Merge remote-tracking branch 'origin/master' into qos-min

CONTRIBUTING.md

@@ -11,3 +11,8 @@ be under the Apache 2 License, as dictated by that
    the terms of any separate license agreement you may have executed
    with Licensor regarding such Contributions.
 ~~~
+
+Contributors must sign-off each commit by adding a `Signed-off-by: ...`
+line to commit messages to certify that they have the right to submit
+the code they are contributing to the project according to the
+[Developer Certificate of Origin (DCO)](https://developercertificate.org/).

rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp

@@ -78,7 +78,6 @@ class MultiThreadedExecutor : public executor::Executor
   size_t number_of_threads_;
   bool yield_before_execute_;
 
-  std::mutex scheduled_timers_mutex_;
   std::set<TimerBase::SharedPtr> scheduled_timers_;
 };
 

rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp

@@ -82,7 +82,6 @@ MultiThreadedExecutor::run(size_t)
       }
       if (any_exec.timer) {
         // Guard against multiple threads getting the same timer.
-        std::lock_guard<std::mutex> lock(scheduled_timers_mutex_);
         if (scheduled_timers_.count(any_exec.timer) != 0) {
           continue;
         }
@@ -96,7 +95,7 @@ MultiThreadedExecutor::run(size_t)
     execute_any_executable(any_exec);
 
     if (any_exec.timer) {
-      std::lock_guard<std::mutex> lock(scheduled_timers_mutex_);
+      std::lock_guard<std::mutex> wait_lock(wait_mutex_);
       auto it = scheduled_timers_.find(any_exec.timer);
       if (it != scheduled_timers_.end()) {
         scheduled_timers_.erase(it);

rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp

@@ -43,6 +43,7 @@
 #include "rclcpp/node_interfaces/node_logging_interface.hpp"
 #include "rclcpp/node_interfaces/node_parameters_interface.hpp"
 #include "rclcpp/node_interfaces/node_services_interface.hpp"
+#include "rclcpp/node_interfaces/node_time_source_interface.hpp"
 #include "rclcpp/node_interfaces/node_timers_interface.hpp"
 #include "rclcpp/node_interfaces/node_topics_interface.hpp"
 #include "rclcpp/node_interfaces/node_waitables_interface.hpp"
@@ -254,6 +255,26 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface,
   rcl_interfaces::msg::SetParametersResult
   set_parameters_atomically(const std::vector<rclcpp::Parameter> & parameters);
 
+  template<typename ParameterT>
+  void
+  set_parameter_if_not_set(
+    const std::string & name,
+    const ParameterT & value);
+
+  /// Set a map of parameters with the same prefix.
+  /**
+   * For each key in the map, a parameter with a name of "name.key" will be set
+   * to the value in the map.
+   *
+   * \param[in] name The prefix of the parameters to set.
+   * \param[in] values The parameters to set in the given prefix.
+   */
+  template<typename MapValueT>
+  void
+  set_parameters_if_not_set(
+    const std::string & name,
+    const std::map<std::string, MapValueT> & values);
+
   RCLCPP_LIFECYCLE_PUBLIC
   std::vector<rclcpp::Parameter>
   get_parameters(const std::vector<std::string> & names) const;
@@ -272,6 +293,49 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface,
   bool
   get_parameter(const std::string & name, ParameterT & parameter) const;
 
+  /// Assign the value of the map parameter if set into the values argument.
+  /**
+   * Parameter names that are part of a map are of the form "name.member".
+   * This API gets all parameters that begin with "name", storing them into the
+   * map with the name of the parameter and their value.
+   * If there are no members in the named map, then the "values" argument is not changed.
+   *
+   * \param[in] name The prefix of the parameters to get.
+   * \param[out] values The map of output values, with one std::string,MapValueT
+   *                    per parameter.
+   * \returns true if values was changed, false otherwise
+   */
+  template<typename MapValueT>
+  bool
+  get_parameters(
+    const std::string & name,
+    std::map<std::string, MapValueT> & values) const;
+
+  template<typename ParameterT>
+  bool
+  get_parameter_or(
+    const std::string & name,
+    ParameterT & value,
+    const ParameterT & alternative_value) const;
+
+  /// Get the parameter value; if not set, set the "alternative value" and store it in the node.
+  /**
+   * If the parameter is set, then the "value" argument is assigned the value
+   * in the parameter.
+   * If the parameter is not set, then the "value" argument is assigned the "alternative_value",
+   * and the parameter is set to the "alternative_value" on the node.
+   *
+   * \param[in] name The name of the parameter to get.
+   * \param[out] value The output where the value of the parameter should be assigned.
+   * \param[in] alternative_value Value to be stored in output and parameter if the parameter was not set.
+   */
+  template<typename ParameterT>
+  void
+  get_parameter_or_set(
+    const std::string & name,
+    ParameterT & value,
+    const ParameterT & alternative_value);
+
   RCLCPP_LIFECYCLE_PUBLIC
   std::vector<rcl_interfaces::msg::ParameterDescriptor>
   describe_parameters(const std::vector<std::string> & names) const;
@@ -383,6 +447,11 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface,
   rclcpp::node_interfaces::NodeParametersInterface::SharedPtr
   get_node_parameters_interface();
 
+  /// Return the Node's internal NodeParametersInterface implementation.
+  RCLCPP_LIFECYCLE_PUBLIC
+  rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr
+  get_node_time_source_interface();
+
   /// Return the Node's internal NodeWaitablesInterface implementation.
   RCLCPP_LIFECYCLE_PUBLIC
   rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr
@@ -513,6 +582,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface,
   rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_;
   rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_;
   rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_;
+  rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr node_time_source_;
   rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_;
 
   bool use_intra_process_comms_;

rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp

@@ -15,9 +15,11 @@
 #ifndef RCLCPP_LIFECYCLE__LIFECYCLE_NODE_IMPL_HPP_
 #define RCLCPP_LIFECYCLE__LIFECYCLE_NODE_IMPL_HPP_
 
+#include <map>
 #include <memory>
 #include <string>
 #include <utility>
+#include <vector>
 
 #include "rclcpp/contexts/default_context.hpp"
 #include "rclcpp/intra_process_manager.hpp"
@@ -205,5 +207,91 @@ LifecycleNode::register_param_change_callback(CallbackT && callback)
   this->node_parameters_->register_param_change_callback(std::forward<CallbackT>(callback));
 }
 
+template<typename ParameterT>
+void
+LifecycleNode::set_parameter_if_not_set(
+  const std::string & name,
+  const ParameterT & value)
+{
+  rclcpp::Parameter parameter;
+  if (!this->get_parameter(name, parameter)) {
+    this->set_parameters({
+        rclcpp::Parameter(name, value),
+      });
+  }
+}
+
+// this is a partially-specialized version of set_parameter_if_not_set above,
+// where our concrete type for ParameterT is std::map, but the to-be-determined
+// type is the value in the map.
+template<typename MapValueT>
+void
+LifecycleNode::set_parameters_if_not_set(
+  const std::string & name,
+  const std::map<std::string, MapValueT> & values)
+{
+  std::vector<rclcpp::Parameter> params;
+
+  for (const auto & val : values) {
+    std::string param_name = name + "." + val.first;
+    rclcpp::Parameter parameter;
+    if (!this->get_parameter(param_name, parameter)) {
+      params.push_back(rclcpp::Parameter(param_name, val.second));
+    }
+  }
+
+  this->set_parameters(params);
+}
+
+// this is a partially-specialized version of get_parameter above,
+// where our concrete type for ParameterT is std::map, but the to-be-determined
+// type is the value in the map.
+template<typename MapValueT>
+bool
+LifecycleNode::get_parameters(
+  const std::string & name,
+  std::map<std::string, MapValueT> & values) const
+{
+  std::map<std::string, rclcpp::Parameter> params;
+  bool result = node_parameters_->get_parameters_by_prefix(name, params);
+  if (result) {
+    for (const auto & param : params) {
+      values[param.first] = param.second.get_value<MapValueT>();
+    }
+  }
+
+  return result;
+}
+
+template<typename ParameterT>
+bool
+LifecycleNode::get_parameter_or(
+  const std::string & name,
+  ParameterT & value,
+  const ParameterT & alternative_value) const
+{
+  bool got_parameter = get_parameter(name, value);
+  if (!got_parameter) {
+    value = alternative_value;
+  }
+  return got_parameter;
+}
+
+template<typename ParameterT>
+void
+LifecycleNode::get_parameter_or_set(
+  const std::string & name,
+  ParameterT & value,
+  const ParameterT & alternative_value)
+{
+  bool got_parameter = get_parameter(name, value);
+  if (!got_parameter) {
+    this->set_parameters({
+        rclcpp::Parameter(name, alternative_value),
+      });
+    value = alternative_value;
+  }
+}
+
 }  // namespace rclcpp_lifecycle
 #endif  // RCLCPP_LIFECYCLE__LIFECYCLE_NODE_IMPL_HPP_

rclcpp_lifecycle/src/lifecycle_node.cpp

@@ -33,6 +33,7 @@
 #include "rclcpp/node_interfaces/node_logging.hpp"
 #include "rclcpp/node_interfaces/node_parameters.hpp"
 #include "rclcpp/node_interfaces/node_services.hpp"
+#include "rclcpp/node_interfaces/node_time_source.hpp"
 #include "rclcpp/node_interfaces/node_timers.hpp"
 #include "rclcpp/node_interfaces/node_topics.hpp"
 #include "rclcpp/node_interfaces/node_waitables.hpp"
@@ -81,6 +82,15 @@ LifecycleNode::LifecycleNode(
       options.start_parameter_event_publisher(),
       options.parameter_event_qos_profile()
     )),
+  node_time_source_(new rclcpp::node_interfaces::NodeTimeSource(
+      node_base_,
+      node_topics_,
+      node_graph_,
+      node_services_,
+      node_logging_,
+      node_clock_,
+      node_parameters_
+    )),
   node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())),
   use_intra_process_comms_(options.use_intra_process_comms()),
   impl_(new LifecycleNodeInterfaceImpl(node_base_, node_services_))
@@ -274,6 +284,12 @@ LifecycleNode::get_node_logging_interface()
   return node_logging_;
 }
 
+rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr
+LifecycleNode::get_node_time_source_interface()
+{
+  return node_time_source_;
+}
+
 rclcpp::node_interfaces::NodeTimersInterface::SharedPtr
 LifecycleNode::get_node_timers_interface()
 {