Add parameter-related templates to LifecycleNode (#645)

* Add parameter-related templates to LifecycleNode

Signed-off-by: vinnamkim <vinnam.kim@gmail.com>

* Update rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp

Co-Authored-By: vinnamkim <vinnam.kim@gmail.com>

* Update rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp

rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp

@@ -255,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;
@@ -273,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;

rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp

@@ -15,6 +15,7 @@
 #ifndef RCLCPP_LIFECYCLE__LIFECYCLE_NODE_IMPL_HPP_
 #define RCLCPP_LIFECYCLE__LIFECYCLE_NODE_IMPL_HPP_
 
+#include <map>
 #include <memory>
 #include <string>
 #include <utility>
@@ -201,5 +202,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_