diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt index 608babbebc..ca30488f79 100644 --- 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/moveit_kinematics/CMakeLists.txt b/moveit_kinematics/CMakeLists.txt index 21b03dfb14..a453674aba 100644 --- 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 index f281644753..5ce836e33a 100644 --- 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/moveit_planners/stomp/include/stomp_moveit/cost_functions.hpp b/moveit_planners/stomp/include/stomp_moveit/cost_functions.hpp index b910cd3393..97350bec21 100644 --- 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(sigma) * 4; const long kernel_end = mu + static_cast(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(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_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp b/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp index 1c0693e638..e74260e6a0 100644 --- a/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp +++ b/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"), diff --git a/moveit_ros/move_group/CMakeLists.txt b/moveit_ros/move_group/CMakeLists.txt index bd48293e7a..2cdebb6ee9 100644 --- a/moveit_ros/move_group/CMakeLists.txt +++ b/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 diff --git a/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp b/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp index ff7959e439..602b16a6ee 100644 --- a/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp @@ -106,7 +106,6 @@ void MoveGroupExecuteTrajectoryAction::executePath(const std::shared_ptrtrajectory_execution_manager_->clear(); if (context_->trajectory_execution_manager_->push(goal->get_goal()->trajectory, goal->get_goal()->controller_names)) { setExecuteTrajectoryState(MONITOR, goal); diff --git a/moveit_ros/planning/plan_execution/src/plan_execution.cpp b/moveit_ros/planning/plan_execution/src/plan_execution.cpp index bfbc2c37ac..173bceccad 100644 --- a/moveit_ros/planning/plan_execution/src/plan_execution.cpp +++ b/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; diff --git a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.hpp b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.hpp index b305371d54..86c190b3ca 100644 --- a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.hpp +++ b/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); diff --git a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp index fd913c7960..00fa65ea17 100644 --- a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp +++ b/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());