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 <zelenak@picknik.ai> * 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 <destogl@users.noreply.github.com> Co-authored-by: AndyZe <zelenak@picknik.ai> Co-authored-by: bijoua <bijou.abraham@technipfmc.com> Co-authored-by: bijoua29 <73511637+bijoua29@users.noreply.github.com>
pal-robotics-forks · Mar 16, 2023 · ee7faf2 · ee7faf2
1 parent bbd9d59
commit ee7faf2
Show file tree

Hide file tree

Showing 15 changed files with 128 additions and 402 deletions.
diff --git a/controller_interface/CMakeLists.txt b/controller_interface/CMakeLists.txt
@@ -11,6 +11,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
 )
 
 find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
 foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
   find_package(${Dependency} REQUIRED)
 endforeach()

diff --git a/controller_interface/include/controller_interface/controller_parameters.hpp b/controller_interface/include/controller_interface/controller_parameters.hpp
diff --git a/controller_interface/package.xml b/controller_interface/package.xml
@@ -10,6 +10,7 @@
 
   <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>

diff --git a/controller_interface/src/controller_parameters.cpp b/controller_interface/src/controller_parameters.cpp
diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt
@@ -17,6 +17,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
 )
 
 find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
 foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
   find_package(${Dependency} REQUIRED)
 endforeach()

diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml
@@ -9,6 +9,7 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <depend>backward_ros</depend>
   <depend>control_msgs</depend>
   <depend>lifecycle_msgs</depend>
   <depend>pluginlib</depend>

diff --git a/joint_limits/CMakeLists.txt b/joint_limits/CMakeLists.txt
@@ -13,6 +13,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
 )
 
 find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
 foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
   find_package(${Dependency} REQUIRED)
 endforeach()

diff --git a/joint_limits/include/joint_limits/joint_limiter_interface.hpp b/joint_limits/include/joint_limits/joint_limiter_interface.hpp
@@ -84,6 +84,7 @@ class JointLimiterInterface
 
 protected:
   size_t number_of_joints_;
+  std::vector<std::string> joint_names_;
   std::vector<LimitsType> joint_limits_;
   rclcpp::Node::SharedPtr node_;
 };

diff --git a/joint_limits/package.xml b/joint_limits/package.xml
@@ -14,6 +14,7 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <depend>backward_ros</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_lifecycle</depend>

diff --git a/joint_limits/src/joint_limiter_interface.cpp b/joint_limits/src/joint_limiter_interface.cpp
@@ -30,6 +30,7 @@ bool JointLimiterInterface<JointLimits>::init(
   const std::string & /*robot_description_topic*/)
 {
   number_of_joints_ = joint_names.size();
+  joint_names_ = joint_names;
   joint_limits_.resize(number_of_joints_);
   node_ = node;