From 3eb8fd3f12274194624e47f908aa520004a53e29 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 29 Aug 2022 10:12:56 +0200 Subject: [PATCH 01/20] Add setTrajectoryConstraints() to PlanningComponent --- .../include/moveit/moveit_cpp/planning_component.h | 4 ++++ .../planning/moveit_cpp/src/planning_component.cpp | 10 +++++++++- 2 files changed, 13 insertions(+), 1 deletion(-) 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 7a8c94ca5b..6f4f4577a5 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 @@ -154,6 +154,9 @@ class PlanningComponent /** \brief Set the path constraints generated from a moveit msg Constraints */ bool setPathConstraints(const moveit_msgs::msg::Constraints& path_constraints); + /** \brief Set the trajectory constraints generated from a moveit msg Constraints */ + bool setTrajectoryConstraints(const moveit_msgs::msg::TrajectoryConstraints& trajectory_constraints); + /** \brief Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() using * default parameters. */ PlanSolution plan(); @@ -182,6 +185,7 @@ class PlanningComponent moveit::core::RobotStatePtr considered_start_state_; 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/planning_component.cpp b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp index f1d8d72d27..5c2a35f1c4 100644 --- a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp @@ -107,6 +107,12 @@ bool PlanningComponent::setPathConstraints(const moveit_msgs::msg::Constraints& return true; } +bool PlanningComponent::setTrajectoryConstraints(const moveit_msgs::msg::TrajectoryConstraints& trajectory_constraints) +{ + current_trajectory_constraints_ = trajectory_constraints; + return true; +} + PlanningComponent::PlanSolution PlanningComponent::plan(const PlanRequestParameters& parameters) { last_plan_solution_ = std::make_shared(); @@ -156,7 +162,9 @@ PlanningComponent::PlanSolution PlanningComponent::plan(const PlanRequestParamet req.goal_constraints = current_goal_constraints_; // Set path constraints - req.path_constraints = current_path_constraints_; + req.path_constraints = current_path_constraints_; // TODO pass as function argument + // Set trajectory constraints + req.trajectory_constraints = current_trajectory_constraints_; // Run planning attempt ::planning_interface::MotionPlanResponse res; From 0d7442acf86439af668bed904af3f39ee81275b8 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 29 Aug 2022 10:13:39 +0200 Subject: [PATCH 02/20] Add planning time to PlanningComponent::PlanSolution --- .../moveit_cpp/include/moveit/moveit_cpp/planning_component.h | 3 +++ moveit_ros/planning/moveit_cpp/src/planning_component.cpp | 4 +++- 2 files changed, 6 insertions(+), 1 deletion(-) 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 6f4f4577a5..60b72ee8c9 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 @@ -64,6 +64,9 @@ class PlanningComponent /// The trajectory of the robot (may not contain joints that are the same as for the start_state_) robot_trajectory::RobotTrajectoryPtr trajectory; + // Planning time (seconds) + double planning_time; + /// Reason why the plan failed moveit::core::MoveItErrorCode error_code; diff --git a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp index 5c2a35f1c4..17ea2ce112 100644 --- a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp @@ -162,7 +162,7 @@ PlanningComponent::PlanSolution PlanningComponent::plan(const PlanRequestParamet req.goal_constraints = current_goal_constraints_; // Set path constraints - req.path_constraints = current_path_constraints_; // TODO pass as function argument + req.path_constraints = current_path_constraints_; // Set trajectory constraints req.trajectory_constraints = current_trajectory_constraints_; @@ -185,6 +185,8 @@ PlanningComponent::PlanSolution PlanningComponent::plan(const PlanRequestParamet } last_plan_solution_->start_state = req.start_state; last_plan_solution_->trajectory = res.trajectory_; + last_plan_solution_->planning_time = res.planning_time_; + // TODO(henningkayser): Visualize trajectory // std::vector eef_links; // if (joint_model_group->getEndEffectorTips(eef_links)) From cbf6eaa712e945174926b8d18727032ce3a8de4b Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Sun, 18 Sep 2022 21:49:04 +0200 Subject: [PATCH 03/20] Replace PlanSolution with MotionPlanResponse --- .../planning_interface/planning_response.h | 19 +++++++++-- .../src/moveit_planning_pipeline.cpp | 10 +++--- .../moveit/moveit_cpp/planning_component.h | 32 +++-------------- .../moveit_cpp/src/planning_component.cpp | 34 +++++++++---------- 4 files changed, 44 insertions(+), 51 deletions(-) diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h b/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h index 3c60f14445..3b2a042da2 100644 --- a/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h @@ -37,23 +37,38 @@ #pragma once #include +#include #include #include #include namespace planning_interface { +/// \brief Response to a planning query struct MotionPlanResponse { - MotionPlanResponse() : planning_time_(0.0) + // Constructor + MotionPlanResponse() : trajectory_(nullptr), planning_time_(0.0), error_code_(moveit::core::MoveItErrorCode::FAILURE) { } + // Construct a ROS message from struct data void getMessage(moveit_msgs::msg::MotionPlanResponse& msg) const; + // Trajectory generated by the queried planner robot_trajectory::RobotTrajectoryPtr trajectory_; + // Time to plan the respond to the planning query double planning_time_; - moveit_msgs::msg::MoveItErrorCodes error_code_; + // Result status of the query + moveit::core::MoveItErrorCode error_code_; + /// The full starting state used for planning + moveit_msgs::msg::RobotState start_state_; + + // Enable checking of query success or failure, for example if(response) ... + explicit operator bool() const + { + return bool(error_code_); + } }; struct MotionPlanDetailedResponse diff --git a/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/src/moveit_planning_pipeline.cpp b/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/src/moveit_planning_pipeline.cpp index ce5be3e98a..912f51f87b 100644 --- a/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/src/moveit_planning_pipeline.cpp +++ b/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/src/moveit_planning_pipeline.cpp @@ -132,17 +132,17 @@ moveit_msgs::msg::MotionPlanResponse MoveItPlanningPipeline::plan( // Plan motion auto plan_solution = planning_components->plan(plan_params); - if (plan_solution.error_code != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) + if (plan_solution.error_code_ != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { - response.error_code = plan_solution.error_code; + response.error_code = plan_solution.error_code_; return response; } // Transform solution into MotionPlanResponse and publish it - response.trajectory_start = plan_solution.start_state; + response.trajectory_start = plan_solution.start_state_; response.group_name = motion_plan_req.group_name; - plan_solution.trajectory->getRobotTrajectoryMsg(response.trajectory); - response.error_code = plan_solution.error_code; + plan_solution.trajectory_->getRobotTrajectoryMsg(response.trajectory); + response.error_code = plan_solution.error_code_; return response; } 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 60b72ee8c9..07ff65afec 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 @@ -43,6 +43,7 @@ #include #include #include +#include namespace moveit_cpp { @@ -51,31 +52,8 @@ MOVEIT_CLASS_FORWARD(PlanningComponent); // Defines PlanningComponentPtr, Const class PlanningComponent { public: - MOVEIT_STRUCT_FORWARD(PlanSolution); - using MoveItErrorCode [[deprecated("Use moveit::core::MoveItErrorCode")]] = moveit::core::MoveItErrorCode; - /// The representation of a plan solution - struct PlanSolution - { - /// The full starting state used for planning - moveit_msgs::msg::RobotState start_state; - - /// The trajectory of the robot (may not contain joints that are the same as for the start_state_) - robot_trajectory::RobotTrajectoryPtr trajectory; - - // Planning time (seconds) - double planning_time; - - /// Reason why the plan failed - moveit::core::MoveItErrorCode error_code; - - explicit operator bool() const - { - return bool(error_code); - } - }; - /// Planner parameters provided with the MotionPlanRequest struct PlanRequestParameters { @@ -162,17 +140,17 @@ class PlanningComponent /** \brief Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() using * default parameters. */ - PlanSolution plan(); + 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. */ - PlanSolution plan(const PlanRequestParameters& parameters); + planning_interface::MotionPlanResponse plan(const PlanRequestParameters& parameters); /** \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. */ bool execute(bool blocking = true); /** \brief Return the last plan solution*/ - const PlanSolutionPtr getLastPlanSolution(); + planning_interface::MotionPlanResponse const& getLastMotionPlanResponse(); private: // Core properties and instances @@ -192,7 +170,7 @@ class PlanningComponent PlanRequestParameters plan_request_parameters_; moveit_msgs::msg::WorkspaceParameters workspace_parameters_; bool workspace_parameters_set_ = false; - PlanSolutionPtr last_plan_solution_; + planning_interface::MotionPlanResponse last_plan_solution_; // common properties for goals // TODO(henningkayser): support goal tolerances diff --git a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp index 17ea2ce112..ee4ddb299e 100644 --- a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp @@ -113,14 +113,14 @@ bool PlanningComponent::setTrajectoryConstraints(const moveit_msgs::msg::Traject return true; } -PlanningComponent::PlanSolution PlanningComponent::plan(const PlanRequestParameters& parameters) +planning_interface::MotionPlanResponse PlanningComponent::plan(const PlanRequestParameters& parameters) { - last_plan_solution_ = std::make_shared(); + last_plan_solution_ = planning_interface::MotionPlanResponse(); if (!joint_model_group_) { RCLCPP_ERROR(LOGGER, "Failed to retrieve joint model group for name '%s'.", group_name_.c_str()); - last_plan_solution_->error_code = moveit::core::MoveItErrorCode::INVALID_GROUP_NAME; - return *last_plan_solution_; + last_plan_solution_.error_code_ = moveit::core::MoveItErrorCode::INVALID_GROUP_NAME; + return last_plan_solution_; } // Clone current planning scene @@ -156,8 +156,8 @@ PlanningComponent::PlanSolution PlanningComponent::plan(const PlanRequestParamet if (current_goal_constraints_.empty()) { RCLCPP_ERROR(LOGGER, "No goal constraints set for planning request"); - last_plan_solution_->error_code = moveit::core::MoveItErrorCode::INVALID_GOAL_CONSTRAINTS; - return *last_plan_solution_; + last_plan_solution_.error_code_ = moveit::core::MoveItErrorCode::INVALID_GOAL_CONSTRAINTS; + return last_plan_solution_; } req.goal_constraints = current_goal_constraints_; @@ -171,21 +171,21 @@ PlanningComponent::PlanSolution PlanningComponent::plan(const PlanRequestParamet if (planning_pipeline_names_.find(parameters.planning_pipeline) == planning_pipeline_names_.end()) { RCLCPP_ERROR(LOGGER, "No planning pipeline available for name '%s'", parameters.planning_pipeline.c_str()); - last_plan_solution_->error_code = moveit::core::MoveItErrorCode::FAILURE; - return *last_plan_solution_; + last_plan_solution_.error_code_ = moveit::core::MoveItErrorCode::FAILURE; + return last_plan_solution_; } const planning_pipeline::PlanningPipelinePtr pipeline = moveit_cpp_->getPlanningPipelines().at(parameters.planning_pipeline); pipeline->generatePlan(planning_scene, req, res); - last_plan_solution_->error_code = res.error_code_.val; + last_plan_solution_.error_code_ = res.error_code_.val; if (res.error_code_.val != res.error_code_.SUCCESS) { RCLCPP_ERROR(LOGGER, "Could not compute plan successfully"); - return *last_plan_solution_; + return last_plan_solution_; } - last_plan_solution_->start_state = req.start_state; - last_plan_solution_->trajectory = res.trajectory_; - last_plan_solution_->planning_time = res.planning_time_; + last_plan_solution_.trajectory_ = res.trajectory_; + last_plan_solution_.planning_time_ = res.planning_time_; + last_plan_solution_.start_state_ = req.start_state; // TODO(henningkayser): Visualize trajectory // std::vector eef_links; @@ -199,10 +199,10 @@ PlanningComponent::PlanSolution PlanningComponent::plan(const PlanRequestParamet // visual_tools_->publishRobotState(last_solution_trajectory_->getLastWayPoint(), rviz_visual_tools::TRANSLUCENT); // } //} - return *last_plan_solution_; + return last_plan_solution_; } -PlanningComponent::PlanSolution PlanningComponent::plan() +planning_interface::MotionPlanResponse PlanningComponent::plan() { return plan(plan_request_parameters_); } @@ -316,10 +316,10 @@ bool PlanningComponent::execute(bool blocking) // RCLCPP_ERROR("Failed to parameterize trajectory"); // return false; //} - return moveit_cpp_->execute(group_name_, last_plan_solution_->trajectory, blocking); + return moveit_cpp_->execute(group_name_, last_plan_solution_.trajectory_, blocking); } -const PlanningComponent::PlanSolutionPtr PlanningComponent::getLastPlanSolution() +planning_interface::MotionPlanResponse const& PlanningComponent::getLastMotionPlanResponse() { return last_plan_solution_; } From ebc5fb6ce7bbe0fd4c6d92117571270703507bc0 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Wed, 28 Sep 2022 14:46:23 +0200 Subject: [PATCH 04/20] Address review --- moveit_ros/planning/moveit_cpp/src/planning_component.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp index ee4ddb299e..a9c171f325 100644 --- a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp @@ -177,7 +177,7 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const PlanRequest const planning_pipeline::PlanningPipelinePtr pipeline = moveit_cpp_->getPlanningPipelines().at(parameters.planning_pipeline); pipeline->generatePlan(planning_scene, req, res); - last_plan_solution_.error_code_ = res.error_code_.val; + last_plan_solution_.error_code_ = res.error_code_; if (res.error_code_.val != res.error_code_.SUCCESS) { RCLCPP_ERROR(LOGGER, "Could not compute plan successfully"); From 107f6cc566d55fea9e5176ebd5293113fab46b77 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Wed, 6 Jul 2022 13:50:16 +0200 Subject: [PATCH 05/20] Add MultiPipelinePlanRequestParameters Add plan(const MultiPipelinePlanRequestParameters& parameters) Add mutex to avoid segfaults Add optional stop_criterion_callback and solution_selection_callback Remove stop_criterion_callback Make default solution_selection_callback = nullptr Remove parameter handling copy&paste code in favor of a template Add TODO to refactor pushBack() method into insert() Fix selection criteria and add RCLCPP_INFO output Changes due to rebase and formatting Fix race condition and segfault when no solution is found Satisfy clang tidy Remove mutex and thread safety TODOs Add stopping functionality to parallel planning Remove unnecessary TODOs --- .../moveit/moveit_cpp/planning_component.h | 100 +++++++++- .../moveit_cpp/src/planning_component.cpp | 175 ++++++++++++++++-- .../planning_pipeline/planning_pipeline.h | 15 +- .../src/planning_pipeline.cpp | 25 ++- 4 files changed, 282 insertions(+), 33 deletions(-) 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 07ff65afec..3346acff35 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 @@ -54,6 +54,36 @@ class PlanningComponent public: using MoveItErrorCode [[deprecated("Use moveit::core::MoveItErrorCode")]] = moveit::core::MoveItErrorCode; + class PlanSolutions + { + public: + /// \brief Constructor + 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 created a sorted container e.g. according to a user specified criteria + void pushBack(planning_interface::MotionPlanResponse plan_solution) + { + std::lock_guard lock_guard(solutions_mutex_); + solutions_.push_back(plan_solution); + } + + /// \brief Get solutions + std::vector const& getSolutions() + { + return solutions_; + } + + private: + std::vector solutions_; + std::mutex solutions_mutex_; + }; + /// Planner parameters provided with the MotionPlanRequest struct PlanRequestParameters { @@ -64,18 +94,64 @@ class PlanningComponent double max_velocity_scaling_factor; double max_acceleration_scaling_factor; - void load(const rclcpp::Node::SharedPtr& node) + template + void declareOrGetParam(rclcpp::Node::SharedPtr const& node, std::string const& param_name, T& output_value, + T default_value) + { + // Try to get parameter or use default + if (!node->get_parameter_or(param_name, output_value, default_value)) + { + RCLCPP_WARN(node->get_logger(), + "Parameter \'%s\' not found in config use default value instead, check parameter type and " + "namespace in YAML file", + (param_name).c_str()); + } + } + + void load(const rclcpp::Node::SharedPtr& node, const std::string& param_namespace = "") { + // Set namespace std::string ns = "plan_request_params."; - node->get_parameter_or(ns + "planner_id", planner_id, std::string("")); - node->get_parameter_or(ns + "planning_pipeline", planning_pipeline, std::string("")); - node->get_parameter_or(ns + "planning_time", planning_time, 1.0); - node->get_parameter_or(ns + "planning_attempts", planning_attempts, 5); - node->get_parameter_or(ns + "max_velocity_scaling_factor", max_velocity_scaling_factor, 1.0); - node->get_parameter_or(ns + "max_acceleration_scaling_factor", max_acceleration_scaling_factor, 1.0); + if (!param_namespace.empty()) + { + ns = param_namespace + ".plan_request_params."; + } + + // Declare parameters + declareOrGetParam(node, ns + "planner_id", planner_id, std::string("")); + declareOrGetParam(node, ns + "planning_pipeline", planning_pipeline, std::string("")); + declareOrGetParam(node, ns + "planning_time", planning_time, 1.0); + declareOrGetParam(node, ns + "planning_attempts", planning_attempts, 5); + declareOrGetParam(node, ns + "max_velocity_scaling_factor", max_velocity_scaling_factor, 1.0); + declareOrGetParam(node, ns + "max_acceleration_scaling_factor", max_acceleration_scaling_factor, 1.0); + } + }; + + /// Planner parameters provided with the MotionPlanRequest + struct MultiPipelinePlanRequestParameters + { + std::vector multi_plan_request_parameters; + + void load(const rclcpp::Node::SharedPtr& node, const std::vector& planning_pipeline_names) + { + multi_plan_request_parameters.reserve(planning_pipeline_names.size()); + + for (auto const& planning_pipeline_name : planning_pipeline_names) + { + PlanRequestParameters parameters; + parameters.load(node, planning_pipeline_name); + multi_plan_request_parameters.push_back(parameters); + } } }; + /// \brief A solution callback function type for the parallel planning API of planning component + typedef std::function const& 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); @@ -143,7 +219,13 @@ 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, const bool update_last_solution = true); + + /** \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 MultiPipelinePlanRequestParameters& parameters, + SolutionCallbackFunction solution_selection_callback = nullptr, + StoppingCriterionFunction stopping_criterion_callback = 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. */ @@ -160,6 +242,8 @@ class PlanningComponent // The robot_model_ member variable of MoveItCpp class will manually free the joint_model_group_ resources const moveit::core::JointModelGroup* joint_model_group_; + std::mutex plan_mutex_; + // Planning std::set planning_pipeline_names_; // The start state used in the planning motion request diff --git a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp index a9c171f325..3e3ee299f5 100644 --- a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp @@ -41,6 +41,7 @@ #include #include #include +#include namespace moveit_cpp { @@ -59,7 +60,7 @@ PlanningComponent::PlanningComponent(const std::string& group_name, const MoveIt planning_pipeline_names_ = moveit_cpp_->getPlanningPipelineNames(group_name); plan_request_parameters_.load(node_); RCLCPP_DEBUG_STREAM( - LOGGER, "Plan request parameters loaded with --" + 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 << "," @@ -113,19 +114,24 @@ bool PlanningComponent::setTrajectoryConstraints(const moveit_msgs::msg::Traject return true; } -planning_interface::MotionPlanResponse PlanningComponent::plan(const PlanRequestParameters& parameters) +planning_interface::MotionPlanResponse PlanningComponent::plan(const PlanRequestParameters& parameters, const bool update_last_solution) { - last_plan_solution_ = planning_interface::MotionPlanResponse(); + auto plan_solution = planning_interface::MotionPlanResponse(); + + // check if joint_model_group exists if (!joint_model_group_) { RCLCPP_ERROR(LOGGER, "Failed to retrieve joint model group for name '%s'.", group_name_.c_str()); - last_plan_solution_.error_code_ = moveit::core::MoveItErrorCode::INVALID_GROUP_NAME; - return last_plan_solution_; + plan_solution.error_code_ = moveit::core::MoveItErrorCode::INVALID_GROUP_NAME; + if (update_last_solution) + { + last_plan_solution_ = plan_solution; + } + return plan_solution; } // Clone current planning scene - planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor = - moveit_cpp_->getPlanningSceneMonitorNonConst(); + 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); @@ -156,11 +162,14 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const PlanRequest if (current_goal_constraints_.empty()) { RCLCPP_ERROR(LOGGER, "No goal constraints set for planning request"); - last_plan_solution_.error_code_ = moveit::core::MoveItErrorCode::INVALID_GOAL_CONSTRAINTS; - return last_plan_solution_; + plan_solution.error_code_ = moveit::core::MoveItErrorCode::INVALID_GOAL_CONSTRAINTS; + if (update_last_solution) + { + last_plan_solution_ = plan_solution; + } + return plan_solution; } req.goal_constraints = current_goal_constraints_; - // Set path constraints req.path_constraints = current_path_constraints_; // Set trajectory constraints @@ -171,21 +180,31 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const PlanRequest if (planning_pipeline_names_.find(parameters.planning_pipeline) == planning_pipeline_names_.end()) { RCLCPP_ERROR(LOGGER, "No planning pipeline available for name '%s'", parameters.planning_pipeline.c_str()); - last_plan_solution_.error_code_ = moveit::core::MoveItErrorCode::FAILURE; - return last_plan_solution_; + plan_solution.error_code_ = moveit::core::MoveItErrorCode::FAILURE; + if (update_last_solution) + { + last_plan_solution_ = plan_solution; + } + return plan_solution; } const planning_pipeline::PlanningPipelinePtr pipeline = moveit_cpp_->getPlanningPipelines().at(parameters.planning_pipeline); pipeline->generatePlan(planning_scene, req, res); - last_plan_solution_.error_code_ = res.error_code_; + + plan_solution.error_code_ = res.error_code_; if (res.error_code_.val != res.error_code_.SUCCESS) { RCLCPP_ERROR(LOGGER, "Could not compute plan successfully"); - return last_plan_solution_; + if (update_last_solution) + { + last_plan_solution_ = plan_solution; + } + return plan_solution; } - last_plan_solution_.trajectory_ = res.trajectory_; - last_plan_solution_.planning_time_ = res.planning_time_; - last_plan_solution_.start_state_ = req.start_state; + 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; @@ -199,7 +218,127 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const PlanRequest // visual_tools_->publishRobotState(last_solution_trajectory_->getLastWayPoint(), rviz_visual_tools::TRANSLUCENT); // } //} - return last_plan_solution_; + + if (update_last_solution) + { + last_plan_solution_ = plan_solution; + } + return plan_solution; +} + +planning_interface::MotionPlanResponse PlanningComponent::plan(const MultiPipelinePlanRequestParameters& parameters, + SolutionCallbackFunction solution_selection_callback, + StoppingCriterionFunction stopping_criterion_callback) +{ + // Create solutions container + PlanningComponent::PlanSolutions planning_solutions{ parameters.multi_plan_request_parameters.size() }; + std::vector planning_threads; + planning_threads.reserve(parameters.multi_plan_request_parameters.size()); + + // Launch planning threads + for (auto const& plan_request_parameter : parameters.multi_plan_request_parameters) + { + auto planning_thread = std::thread([&]() { + try + { + auto solution = plan(plan_request_parameter, false); + planning_solutions.pushBack(solution); + } + catch (const std::exception& e) + { + RCLCPP_ERROR_STREAM(LOGGER, "Planning pipeline '" << plan_request_parameter.planning_pipeline.c_str() + << "' threw exception '" << e.what() << "'"); + auto plan_solution = std::make_shared(); + plan_solution->error_code_ = moveit::core::MoveItErrorCode::FAILURE; + } + }); + planning_threads.push_back(std::move(planning_thread)); + } + + // Launch monitor thread if a stopping criterion is provided as function arguments. Otherwise, + // the parallel planning process takes as long as the longest planner takes. + if (stopping_criterion_callback != nullptr) + { + auto monitor_thread = std::thread([&]() { + while (planning_solutions.getSolutions().size() < planning_threads.size()) + { + if (stopping_criterion_callback(planning_solutions, parameters)) + { + // Terminate planning pipelines + for (auto const& plan_request_parameter : parameters.multi_plan_request_parameters) + { + auto const& planning_pipeline = + moveit_cpp_->getPlanningPipelines().at(plan_request_parameter.planning_pipeline); + if (planning_pipeline->isActive()) + { + planning_pipeline->terminate(); + } + } + } + } + using namespace std::chrono_literals; + std::this_thread::sleep_for(0.1s); + }); + if (monitor_thread.joinable()) + { + monitor_thread.join(); + } + } + // Wait for threads to finish + for (auto& planning_thread : planning_threads) + { + if (planning_thread.joinable()) + { + planning_thread.join(); + } + } + + // Select solution + // If no custom solution selection callback is provided, choose the shortest path available + if (solution_selection_callback == nullptr) + { + RCLCPP_DEBUG(LOGGER, "No custom solution selection callback provided, automatically choose shortest path"); + auto const& solutions = planning_solutions.getSolutions(); + + // TODO Make debug before merge + for (auto const& solution : solutions) + { + RCLCPP_INFO_STREAM(LOGGER, + "Solution error code is: " << moveit::core::error_code_to_string(solution.error_code_.val)); + if (solution.trajectory_ != nullptr) + { + RCLCPP_INFO_STREAM(LOGGER, "Solution path length is: " << robot_trajectory::path_length(*solution.trajectory_)); + } + } + + // Find trajectory with minimal path + auto const shortest_trajectory = std::min_element(solutions.begin(), solutions.end(), + [](planning_interface::MotionPlanResponse const& solution_a, + planning_interface::MotionPlanResponse const& solution_b) { + // If both solutions were successful, check which path is shorter + if (solution_a && solution_b) + { + return robot_trajectory::path_length(*solution_a.trajectory_) < + robot_trajectory::path_length(*solution_b.trajectory_); + } + // If only solution a is successful, return a + else if (solution_a) + { + return true; + } + // Else return solution b, either because it is successful or not + return false; + }); + if (shortest_trajectory->trajectory_ != nullptr) + { + RCLCPP_INFO_STREAM(LOGGER, "Chosen solution with path length: " + << robot_trajectory::path_length(*shortest_trajectory->trajectory_)); + } + return *shortest_trajectory; + } + + // Return best solution determined by user defined callback + return solution_selection_callback(planning_solutions.getSolutions()); } planning_interface::MotionPlanResponse PlanningComponent::plan() diff --git a/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h b/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h index 3c3527da32..0a248b9fab 100644 --- a/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h +++ b/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h @@ -44,6 +44,7 @@ #include #include +#include #include "moveit_planning_pipeline_export.h" @@ -131,8 +132,7 @@ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline \param req The request for motion planning \param res The motion planning response */ bool generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res) const; + const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res); /** \brief Call the motion planner plugin and the sequence of planning request adapters (if any). \param planning_scene The planning scene where motion planning is to be done @@ -144,7 +144,7 @@ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline invalid in all situations. */ bool generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& adapter_added_state_index) const; + std::vector& adapter_added_state_index); /** \brief Request termination, if a generatePlan() function is currently computing plans */ void terminate() const; @@ -173,9 +173,18 @@ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline return robot_model_; } + /** \brief Get current status of the planning pipeline */ + bool isActive() const + { + return active_; + } + private: void configure(); + // Flag that indicates whether or not the planning pipeline is currently solving a planning problem + std::atomic active_; + std::shared_ptr node_; std::string parameter_namespace_; /// Flag indicating whether motion plans should be published as a moveit_msgs::msg::DisplayTrajectory diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp index 3fe5ea1bce..8ece62614f 100644 --- a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp +++ b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp @@ -53,7 +53,7 @@ planning_pipeline::PlanningPipeline::PlanningPipeline(const moveit::core::RobotM const std::string& parameter_namespace, const std::string& planner_plugin_param_name, const std::string& adapter_plugins_param_name) - : node_(node), parameter_namespace_(parameter_namespace), robot_model_(model) + : active_{ false }, node_(node), parameter_namespace_(parameter_namespace), robot_model_(model) { std::string planner_plugin_fullname = parameter_namespace_ + "." + planner_plugin_param_name; if (parameter_namespace_.empty()) @@ -85,7 +85,8 @@ planning_pipeline::PlanningPipeline::PlanningPipeline(const moveit::core::RobotM const std::string& parameter_namespace, const std::string& planner_plugin_name, const std::vector& adapter_plugin_names) - : node_(node) + : active_{ false } + , node_(node) , parameter_namespace_(parameter_namespace) , planner_plugin_name_(planner_plugin_name) , adapter_plugin_names_(adapter_plugin_names) @@ -231,7 +232,7 @@ void planning_pipeline::PlanningPipeline::checkSolutionPaths(bool flag) bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res) const + planning_interface::MotionPlanResponse& res) { std::vector dummy; return generatePlan(planning_scene, req, res, dummy); @@ -240,16 +241,24 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& adapter_added_state_index) const + std::vector& adapter_added_state_index) { + // Set planning pipeline active + active_ = true; + // broadcast the request we are about to work on, if needed if (publish_received_requests_) + { received_request_publisher_->publish(req); + } adapter_added_state_index.clear(); if (!planner_instance_) { RCLCPP_ERROR(LOGGER, "No planning plugin loaded. Cannot plan."); + // Set planning pipeline to inactive + + active_ = false; return false; } @@ -278,6 +287,9 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla catch (std::exception& ex) { RCLCPP_ERROR(LOGGER, "Exception caught: '%s'", ex.what()); + // Set planning pipeline to inactive + + active_ = false; return false; } bool valid = true; @@ -401,11 +413,16 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla "equivalent?"); } + // Set planning pipeline to inactive + + active_ = false; return solved && valid; } void planning_pipeline::PlanningPipeline::terminate() const { if (planner_instance_) + { planner_instance_->terminate(); + } } From 2dac7f400b68f5233032db4a715fb915fbcea4fe Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Wed, 28 Sep 2022 15:42:17 +0200 Subject: [PATCH 06/20] Fix unused plan solution with failure --- moveit_ros/planning/moveit_cpp/src/planning_component.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp index 3e3ee299f5..8834412d87 100644 --- a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp @@ -248,8 +248,9 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const MultiPipeli { RCLCPP_ERROR_STREAM(LOGGER, "Planning pipeline '" << plan_request_parameter.planning_pipeline.c_str() << "' threw exception '" << e.what() << "'"); - auto plan_solution = std::make_shared(); - plan_solution->error_code_ = moveit::core::MoveItErrorCode::FAILURE; + auto plan_solution = planning_interface::MotionPlanResponse(); + plan_solution.error_code_ = moveit::core::MoveItErrorCode::FAILURE; + planning_solutions.pushBack(plan_solution); } }); planning_threads.push_back(std::move(planning_thread)); From e09b18e1f886bf98031fa5ea02009328169e52b6 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Wed, 28 Sep 2022 16:46:26 +0200 Subject: [PATCH 07/20] Add sanity check for number of parallel planning problems --- .../planning/moveit_cpp/src/planning_component.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp index 8834412d87..8220c08429 100644 --- a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp @@ -235,6 +235,17 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const MultiPipeli 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) + { + 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); + } + // Launch planning threads for (auto const& plan_request_parameter : parameters.multi_plan_request_parameters) { From 7d89312c1ef46814bad97f74515df6a1c6e0b9a7 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 29 Sep 2022 10:19:30 +0200 Subject: [PATCH 08/20] Check stopping criterion when new solution is generated + make thread safe --- .../moveit/moveit_cpp/planning_component.h | 7 ++++--- .../moveit_cpp/src/planning_component.cpp | 21 +++++-------------- 2 files changed, 9 insertions(+), 19 deletions(-) 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 3346acff35..d88cc7406a 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 @@ -130,9 +130,8 @@ class PlanningComponent /// Planner parameters provided with the MotionPlanRequest struct MultiPipelinePlanRequestParameters { - std::vector multi_plan_request_parameters; - - void load(const rclcpp::Node::SharedPtr& node, const std::vector& planning_pipeline_names) + MultiPipelinePlanRequestParameters(const rclcpp::Node::SharedPtr& node, + const std::vector& planning_pipeline_names) { multi_plan_request_parameters.reserve(planning_pipeline_names.size()); @@ -143,6 +142,8 @@ class PlanningComponent multi_plan_request_parameters.push_back(parameters); } } + // Plan request parameters for the individual planning pipelines which run concurrently + std::vector multi_plan_request_parameters; }; /// \brief A solution callback function type for the parallel planning API of planning component diff --git a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp index 8220c08429..4d0d5de881 100644 --- a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp @@ -250,10 +250,10 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const MultiPipeli for (auto const& plan_request_parameter : parameters.multi_plan_request_parameters) { auto planning_thread = std::thread([&]() { + auto plan_solution = planning_interface::MotionPlanResponse(); try { auto solution = plan(plan_request_parameter, false); - planning_solutions.pushBack(solution); } catch (const std::exception& e) { @@ -263,16 +263,9 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const MultiPipeli plan_solution.error_code_ = moveit::core::MoveItErrorCode::FAILURE; planning_solutions.pushBack(plan_solution); } - }); - planning_threads.push_back(std::move(planning_thread)); - } + planning_solutions.pushBack(plan_solution); - // Launch monitor thread if a stopping criterion is provided as function arguments. Otherwise, - // the parallel planning process takes as long as the longest planner takes. - if (stopping_criterion_callback != nullptr) - { - auto monitor_thread = std::thread([&]() { - while (planning_solutions.getSolutions().size() < planning_threads.size()) + if (stopping_criterion_callback != nullptr) { if (stopping_criterion_callback(planning_solutions, parameters)) { @@ -288,14 +281,10 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const MultiPipeli } } } - using namespace std::chrono_literals; - std::this_thread::sleep_for(0.1s); }); - if (monitor_thread.joinable()) - { - monitor_thread.join(); - } + planning_threads.push_back(std::move(planning_thread)); } + // Wait for threads to finish for (auto& planning_thread : planning_threads) { From 9dc1ddec8330a9a4b3cdb65ff2552ac55941501c Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 29 Sep 2022 11:00:43 +0200 Subject: [PATCH 09/20] Add terminatePlanningPipeline() to MoveItCpp interface --- .../include/moveit/moveit_cpp/moveit_cpp.h | 3 +++ .../planning/moveit_cpp/src/moveit_cpp.cpp | 19 +++++++++++++++++++ .../moveit_cpp/src/planning_component.cpp | 7 +------ .../planning_pipeline/planning_pipeline.h | 2 +- 4 files changed, 24 insertions(+), 7 deletions(-) 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 68db705ba2..93a2ce34d1 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 @@ -172,6 +172,9 @@ class MoveItCpp const robot_trajectory::RobotTrajectoryPtr& robot_trajectory, bool blocking = true); + /** \brief Utility to terminate all active planning pipelines */ + bool terminatePlanningPipeline(std::string const& pipeline_name); + private: // Core properties and instances rclcpp::Node::SharedPtr node_; diff --git a/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp b/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp index 8f91cb8127..e07a97db92 100644 --- a/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp +++ b/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp @@ -276,6 +276,25 @@ MoveItCpp::execute(const std::string& group_name, const robot_trajectory::RobotT return moveit_controller_manager::ExecutionStatus::RUNNING; } +bool MoveItCpp::terminatePlanningPipeline(std::string const& pipeline_name) +{ + try + { + auto const& planning_pipeline = planning_pipelines_.at(pipeline_name); + if (planning_pipeline->isActive()) + { + planning_pipeline->terminate(); + } + return true; + } + catch (const std::out_of_range& oor) + { + RCLCPP_ERROR(LOGGER, "Cannot terminate pipeline '%s' because no pipeline with that name exists", + pipeline_name.c_str()); + return false; + } +} + const std::shared_ptr& MoveItCpp::getTFBuffer() const { return planning_scene_monitor_->getTFClient(); diff --git a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp index 4d0d5de881..2cc3999c9e 100644 --- a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp @@ -272,12 +272,7 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const MultiPipeli // Terminate planning pipelines for (auto const& plan_request_parameter : parameters.multi_plan_request_parameters) { - auto const& planning_pipeline = - moveit_cpp_->getPlanningPipelines().at(plan_request_parameter.planning_pipeline); - if (planning_pipeline->isActive()) - { - planning_pipeline->terminate(); - } + moveit_cpp_->terminatePlanningPipeline(plan_request_parameter.planning_pipeline); } } } diff --git a/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h b/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h index 0a248b9fab..285d1ab41b 100644 --- a/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h +++ b/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h @@ -174,7 +174,7 @@ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline } /** \brief Get current status of the planning pipeline */ - bool isActive() const + [[nodiscard]] bool isActive() const { return active_; } From 8e7fedebf245ed681b8bcd7a691f86ae7c236a9a Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 29 Sep 2022 13:27:33 +0200 Subject: [PATCH 10/20] Format! --- .../moveit/moveit_cpp/planning_component.h | 11 +++-- .../moveit_cpp/src/planning_component.cpp | 44 ++++++++++--------- 2 files changed, 30 insertions(+), 25 deletions(-) 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 d88cc7406a..90f91f943b 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 @@ -147,7 +147,9 @@ class PlanningComponent }; /// \brief A solution callback function type for the parallel planning API of planning component - typedef std::function const& solutions)> SolutionCallbackFunction; + typedef std::function const& solutions)> + SolutionCallbackFunction; /// \brief A stopping criterion callback function for the parallel planning API of planning component typedef std::function @@ -220,13 +222,14 @@ 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, const bool update_last_solution = true); + planning_interface::MotionPlanResponse plan(const PlanRequestParameters& parameters, + const bool update_last_solution = true); /** \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 MultiPipelinePlanRequestParameters& parameters, - SolutionCallbackFunction solution_selection_callback = nullptr, - StoppingCriterionFunction stopping_criterion_callback = nullptr); + SolutionCallbackFunction solution_selection_callback = nullptr, + StoppingCriterionFunction stopping_criterion_callback = 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. */ diff --git a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp index 2cc3999c9e..1c0c77c98b 100644 --- a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp @@ -114,7 +114,8 @@ bool PlanningComponent::setTrajectoryConstraints(const moveit_msgs::msg::Traject return true; } -planning_interface::MotionPlanResponse PlanningComponent::plan(const PlanRequestParameters& parameters, const bool update_last_solution) +planning_interface::MotionPlanResponse PlanningComponent::plan(const PlanRequestParameters& parameters, + const bool update_last_solution) { auto plan_solution = planning_interface::MotionPlanResponse(); @@ -195,7 +196,7 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const PlanRequest if (res.error_code_.val != res.error_code_.SUCCESS) { RCLCPP_ERROR(LOGGER, "Could not compute plan successfully"); - if (update_last_solution) + if (update_last_solution) { last_plan_solution_ = plan_solution; } @@ -227,8 +228,8 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const PlanRequest } planning_interface::MotionPlanResponse PlanningComponent::plan(const MultiPipelinePlanRequestParameters& parameters, - SolutionCallbackFunction solution_selection_callback, - StoppingCriterionFunction stopping_criterion_callback) + SolutionCallbackFunction solution_selection_callback, + StoppingCriterionFunction stopping_criterion_callback) { // Create solutions container PlanningComponent::PlanSolutions planning_solutions{ parameters.multi_plan_request_parameters.size() }; @@ -308,23 +309,24 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const MultiPipeli } // Find trajectory with minimal path - auto const shortest_trajectory = std::min_element(solutions.begin(), solutions.end(), - [](planning_interface::MotionPlanResponse const& solution_a, - planning_interface::MotionPlanResponse const& solution_b) { - // If both solutions were successful, check which path is shorter - if (solution_a && solution_b) - { - return robot_trajectory::path_length(*solution_a.trajectory_) < - robot_trajectory::path_length(*solution_b.trajectory_); - } - // If only solution a is successful, return a - else if (solution_a) - { - return true; - } - // Else return solution b, either because it is successful or not - return false; - }); + auto const shortest_trajectory = + std::min_element(solutions.begin(), solutions.end(), + [](planning_interface::MotionPlanResponse const& solution_a, + planning_interface::MotionPlanResponse const& solution_b) { + // If both solutions were successful, check which path is shorter + if (solution_a && solution_b) + { + return robot_trajectory::path_length(*solution_a.trajectory_) < + robot_trajectory::path_length(*solution_b.trajectory_); + } + // If only solution a is successful, return a + else if (solution_a) + { + return true; + } + // Else return solution b, either because it is successful or not + return false; + }); if (shortest_trajectory->trajectory_ != nullptr) { RCLCPP_INFO_STREAM(LOGGER, "Chosen solution with path length: " From 4b0fbf18a694533b35f1f8285693ec635df77846 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 29 Sep 2022 15:57:47 +0200 Subject: [PATCH 11/20] Bug fixes --- moveit_ros/planning/moveit_cpp/src/planning_component.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp index 1c0c77c98b..7e1055420f 100644 --- a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp @@ -239,7 +239,7 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const MultiPipeli // 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) + if (parameters.multi_plan_request_parameters.size() > hardware_concurrency && hardware_concurrency != 0) { RCLCPP_WARN( LOGGER, @@ -254,7 +254,7 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const MultiPipeli auto plan_solution = planning_interface::MotionPlanResponse(); try { - auto solution = plan(plan_request_parameter, false); + plan_solution = plan(plan_request_parameter, false); } catch (const std::exception& e) { @@ -262,7 +262,6 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const MultiPipeli << "' threw exception '" << e.what() << "'"); auto plan_solution = planning_interface::MotionPlanResponse(); plan_solution.error_code_ = moveit::core::MoveItErrorCode::FAILURE; - planning_solutions.pushBack(plan_solution); } planning_solutions.pushBack(plan_solution); @@ -271,6 +270,7 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const MultiPipeli 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 (auto const& plan_request_parameter : parameters.multi_plan_request_parameters) { moveit_cpp_->terminatePlanningPipeline(plan_request_parameter.planning_pipeline); From d8e78b28ea052c139c2f7e5ac00a24cc7381cc7d Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 29 Sep 2022 15:58:36 +0200 Subject: [PATCH 12/20] Move getShortestSolution callback into own function --- .../moveit/moveit_cpp/planning_component.h | 11 ++- .../moveit_cpp/src/planning_component.cpp | 80 ++++++++----------- 2 files changed, 42 insertions(+), 49 deletions(-) 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 90f91f943b..e0e476f354 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 @@ -49,6 +49,10 @@ namespace moveit_cpp { MOVEIT_CLASS_FORWARD(PlanningComponent); // Defines PlanningComponentPtr, ConstPtr, WeakPtr... etc +/// \brief A function to choose the solution with the shortest path from a vector of solutions +planning_interface::MotionPlanResponse +getShortestSolution(std::vector const& solutions); + class PlanningComponent { public: @@ -227,9 +231,10 @@ class PlanningComponent /** \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 MultiPipelinePlanRequestParameters& parameters, - SolutionCallbackFunction solution_selection_callback = nullptr, - StoppingCriterionFunction stopping_criterion_callback = nullptr); + planning_interface::MotionPlanResponse + plan(const MultiPipelinePlanRequestParameters& parameters, + SolutionCallbackFunction solution_selection_callback = &getShortestSolution, + StoppingCriterionFunction stopping_criterion_callback = 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. */ diff --git a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp index 7e1055420f..dbdce5b104 100644 --- a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp @@ -290,52 +290,7 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const MultiPipeli } } - // Select solution - // If no custom solution selection callback is provided, choose the shortest path available - if (solution_selection_callback == nullptr) - { - RCLCPP_DEBUG(LOGGER, "No custom solution selection callback provided, automatically choose shortest path"); - auto const& solutions = planning_solutions.getSolutions(); - - // TODO Make debug before merge - for (auto const& solution : solutions) - { - RCLCPP_INFO_STREAM(LOGGER, - "Solution error code is: " << moveit::core::error_code_to_string(solution.error_code_.val)); - if (solution.trajectory_ != nullptr) - { - RCLCPP_INFO_STREAM(LOGGER, "Solution path length is: " << robot_trajectory::path_length(*solution.trajectory_)); - } - } - - // Find trajectory with minimal path - auto const shortest_trajectory = - std::min_element(solutions.begin(), solutions.end(), - [](planning_interface::MotionPlanResponse const& solution_a, - planning_interface::MotionPlanResponse const& solution_b) { - // If both solutions were successful, check which path is shorter - if (solution_a && solution_b) - { - return robot_trajectory::path_length(*solution_a.trajectory_) < - robot_trajectory::path_length(*solution_b.trajectory_); - } - // If only solution a is successful, return a - else if (solution_a) - { - return true; - } - // Else return solution b, either because it is successful or not - return false; - }); - if (shortest_trajectory->trajectory_ != nullptr) - { - RCLCPP_INFO_STREAM(LOGGER, "Chosen solution with path length: " - << robot_trajectory::path_length(*shortest_trajectory->trajectory_)); - } - return *shortest_trajectory; - } - - // Return best solution determined by user defined callback + // Return best solution determined by user defined callback (Default: Shortest path) return solution_selection_callback(planning_solutions.getSolutions()); } @@ -460,4 +415,37 @@ planning_interface::MotionPlanResponse const& PlanningComponent::getLastMotionPl { return last_plan_solution_; } + +planning_interface::MotionPlanResponse +getShortestSolution(std::vector const& solutions) +{ + // Find trajectory with minimal path + auto const shortest_trajectory = std::min_element(solutions.begin(), solutions.end(), + [](planning_interface::MotionPlanResponse const& solution_a, + planning_interface::MotionPlanResponse const& solution_b) { + // If both solutions were successful, check which path is shorter + if (solution_a && solution_b) + { + return robot_trajectory::path_length(*solution_a.trajectory_) < + robot_trajectory::path_length(*solution_b.trajectory_); + } + // If only solution a is successful, return a + else if (solution_a) + { + return true; + } + // Else return solution b, either because it is successful or not + return false; + }); + if (shortest_trajectory->trajectory_ != nullptr) + { + RCLCPP_INFO(LOGGER, "Chosen solution with shortest path length: '%f'", + robot_trajectory::path_length(*shortest_trajectory->trajectory_)); + } + else + { + RCLCPP_INFO_STREAM(LOGGER, "Could not determine shortest path"); + } + return *shortest_trajectory; +} } // namespace moveit_cpp From 73d58ec35811590d5987ece52aa6cd7bcf7dcdff Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Fri, 28 Oct 2022 14:50:20 +0200 Subject: [PATCH 13/20] No east const --- .../include/moveit/moveit_cpp/moveit_cpp.h | 2 +- .../moveit/moveit_cpp/planning_component.h | 16 ++++++++-------- .../planning/moveit_cpp/src/moveit_cpp.cpp | 4 ++-- .../moveit_cpp/src/planning_component.cpp | 12 ++++++------ 4 files changed, 17 insertions(+), 17 deletions(-) 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 93a2ce34d1..678f2a9210 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 @@ -173,7 +173,7 @@ class MoveItCpp bool blocking = true); /** \brief Utility to terminate all active planning pipelines */ - bool terminatePlanningPipeline(std::string const& pipeline_name); + bool terminatePlanningPipeline(const std::string& pipeline_name); private: // Core properties and instances 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 e0e476f354..8f234c2115 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 @@ -51,7 +51,7 @@ MOVEIT_CLASS_FORWARD(PlanningComponent); // Defines PlanningComponentPtr, Const /// \brief A function to choose the solution with the shortest path from a vector of solutions planning_interface::MotionPlanResponse -getShortestSolution(std::vector const& solutions); +getShortestSolution(const std::vector& solutions); class PlanningComponent { @@ -78,7 +78,7 @@ class PlanningComponent } /// \brief Get solutions - std::vector const& getSolutions() + const std::vector& getSolutions() { return solutions_; } @@ -99,7 +99,7 @@ class PlanningComponent double max_acceleration_scaling_factor; template - void declareOrGetParam(rclcpp::Node::SharedPtr const& node, std::string const& param_name, T& output_value, + void declareOrGetParam(const rclcpp::Node::SharedPtr& node, const std::string& param_name, T& output_value, T default_value) { // Try to get parameter or use default @@ -139,7 +139,7 @@ class PlanningComponent { multi_plan_request_parameters.reserve(planning_pipeline_names.size()); - for (auto const& planning_pipeline_name : planning_pipeline_names) + for (const auto& planning_pipeline_name : planning_pipeline_names) { PlanRequestParameters parameters; parameters.load(node, planning_pipeline_name); @@ -152,11 +152,11 @@ class PlanningComponent /// \brief A solution callback function type for the parallel planning API of planning component typedef std::function const& solutions)> + const std::vector& solutions)> SolutionCallbackFunction; /// \brief A stopping criterion callback function for the parallel planning API of planning component - typedef std::function + typedef std::function StoppingCriterionFunction; /** \brief Constructor */ @@ -241,7 +241,7 @@ class PlanningComponent bool execute(bool blocking = true); /** \brief Return the last plan solution*/ - planning_interface::MotionPlanResponse const& getLastMotionPlanResponse(); + const planning_interface::MotionPlanResponse& getLastMotionPlanResponse(); private: // Core properties and instances diff --git a/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp b/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp index e07a97db92..7599d0c49f 100644 --- a/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp +++ b/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp @@ -276,11 +276,11 @@ MoveItCpp::execute(const std::string& group_name, const robot_trajectory::RobotT return moveit_controller_manager::ExecutionStatus::RUNNING; } -bool MoveItCpp::terminatePlanningPipeline(std::string const& pipeline_name) +bool MoveItCpp::terminatePlanningPipeline(const std::string& pipeline_name) { try { - auto const& planning_pipeline = planning_pipelines_.at(pipeline_name); + const auto& planning_pipeline = planning_pipelines_.at(pipeline_name); if (planning_pipeline->isActive()) { planning_pipeline->terminate(); diff --git a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp index dbdce5b104..85450596da 100644 --- a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp @@ -248,7 +248,7 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const MultiPipeli } // Launch planning threads - for (auto const& plan_request_parameter : parameters.multi_plan_request_parameters) + for (const auto& plan_request_parameter : parameters.multi_plan_request_parameters) { auto planning_thread = std::thread([&]() { auto plan_solution = planning_interface::MotionPlanResponse(); @@ -271,7 +271,7 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const MultiPipeli { // Terminate planning pipelines RCLCPP_ERROR_STREAM(LOGGER, "Stopping criterion met: Terminating planning pipelines that are still active"); - for (auto const& plan_request_parameter : parameters.multi_plan_request_parameters) + for (const auto& plan_request_parameter : parameters.multi_plan_request_parameters) { moveit_cpp_->terminatePlanningPipeline(plan_request_parameter.planning_pipeline); } @@ -411,18 +411,18 @@ bool PlanningComponent::execute(bool blocking) return moveit_cpp_->execute(group_name_, last_plan_solution_.trajectory_, blocking); } -planning_interface::MotionPlanResponse const& PlanningComponent::getLastMotionPlanResponse() +const planning_interface::MotionPlanResponse& PlanningComponent::getLastMotionPlanResponse() { return last_plan_solution_; } planning_interface::MotionPlanResponse -getShortestSolution(std::vector const& solutions) +getShortestSolution(const std::vector& solutions) { // Find trajectory with minimal path auto const shortest_trajectory = std::min_element(solutions.begin(), solutions.end(), - [](planning_interface::MotionPlanResponse const& solution_a, - planning_interface::MotionPlanResponse const& 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) { From 6dcbec546f444e8c2c72e8256e1a0c4fc8c1b219 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Fri, 28 Oct 2022 15:58:18 +0200 Subject: [PATCH 14/20] Remove PlanSolutions and make planner_id accessible --- .../planning_interface/planning_response.h | 2 ++ .../moveit/moveit_cpp/planning_component.h | 32 +---------------- .../moveit_cpp/src/planning_component.cpp | 36 ++----------------- 3 files changed, 5 insertions(+), 65 deletions(-) diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h b/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h index 3b2a042da2..c16bd0b397 100644 --- a/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h @@ -63,6 +63,7 @@ struct MotionPlanResponse moveit::core::MoveItErrorCode error_code_; /// The full starting state used for planning moveit_msgs::msg::RobotState start_state_; + std::string planner_id_; // Enable checking of query success or failure, for example if(response) ... explicit operator bool() const @@ -79,6 +80,7 @@ struct MotionPlanDetailedResponse std::vector description_; std::vector processing_time_; moveit_msgs::msg::MoveItErrorCodes error_code_; + std::string planner_id_; }; } // namespace planning_interface 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 8f234c2115..59a161f85f 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,6 +39,7 @@ #include #include +#include #include #include #include @@ -57,37 +58,6 @@ class PlanningComponent { public: using MoveItErrorCode [[deprecated("Use moveit::core::MoveItErrorCode")]] = moveit::core::MoveItErrorCode; - - class PlanSolutions - { - public: - /// \brief Constructor - 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 created a sorted container e.g. according to a user specified criteria - void pushBack(planning_interface::MotionPlanResponse plan_solution) - { - std::lock_guard lock_guard(solutions_mutex_); - solutions_.push_back(plan_solution); - } - - /// \brief Get solutions - const std::vector& getSolutions() - { - return solutions_; - } - - private: - std::vector solutions_; - std::mutex solutions_mutex_; - }; - /// Planner parameters provided with the MotionPlanRequest struct PlanRequestParameters { diff --git a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp index 85450596da..0caa67e7cd 100644 --- a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp @@ -232,7 +232,7 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const MultiPipeli StoppingCriterionFunction stopping_criterion_callback) { // Create solutions container - PlanningComponent::PlanSolutions planning_solutions{ parameters.multi_plan_request_parameters.size() }; + PlanSolutions planning_solutions{ parameters.multi_plan_request_parameters.size() }; std::vector planning_threads; planning_threads.reserve(parameters.multi_plan_request_parameters.size()); @@ -263,6 +263,7 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const MultiPipeli auto 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) @@ -415,37 +416,4 @@ const planning_interface::MotionPlanResponse& PlanningComponent::getLastMotionPl { return last_plan_solution_; } - -planning_interface::MotionPlanResponse -getShortestSolution(const std::vector& 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) { - // If both solutions were successful, check which path is shorter - if (solution_a && solution_b) - { - return robot_trajectory::path_length(*solution_a.trajectory_) < - robot_trajectory::path_length(*solution_b.trajectory_); - } - // If only solution a is successful, return a - else if (solution_a) - { - return true; - } - // Else return solution b, either because it is successful or not - return false; - }); - if (shortest_trajectory->trajectory_ != nullptr) - { - RCLCPP_INFO(LOGGER, "Chosen solution with shortest path length: '%f'", - robot_trajectory::path_length(*shortest_trajectory->trajectory_)); - } - else - { - RCLCPP_INFO_STREAM(LOGGER, "Could not determine shortest path"); - } - return *shortest_trajectory; -} } // namespace moveit_cpp From f3d7bc1f609f32d9d568b724534cb705e456cae5 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Fri, 28 Oct 2022 16:00:21 +0200 Subject: [PATCH 15/20] Make solution executable --- moveit_ros/planning/moveit_cpp/src/planning_component.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp index 0caa67e7cd..e3738709a9 100644 --- a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp @@ -292,7 +292,8 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const MultiPipeli } // Return best solution determined by user defined callback (Default: Shortest path) - return solution_selection_callback(planning_solutions.getSolutions()); + last_plan_solution_ = solution_selection_callback(planning_solutions.getSolutions()); + return last_plan_solution_; } planning_interface::MotionPlanResponse PlanningComponent::plan() From 56f4231d978cbaacd17342189ea95e7d94e47580 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 3 Nov 2022 10:57:14 +0100 Subject: [PATCH 16/20] Rename update_last_solution to store_solution --- .../include/moveit/moveit_cpp/planning_component.h | 3 +-- .../planning/moveit_cpp/src/planning_component.cpp | 14 +++++++------- 2 files changed, 8 insertions(+), 9 deletions(-) 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 59a161f85f..d66b7bbcc4 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 @@ -196,8 +196,7 @@ 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, - const bool update_last_solution = true); + planning_interface::MotionPlanResponse plan(const PlanRequestParameters& parameters, const bool store_solution = true); /** \brief Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() using the * provided PlanRequestParameters. */ diff --git a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp index e3738709a9..1691fe69b3 100644 --- a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp @@ -115,7 +115,7 @@ bool PlanningComponent::setTrajectoryConstraints(const moveit_msgs::msg::Traject } planning_interface::MotionPlanResponse PlanningComponent::plan(const PlanRequestParameters& parameters, - const bool update_last_solution) + const bool store_solution) { auto plan_solution = planning_interface::MotionPlanResponse(); @@ -124,7 +124,7 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const PlanRequest { 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; - if (update_last_solution) + if (store_solution) { last_plan_solution_ = plan_solution; } @@ -164,7 +164,7 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const PlanRequest { RCLCPP_ERROR(LOGGER, "No goal constraints set for planning request"); plan_solution.error_code_ = moveit::core::MoveItErrorCode::INVALID_GOAL_CONSTRAINTS; - if (update_last_solution) + if (store_solution) { last_plan_solution_ = plan_solution; } @@ -182,7 +182,7 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const PlanRequest { RCLCPP_ERROR(LOGGER, "No planning pipeline available for name '%s'", parameters.planning_pipeline.c_str()); plan_solution.error_code_ = moveit::core::MoveItErrorCode::FAILURE; - if (update_last_solution) + if (store_solution) { last_plan_solution_ = plan_solution; } @@ -196,7 +196,7 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const PlanRequest if (res.error_code_.val != res.error_code_.SUCCESS) { RCLCPP_ERROR(LOGGER, "Could not compute plan successfully"); - if (update_last_solution) + if (store_solution) { last_plan_solution_ = plan_solution; } @@ -220,7 +220,7 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const PlanRequest // } //} - if (update_last_solution) + if (store_solution) { last_plan_solution_ = plan_solution; } @@ -260,7 +260,7 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const MultiPipeli { RCLCPP_ERROR_STREAM(LOGGER, "Planning pipeline '" << plan_request_parameter.planning_pipeline.c_str() << "' threw exception '" << e.what() << "'"); - auto plan_solution = planning_interface::MotionPlanResponse(); + plan_solution = planning_interface::MotionPlanResponse(); plan_solution.error_code_ = moveit::core::MoveItErrorCode::FAILURE; } plan_solution.planner_id_ = plan_request_parameter.planner_id; From ca722657bab2180c08ebbbc78223969a441c6a87 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 3 Nov 2022 11:04:46 +0100 Subject: [PATCH 17/20] Alphabetize includes and include plan_solutions.hpp instead of .h --- .../include/moveit/moveit_cpp/planning_component.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) 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 d66b7bbcc4..535876b53d 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 @@ -37,14 +37,14 @@ #pragma once -#include -#include -#include -#include #include +#include +#include +#include #include +#include #include -#include +#include namespace moveit_cpp { From c872260c7ec8019e11fdeb89f05d1b73dd595742 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 3 Nov 2022 11:14:52 +0100 Subject: [PATCH 18/20] Address review --- .../include/moveit/moveit_cpp/planning_component.h | 8 +++----- .../include/moveit/planning_pipeline/planning_pipeline.h | 3 ++- 2 files changed, 5 insertions(+), 6 deletions(-) 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 535876b53d..f6fd3ea259 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 @@ -50,7 +50,7 @@ namespace moveit_cpp { MOVEIT_CLASS_FORWARD(PlanningComponent); // Defines PlanningComponentPtr, ConstPtr, WeakPtr... etc -/// \brief A function to choose the solution with the shortest path from a vector of solutions +/** \brief A function to choose the solution with the shortest path from a vector of solutions */ planning_interface::MotionPlanResponse getShortestSolution(const std::vector& solutions); @@ -120,11 +120,11 @@ class PlanningComponent std::vector multi_plan_request_parameters; }; - /// \brief A solution callback function type for the parallel planning API of planning component + /** \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 + /** \brief A stopping criterion callback function for the parallel planning API of planning component */ typedef std::function StoppingCriterionFunction; @@ -220,8 +220,6 @@ class PlanningComponent // The robot_model_ member variable of MoveItCpp class will manually free the joint_model_group_ resources const moveit::core::JointModelGroup* joint_model_group_; - std::mutex plan_mutex_; - // Planning std::set planning_pipeline_names_; // The start state used in the planning motion request diff --git a/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h b/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h index 285d1ab41b..d9015480f5 100644 --- a/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h +++ b/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h @@ -36,6 +36,8 @@ #pragma once +#include + #include #include #include @@ -44,7 +46,6 @@ #include #include -#include #include "moveit_planning_pipeline_export.h" From 5c73e317c013d0375258be99d803e793eb272a7a Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 3 Nov 2022 12:15:47 +0100 Subject: [PATCH 19/20] Add missing header --- .../moveit/moveit_cpp/plan_solutions.hpp | 105 ++++++++++++++++++ .../moveit/moveit_cpp/planning_component.h | 5 - 2 files changed, 105 insertions(+), 5 deletions(-) create mode 100644 moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/plan_solutions.hpp diff --git a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/plan_solutions.hpp b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/plan_solutions.hpp new file mode 100644 index 0000000000..0ee7771be0 --- /dev/null +++ b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/plan_solutions.hpp @@ -0,0 +1,105 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, 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 safe data structure for MotionPlanResponses and free functions to analyze them */ + +#pragma once + +#include +#include +#include + +namespace moveit_cpp +{ +MOVEIT_CLASS_FORWARD(PlanSolutions); // Defines PlanningComponentPtr, ConstPtr, WeakPtr... etc + +/** \brief A container to thread-safely store multiple MotionPlanResponses for later usage */ +class PlanSolutions +{ +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 created a sorted container e.g. according to a user specified criteria + */ + void pushBack(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! + */ +planning_interface::MotionPlanResponse +getShortestSolution(const std::vector& 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) { + // If both solutions were successful, check which path is shorter + if (solution_a && solution_b) + { + return robot_trajectory::path_length(*solution_a.trajectory_) < + robot_trajectory::path_length(*solution_b.trajectory_); + } + // If only solution a is successful, return a + else if (solution_a) + { + return true; + } + // Else return solution b, either because it is successful or not + return false; + }); + return *shortest_trajectory; +} +} // namespace moveit_cpp 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 f6fd3ea259..79ce7ed721 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 @@ -49,11 +49,6 @@ namespace moveit_cpp { MOVEIT_CLASS_FORWARD(PlanningComponent); // Defines PlanningComponentPtr, ConstPtr, WeakPtr... etc - -/** \brief A function to choose the solution with the shortest path from a vector of solutions */ -planning_interface::MotionPlanResponse -getShortestSolution(const std::vector& solutions); - class PlanningComponent { public: From 5c35acf70b0f1af0cec4543917b26d487a1b59c6 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 3 Nov 2022 14:51:01 +0100 Subject: [PATCH 20/20] Apply suggestions from code review Co-authored-by: AndyZe --- .../include/moveit/planning_interface/planning_response.h | 6 +++--- .../global_planner_plugins/src/moveit_planning_pipeline.cpp | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h b/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h index c16bd0b397..72db4fa777 100644 --- a/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h @@ -47,12 +47,12 @@ namespace planning_interface /// \brief Response to a planning query struct MotionPlanResponse { - // Constructor + /** \brief Constructor */ MotionPlanResponse() : trajectory_(nullptr), planning_time_(0.0), error_code_(moveit::core::MoveItErrorCode::FAILURE) { } - // Construct a ROS message from struct data + /** \brief Construct a ROS message from struct data */ void getMessage(moveit_msgs::msg::MotionPlanResponse& msg) const; // Trajectory generated by the queried planner @@ -65,7 +65,7 @@ struct MotionPlanResponse moveit_msgs::msg::RobotState start_state_; std::string planner_id_; - // Enable checking of query success or failure, for example if(response) ... + // \brief Enable checking of query success or failure, for example if(response) ... explicit operator bool() const { return bool(error_code_); diff --git a/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/src/moveit_planning_pipeline.cpp b/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/src/moveit_planning_pipeline.cpp index 912f51f87b..fa9995cb7e 100644 --- a/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/src/moveit_planning_pipeline.cpp +++ b/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/src/moveit_planning_pipeline.cpp @@ -132,7 +132,7 @@ moveit_msgs::msg::MotionPlanResponse MoveItPlanningPipeline::plan( // Plan motion auto plan_solution = planning_components->plan(plan_params); - if (plan_solution.error_code_ != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) + if (!bool(plan_solution.error_code_)) { response.error_code = plan_solution.error_code_; return response;