Modify simple joint limiting plugin (same as changes to moveit2 filte…

…r) (#6) * Merge error handling possilibity on read and write. * Ros2 control extensions rolling joint limits plugins (#5) * Added initial structures for joint-limit plugins. * Move Ruckig limiter to package joint_limits_enforcement_plugins and make it working. Co-authored-by: AndyZe <[email protected]> * Add option to automatically update parameters after getting them from parameter server. * Modify simple joint limiting plugin (same as changes to moveit2 filter) * Add backward_ros dependency for crash stack trace * Check for required inputs in simple joint limiter * Change services history QOS to 'keep all' so client req are not dropped * Add missing on 'pluginlib' dependency explicitly. * Update ControllerParameters structure to support custom prefix and use in filters. * Update messge. * Change controller param changed msg log level to info instead of error --------- Co-authored-by: Denis Štogl <[email protected]> Co-authored-by: AndyZe <[email protected]> Co-authored-by: bijoua <[email protected]> Co-authored-by: bijoua29 <[email protected]>
destogl · Feb 9, 2023 · a667d5d · a667d5d
1 parent 2c73d4b
commit a667d5d
Show file tree

Hide file tree

Showing 25 changed files with 846 additions and 189 deletions.
diff --git a/controller_interface/CMakeLists.txt b/controller_interface/CMakeLists.txt
@@ -6,6 +6,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
 endif()
 
 find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
 find_package(hardware_interface REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
 

diff --git a/controller_interface/include/controller_interface/controller_parameters.hpp b/controller_interface/include/controller_interface/controller_parameters.hpp
@@ -27,106 +27,147 @@
 
 namespace controller_interface
 {
+using rcl_interfaces::msg::ParameterType;
+
+namespace impl
+{
+  inline std::string normalize_params_prefix(std::string prefix)
+  {
+    if (!prefix.empty()) {
+      if ('.' != prefix.back()) {
+        prefix += '.';
+      }
+    }
+        return prefix;
+  }
+}  // namespace impl
+
 struct Parameter
 {
   Parameter() = default;
 
-  explicit Parameter(const std::string & name, bool configurable = false)
-  : name(name), configurable(configurable)
+  explicit Parameter(const std::string & name, const uint8_t type, const bool configurable = false)
+  : name(name), type(type), configurable(configurable)
   {
   }
 
   std::string name;
+  uint8_t type;
   bool configurable;
 };
 
 class ControllerParameters
 {
 public:
   ControllerParameters(
+    const std::string & params_prefix = "",
     int nr_bool_params = 0, int nr_double_params = 0, int nr_string_params = 0,
-    int nr_string_list_params = 0);
+    int nr_double_array_params = 0, int nr_string_list_params = 0);
 
   virtual ~ControllerParameters() = default;
 
-  virtual void declare_parameters(rclcpp::Node::SharedPtr node);
+  void declare_parameters(rclcpp::Node::SharedPtr node);
+
+  void declare_parameters(
+    const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & node_logger,
+    const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_params);
 
   /**
-   * Gets all defined parameters from.
+   * Gets all defined parameters from parameter server. If parameters are not declared, they will
+   * be declared first.
+   * See also documentation of another `get_parameters` method.
    *
-   * \param[node] shared pointer to the node where parameters should be read.
-   * \return true if all parameters are read Successfully, false if a parameter is not provided or
+   * \param[in] node shared pointer to the node where parameters should be read.
+   * \return true if all parameters are read successfully, false if a parameter is not provided or
    * parameter configuration is wrong.
    */
-  virtual bool get_parameters(rclcpp::Node::SharedPtr node, bool check_validity = true);
+  bool get_parameters(rclcpp::Node::SharedPtr node, bool check_validity = true, bool update = true);
 
-  virtual rcl_interfaces::msg::SetParametersResult set_parameter_callback(
+  /**
+   * Gets all defined parameters from parameter server after parameters are declared.
+   * Optionally, check parameters' validity and update storage.
+   *
+   * \param[in] check_validity check validity after getting parameters (default: **true**).
+   * \param[in] update update parameters in storage after getting them (default: **true**).
+   * \return true if all parameters are read successfully, false if a parameter is not provided or
+   * parameter configuration is wrong.
+   */
+  bool get_parameters(bool check_validity = true, bool update = true);
+
+  /**
+   * Update all storage variables from the parameter maps.
+   * Parameters will be only updated if they are previously read from parameter server or dynamic
+   * reconfigure callback occurred.
+   *
+   * \param[in] check_validity check validity before updating parameters (default: **true**).
+   * \return is update was successful, i.e., parameters are valid. If \p check_validity is set
+   * **false**, the result is always **true**.
+   */
+  bool update(bool check_validity = true);
+
+  rcl_interfaces::msg::SetParametersResult set_parameter_callback(
     const std::vector<rclcpp::Parameter> & parameters);
 
+protected:
   /**
-   * Default implementation of parameter check. No check is done. Always returns true.
+   * Override this method to implement custom parameters check.
+   * Default implementation does not checks anything, always returns true.
    *
-   * \return true
+   * \return **true** if parameters are valid, **false** otherwise.
    */
   virtual bool check_if_parameters_are_valid() { return true; }
 
-  virtual void update() = 0;
-
-  // TODO(destogl): Test this: "const rclcpp::Node::SharedPtr & node"
+  /**
+   * Mapping between parameter storage and implementation members.
+   * The order of parameter in storage is the order of adding parameters to the storage.
+   * E.g., to access the value of 5-th string parameter added to storage use:
+   * `fifth_string_param_ = string_parameters_[4].second;
+   * where `fifth_string_param_` is the name of the member of a child class.
+   */
+  virtual void update_storage() = 0;
 
 protected:
   void add_bool_parameter(
-    const std::string & name, bool configurable = false, bool default_value = false)
+    const std::string & name, const bool & configurable = false, const bool & default_value = false)
   {
-    bool_parameters_.push_back({Parameter(name, configurable), default_value});
+    bool_parameters_.push_back({
+      Parameter(params_prefix_ + name, ParameterType::PARAMETER_BOOL, configurable), default_value});
   }
 
   void add_double_parameter(
-    const std::string & name, bool configurable = false,
-    double default_value = std::numeric_limits<double>::quiet_NaN())
+    const std::string & name, const bool & configurable = false,
+    const double & default_value = std::numeric_limits<double>::quiet_NaN())
   {
-    double_parameters_.push_back({Parameter(name, configurable), default_value});
+    double_parameters_.push_back({
+      Parameter(params_prefix_ + name, ParameterType::PARAMETER_DOUBLE, configurable), default_value});
   }
 
   void add_string_parameter(
-    const std::string & name, bool configurable = false, const std::string & default_value = "")
+    const std::string & name, const bool & configurable = false,
+    const std::string & default_value = "")
   {
-    string_parameters_.push_back({Parameter(name, configurable), default_value});
+    string_parameters_.push_back({
+      Parameter(params_prefix_ + name, ParameterType::PARAMETER_STRING, configurable), default_value});
   }
 
-  void add_string_list_parameter(
-    const std::string & name, bool configurable = false,
-    const std::vector<std::string> & default_value = {})
+  void add_double_array_parameter(
+    const std::string & name, const bool & configurable = false,
+    const std::vector<double> & default_value = {std::numeric_limits<double>::quiet_NaN()})
   {
-    string_list_parameters_.push_back({Parameter(name, configurable), default_value});
+    double_array_parameters_.push_back({
+      Parameter(params_prefix_ + name, ParameterType::PARAMETER_DOUBLE_ARRAY, configurable), default_value});
   }
 
-  template <typename ParameterT>
-  bool get_parameters_from_list(
-    const rclcpp::Node::SharedPtr node, std::vector<std::pair<Parameter, ParameterT>> & parameters)
+    void add_string_list_parameter(
+    const std::string & name, const bool & configurable = false,
+    const std::vector<std::string> & default_value = {})
   {
-    bool ret = true;
-    for (auto & parameter : parameters)
-    {
-      try
-      {
-        rclcpp::Parameter input_parameter;  // TODO(destogl): will this be allocated each time?
-        ret = node->get_parameter(parameter.first.name, input_parameter);
-        parameter.second = input_parameter.get_value<ParameterT>();
-      }
-      catch (rclcpp::ParameterTypeException & e)
-      {
-        RCUTILS_LOG_ERROR_NAMED(
-          logger_name_.c_str(), "'%s' parameter has wrong type", parameter.first.name.c_str());
-        ret = false;
-        break;
-      }
-    }
-    return ret;
+    string_list_parameters_.push_back({
+      Parameter(params_prefix_ + name, ParameterType::PARAMETER_STRING_ARRAY, configurable), default_value});
   }
 
-  template <typename ParameterT>
-  bool empty_parameter_in_list(const std::vector<std::pair<Parameter, ParameterT>> & parameters)
+  template <typename PT>
+  bool empty_parameter_in_list(const std::vector<std::pair<Parameter, PT>> & parameters)
   {
     bool ret = false;
     for (const auto & parameter : parameters)
@@ -156,9 +197,9 @@ class ControllerParameters
     return ret;
   }
 
-  template <typename ParameterT>
+  template <typename PT>
   bool find_and_assign_parameter_value(
-    std::vector<std::pair<Parameter, ParameterT>> & parameter_list,
+    std::vector<std::pair<Parameter, PT>> & parameter_list,
     const rclcpp::Parameter & input_parameter)
   {
     auto is_in_list = [&](const auto & parameter) {
@@ -176,8 +217,8 @@ class ControllerParameters
       }
       else
       {
-        it->second = input_parameter.get_value<ParameterT>();
-        RCUTILS_LOG_ERROR_NAMED(
+        it->second = input_parameter.get_value<PT>();
+        RCUTILS_LOG_INFO_NAMED(
           logger_name_.c_str(), "'%s' parameter is updated to value: %s", it->first.name.c_str(),
           input_parameter.value_to_string().c_str());
         return true;
@@ -189,22 +230,97 @@ class ControllerParameters
     }
   }
 
+  // Storage members
   std::vector<std::pair<Parameter, bool>> bool_parameters_;
   std::vector<std::pair<Parameter, double>> double_parameters_;
+  std::vector<std::pair<Parameter, std::vector<double>>> double_array_parameters_;
   std::vector<std::pair<Parameter, std::string>> string_parameters_;
   std::vector<std::pair<Parameter, std::vector<std::string>>> string_list_parameters_;
 
+  // Input/Output members to ROS 2
   std::string logger_name_;
+  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr params_interface_;
+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface_;
+
+  bool declared_;
+  bool up_to_date_;
+  std::string params_prefix_;
 
 private:
-  template <typename ParameterT>
+  template <typename PT>
   void declare_parameters_from_list(
-    rclcpp::Node::SharedPtr node, const std::vector<std::pair<Parameter, ParameterT>> & parameters)
+    rclcpp::Node::SharedPtr node, const std::vector<std::pair<Parameter, PT>> & parameters)
+  {
+    for (const auto & parameter : parameters)
+    {
+      node->declare_parameter<PT>(parameter.first.name, parameter.second);
+    }
+  }
+
+  template <typename PT>
+  void declare_parameters_from_list(const std::vector<std::pair<Parameter, PT>> & parameters)
   {
     for (const auto & parameter : parameters)
     {
-      node->declare_parameter<ParameterT>(parameter.first.name, parameter.second);
+      // declare parameter only if it does not already exists
+      if (!params_interface_->has_parameter(parameter.first.name))
+      {
+        rclcpp::ParameterValue default_parameter_value(parameter.second);
+        rcl_interfaces::msg::ParameterDescriptor descr;
+        descr.name = parameter.first.name;
+        descr.type = parameter.first.type;
+        descr.read_only = false;
+
+        params_interface_->declare_parameter(parameter.first.name, default_parameter_value, descr);
+      }
+    }
+  }
+
+  template <typename PT>
+  bool get_parameters_from_list(
+    const rclcpp::Node::SharedPtr node, std::vector<std::pair<Parameter, PT>> & parameters)
+  {
+    bool ret = true;
+    for (auto & parameter : parameters)
+    {
+      try
+      {
+        rclcpp::Parameter input_parameter;  // TODO(destogl): will this be allocated each time?
+        ret = node->get_parameter(parameter.first.name, input_parameter);
+        parameter.second = input_parameter.get_value<PT>();
+      }
+      catch (rclcpp::ParameterTypeException & e)
+      {
+        RCUTILS_LOG_ERROR_NAMED(
+          logger_name_.c_str(), "'%s' parameter has wrong type", parameter.first.name.c_str());
+        ret = false;
+        break;
+      }
+    }
+    return ret;
+  }
+
+  template <typename PT>
+  bool get_parameters_from_list(std::vector<std::pair<Parameter, PT>> & parameters)
+  {
+    bool ret = true;
+    for (auto & parameter : parameters)
+    {
+      try
+      {
+        rclcpp::Parameter input_parameter;  // TODO(destogl): will this be allocated each time?
+        ret = params_interface_->get_parameter(parameter.first.name, input_parameter);
+        parameter.second = input_parameter.get_value<PT>();
+      }
+      catch (rclcpp::ParameterTypeException & e)
+      {
+        RCUTILS_LOG_ERROR_NAMED(
+          logger_name_.c_str(), "'%s' parameter has wrong type", parameter.first.name.c_str());
+        ret = false;
+        break;
+      }
     }
+    return ret;
   }
 };
 

diff --git a/controller_interface/package.xml b/controller_interface/package.xml
@@ -10,10 +10,12 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <build_depend>backward_ros</build_depend>
   <build_depend>hardware_interface</build_depend>
   <build_depend>rclcpp_lifecycle</build_depend>
   <build_depend>sensor_msgs</build_depend>
 
+  <exec_depend>backward_ros</exec_depend>
   <exec_depend>hardware_interface</exec_depend>
   <exec_depend>rclcpp_lifecycle</exec_depend>
   <exec_depend>sensor_msgs</exec_depend>