Refactor MoveIt Simple Controller Manager to use generate param library

moveit_plugins/moveit_simple_controller_manager/CMakeLists.txt

@@ -11,12 +11,18 @@ find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(control_msgs REQUIRED)
 find_package(rclcpp_action REQUIRED)
+find_package(generate_parameter_library REQUIRED)
 
 # Finds Boost Components
 include(ConfigExtras.cmake)
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS control_msgs rclcpp rclcpp_action moveit_core
-                                 pluginlib)
+                                 pluginlib generate_parameter_library)
+
+generate_parameter_library(
+  moveit_simple_controller_manager_parameters # cmake target name for the parameter library
+  src/moveit_simple_controller_manager_parameters.yaml # path to input yaml file
+)
 
 include_directories(include)
 
@@ -32,6 +38,16 @@ set_target_properties(
 ament_target_dependencies(moveit_simple_controller_manager
                           ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost)
 
+target_link_libraries(moveit_simple_controller_manager moveit_simple_controller_manager_parameters)
+
+install(
+  TARGETS moveit_simple_controller_manager_parameters
+  EXPORT ${PROJECT_NAME}Targets
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
 install(
   TARGETS moveit_simple_controller_manager
   EXPORT moveit_simple_controller_managerTargets

moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/follow_joint_trajectory_controller_handle.h

@@ -63,6 +63,11 @@ class FollowJointTrajectoryControllerHandle
 
   // TODO(JafarAbdi): Revise parameter lookup
   // void configure(XmlRpc::XmlRpcValue& config) override;
+  void setPathTolerance(double path_tolerance);
+
+  void setGoalTolerance(double goal_tolerance);
+
+  void setGoalTimeTolerance(double goal_time_tolerance);
 
 protected:
   static control_msgs::msg::JointTolerance& getTolerance(std::vector<control_msgs::msg::JointTolerance>& tolerances,

moveit_plugins/moveit_simple_controller_manager/package.xml

@@ -25,6 +25,7 @@
   <depend version_gte="1.11.2">pluginlib</depend>
   <depend>control_msgs</depend>
   <depend>rclcpp_action</depend>
+  <depend>generate_parameter_library</depend>
 
   <export>
     <build_type>ament_cmake</build_type>

moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp

@@ -36,6 +36,7 @@
 /* Author: Michael Ferguson, Ioan Sucan, E. Gil Jones */
 
 #include <moveit_simple_controller_manager/follow_joint_trajectory_controller_handle.h>
+#include <rclcpp/duration.hpp>
 
 using namespace std::placeholders;
 
@@ -97,6 +98,19 @@ bool FollowJointTrajectoryControllerHandle::sendTrajectory(const moveit_msgs::ms
   return true;
 }
 
+void FollowJointTrajectoryControllerHandle::setPathTolerance(double /*path_tolerance*/)
+{
+}
+
+void FollowJointTrajectoryControllerHandle::setGoalTolerance(double /*goal_tolerance*/)
+{
+}
+
+void FollowJointTrajectoryControllerHandle::setGoalTimeTolerance(double goal_time_tolerance)
+{
+  goal_template_.goal_time_tolerance = rclcpp::Duration::from_seconds(goal_time_tolerance);
+}
+
 // TODO(JafarAbdi): Revise parameter lookup
 // void FollowJointTrajectoryControllerHandle::configure(XmlRpc::XmlRpcValue& config)
 //{

moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp

@@ -49,6 +49,8 @@
 #include <map>
 #include <moveit/utils/logger.hpp>
 
+#include <moveit_simple_controller_manager_parameters.hpp>
+
 namespace
 {
 /**
@@ -101,62 +103,33 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo
   void initialize(const rclcpp::Node::SharedPtr& node) override
   {
     node_ = node;
-    if (!node_->has_parameter(makeParameterName(PARAM_BASE_NAME, "controller_names")))
-    {
-      RCLCPP_ERROR_STREAM(getLogger(), "No controller_names specified.");
-      return;
-    }
-    rclcpp::Parameter controller_names_param;
-    node_->get_parameter(makeParameterName(PARAM_BASE_NAME, "controller_names"), controller_names_param);
-    if (controller_names_param.get_type() != rclcpp::ParameterType::PARAMETER_STRING_ARRAY)
+    try
     {
-      RCLCPP_ERROR(getLogger(), "Parameter controller_names should be specified as a string array");
-      return;
-    }
-    std::vector<std::string> controller_names = controller_names_param.as_string_array();
-    /* actually create each controller */
-    for (const std::string& controller_name : controller_names)
-    {
-      try
-      {
-        std::string action_ns;
-        const std::string& action_ns_param = makeParameterName(PARAM_BASE_NAME, controller_name, "action_ns");
-        if (!node_->get_parameter(action_ns_param, action_ns))
-        {
-          RCLCPP_ERROR_STREAM(getLogger(), "No action namespace specified for controller `"
-                                               << controller_name << "` through parameter `" << action_ns_param << '`');
-          continue;
-        }
+      const auto controller_manager_param_prefix = "moveit_simple_controller_manager";
+      param_listener_ =
+          std::make_shared<moveit_simple_controller_manager::ParamListener>(node_, controller_manager_param_prefix);
+      params_ = param_listener_->get_params();
 
-        std::string type;
-        if (!node_->get_parameter(makeParameterName(PARAM_BASE_NAME, controller_name, "type"), type))
-        {
-          RCLCPP_ERROR_STREAM(getLogger(), "No type specified for controller " << controller_name);
-          continue;
-        }
+      std::vector<std::string> controller_names = params_.controller_names;
 
-        std::vector<std::string> controller_joints;
-        if (!node_->get_parameter(makeParameterName(PARAM_BASE_NAME, controller_name, "joints"), controller_joints) ||
-            controller_joints.empty())
-        {
-          RCLCPP_ERROR_STREAM(getLogger(), "No joints specified for controller " << controller_name);
-          continue;
-        }
+      /* actually create each controller */
+      for (const auto& controller : params_.controller_names_map)
+      {
+        const std::string controller_name = controller.first;
+        const std::string action_ns = controller.second.action_ns;
+        const std::string controller_type = controller.second.type;
+        const std::vector<std::string> controller_joints = controller.second.joints;
 
         ActionBasedControllerHandleBasePtr new_handle;
-        if (type == "GripperCommand")
+        if (controller_type == "GripperCommand")
         {
-          double max_effort;
-          const std::string& max_effort_param = makeParameterName(PARAM_BASE_NAME, controller_name, "max_effort");
-          if (!node->get_parameter(max_effort_param, max_effort))
-          {
-            RCLCPP_INFO_STREAM(getLogger(), "Max effort set to 0.0");
-            max_effort = 0.0;
-          }
+          const double max_effort = controller.second.max_effort;
+          RCLCPP_INFO_STREAM(LOGGER, "Max effort set to " << max_effort);
 
           new_handle = std::make_shared<GripperControllerHandle>(node_, controller_name, action_ns, max_effort);
-          bool parallel_gripper = false;
-          if (node_->get_parameter(makeParameterName(PARAM_BASE_NAME, "parallel"), parallel_gripper) && parallel_gripper)
+
+          bool parallel_gripper = controller.second.parallel;
+          if (parallel_gripper)
           {
             if (controller_joints.size() != 2)
             {
@@ -169,23 +142,28 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo
           }
           else
           {
-            std::string command_joint;
-            if (!node_->get_parameter(makeParameterName(PARAM_BASE_NAME, "command_joint"), command_joint))
+            std::string command_joint = controller.second.command_joint;
+            if (command_joint.empty())
+            {
               command_joint = controller_joints[0];
-
+            }
             static_cast<GripperControllerHandle*>(new_handle.get())->setCommandJoint(command_joint);
           }
 
-          bool allow_failure;
-          node_->get_parameter_or(makeParameterName(PARAM_BASE_NAME, "allow_failure"), allow_failure, false);
+          const bool allow_failure = controller.second.allow_failure;
           static_cast<GripperControllerHandle*>(new_handle.get())->allowFailure(allow_failure);
 
           RCLCPP_INFO_STREAM(getLogger(), "Added GripperCommand controller for " << controller_name);
           controllers_[controller_name] = new_handle;
         }
-        else if (type == "FollowJointTrajectory")
+        else if (controller_type == "FollowJointTrajectory")
         {
           new_handle = std::make_shared<FollowJointTrajectoryControllerHandle>(node_, controller_name, action_ns);
+
+          // static_cast<FollowJointTrajectoryControllerHandle*>(new_handle.get())->setPathTolerance(controller.second.path_tolerance);
+          // static_cast<FollowJointTrajectoryControllerHandle*>(new_handle.get())->setGoalTolerance(controller.second.goal_tolerance);
+          static_cast<FollowJointTrajectoryControllerHandle*>(new_handle.get())
+              ->setGoalTimeTolerance(controller.second.goal_time_tolerance);
           RCLCPP_INFO_STREAM(getLogger(), "Added FollowJointTrajectory controller for " << controller_name);
           controllers_[controller_name] = new_handle;
         }
@@ -201,8 +179,7 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo
         }
 
         moveit_controller_manager::MoveItControllerManager::ControllerState state;
-        node_->get_parameter_or(makeParameterName(PARAM_BASE_NAME, controller_name, "default"), state.default_, false);
-        state.active_ = true;
+        state.active_ = controller.second.make_default;
 
         controller_states_[controller_name] = state;
 
@@ -216,6 +193,7 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo
       }
     }
   }
+
   /*
    * Get a controller, by controller name (which was specified in the controllers.yaml
    */
@@ -298,6 +276,9 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo
   rclcpp::Node::SharedPtr node_;
   std::map<std::string, ActionBasedControllerHandleBasePtr> controllers_;
   std::map<std::string, moveit_controller_manager::MoveItControllerManager::ControllerState> controller_states_;
+
+  std::shared_ptr<moveit_simple_controller_manager::ParamListener> param_listener_;
+  moveit_simple_controller_manager::Params params_;
 };
 
 }  // end namespace moveit_simple_controller_manager

moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager_parameters.yaml

@@ -0,0 +1,103 @@
+moveit_simple_controller_manager:
+  controller_names: {
+    type: string_array,
+    default_value: [],
+    description: "List of controllers for MoveIt's controller manager",
+    validation: {
+      not_empty<>: []
+    }
+  }
+  __map_controller_names:
+    action_ns: {
+      type: string,
+      default_value: "",
+      description: "Action namespace for the controller. The action topic names will start with `/controller_name/action_ns`",
+      validation: {
+        not_empty<>: []
+      }
+    }
+    type: {
+      type: string,
+      default_value: "",
+      description: "Controller type. FollowJointTrajectory or GripperCommand",
+      validation: {
+        not_empty<>: []
+      }
+    }
+    make_default: {
+      type: bool,
+      default_value: false,
+      description: "This param was originally named default. Marking a controller as
+       default makes MoveIt prefer this controller when multiple options are available.",
+      validation: {
+        not_empty<>: []
+      }
+    }
+    joints: {
+      type: string_array,
+      default_value: [],
+      description: "list of joints to actuate",
+      validation: {
+        not_empty<>: []
+      }
+    }
+    # parameters for gripper controllers
+    max_effort: {
+      type: double,
+      default_value: 0.0,
+      description: "Max Effort. Parameter to be set if using GripperCommand"
+    }
+    parallel: {
+      type: bool,
+      default_value: false,
+      description: "Parallel gripper require exactly two joints. If true, specify only two joints. Parameter to be set if using GripperCommand"
+    }
+    command_joint: {
+      type: string,
+      default_value: "",
+      description: "If not parallel, set the joint to command. Parameter to be set if using GripperCommand"
+    }
+    allow_failure: {
+      type: bool,
+      default_value: false,
+      description: "Parameter to be set if using GripperCommand"
+    }
+    # parameters for follow joint trajectory controllers
+    goal_time_tolerance: {
+      type: double,
+      default_value: 0.0,
+      description: "Parameter to be set if using FollowJointTrajectory"
+    }
+    # __map_joints:
+    #   path_tolerance:
+    #     position: {
+    #       type: double,
+    #       default_value: 0.0,
+    #       description: "Parameter to be set if using FollowJointTrajectory"
+    #     }
+    #     velocity: {
+    #       type: double,
+    #       default_value: 0.0,
+    #       description: "Parameter to be set if using FollowJointTrajectory"
+    #     }
+    #     acceleration: {
+    #       type: double,
+    #       default_value: 0.0,
+    #       description: "Parameter to be set if using FollowJointTrajectory"
+    #     }
+    #   goal_tolerance:
+    #     position: {
+    #       type: double,
+    #       default_value: 0.0,
+    #       description: "Parameter to be set if using FollowJointTrajectory"
+    #     }
+    #     velocity: {
+    #       type: double,
+    #       default_value: 0.0,
+    #       description: "Parameter to be set if using FollowJointTrajectory"
+    #     }
+    #     acceleration: {
+    #       type: double,
+    #       default_value: 0.0,
+    #       description: "Parameter to be set if using FollowJointTrajectory"
+    #     }