diff --git a/moveit_ros/planning/CMakeLists.txt b/moveit_ros/planning/CMakeLists.txt index 9dcf18a6f0..6585695658 100644 --- a/moveit_ros/planning/CMakeLists.txt +++ b/moveit_ros/planning/CMakeLists.txt @@ -33,6 +33,7 @@ set(THIS_PACKAGE_INCLUDE_DIRS robot_model_loader/include constraint_sampler_manager_loader/include planning_pipeline/include + planning_pipeline_interfaces/include planning_scene_monitor/include trajectory_execution_manager/include plan_execution/include @@ -46,6 +47,7 @@ set(THIS_PACKAGE_LIBRARIES moveit_robot_model_loader moveit_constraint_sampler_manager_loader moveit_planning_pipeline + moveit_planning_pipeline_interfaces moveit_trajectory_execution_manager moveit_plan_execution moveit_planning_scene_monitor @@ -83,6 +85,7 @@ add_subdirectory(kinematics_plugin_loader) add_subdirectory(robot_model_loader) add_subdirectory(constraint_sampler_manager_loader) add_subdirectory(planning_pipeline) +add_subdirectory(planning_pipeline_interfaces) add_subdirectory(planning_request_adapter_plugins) add_subdirectory(planning_scene_monitor) add_subdirectory(planning_components_tools) diff --git a/moveit_ros/planning/moveit_cpp/CMakeLists.txt b/moveit_ros/planning/moveit_cpp/CMakeLists.txt index 3e920a5e89..facf2e1310 100644 --- a/moveit_ros/planning/moveit_cpp/CMakeLists.txt +++ b/moveit_ros/planning/moveit_cpp/CMakeLists.txt @@ -1,6 +1,5 @@ add_library(moveit_cpp SHARED src/moveit_cpp.cpp - src/parallel_planning_callbacks.cpp src/planning_component.cpp ) set_target_properties(moveit_cpp PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") @@ -11,6 +10,7 @@ ament_target_dependencies(moveit_cpp target_link_libraries(moveit_cpp moveit_planning_scene_monitor moveit_planning_pipeline + moveit_planning_pipeline_interfaces moveit_trajectory_execution_manager) install(DIRECTORY include/ DESTINATION include/moveit_ros_planning) diff --git a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h index 644bdb3faa..32d8a54714 100644 --- a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h +++ b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h @@ -150,7 +150,7 @@ class MoveItCpp moveit::core::RobotStatePtr getCurrentState(double wait_seconds = 0.0); /** \brief Get all loaded planning pipeline instances mapped to their reference names */ - const std::map& getPlanningPipelines() const; + const std::unordered_map& getPlanningPipelines() const; /** \brief Get the stored instance of the planning scene monitor */ const planning_scene_monitor::PlanningSceneMonitorPtr& getPlanningSceneMonitor() const; @@ -189,8 +189,8 @@ class MoveItCpp planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; // Planning - std::map planning_pipelines_; - std::map> groups_algorithms_map_; + std::unordered_map planning_pipelines_; + std::unordered_map> groups_algorithms_map_; // Execution trajectory_execution_manager::TrajectoryExecutionManagerPtr trajectory_execution_manager_; diff --git a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h index 6711abc336..78e9a8b94c 100644 --- a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h +++ b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h @@ -39,7 +39,9 @@ #include #include -#include +#include +#include +#include #include #include #include @@ -102,28 +104,19 @@ class PlanningComponent MultiPipelinePlanRequestParameters(const rclcpp::Node::SharedPtr& node, const std::vector& planning_pipeline_names) { - multi_plan_request_parameters.reserve(planning_pipeline_names.size()); + plan_request_parameter_vector.reserve(planning_pipeline_names.size()); for (const auto& planning_pipeline_name : planning_pipeline_names) { PlanRequestParameters parameters; parameters.load(node, planning_pipeline_name); - multi_plan_request_parameters.push_back(parameters); + plan_request_parameter_vector.push_back(parameters); } } // Plan request parameters for the individual planning pipelines which run concurrently - std::vector multi_plan_request_parameters; + std::vector plan_request_parameter_vector; }; - /** \brief A solution callback function type for the parallel planning API of planning component */ - typedef std::function& solutions)> - SolutionCallbackFunction; - /** \brief A stopping criterion callback function for the parallel planning API of planning component */ - typedef std::function - StoppingCriterionFunction; - /** \brief Constructor */ PlanningComponent(const std::string& group_name, const rclcpp::Node::SharedPtr& node); PlanningComponent(const std::string& group_name, const MoveItCppPtr& moveit_cpp); @@ -191,15 +184,18 @@ class PlanningComponent planning_interface::MotionPlanResponse plan(); /** \brief Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() using the * provided PlanRequestParameters. */ - planning_interface::MotionPlanResponse plan(const PlanRequestParameters& parameters); + planning_interface::MotionPlanResponse plan(const PlanRequestParameters& parameters, + planning_scene::PlanningScenePtr planning_scene = nullptr); /** \brief Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() using the * provided PlanRequestParameters. This defaults to taking the full planning time (null stopping_criterion_callback) * and finding the shortest solution in joint space. */ planning_interface::MotionPlanResponse plan(const MultiPipelinePlanRequestParameters& parameters, - const SolutionCallbackFunction& solution_selection_callback = &getShortestSolution, - StoppingCriterionFunction stopping_criterion_callback = nullptr); + const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function = + &moveit::planning_pipeline_interfaces::getShortestSolution, + const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_callback = nullptr, + planning_scene::PlanningScenePtr planning_scene = nullptr); /** \brief Execute the latest computed solution trajectory computed by plan(). By default this function terminates * after the execution is complete. The execution can be run in background by setting blocking to false. */ @@ -208,6 +204,15 @@ class PlanningComponent return false; }; + /** \brief Utility function to get a MotionPlanRequest from PlanRequestParameters and the internal state of the + * PlanningComponent instance */ + ::planning_interface::MotionPlanRequest getMotionPlanRequest(const PlanRequestParameters& plan_request_parameters); + + /** \brief Utility function to get a Vector of MotionPlanRequest from a vector of PlanRequestParameters and the + * internal state of the PlanningComponent instance */ + std::vector<::planning_interface::MotionPlanRequest> + getMotionPlanRequestVector(const MultiPipelinePlanRequestParameters& multi_pipeline_plan_request_parameters); + private: // Core properties and instances rclcpp::Node::SharedPtr node_; @@ -222,7 +227,6 @@ class PlanningComponent std::vector current_goal_constraints_; moveit_msgs::msg::Constraints current_path_constraints_; moveit_msgs::msg::TrajectoryConstraints current_trajectory_constraints_; - PlanRequestParameters plan_request_parameters_; moveit_msgs::msg::WorkspaceParameters workspace_parameters_; bool workspace_parameters_set_ = false; diff --git a/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp b/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp index 60d1b86202..3b057208f2 100644 --- a/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp +++ b/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp @@ -37,6 +37,7 @@ #include #include +#include #include #include #include @@ -44,7 +45,6 @@ namespace moveit_cpp { static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros_planning_interface.moveit_cpp"); -constexpr char PLANNING_PLUGIN_PARAM[] = "planning_plugin"; MoveItCpp::MoveItCpp(const rclcpp::Node::SharedPtr& node) : MoveItCpp(node, Options(node)) { @@ -129,34 +129,13 @@ bool MoveItCpp::loadPlanningSceneMonitor(const PlanningSceneMonitorOptions& opti bool MoveItCpp::loadPlanningPipelines(const PlanningPipelineOptions& options) { // TODO(henningkayser): Use parent namespace for planning pipeline config lookup - // ros::NodeHandle node_handle(options.parent_namespace.empty() ? "~" : options.parent_namespace); - for (const auto& planning_pipeline_name : options.pipeline_names) - { - if (planning_pipelines_.count(planning_pipeline_name) > 0) - { - RCLCPP_WARN(LOGGER, "Skipping duplicate entry for planning pipeline '%s'.", planning_pipeline_name.c_str()); - continue; - } - RCLCPP_INFO(LOGGER, "Loading planning pipeline '%s'", planning_pipeline_name.c_str()); - planning_pipeline::PlanningPipelinePtr pipeline; - pipeline = std::make_shared(robot_model_, node_, planning_pipeline_name, - PLANNING_PLUGIN_PARAM); - - if (!pipeline->getPlannerManager()) - { - RCLCPP_ERROR(LOGGER, "Failed to initialize planning pipeline '%s'.", planning_pipeline_name.c_str()); - continue; - } - - planning_pipelines_[planning_pipeline_name] = pipeline; - } - + planning_pipelines_ = + moveit::planning_pipeline_interfaces::createPlanningPipelineMap(options.pipeline_names, robot_model_, node_); if (planning_pipelines_.empty()) { RCLCPP_ERROR(LOGGER, "Failed to load any planning pipelines."); return false; } - return true; } @@ -191,7 +170,7 @@ moveit::core::RobotStatePtr MoveItCpp::getCurrentState(double wait) return current_state; } -const std::map& MoveItCpp::getPlanningPipelines() const +const std::unordered_map& MoveItCpp::getPlanningPipelines() const { return planning_pipelines_; } diff --git a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp index 37cb99dea4..35bfb16a1e 100644 --- a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp @@ -57,15 +57,6 @@ PlanningComponent::PlanningComponent(const std::string& group_name, const MoveIt RCLCPP_FATAL_STREAM(LOGGER, error); throw std::runtime_error(error); } - plan_request_parameters_.load(node_); - RCLCPP_DEBUG_STREAM( - LOGGER, "Default plan request parameters loaded with --" - << " planning_pipeline: " << plan_request_parameters_.planning_pipeline << ',' - << " planner_id: " << plan_request_parameters_.planner_id << ',' - << " planning_time: " << plan_request_parameters_.planning_time << ',' - << " planning_attempts: " << plan_request_parameters_.planning_attempts << ',' - << " max_velocity_scaling_factor: " << plan_request_parameters_.max_velocity_scaling_factor << ',' - << " max_acceleration_scaling_factor: " << plan_request_parameters_.max_acceleration_scaling_factor); } PlanningComponent::PlanningComponent(const std::string& group_name, const rclcpp::Node::SharedPtr& node) @@ -124,159 +115,110 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const PlanRequest return plan_solution; } - // Clone current planning scene - planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor = moveit_cpp_->getPlanningSceneMonitor(); - planning_scene_monitor->updateFrameTransforms(); - const planning_scene::PlanningScenePtr planning_scene = [planning_scene_monitor] { - planning_scene_monitor::LockedPlanningSceneRO ls(planning_scene_monitor); - return planning_scene::PlanningScene::clone(ls); - }(); - planning_scene_monitor.reset(); // release this pointer - - // Init MotionPlanRequest - ::planning_interface::MotionPlanRequest req; - req.group_name = group_name_; - req.planner_id = parameters.planner_id; - req.num_planning_attempts = std::max(1, parameters.planning_attempts); - req.allowed_planning_time = parameters.planning_time; - req.max_velocity_scaling_factor = parameters.max_velocity_scaling_factor; - req.max_acceleration_scaling_factor = parameters.max_acceleration_scaling_factor; - if (workspace_parameters_set_) - req.workspace_parameters = workspace_parameters_; - - // Set start state - moveit::core::RobotStatePtr start_state = considered_start_state_; - if (!start_state) - start_state = moveit_cpp_->getCurrentState(); - start_state->update(); - moveit::core::robotStateToRobotStateMsg(*start_state, req.start_state); - planning_scene->setCurrentState(*start_state); - - // Set goal constraints + // Check if goal constraints exist if (current_goal_constraints_.empty()) { RCLCPP_ERROR(LOGGER, "No goal constraints set for planning request"); - plan_solution.error_code_ = moveit::core::MoveItErrorCode::INVALID_GOAL_CONSTRAINTS; + plan_solution.error_code = moveit::core::MoveItErrorCode::INVALID_GOAL_CONSTRAINTS; return plan_solution; } - req.goal_constraints = current_goal_constraints_; - // Set path constraints - req.path_constraints = current_path_constraints_; - // Set trajectory constraints - req.trajectory_constraints = current_trajectory_constraints_; + + if (!planning_scene) + { // Clone current planning scene + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor = moveit_cpp_->getPlanningSceneMonitor(); + planning_scene_monitor->updateFrameTransforms(); + planning_scene = [planning_scene_monitor] { + planning_scene_monitor::LockedPlanningSceneRO ls(planning_scene_monitor); + return planning_scene::PlanningScene::clone(ls); + }(); + planning_scene_monitor.reset(); // release this pointer} + } + // Init MotionPlanRequest + ::planning_interface::MotionPlanRequest request = getMotionPlanRequest(parameters); + + // Set start state + planning_scene->setCurrentState(request.start_state); // Run planning attempt - ::planning_interface::MotionPlanResponse res; - const auto& pipelines = moveit_cpp_->getPlanningPipelines(); - auto it = pipelines.find(parameters.planning_pipeline); - if (it == pipelines.end()) + return moveit::planning_pipeline_interfaces::planWithSinglePipeline(request, planning_scene, + moveit_cpp_->getPlanningPipelines()); +} + +planning_interface::MotionPlanResponse PlanningComponent::plan( + const MultiPipelinePlanRequestParameters& parameters, + const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function, + const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_callback, + planning_scene::PlanningScenePtr planning_scene) +{ + auto plan_solution = planning_interface::MotionPlanResponse(); + + // check if joint_model_group exists + if (!joint_model_group_) { - RCLCPP_ERROR(LOGGER, "No planning pipeline available for name '%s'", parameters.planning_pipeline.c_str()); - plan_solution.error_code_ = moveit::core::MoveItErrorCode::FAILURE; + RCLCPP_ERROR(LOGGER, "Failed to retrieve joint model group for name '%s'.", group_name_.c_str()); + plan_solution.error_code = moveit::core::MoveItErrorCode::INVALID_GROUP_NAME; return plan_solution; } - const planning_pipeline::PlanningPipelinePtr pipeline = it->second; - pipeline->generatePlan(planning_scene, req, res); - plan_solution.error_code_ = res.error_code_; - if (res.error_code_.val != res.error_code_.SUCCESS) + // Check if goal constraints exist + if (current_goal_constraints_.empty()) { - RCLCPP_ERROR(LOGGER, "Could not compute plan successfully"); + RCLCPP_ERROR(LOGGER, "No goal constraints set for planning request"); + plan_solution.error_code_ = moveit::core::MoveItErrorCode::INVALID_GOAL_CONSTRAINTS; return plan_solution; } - plan_solution.trajectory_ = res.trajectory_; - plan_solution.planning_time_ = res.planning_time_; - plan_solution.start_state_ = req.start_state; - plan_solution.error_code_ = res.error_code_.val; - - // TODO(henningkayser): Visualize trajectory - // std::vector eef_links; - // if (joint_model_group->getEndEffectorTips(eef_links)) - //{ - // for (const auto& eef_link : eef_links) - // { - // RCLCPP_INFO_STREAM("Publishing trajectory for end effector " << eef_link->getName()); - // visual_tools_->publishTrajectoryLine(last_solution_trajectory_, eef_link); - // visual_tools_->publishTrajectoryPath(last_solution_trajectory_, false); - // visual_tools_->publishRobotState(last_solution_trajectory_->getLastWayPoint(), rviz_visual_tools::TRANSLUCENT); - // } - //} - return plan_solution; -} + if (!planning_scene) + { // Clone current planning scene + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor = moveit_cpp_->getPlanningSceneMonitor(); + planning_scene_monitor->updateFrameTransforms(); + planning_scene = [planning_scene_monitor] { + planning_scene_monitor::LockedPlanningSceneRO ls(planning_scene_monitor); + return planning_scene::PlanningScene::clone(ls); + }(); + planning_scene_monitor.reset(); // release this pointer} + } + // Init MotionPlanRequest + std::vector<::planning_interface::MotionPlanRequest> requests = getMotionPlanRequestVector(parameters); -planning_interface::MotionPlanResponse -PlanningComponent::plan(const MultiPipelinePlanRequestParameters& parameters, - const SolutionCallbackFunction& solution_selection_callback, - StoppingCriterionFunction stopping_criterion_callback) -{ - // Create solutions container - PlanSolutions planning_solutions{ parameters.multi_plan_request_parameters.size() }; - std::vector planning_threads; - planning_threads.reserve(parameters.multi_plan_request_parameters.size()); - - // Print a warning if more parallel planning problems than available concurrent threads are defined. If - // std::thread::hardware_concurrency() is not defined, the command returns 0 so the check does not work - auto const hardware_concurrency = std::thread::hardware_concurrency(); - if (parameters.multi_plan_request_parameters.size() > hardware_concurrency && hardware_concurrency != 0) + // Set start state + for (const auto& request : requests) { - RCLCPP_WARN( - LOGGER, - "More parallel planning problems defined ('%ld') than possible to solve concurrently with the hardware ('%d')", - parameters.multi_plan_request_parameters.size(), hardware_concurrency); + planning_scene->setCurrentState(request.start_state); } - // Launch planning threads - for (const auto& plan_request_parameter : parameters.multi_plan_request_parameters) + auto const motion_plan_response_vector = moveit::planning_pipeline_interfaces::planWithParallelPipelines( + requests, planning_scene, moveit_cpp_->getPlanningPipelines(), stopping_criterion_callback, + solution_selection_function); + + try { - auto planning_thread = std::thread([&]() { - auto plan_solution = planning_interface::MotionPlanResponse(); - try - { - plan_solution = plan(plan_request_parameter); - } - catch (const std::exception& e) - { - RCLCPP_ERROR_STREAM(LOGGER, "Planning pipeline '" << plan_request_parameter.planning_pipeline.c_str() - << "' threw exception '" << e.what() << '\''); - plan_solution = planning_interface::MotionPlanResponse(); - plan_solution.error_code_ = moveit::core::MoveItErrorCode::FAILURE; - } - plan_solution.planner_id_ = plan_request_parameter.planner_id; - planning_solutions.pushBack(plan_solution); - - if (stopping_criterion_callback != nullptr) - { - if (stopping_criterion_callback(planning_solutions, parameters)) - { - // Terminate planning pipelines - RCLCPP_ERROR_STREAM(LOGGER, "Stopping criterion met: Terminating planning pipelines that are still active"); - for (const auto& plan_request_parameter : parameters.multi_plan_request_parameters) - { - moveit_cpp_->terminatePlanningPipeline(plan_request_parameter.planning_pipeline); - } - } - } - }); - planning_threads.push_back(std::move(planning_thread)); + // If a solution_selection function is passed to the parallel pipeline interface, the returned vector contains only + // the selected solution + plan_solution = motion_plan_response_vector.at(0); } - - // Wait for threads to finish - for (auto& planning_thread : planning_threads) + catch (std::out_of_range&) { - if (planning_thread.joinable()) - { - planning_thread.join(); - } + RCLCPP_ERROR(LOGGER, "MotionPlanResponse vector was empty after parallel planning"); + plan_solution.error_code = moveit::core::MoveItErrorCode::INVALID_GOAL_CONSTRAINTS; } - - // Return best solution determined by user defined callback (Default: Shortest path) - return solution_selection_callback(planning_solutions.getSolutions()); + // Run planning attempt + return plan_solution; } planning_interface::MotionPlanResponse PlanningComponent::plan() { - return plan(plan_request_parameters_); + PlanRequestParameters plan_request_parameters; + plan_request_parameters.load(node_); + RCLCPP_DEBUG_STREAM( + LOGGER, "Default plan request parameters loaded with --" + << " planning_pipeline: " << plan_request_parameters.planning_pipeline << ',' + << " planner_id: " << plan_request_parameters.planner_id << ',' + << " planning_time: " << plan_request_parameters.planning_time << ',' + << " planning_attempts: " << plan_request_parameters.planning_attempts << ',' + << " max_velocity_scaling_factor: " << plan_request_parameters.max_velocity_scaling_factor << ',' + << " max_acceleration_scaling_factor: " << plan_request_parameters.max_acceleration_scaling_factor); + return plan(plan_request_parameters); } bool PlanningComponent::setStartState(const moveit::core::RobotState& start_state) @@ -373,4 +315,46 @@ bool PlanningComponent::setGoal(const std::string& goal_state_name) goal_state.setToDefaultValues(joint_model_group_, goal_state_name); return setGoal(goal_state); } + +::planning_interface::MotionPlanRequest +PlanningComponent::getMotionPlanRequest(const PlanRequestParameters& plan_request_parameters) +{ + ::planning_interface::MotionPlanRequest request; + request.group_name = group_name_; + request.pipeline_id = plan_request_parameters.planning_pipeline; + request.planner_id = plan_request_parameters.planner_id; + request.num_planning_attempts = std::max(1, plan_request_parameters.planning_attempts); + request.allowed_planning_time = plan_request_parameters.planning_time; + request.max_velocity_scaling_factor = plan_request_parameters.max_velocity_scaling_factor; + request.max_acceleration_scaling_factor = plan_request_parameters.max_acceleration_scaling_factor; + if (workspace_parameters_set_) + { + request.workspace_parameters = workspace_parameters_; + } + request.goal_constraints = current_goal_constraints_; + request.path_constraints = current_path_constraints_; + request.trajectory_constraints = current_trajectory_constraints_; + + // Set start state + moveit::core::RobotStatePtr start_state = considered_start_state_; + if (!start_state) + { + start_state = moveit_cpp_->getCurrentState(); + } + start_state->update(); + moveit::core::robotStateToRobotStateMsg(*start_state, request.start_state); + return request; +} + +std::vector<::planning_interface::MotionPlanRequest> PlanningComponent::getMotionPlanRequestVector( + const MultiPipelinePlanRequestParameters& multi_pipeline_plan_request_parameters) +{ + std::vector<::planning_interface::MotionPlanRequest> motion_plan_requests; + motion_plan_requests.reserve(multi_pipeline_plan_request_parameters.plan_request_parameter_vector.size()); + for (auto const& plan_request_parameters : multi_pipeline_plan_request_parameters.plan_request_parameter_vector) + { + motion_plan_requests.push_back(getMotionPlanRequest(plan_request_parameters)); + } + return motion_plan_requests; +} } // namespace moveit_cpp diff --git a/moveit_ros/planning/planning_pipeline_interfaces/CMakeLists.txt b/moveit_ros/planning/planning_pipeline_interfaces/CMakeLists.txt new file mode 100644 index 0000000000..21025589ab --- /dev/null +++ b/moveit_ros/planning/planning_pipeline_interfaces/CMakeLists.txt @@ -0,0 +1,21 @@ +add_library(moveit_planning_pipeline_interfaces SHARED + src/planning_pipeline_interfaces.cpp + src/plan_responses_container.cpp + src/solution_selection_functions.cpp + src/stopping_criterion_function.cpp +) + +include(GenerateExportHeader) +generate_export_header(moveit_planning_pipeline_interfaces) +target_include_directories(moveit_planning_pipeline_interfaces PUBLIC $) +set_target_properties(moveit_planning_pipeline_interfaces PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +target_link_libraries(moveit_planning_pipeline_interfaces moveit_planning_pipeline) + +ament_target_dependencies(moveit_planning_pipeline_interfaces + moveit_core + moveit_msgs + rclcpp +) + +install(DIRECTORY include/ DESTINATION include/moveit_ros_planning) +install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_planning_pipeline_interfaces_export.h DESTINATION include/moveit_ros_planning) diff --git a/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/plan_responses_container.hpp b/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/plan_responses_container.hpp new file mode 100644 index 0000000000..779a7dd9d4 --- /dev/null +++ b/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/plan_responses_container.hpp @@ -0,0 +1,74 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Sebastian Jahr + Desc: A thread safe container to store motion plan responses */ + +#pragma once + +#include +#include + +namespace moveit +{ +namespace planning_pipeline_interfaces +{ +MOVEIT_CLASS_FORWARD(PlanResponsesContainer); // Defines PlanningComponentPtr, ConstPtr, WeakPtr... etc +/** \brief A container to thread-safely store multiple MotionPlanResponses */ +class PlanResponsesContainer +{ +public: + /** \brief Constructor + * \param [in] expected_size Number of expected solutions + */ + PlanResponsesContainer(const size_t expected_size = 0); + + /** \brief Thread safe method to add PlanResponsesContainer to this data structure TODO(sjahr): Refactor this method to an + * insert method similar to https://github.com/ompl/ompl/blob/main/src/ompl/base/src/ProblemDefinition.cpp#L54-L161. + * This way, it is possible to create a sorted container e.g. according to a user specified criteria + * \param [in] plan_solution MotionPlanResponse to push back into the vector + */ + void pushBack(const ::planning_interface::MotionPlanResponse& plan_solution); + + /** \brief Get solutions + * \return Read-only access to the responses vector + */ + const std::vector<::planning_interface::MotionPlanResponse>& getSolutions() const; + +private: + std::vector<::planning_interface::MotionPlanResponse> solutions_; + std::mutex solutions_mutex_; +}; +} // namespace planning_pipeline_interfaces +} // namespace moveit diff --git a/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/planning_pipeline_interfaces.hpp b/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/planning_pipeline_interfaces.hpp new file mode 100644 index 0000000000..e4b0ec11e8 --- /dev/null +++ b/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/planning_pipeline_interfaces.hpp @@ -0,0 +1,118 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Sebastian Jahr + Desc: Functions to create and use a map of PlanningPipelines to solve MotionPlanRequests */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace moveit +{ +namespace planning_pipeline_interfaces +{ +/** \brief A stopping criterion callback function for the parallel planning API of planning component + * \param [in] plan_responses_container Container with responses to be taken into account for the stopping decision + * \param [in] plan_requests Motion plan requests for the parallel planner + * \return True to stop parallel planning and abort planning pipelines that haven't terminated by now + */ +typedef std::function& plan_requests)> + StoppingCriterionFunction; + +/** \brief A solution callback function type for the parallel planning API of planning component + * \param [in] solutions Motion plan responses to choose from + * \return Selected motion plan response + */ +typedef std::function<::planning_interface::MotionPlanResponse( + const std::vector<::planning_interface::MotionPlanResponse>& solutions)> + SolutionSelectionFunction; + +/** \brief Function to calculate the MotionPlanResponse for a given MotionPlanRequest and a PlanningScene + * \param [in] motion_plan_request Motion planning problem to be solved + * \param [in] planning_scene Planning scene for which the given planning problem needs to be solved + * \param [in] planning_pipelines Pipelines available to solve the problem, if the requested pipeline is not provided + * the MotionPlanResponse will be FAILURE \return MotionPlanResponse for the given planning problem + */ +::planning_interface::MotionPlanResponse planWithSinglePipeline( + const ::planning_interface::MotionPlanRequest& motion_plan_request, + const ::planning_scene::PlanningSceneConstPtr& planning_scene, + const std::unordered_map& planning_pipelines); + +/** \brief Function to solve multiple planning problems in parallel threads with multiple planning pipelines at the same + time + * \param [in] motion_plan_request Motion planning problems to be solved + * \param [in] planning_scene Planning scene for which the given planning problem needs to be solved + * \param [in] planning_pipelines Pipelines available to solve the problems, if a requested pipeline is not provided the + MotionPlanResponse will be FAILURE + * \param [in] stopping_criterion_callback If this function returns true, the planning pipelines that are still running + will be terminated and the existing solutions will be evaluated. If no callback is provided, all planning pipelines + terminate after the max. planning time defined in the MotionPlanningRequest is reached. + * \param [in] solution_selection_function Function to select a specific solution out of all available solution. If no + function is provided, all solutions are returned. + + \return If a solution_selection_function is provided a vector containing the selected response is returned, otherwise + the vector contains all solutions produced. +*/ +const std::vector<::planning_interface::MotionPlanResponse> planWithParallelPipelines( + const std::vector<::planning_interface::MotionPlanRequest>& motion_plan_requests, + const ::planning_scene::PlanningSceneConstPtr& planning_scene, + const std::unordered_map& planning_pipelines, + const StoppingCriterionFunction& stopping_criterion_callback = nullptr, + const SolutionSelectionFunction& solution_selection_function = nullptr); + +/** \brief Utility function to create a map of named planning pipelines + * \param [in] pipeline_names Vector of planning pipeline names to be used. Each name is also the namespace from which + * the pipeline parameters are loaded + * \param [in] robot_model Robot model used to initialize the pipelines + * \param [in] node Node used to load parameters + * \param [in] parameter_namespace Optional prefix for the pipeline parameter + * namespace. Empty by default, so only the pipeline name is used as namespace + * \param [in] planning_plugin_param_name + * Optional name of the planning plugin namespace + * \param [in] adapter_plugins_param_name Optional name of the adapter plugin namespace + * \return Map of PlanningPipelinePtr's associated with a name for faster look-up + */ +std::unordered_map +createPlanningPipelineMap(const std::vector& pipeline_names, + const moveit::core::RobotModelConstPtr& robot_model, const rclcpp::Node::SharedPtr& node, + const std::string& parameter_namespace = std::string(), + const std::string& planning_plugin_param_name = "planning_plugin", + const std::string& adapter_plugins_param_name = "request_adapters"); +} // namespace planning_pipeline_interfaces +} // namespace moveit diff --git a/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/solution_selection_functions.hpp b/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/solution_selection_functions.hpp new file mode 100644 index 0000000000..535a9aecc2 --- /dev/null +++ b/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/solution_selection_functions.hpp @@ -0,0 +1,54 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Sebastian Jahr + Desc: Solution selection function implementations */ + +#pragma once + +#include + +namespace moveit +{ +namespace planning_pipeline_interfaces +{ +/** \brief Function that returns the shortest solution out of a vector of solutions based on robot_trajectory::path_length(...) + * \param [in] solutions Vector of solutions to chose the shortest one from + * \return Shortest solution, trajectory of the returned MotionPlanResponse is a nullptr if no solution is found! + */ +::planning_interface::MotionPlanResponse +getShortestSolution(const std::vector<::planning_interface::MotionPlanResponse>& solutions); + +} // namespace planning_pipeline_interfaces +} // namespace moveit diff --git a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/parallel_planning_callbacks.hpp b/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/stopping_criterion_functions.hpp similarity index 80% rename from moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/parallel_planning_callbacks.hpp rename to moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/stopping_criterion_functions.hpp index e70742de55..5549090bcb 100644 --- a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/parallel_planning_callbacks.hpp +++ b/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/stopping_criterion_functions.hpp @@ -32,17 +32,20 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Sebastian Jahr - Desc: Common callback functions for parallel planning */ +/* Author: AndyZe, Sebastian Jahr + Desc: Stopping criterion function implementations */ #pragma once -#include +#include -namespace moveit_cpp +namespace moveit +{ +namespace planning_pipeline_interfaces { /** \brief A callback function that can be used as a parallel planning stop criterion. * It stops parallel planning as soon as any planner finds a solution. */ -bool stopAtFirstSolution(PlanSolutions const& plan_solutions, - PlanningComponent::MultiPipelinePlanRequestParameters const& /*plan_request_parameters*/); -} // namespace moveit_cpp +bool stopAtFirstSolution(const PlanResponsesContainer& plan_responses_container, + const std::vector<::planning_interface::MotionPlanRequest>& plan_requests); +} // namespace planning_pipeline_interfaces +} // namespace moveit diff --git a/moveit_ros/planning/planning_pipeline_interfaces/src/plan_responses_container.cpp b/moveit_ros/planning/planning_pipeline_interfaces/src/plan_responses_container.cpp new file mode 100644 index 0000000000..88967e0894 --- /dev/null +++ b/moveit_ros/planning/planning_pipeline_interfaces/src/plan_responses_container.cpp @@ -0,0 +1,60 @@ + +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Sebastian Jahr */ + +#include + +namespace moveit +{ +namespace planning_pipeline_interfaces +{ +PlanResponsesContainer::PlanResponsesContainer(const size_t expected_size) +{ + solutions_.reserve(expected_size); +} + +void PlanResponsesContainer::pushBack(const ::planning_interface::MotionPlanResponse& plan_solution) +{ + std::lock_guard lock_guard(solutions_mutex_); + solutions_.push_back(plan_solution); +} + +const std::vector<::planning_interface::MotionPlanResponse>& PlanResponsesContainer::getSolutions() const +{ + return solutions_; +} +} // namespace planning_pipeline_interfaces +} // namespace moveit diff --git a/moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp b/moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp new file mode 100644 index 0000000000..d689397f44 --- /dev/null +++ b/moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp @@ -0,0 +1,192 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Sebastian Jahr */ + +#include + +#include + +namespace moveit +{ +namespace planning_pipeline_interfaces +{ +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.::planning_interface.planning_pipeline_interfaces"); + +::planning_interface::MotionPlanResponse +planWithSinglePipeline(const ::planning_interface::MotionPlanRequest& motion_plan_request, + const ::planning_scene::PlanningSceneConstPtr& planning_scene, + const std::unordered_map& planning_pipelines) +{ + ::planning_interface::MotionPlanResponse motion_plan_response; + auto it = planning_pipelines.find(motion_plan_request.pipeline_id); + if (it == planning_pipelines.end()) + { + RCLCPP_ERROR(LOGGER, "No planning pipeline available for name '%s'", motion_plan_request.pipeline_id.c_str()); + motion_plan_response.error_code = moveit::core::MoveItErrorCode::FAILURE; + return motion_plan_response; + } + const planning_pipeline::PlanningPipelinePtr pipeline = it->second; + pipeline->generatePlan(planning_scene, motion_plan_request, motion_plan_response); + return motion_plan_response; +} + +const std::vector<::planning_interface::MotionPlanResponse> planWithParallelPipelines( + const std::vector<::planning_interface::MotionPlanRequest>& motion_plan_requests, + const ::planning_scene::PlanningSceneConstPtr& planning_scene, + const std::unordered_map& planning_pipelines, + const StoppingCriterionFunction& stopping_criterion_callback, + const SolutionSelectionFunction& solution_selection_function) +{ + // Create solutions container + PlanResponsesContainer plan_responses_container{ motion_plan_requests.size() }; + std::vector planning_threads; + planning_threads.reserve(motion_plan_requests.size()); + + // Print a warning if more parallel planning problems than available concurrent threads are defined. If + // std::thread::hardware_concurrency() is not defined, the command returns 0 so the check does not work + auto const hardware_concurrency = std::thread::hardware_concurrency(); + if (motion_plan_requests.size() > hardware_concurrency && hardware_concurrency != 0) + { + RCLCPP_WARN(LOGGER, + "More parallel planning problems defined ('%ld') than possible to solve concurrently with the " + "hardware ('%d')", + motion_plan_requests.size(), hardware_concurrency); + } + + // Launch planning threads + for (const auto& request : motion_plan_requests) + { + auto planning_thread = std::thread([&]() { + auto plan_solution = ::planning_interface::MotionPlanResponse(); + try + { + // Use planning scene if provided, otherwise the planning scene from planning scene monitor is used + plan_solution = planWithSinglePipeline(request, planning_scene, planning_pipelines); + } + catch (const std::exception& e) + { + RCLCPP_ERROR(LOGGER, "Planning pipeline '%s' threw exception '%s'", request.pipeline_id.c_str(), e.what()); + plan_solution = ::planning_interface::MotionPlanResponse(); + plan_solution.error_code = moveit::core::MoveItErrorCode::FAILURE; + } + plan_solution.planner_id = request.planner_id; + plan_responses_container.pushBack(plan_solution); + + if (stopping_criterion_callback != nullptr) + { + if (stopping_criterion_callback(plan_responses_container, motion_plan_requests)) + { + // Terminate planning pipelines + RCLCPP_INFO(LOGGER, "Stopping criterion met: Terminating planning pipelines that are still active"); + for (const auto& request : motion_plan_requests) + { + try + { + const auto& planning_pipeline = planning_pipelines.at(request.pipeline_id); + if (planning_pipeline->isActive()) + { + planning_pipeline->terminate(); + } + } + catch (const std::out_of_range&) + { + RCLCPP_WARN(LOGGER, "Cannot terminate pipeline '%s' because no pipeline with that name exists", + request.pipeline_id.c_str()); + } + } + } + } + }); + planning_threads.push_back(std::move(planning_thread)); + } + + // Wait for threads to finish + for (auto& planning_thread : planning_threads) + { + if (planning_thread.joinable()) + { + planning_thread.join(); + } + } + + // If a solution selection function is provided, it is used to compute the return value + if (solution_selection_function) + { + std::vector<::planning_interface::MotionPlanResponse> solutions; + solutions.reserve(1); + solutions.push_back(solution_selection_function(plan_responses_container.getSolutions())); + return solutions; + } + + // Otherwise, just return the unordered list of solutions + return plan_responses_container.getSolutions(); +} + +std::unordered_map +createPlanningPipelineMap(const std::vector& pipeline_names, + const moveit::core::RobotModelConstPtr& robot_model, const rclcpp::Node::SharedPtr& node, + const std::string& parameter_namespace, const std::string& planning_plugin_param_name, + const std::string& adapter_plugins_param_name) +{ + std::unordered_map planning_pipelines; + for (const auto& planning_pipeline_name : pipeline_names) + { + // Check if pipeline already exists + if (planning_pipelines.count(planning_pipeline_name) > 0) + { + RCLCPP_WARN(LOGGER, "Skipping duplicate entry for planning pipeline '%s'.", planning_pipeline_name.c_str()); + continue; + } + + // Create planning pipeline + planning_pipeline::PlanningPipelinePtr pipeline = + std::make_shared(robot_model, node, + parameter_namespace + planning_pipeline_name, + planning_plugin_param_name, adapter_plugins_param_name); + + if (!pipeline->getPlannerManager()) + { + RCLCPP_ERROR(LOGGER, "Failed to initialize planning pipeline '%s'.", planning_pipeline_name.c_str()); + continue; + } + + planning_pipelines[planning_pipeline_name] = pipeline; + } + + return planning_pipelines; +} + +} // namespace planning_pipeline_interfaces +} // namespace moveit diff --git a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/plan_solutions.hpp b/moveit_ros/planning/planning_pipeline_interfaces/src/solution_selection_functions.cpp similarity index 59% rename from moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/plan_solutions.hpp rename to moveit_ros/planning/planning_pipeline_interfaces/src/solution_selection_functions.cpp index 203e58c31b..7104a675a2 100644 --- a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/plan_solutions.hpp +++ b/moveit_ros/planning/planning_pipeline_interfaces/src/solution_selection_functions.cpp @@ -1,7 +1,7 @@ /********************************************************************* * Software License Agreement (BSD License) * - * Copyright (c) 2019, PickNik Inc. + * Copyright (c) 2023, PickNik Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -32,61 +32,21 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Sebastian Jahr - Desc: A safe data structure for MotionPlanResponses and free functions to analyze them */ +/* Author: Sebastian Jahr */ -#pragma once +#include -#include -#include -#include -#include - -namespace moveit_cpp +namespace moveit { -MOVEIT_CLASS_FORWARD(PlanSolutions); // Defines PlanningComponentPtr, ConstPtr, WeakPtr... etc - -/** \brief A container to thread-safely store multiple MotionPlanResponses for later usage */ -class PlanSolutions +namespace planning_pipeline_interfaces { -public: - PlanSolutions(const size_t expected_size = 0) - { - solutions_.reserve(expected_size); - } - - /** \brief Thread safe method to add PlanSolutions to this data structure TODO(sjahr): Refactor this method to an - * insert method similar to https://github.com/ompl/ompl/blob/main/src/ompl/base/src/ProblemDefinition.cpp#L54-L161. - * This way, it is possible to create a sorted container e.g. according to a user specified criteria - */ - void pushBack(const planning_interface::MotionPlanResponse& plan_solution) - { - std::lock_guard lock_guard(solutions_mutex_); - solutions_.push_back(plan_solution); - } - - /** \brief Get solutions */ - const std::vector& getSolutions() const - { - return solutions_; - } - -private: - std::vector solutions_; - std::mutex solutions_mutex_; -}; - -/** \brief Function that returns the shortest solution out of a vector of solutions based on robot_trajectory::path_length(...) - * \param [in] solutions Vector of solutions to chose the shortest one from - * \return Shortest solution, trajectory_ of the returned MotionPlanResponse is a nullptr if no solution is found! - */ -static inline planning_interface::MotionPlanResponse -getShortestSolution(const std::vector& solutions) +::planning_interface::MotionPlanResponse +getShortestSolution(const std::vector<::planning_interface::MotionPlanResponse>& solutions) { // Find trajectory with minimal path auto const shortest_trajectory = std::min_element(solutions.begin(), solutions.end(), - [](const planning_interface::MotionPlanResponse& solution_a, - const planning_interface::MotionPlanResponse& solution_b) { + [](const ::planning_interface::MotionPlanResponse& solution_a, + const ::planning_interface::MotionPlanResponse& solution_b) { // If both solutions were successful, check which path is shorter if (solution_a && solution_b) { @@ -103,4 +63,6 @@ getShortestSolution(const std::vector& s }); return *shortest_trajectory; } -} // namespace moveit_cpp + +} // namespace planning_pipeline_interfaces +} // namespace moveit diff --git a/moveit_ros/planning/moveit_cpp/src/parallel_planning_callbacks.cpp b/moveit_ros/planning/planning_pipeline_interfaces/src/stopping_criterion_function.cpp similarity index 80% rename from moveit_ros/planning/moveit_cpp/src/parallel_planning_callbacks.cpp rename to moveit_ros/planning/planning_pipeline_interfaces/src/stopping_criterion_function.cpp index 9db08dd0f1..f0bf424cee 100644 --- a/moveit_ros/planning/moveit_cpp/src/parallel_planning_callbacks.cpp +++ b/moveit_ros/planning/planning_pipeline_interfaces/src/stopping_criterion_function.cpp @@ -34,17 +34,17 @@ /* Author: Sebastian Jahr */ -#include +#include -namespace moveit_cpp +namespace moveit { -/** \brief A callback function that can be used as a parallel planning stop criterion. - * It stops parallel planning as soon as any planner finds a solution. */ -bool stopAtFirstSolution(PlanSolutions const& plan_solutions, - PlanningComponent::MultiPipelinePlanRequestParameters const& /*plan_request_parameters*/) +namespace planning_pipeline_interfaces +{ +bool stopAtFirstSolution(const PlanResponsesContainer& plan_responses_container, + const std::vector<::planning_interface::MotionPlanRequest>& /*plan_requests*/) { // Stop at the first successful plan - for (auto const& solution : plan_solutions.getSolutions()) + for (auto const& solution : plan_responses_container.getSolutions()) { // bool(solution) is shorthand to evaluate the error code of the solution, checking for SUCCESS if (bool(solution)) @@ -56,4 +56,5 @@ bool stopAtFirstSolution(PlanSolutions const& plan_solutions, // Return false when parallel planning should continue because it hasn't found a successful solution yet return false; } -} // namespace moveit_cpp +} // namespace planning_pipeline_interfaces +} // namespace moveit