Merge branch 'main' into update_ompl_version

moveit · Jan 27, 2025 · 0f71c9c · 0f71c9c
2 parents 255575c + 524ca4f
commit 0f71c9c
Show file tree

Hide file tree

Showing 61 changed files with 531 additions and 217 deletions.
diff --git a/README.md b/README.md
diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt
@@ -69,13 +69,7 @@ add_subdirectory(utils)
 add_subdirectory(version)
 
 install(
-  TARGETS collision_detector_bullet_plugin
-          collision_detector_fcl_plugin
-          moveit_acceleration_filter
-          moveit_acceleration_filter_parameters
-          moveit_butterworth_filter
-          moveit_butterworth_filter_parameters
-          moveit_collision_detection
+  TARGETS moveit_collision_detection
           moveit_collision_detection_bullet
           moveit_collision_detection_fcl
           moveit_collision_distance_field
@@ -92,8 +86,6 @@ install(
           moveit_robot_model
           moveit_robot_state
           moveit_robot_trajectory
-          moveit_ruckig_filter
-          moveit_ruckig_filter_parameters
           moveit_smoothing_base
           moveit_test_utils
           moveit_trajectory_processing
@@ -104,6 +96,20 @@ install(
   ARCHIVE DESTINATION lib
   RUNTIME DESTINATION bin)
 
+install(
+  TARGETS collision_detector_bullet_plugin
+          collision_detector_fcl_plugin
+          moveit_acceleration_filter
+          moveit_acceleration_filter_parameters
+          moveit_butterworth_filter
+          moveit_butterworth_filter_parameters
+          moveit_ruckig_filter
+          moveit_ruckig_filter_parameters
+  EXPORT moveit_core_pluginTargets
+  LIBRARY DESTINATION lib
+  ARCHIVE DESTINATION lib
+  RUNTIME DESTINATION bin)
+
 ament_export_targets(moveit_coreTargets HAS_LIBRARY_TARGET)
 ament_export_dependencies(
   angles

diff --git a/...re/online_signal_smoothing/include/moveit/online_signal_smoothing/acceleration_filter.hpp b/...re/online_signal_smoothing/include/moveit/online_signal_smoothing/acceleration_filter.hpp
@@ -78,7 +78,7 @@ c --------x--- v   |
 #include <moveit/robot_model/robot_model.hpp>
 #include <moveit/robot_state/robot_state.hpp>
 #include <moveit/utils/logger.hpp>
-#include <moveit_acceleration_filter_parameters.hpp>
+#include <moveit_core/moveit_acceleration_filter_parameters.hpp>
 
 #include <osqp.h>
 #include <types.h>

diff --git a/...ore/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.hpp b/...ore/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.hpp
@@ -41,9 +41,9 @@
 
 #include <cstddef>
 
-#include <moveit_butterworth_filter_parameters.hpp>
 #include <moveit/robot_model/robot_model.hpp>
 #include <moveit/online_signal_smoothing/smoothing_base_class.hpp>
+#include <moveit_core/moveit_butterworth_filter_parameters.hpp>
 
 namespace online_signal_smoothing
 {

diff --git a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/ruckig_filter.hpp b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/ruckig_filter.hpp
@@ -42,7 +42,7 @@ Description: Applies jerk/acceleration/velocity limits to online motion commands
 
 #include <moveit/robot_model/robot_model.hpp>
 #include <moveit/online_signal_smoothing/smoothing_base_class.hpp>
-#include <moveit_ruckig_filter_parameters.hpp>
+#include <moveit_core/moveit_ruckig_filter_parameters.hpp>
 
 #include <ruckig/ruckig.hpp>
 

diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp
@@ -2039,8 +2039,8 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL::
   {
     if (consistency_limits[i].size() != sub_groups[i]->getVariableCount())
     {
-      RCLCPP_ERROR(getLogger(), "Number of joints in consistency_limits is %zu but it should be should be %u", i,
-                   sub_groups[i]->getVariableCount());
+      RCLCPP_ERROR(getLogger(), "Number of joints in consistency_limits[%zu] is %lu but it should be should be %u", i,
+                   consistency_limits[i].size(), sub_groups[i]->getVariableCount());
       return false;
     }
   }

diff --git a/moveit_kinematics/CMakeLists.txt b/moveit_kinematics/CMakeLists.txt
@@ -28,12 +28,11 @@ set(THIS_PACKAGE_INCLUDE_DIRS
     cached_ik_kinematics_plugin/include)
 
 set(THIS_PACKAGE_LIBRARIES
-    cached_ik_kinematics_parameters
-    moveit_cached_ik_kinematics_base
-    moveit_cached_ik_kinematics_plugin
-    kdl_kinematics_parameters
-    moveit_kdl_kinematics_plugin
-    srv_kinematics_parameters
+    cached_ik_kinematics_parameters moveit_cached_ik_kinematics_base
+    kdl_kinematics_parameters srv_kinematics_parameters)
+
+set(THIS_PACKAGE_PLUGINS
+    moveit_cached_ik_kinematics_plugin moveit_kdl_kinematics_plugin
     moveit_srv_kinematics_plugin)
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS
@@ -74,6 +73,15 @@ install(
   INCLUDES
   DESTINATION include/moveit_kinematics)
 
+install(
+  TARGETS ${THIS_PACKAGE_PLUGINS}
+  EXPORT ${PROJECT_NAME}PluginTargets
+  LIBRARY DESTINATION lib
+  ARCHIVE DESTINATION lib
+  RUNTIME DESTINATION bin
+  INCLUDES
+  DESTINATION include/moveit_kinematics)
+
 ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET)
 ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
 

diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/CMakeLists.txt b/moveit_kinematics/cached_ik_kinematics_plugin/CMakeLists.txt
@@ -21,7 +21,7 @@ generate_parameter_library(
 
 target_link_libraries(
   moveit_cached_ik_kinematics_base PUBLIC cached_ik_kinematics_parameters
-                                          moveit_kdl_kinematics_plugin)
+                                          kdl_kinematics_parameters)
 
 add_library(moveit_cached_ik_kinematics_plugin SHARED
             src/cached_ik_kinematics_plugin.cpp)

diff --git a/...ematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.hpp b/...ematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.hpp
@@ -55,8 +55,8 @@
 #include <unordered_map>
 #include <utility>
 #include <filesystem>
-#include <cached_ik_kinematics_parameters.hpp>
 #include <moveit/utils/logger.hpp>
+#include <moveit_kinematics/cached_ik_kinematics_parameters.hpp>
 
 namespace cached_ik_kinematics_plugin
 {

diff --git a/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp b/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp
@@ -47,8 +47,8 @@
 #include <Eigen/Geometry>
 #include <tf2_kdl/tf2_kdl.hpp>
 #include <tf2_eigen/tf2_eigen.hpp>
-#include <ikfast_kinematics_parameters.hpp>
 #include <moveit/utils/logger.hpp>
+#include <_PACKAGE_NAME_/ikfast_kinematics_parameters.hpp>
 
 using namespace moveit::core;
 

diff --git a/...tics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.hpp b/...tics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.hpp
@@ -39,7 +39,6 @@
 // ROS
 #include <rclcpp/rclcpp.hpp>
 #include <random_numbers/random_numbers.h>
-#include <kdl_kinematics_parameters.hpp>
 
 // ROS msgs
 #include <geometry_msgs/msg/pose_stamped.hpp>
@@ -58,6 +57,7 @@
 #include <moveit/kdl_kinematics_plugin/joint_mimic.hpp>
 #include <moveit/robot_model/robot_model.hpp>
 #include <moveit/robot_state/robot_state.hpp>
+#include <moveit_kinematics/kdl_kinematics_parameters.hpp>
 
 #include <cfloat>
 

diff --git a/...tics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.hpp b/...tics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.hpp
@@ -43,7 +43,6 @@
 
 // ROS2
 #include <rclcpp/rclcpp.hpp>
-#include <srv_kinematics_parameters.hpp>
 
 // System
 #include <memory>
@@ -56,6 +55,7 @@
 // MoveIt
 #include <moveit/kinematics_base/kinematics_base.hpp>
 #include <moveit/robot_state/robot_state.hpp>
+#include <moveit_kinematics/srv_kinematics_parameters.hpp>
 
 namespace srv_kinematics_plugin
 {

diff --git a/...industrial_motion_planner/include/pilz_industrial_motion_planner/command_list_manager.hpp b/...industrial_motion_planner/include/pilz_industrial_motion_planner/command_list_manager.hpp
@@ -48,7 +48,7 @@
 #include <pilz_industrial_motion_planner/trajectory_blender.hpp>
 #include <pilz_industrial_motion_planner/trajectory_generation_exceptions.hpp>
 
-#include <cartesian_limits_parameters.hpp>
+#include <pilz_industrial_motion_planner/cartesian_limits_parameters.hpp>
 
 namespace pilz_industrial_motion_planner
 {

diff --git a/...ilz_industrial_motion_planner/include/pilz_industrial_motion_planner/limits_container.hpp b/...ilz_industrial_motion_planner/include/pilz_industrial_motion_planner/limits_container.hpp
@@ -37,7 +37,7 @@
 #include <pilz_industrial_motion_planner/joint_limits_container.hpp>
 #include <math.h>
 
-#include "cartesian_limits_parameters.hpp"
+#include <pilz_industrial_motion_planner/cartesian_limits_parameters.hpp>
 
 namespace pilz_industrial_motion_planner
 {

diff --git a/..._motion_planner/include/pilz_industrial_motion_planner/pilz_industrial_motion_planner.hpp b/..._motion_planner/include/pilz_industrial_motion_planner/pilz_industrial_motion_planner.hpp
@@ -45,7 +45,7 @@
 #include <pluginlib/class_loader.hpp>
 #include <memory>
 
-#include <cartesian_limits_parameters.hpp>
+#include <pilz_industrial_motion_planner/cartesian_limits_parameters.hpp>
 
 namespace pilz_industrial_motion_planner
 {

diff --git a/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp b/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp
@@ -44,7 +44,7 @@
 #include <moveit/robot_state/conversions.hpp>
 #include <moveit/utils/logger.hpp>
 
-#include "cartesian_limits_parameters.hpp"
+#include <pilz_industrial_motion_planner/cartesian_limits_parameters.hpp>
 #include <pilz_industrial_motion_planner/joint_limits_aggregator.hpp>
 #include <pilz_industrial_motion_planner/tip_frame_getter.hpp>
 #include <pilz_industrial_motion_planner/trajectory_blend_request.hpp>

diff --git a/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp b/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp
@@ -40,7 +40,7 @@
 #include <pilz_industrial_motion_planner/planning_context_loader_ptp.hpp>
 #include <pilz_industrial_motion_planner/planning_exceptions.hpp>
 
-#include "cartesian_limits_parameters.hpp"
+#include <pilz_industrial_motion_planner/cartesian_limits_parameters.hpp>
 #include <pilz_industrial_motion_planner/joint_limits_aggregator.hpp>
 
 #include <pluginlib/class_list_macros.hpp>

diff --git a/...lz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp b/...lz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp
@@ -53,7 +53,7 @@
 
 #include <rclcpp/rclcpp.hpp>
 
-#include "cartesian_limits_parameters.hpp"
+#include <pilz_industrial_motion_planner/cartesian_limits_parameters.hpp>
 
 const std::string PARAM_MODEL_NO_GRIPPER_NAME{ "robot_description" };
 const std::string PARAM_MODEL_WITH_GRIPPER_NAME{ "robot_description_pg70" };

diff --git a/moveit_planners/stomp/include/stomp_moveit/cost_functions.hpp b/moveit_planners/stomp/include/stomp_moveit/cost_functions.hpp
@@ -168,7 +168,7 @@ CostFn getCostFunctionFromStateValidator(const StateValidatorFn& state_validator
       const long kernel_start = mu - static_cast<long>(sigma) * 4;
       const long kernel_end = mu + static_cast<long>(sigma) * 4;
       const long bounded_kernel_start = std::max(0l, kernel_start);
-      const long bounded_kernel_end = std::min(values.cols() - 1, kernel_end);
+      const long bounded_kernel_end = std::min(static_cast<long>(values.cols()) - 1, kernel_end);
       for (auto j = bounded_kernel_start; j <= bounded_kernel_end; ++j)
       {
         costs(j) = std::exp(-std::pow(j - mu, 2) / (2 * std::pow(sigma, 2))) / (sigma * std::sqrt(2 * M_PI));

diff --git a/moveit_planners/stomp/include/stomp_moveit/stomp_moveit_planning_context.hpp b/moveit_planners/stomp/include/stomp_moveit/stomp_moveit_planning_context.hpp
@@ -41,7 +41,7 @@
 
 #include <moveit/planning_interface/planning_interface.hpp>
 
-#include <stomp_moveit_parameters.hpp>
+#include <moveit_planners_stomp/stomp_moveit_parameters.hpp>
 
 // Forward declaration
 namespace stomp

diff --git a/moveit_planners/stomp/src/stomp_moveit_planning_context.cpp b/moveit_planners/stomp/src/stomp_moveit_planning_context.cpp
@@ -47,7 +47,7 @@
 #include <stomp_moveit/noise_generators.hpp>
 #include <stomp_moveit/cost_functions.hpp>
 #include <stomp_moveit/stomp_moveit_task.hpp>
-#include <stomp_moveit_parameters.hpp>
+#include <moveit_planners_stomp/stomp_moveit_parameters.hpp>
 
 #include <moveit/constraint_samplers/constraint_sampler_manager.hpp>
 #include <moveit/robot_state/conversions.hpp>

diff --git a/...test_configs/prbt_ikfast_manipulator_plugin/src/prbt_manipulator_ikfast_moveit_plugin.cpp b/...test_configs/prbt_ikfast_manipulator_plugin/src/prbt_manipulator_ikfast_moveit_plugin.cpp
@@ -52,7 +52,7 @@
 #include <tf2_kdl/tf2_kdl.hpp>
 #include <moveit/kinematics_base/kinematics_base.hpp>
 #include <moveit/robot_state/robot_state.hpp>
-#include <prbt_ikfast_kinematics_parameters.hpp>
+#include <moveit_resources_prbt_ikfast_manipulator_plugin/prbt_ikfast_kinematics_parameters.hpp>
 
 using namespace moveit::core;
 

diff --git a/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt b/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt
@@ -38,7 +38,7 @@ ament_target_dependencies(moveit_ros_control_interface_trajectory_plugin
                           ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost)
 
 add_library(moveit_ros_control_interface_gripper_plugin SHARED
-            src/gripper_controller_plugin.cpp)
+            src/gripper_command_controller_plugin.cpp)
 set_target_properties(
   moveit_ros_control_interface_gripper_plugin
   PROPERTIES VERSION "${moveit_ros_control_interface_VERSION}")
@@ -47,6 +47,16 @@ target_include_directories(moveit_ros_control_interface_gripper_plugin
 ament_target_dependencies(moveit_ros_control_interface_gripper_plugin
                           ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost)
 
+add_library(moveit_ros_control_interface_parallel_gripper_plugin SHARED
+            src/parallel_gripper_command_controller_plugin.cpp)
+set_target_properties(
+  moveit_ros_control_interface_parallel_gripper_plugin
+  PROPERTIES VERSION "${moveit_ros_control_interface_VERSION}")
+target_include_directories(moveit_ros_control_interface_parallel_gripper_plugin
+                           PRIVATE include)
+ament_target_dependencies(moveit_ros_control_interface_parallel_gripper_plugin
+                          ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost)
+
 add_library(moveit_ros_control_interface_empty_plugin SHARED
             src/empty_controller_plugin.cpp)
 set_target_properties(
@@ -65,6 +75,7 @@ set(TARGET_LIBRARIES
     moveit_ros_control_interface_plugin
     moveit_ros_control_interface_trajectory_plugin
     moveit_ros_control_interface_gripper_plugin
+    moveit_ros_control_interface_parallel_gripper_plugin
     moveit_ros_control_interface_empty_plugin)
 
 # Mark executables and/or libraries for installation

diff --git a/moveit_plugins/moveit_ros_control_interface/moveit_ros_control_interface_plugins.xml b/moveit_plugins/moveit_ros_control_interface/moveit_ros_control_interface_plugins.xml
@@ -10,10 +10,15 @@
     </class>
   </library>
   <library path="moveit_ros_control_interface_gripper_plugin">
-    <class name="position_controllers/GripperActionController" type="moveit_ros_control_interface::GripperControllerAllocator" base_class_type="moveit_ros_control_interface::ControllerHandleAllocator">
+    <class name="position_controllers/GripperActionController" type="moveit_ros_control_interface::GripperCommandControllerAllocator" base_class_type="moveit_ros_control_interface::ControllerHandleAllocator">
         <description></description>
     </class>
-    <class name="effort_controllers/GripperActionController" type="moveit_ros_control_interface::GripperControllerAllocator" base_class_type="moveit_ros_control_interface::ControllerHandleAllocator">
+    <class name="effort_controllers/GripperActionController" type="moveit_ros_control_interface::GripperCommandControllerAllocator" base_class_type="moveit_ros_control_interface::ControllerHandleAllocator">
+        <description></description>
+    </class>
+  </library>
+  <library path="moveit_ros_control_interface_parallel_gripper_plugin">
+    <class name="parallel_gripper_action_controller/GripperActionController" type="moveit_ros_control_interface::ParallelGripperCommandControllerAllocator" base_class_type="moveit_ros_control_interface::ControllerHandleAllocator">
         <description></description>
     </class>
   </library>

diff --git a/...terface/src/gripper_controller_plugin.cpp → ...src/gripper_command_controller_plugin.cpp b/...terface/src/gripper_controller_plugin.cpp → ...src/gripper_command_controller_plugin.cpp
@@ -36,27 +36,27 @@
 
 #include <moveit_ros_control_interface/ControllerHandle.hpp>
 #include <pluginlib/class_list_macros.hpp>
-#include <moveit_simple_controller_manager/gripper_controller_handle.hpp>
+#include <moveit_simple_controller_manager/gripper_command_controller_handle.hpp>
 #include <rclcpp/node.hpp>
 #include <memory>
 
 namespace moveit_ros_control_interface
 {
 /**
- * \brief Simple allocator for moveit_simple_controller_manager::FollowJointTrajectoryControllerHandle instances.
+ * \brief Simple allocator for moveit_simple_controller_manager::GripperCommandControllerHandle instances.
  */
-class GripperControllerAllocator : public ControllerHandleAllocator
+class GripperCommandControllerAllocator : public ControllerHandleAllocator
 {
 public:
   moveit_controller_manager::MoveItControllerHandlePtr alloc(const rclcpp::Node::SharedPtr& node,
                                                              const std::string& name,
                                                              const std::vector<std::string>& /* resources */) override
   {
-    return std::make_shared<moveit_simple_controller_manager::GripperControllerHandle>(node, name, "gripper_cmd");
+    return std::make_shared<moveit_simple_controller_manager::GripperCommandControllerHandle>(node, name, "gripper_cmd");
   }
 };
 
 }  // namespace moveit_ros_control_interface
 
-PLUGINLIB_EXPORT_CLASS(moveit_ros_control_interface::GripperControllerAllocator,
+PLUGINLIB_EXPORT_CLASS(moveit_ros_control_interface::GripperCommandControllerAllocator,
                        moveit_ros_control_interface::ControllerHandleAllocator);