Merge branch 'main' of https://github.com/TohokuQuix/moveit2

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

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})
 

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)

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));

moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp

@@ -200,11 +200,6 @@ void initTrajectoryExecutionManager(py::module& m)
            Stop whatever executions are active, if any.
            )")
 
-      .def("clear", &trajectory_execution_manager::TrajectoryExecutionManager::clear,
-           R"(
-           Clear the trajectories to execute.
-           )")
-
       .def("enable_execution_duration_monitoring",
            &trajectory_execution_manager::TrajectoryExecutionManager::enableExecutionDurationMonitoring,
            py::arg("flag"),

moveit_ros/move_group/CMakeLists.txt

@@ -79,13 +79,19 @@ install(TARGETS move_group list_move_group_capabilities
         RUNTIME DESTINATION lib/moveit_ros_move_group)
 
 install(
-  TARGETS moveit_move_group_default_capabilities
-          moveit_move_group_capabilities_base
+  TARGETS moveit_move_group_capabilities_base
   EXPORT moveit_ros_move_groupTargets
   LIBRARY DESTINATION lib
   ARCHIVE DESTINATION lib
   RUNTIME DESTINATION bin)
 
+install(
+  TARGETS moveit_move_group_default_capabilities
+  EXPORT moveit_move_group_default_capabilitiesTargets
+  LIBRARY DESTINATION lib
+  ARCHIVE DESTINATION lib
+  RUNTIME DESTINATION bin)
+
 install(DIRECTORY include/ DESTINATION include/moveit_ros_move_group)
 
 install(PROGRAMS scripts/load_map scripts/save_map

moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp

@@ -106,7 +106,6 @@ void MoveGroupExecuteTrajectoryAction::executePath(const std::shared_ptr<ExecTra
 {
   RCLCPP_INFO(getLogger(), "Execution request received");
 
-  context_->trajectory_execution_manager_->clear();
   if (context_->trajectory_execution_manager_->push(goal->get_goal()->trajectory, goal->get_goal()->controller_names))
   {
     setExecuteTrajectoryState(MONITOR, goal);

moveit_ros/planning/plan_execution/src/plan_execution.cpp

@@ -401,7 +401,6 @@ moveit_msgs::msg::MoveItErrorCodes plan_execution::PlanExecution::executeAndMoni
     plan.plan_components[component_idx].trajectory->getRobotTrajectoryMsg(msg);
     if (!trajectory_execution_manager_->push(msg, plan.plan_components[component_idx].controller_name))
     {
-      trajectory_execution_manager_->clear();
       RCLCPP_ERROR(logger_, "Apparently trajectory initialization failed");
       execution_complete_ = true;
       result.val = moveit_msgs::msg::MoveItErrorCodes::CONTROL_FAILED;

moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.hpp

@@ -186,9 +186,6 @@ class MOVEIT_TRAJECTORY_EXECUTION_MANAGER_EXPORT TrajectoryExecutionManager
   /// Stop whatever executions are active, if any
   void stopExecution(bool auto_clear = true);
 
-  /// Clear the trajectories to execute
-  void clear();
-
   /// Enable or disable the monitoring of trajectory execution duration. If a controller takes
   /// longer than expected, the trajectory is canceled
   void enableExecutionDurationMonitoring(bool flag);
@@ -286,6 +283,9 @@ class MOVEIT_TRAJECTORY_EXECUTION_MANAGER_EXPORT TrajectoryExecutionManager
   bool executePart(std::size_t part_index);
   bool waitForRobotToStop(const TrajectoryExecutionContext& context, double wait_time = 1.0);
 
+  /// Clear the trajectories to execute
+  void clear();
+
   void stopExecutionInternal();
 
   void receiveEvent(const std_msgs::msg::String::ConstSharedPtr& event);

moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp

@@ -1271,12 +1271,13 @@ void TrajectoryExecutionManager::clear()
 {
   if (execution_complete_)
   {
+    std::scoped_lock slock(execution_state_mutex_);
     for (TrajectoryExecutionContext* trajectory : trajectories_)
       delete trajectory;
     trajectories_.clear();
   }
   else
-    RCLCPP_ERROR(logger_, "Cannot push a new trajectory while another is being executed");
+    RCLCPP_FATAL(logger_, "Expecting execution_complete_ to be true!");
 }
 
 void TrajectoryExecutionManager::executeThread(const ExecutionCompleteCallback& callback,
@@ -1312,7 +1313,13 @@ void TrajectoryExecutionManager::executeThread(const ExecutionCompleteCallback&
 
   // only report that execution finished successfully when the robot actually stopped moving
   if (last_execution_status_ == moveit_controller_manager::ExecutionStatus::SUCCEEDED)
-    waitForRobotToStop(*trajectories_[i - 1]);
+  {
+    std::scoped_lock slock(execution_state_mutex_);
+    if (!execution_complete_)
+    {
+      waitForRobotToStop(*trajectories_[i - 1]);
+    }
+  }
 
   RCLCPP_INFO(logger_, "Completed trajectory execution with status %s ...", last_execution_status_.asString().c_str());