From 9bd1d868d29c0ee07ba5c03979b900de4dc95d67 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 16 Oct 2023 10:57:00 +0200 Subject: [PATCH 01/24] Enable using multiple planners in a row --- .../planning_pipeline/planning_pipeline.h | 171 +++++++++--------- .../src/planning_pipeline.cpp | 91 ++++++---- 2 files changed, 141 insertions(+), 121 deletions(-) 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 99f17b42fe..8e36a55b46 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 @@ -118,14 +118,13 @@ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline motion planning pipeline. \param model The robot model for which this pipeline is initialized. \param node The ROS node that should be used for reading parameters needed for configuration - \param parameter_namespace Parameter - namespace where the planner configurations are stored - \param request_adapter_plugin_names Optional vector of - RequestAdapter plugin names + \param parameter_namespace Parameter namespace where the planner configurations are stored + \param planner_plugin_names Names of the planner plugins to use + \param request_adapter_plugin_names Optional vector of RequestAdapter plugin names \param response_adapter_plugin_names Optional vector of ResponseAdapter plugin names */ PlanningPipeline(const moveit::core::RobotModelConstPtr& model, const std::shared_ptr& node, - const std::string& parameter_namespace, const std::string& planning_plugin_name, + const std::string& parameter_namespace, const std::vector& planner_plugin_names, const std::vector& request_adapter_plugin_names = std::vector(), const std::vector& response_adapter_plugin_names = std::vector()); @@ -162,102 +161,104 @@ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline const bool /*display_computed_motion_plans*/) const { return false; - } - /* - END BLOCK OF DEPRECATED FUNCTIONS - */ - - /** \brief Call the chain of planning request adapters, motion planner plugin, and planning response adapters in - sequence. \param planning_scene The planning scene where motion planning is to be done \param req The request for - motion planning \param res The motion planning response \param publish_received_requests Flag indicating whether - received requests should be published just before beginning processing (useful for debugging) - */ - [[nodiscard]] bool generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res, - const bool publish_received_requests = false) const; + [[deprecated("Please use getPlannerPluginNames().")]] const std::string& getPlannerPluginName() const + { + return pipeline_parameters_.planning_plugins.at(0); + } + [[deprecated("Removed from API.")]] const planning_interface::PlannerManagerPtr& getPlannerManager() + { + return planner_vector_.at(0); + } + /* + END BLOCK OF DEPRECATED FUNCTIONS + */ - /** \brief Request termination, if a generatePlan() function is currently computing plans */ - void terminate() const; + /** \brief Call the chain of planning request adapters, motion planner plugin, and planning response adapters in + sequence. \param planning_scene The planning scene where motion planning is to be done \param req The request for + motion planning \param res The motion planning response \param publish_received_requests Flag indicating whether + received requests should be published just before beginning processing (useful for debugging) + */ + [[nodiscard]] bool generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res, + const bool publish_received_requests = false) const; - /** \brief Get the name of the planning plugin used */ - [[nodiscard]] const std::string& getPlannerPluginName() const - { - return pipeline_parameters_.planning_plugin; - } + /** \brief Request termination, if a generatePlan() function is currently computing plans */ + void terminate() const; - /** \brief Get the names of the planning request adapter plugins used */ - [[nodiscard]] const std::vector& getRequestAdapterPluginNames() const - { - return pipeline_parameters_.request_adapters; - } + /** \brief Get the names of the planning plugins used */ + [[nodiscard]] const std::vector& getPlannerPluginNames() const + { + return pipeline_parameters_.planning_plugins; + } - /** \brief Get the names of the planning response adapter plugins in the order they are processed. */ - [[nodiscard]] const std::vector& getResponseAdapterPluginNames() const - { - return pipeline_parameters_.response_adapters; - } + /** \brief Get the names of the planning request adapter plugins used */ + [[nodiscard]] const std::vector& getRequestAdapterPluginNames() const + { + return pipeline_parameters_.request_adapters; + } - /** \brief Get the planner manager for the loaded planning plugin */ - [[nodiscard]] const planning_interface::PlannerManagerPtr& getPlannerManager() - { - return planner_instance_; - } + /** \brief Get the names of the planning response adapter plugins in the order they are processed. */ + [[nodiscard]] const std::vector& getResponseAdapterPluginNames() const + { + return pipeline_parameters_.response_adapters; + } - /** \brief Get the robot model that this pipeline is using */ - [[nodiscard]] const moveit::core::RobotModelConstPtr& getRobotModel() const - { - return robot_model_; - } + /** \brief Get the robot model that this pipeline is using */ + [[nodiscard]] const moveit::core::RobotModelConstPtr& getRobotModel() const + { + return robot_model_; + } - /** \brief Get current status of the planning pipeline */ - [[nodiscard]] bool isActive() const - { - return active_; - } + /** \brief Get current status of the planning pipeline */ + [[nodiscard]] bool isActive() const + { + return active_; + } -private: - /// \brief Helper function that is called by both constructors to configure and initialize a PlanningPipeline instance - void configure(); + private: + /// \brief Helper function that is called by both constructors to configure and initialize a PlanningPipeline instance + void configure(); - /** - * @brief Helper function to publish the planning pipeline state during the planning process - * - * @param req Current request to publish - * @param res Current pipeline result - * @param pipeline_stage Last pipeline stage that got invoked - */ - void publishPipelineState(moveit_msgs::msg::MotionPlanRequest req, const planning_interface::MotionPlanResponse& res, - const std::string& pipeline_stage) const; + /** + * @brief Helper function to publish the planning pipeline state during the planning process + * + * @param req Current request to publish + * @param res Current pipeline result + * @param pipeline_stage Last pipeline stage that got invoked + */ + void publishPipelineState(moveit_msgs::msg::MotionPlanRequest req, + const planning_interface::MotionPlanResponse& res, const std::string& pipeline_stage) + const; - // Flag that indicates whether or not the planning pipeline is currently solving a planning problem - mutable std::atomic active_; + // Flag that indicates whether or not the planning pipeline is currently solving a planning problem + mutable std::atomic active_; - // ROS node and parameters - std::shared_ptr node_; - const std::string parameter_namespace_; - planning_pipeline_parameters::Params pipeline_parameters_; + // ROS node and parameters + std::shared_ptr node_; + const std::string parameter_namespace_; + planning_pipeline_parameters::Params pipeline_parameters_; - // Planner plugin - std::unique_ptr> planner_plugin_loader_; - planning_interface::PlannerManagerPtr planner_instance_; + // Planner plugin + std::unique_ptr> planner_plugin_loader_; + std::vector planner_vector_; - // Plan request adapters - std::unique_ptr> request_adapter_plugin_loader_; - std::vector planning_request_adapter_vector_; + // Plan request adapters + std::unique_ptr> request_adapter_plugin_loader_; + std::vector planning_request_adapter_vector_; - // Plan response adapters - std::unique_ptr> response_adapter_plugin_loader_; - std::vector planning_response_adapter_vector_; + // Plan response adapters + std::unique_ptr> response_adapter_plugin_loader_; + std::vector planning_response_adapter_vector_; - // Robot model - moveit::core::RobotModelConstPtr robot_model_; + // Robot model + moveit::core::RobotModelConstPtr robot_model_; - /// Publish the planning pipeline progress - rclcpp::Publisher::SharedPtr progress_publisher_; + /// Publish the planning pipeline progress + rclcpp::Publisher::SharedPtr progress_publisher_; - rclcpp::Logger logger_; -}; + rclcpp::Logger logger_; + }; -MOVEIT_CLASS_FORWARD(PlanningPipeline); // Defines PlanningPipelinePtr, ConstPtr, WeakPtr... etc + MOVEIT_CLASS_FORWARD(PlanningPipeline); // Defines PlanningPipelinePtr, ConstPtr, WeakPtr... etc } // namespace planning_pipeline diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp index e17d075489..28817346df 100644 --- a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp +++ b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp @@ -65,7 +65,7 @@ PlanningPipeline::PlanningPipeline(const moveit::core::RobotModelConstPtr& model , robot_model_(model) , logger_(moveit::makeChildLogger("planning_pipeline")) { - pipeline_parameters_.planning_plugin = planner_plugin_name; + pipeline_parameters_.planning_plugins = planner_plugin_names; pipeline_parameters_.request_adapters = request_adapter_plugin_names; pipeline_parameters_.response_adapters = response_adapter_plugin_names; configure(); @@ -74,7 +74,7 @@ PlanningPipeline::PlanningPipeline(const moveit::core::RobotModelConstPtr& model void PlanningPipeline::configure() { // Check if planning plugin name is available - if (pipeline_parameters_.planning_plugin.empty()) + if (pipeline_parameters_.planning_plugins.empty()) { const std::string classes_str = fmt::format("{}", fmt::join(planner_plugin_loader_->getDeclaredClasses(), ", ")); throw std::runtime_error("Planning plugin name is empty. Please choose one of the available plugins: " + @@ -100,30 +100,45 @@ void PlanningPipeline::configure() throw; } - if (pipeline_parameters_.planning_plugin.empty() || pipeline_parameters_.planning_plugin == "UNKNOWN") + if (pipeline_parameters_.planning_plugins.empty() || pipeline_parameters_.planning_plugins.at(0) == "UNKNOWN") { const std::string classes_str = fmt::format("{}", fmt::join(planner_plugin_loader_->getDeclaredClasses(), ", ")); throw std::runtime_error("Planning plugin name is empty or not defined in namespace '" + parameter_namespace_ + "'. Please choose one of the available plugins: " + classes_str); } - try + for (const auto& planner_name : pipeline_parameters_.planning_plugins) { - planner_instance_ = planner_plugin_loader_->createUniqueInstance(pipeline_parameters_.planning_plugin); - if (!planner_instance_->initialize(robot_model_, node_, parameter_namespace_)) + planning_interface::PlannerManagerPtr planner_instance; + + // Load plugin + try { - throw std::runtime_error("Unable to initialize planning plugin"); + planner_instance = planner_plugin_loader_->createUniqueInstance(planner_name); } - RCLCPP_INFO(logger_, "Using planning interface '%s'", planner_instance_->getDescription().c_str()); - } - catch (pluginlib::PluginlibException& ex) - { - const std::string classes_str = fmt::format("{}", fmt::join(planner_plugin_loader_->getDeclaredClasses(), ", ")); - RCLCPP_FATAL(logger_, - "Exception while loading planner '%s': %s" - "Available plugins: %s", - pipeline_parameters_.planning_plugin.c_str(), ex.what(), classes_str.c_str()); - throw; + catch (pluginlib::PluginlibException& ex) + { + std::string classes_str = fmt::format("{}", fmt::join(planner_plugin_loader_->getDeclaredClasses(), ", ")); + RCLCPP_FATAL(LOGGER, + "Exception while loading planner '%s': %s" + "Available plugins: %s", + planner_name.c_str(), ex.what(), classes_str.c_str()); + throw; + } + + // Check if planner is not NULL + if (!planner_instance) + { + throw std::runtime_error("Unable to initialize planning plugin " + planner_name); + } + + // Initialize planner + if (!planner_instance->initialize(robot_model_, node_, parameter_namespace_)) + { + throw std::runtime_error("Unable to initialize planning plugin " + planner_name); + } + RCLCPP_INFO(LOGGER, "Successfully loaded planner '%s'", planner_instance->getDescription().c_str()); + planner_vector_.push_back(std::move(planner_instance)); } // Load the planner request adapters @@ -204,7 +219,7 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& planning_interface::MotionPlanResponse& res, const bool publish_received_requests) const { - assert(planner_instance_ != nullptr); + assert(!planner_vector_.empty()); // Set planning pipeline active active_ = true; @@ -241,22 +256,25 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& } } - // Call planner - if (res.error_code) + // Call planners + for (const auto& planner : planner_vector_) { - planning_interface::PlanningContextPtr context = - planner_instance_->getPlanningContext(planning_scene, mutable_request, res.error_code); - if (context) + if (res.error_code) { - context->solve(res); - publishPipelineState(mutable_request, res, planner_instance_->getDescription()); - } - else - { - RCLCPP_ERROR(node_->get_logger(), - "Failed to create PlanningContext for planner '%s'. Aborting planning pipeline.", - planner_instance_->getDescription().c_str()); - res.error_code = moveit::core::MoveItErrorCode::PLANNING_FAILED; + planning_interface::PlanningContextPtr context = + planner->getPlanningContext(planning_scene, mutable_request, res.error_code); + if (context) + { + context->solve(res); + publishPipelineState(mutable_request, res, planner->getDescription()); + } + else + { + RCLCPP_ERROR(node_->get_logger(), + "Failed to create PlanningContext for planner '%s'. Aborting planning pipeline.", + planner->getDescription().c_str()); + res.error_code = moveit::core::MoveItErrorCode::PLANNING_FAILED; + } } } @@ -305,9 +323,10 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& void PlanningPipeline::terminate() const { - if (planner_instance_) - { - planner_instance_->terminate(); - } + for (const auto& planner : planner_vector_) + if (planner) + { + planner->terminate(); + } } } // namespace planning_pipeline From 01ff6fb1adc7565216e85a8b75be0621a0835d92 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 16 Oct 2023 15:08:10 +0200 Subject: [PATCH 02/24] Update configs --- .../default_configs/chomp_planning.yaml | 3 ++- .../default_configs/ompl_planning.yaml | 5 ++++- .../pilz_industrial_motion_planner_planning.yaml | 12 ++++++++++-- .../default_configs/stomp_planning.yaml | 4 ++-- .../planning_pipeline/src/planning_pipeline.cpp | 2 +- 5 files changed, 19 insertions(+), 7 deletions(-) diff --git a/moveit_configs_utils/default_configs/chomp_planning.yaml b/moveit_configs_utils/default_configs/chomp_planning.yaml index b632d65c05..93ee55c4a7 100644 --- a/moveit_configs_utils/default_configs/chomp_planning.yaml +++ b/moveit_configs_utils/default_configs/chomp_planning.yaml @@ -1,4 +1,5 @@ -planning_plugin: chomp_interface/CHOMPPlanner +planning_plugin: + - chomp_interface/CHOMPPlanner enable_failure_recovery: true # The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline. request_adapters: diff --git a/moveit_configs_utils/default_configs/ompl_planning.yaml b/moveit_configs_utils/default_configs/ompl_planning.yaml index 9c02c44bdd..4588751d52 100644 --- a/moveit_configs_utils/default_configs/ompl_planning.yaml +++ b/moveit_configs_utils/default_configs/ompl_planning.yaml @@ -1,4 +1,7 @@ -planning_plugin: ompl_interface/OMPLPlanner +planning_plugins: + - ompl_interface/OMPLPlanner +start_state_max_bounds_error: 0.1 +jiggle_fraction: 0.05 # The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline. request_adapters: - default_planning_request_adapters/ResolveConstraintFrames diff --git a/moveit_configs_utils/default_configs/pilz_industrial_motion_planner_planning.yaml b/moveit_configs_utils/default_configs/pilz_industrial_motion_planner_planning.yaml index 9925a81a8b..d0a07ce4c2 100644 --- a/moveit_configs_utils/default_configs/pilz_industrial_motion_planner_planning.yaml +++ b/moveit_configs_utils/default_configs/pilz_industrial_motion_planner_planning.yaml @@ -1,6 +1,14 @@ -planning_plugin: pilz_industrial_motion_planner/CommandPlanner -# The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline. +planning_plugins: + - pilz_industrial_motion_planner/CommandPlanner default_planner_config: PTP +request_adapters: + - default_planning_request_adapters/ResolveConstraintFrames + - default_planning_request_adapters/ValidateWorkspaceBounds + - default_planning_request_adapters/CheckStartStateBounds + - default_planning_request_adapters/CheckStartStateCollision +response_adapters: + - default_planning_response_adapters/ValidateSolution + - default_planning_response_adapters/DisplayMotionPath capabilities: >- pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService diff --git a/moveit_configs_utils/default_configs/stomp_planning.yaml b/moveit_configs_utils/default_configs/stomp_planning.yaml index 38120524cb..e811ac9e91 100644 --- a/moveit_configs_utils/default_configs/stomp_planning.yaml +++ b/moveit_configs_utils/default_configs/stomp_planning.yaml @@ -1,5 +1,5 @@ -planning_plugin: stomp_moveit/StompPlanner -# The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline. +planning_plugins: + - stomp_moveit/StompPlanner request_adapters: - default_planning_request_adapters/ResolveConstraintFrames - default_planning_request_adapters/ValidateWorkspaceBounds diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp index 28817346df..4332addf95 100644 --- a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp +++ b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp @@ -129,7 +129,7 @@ void PlanningPipeline::configure() // Check if planner is not NULL if (!planner_instance) { - throw std::runtime_error("Unable to initialize planning plugin " + planner_name); + throw std::runtime_error("Unable to initialize planning plugin " + planner_name + ". Planner instance is nullptr."); } // Initialize planner From 77cd9066a02741978ee239558daefa8eab33fc59 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Tue, 17 Oct 2023 12:05:58 +0200 Subject: [PATCH 03/24] Enable planner chaining --- .../src/planning_pipeline.cpp | 42 ++++++++++++++++++- 1 file changed, 41 insertions(+), 1 deletion(-) diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp index 4332addf95..45fcb5ba3c 100644 --- a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp +++ b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp @@ -40,6 +40,40 @@ namespace planning_pipeline { +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros_planning.planning_pipeline"); + +/** + * @brief Transform a joint trajectory into a vector of joint constraints + * + * @param trajectory Reference trajectory from which the joint constraints are created + * @return A vector of joint constraints each corresponding to a waypoint of the reference trajectory. + */ +[[nodiscard]] std::vector +getTrajectoryConstraints(const robot_trajectory::RobotTrajectoryPtr& trajectory) +{ + const std::vector joint_names = + trajectory->getFirstWayPoint().getJointModelGroup(trajectory->getGroupName())->getActiveJointModelNames(); + + std::vector trajectory_constraints; + // Iterate through waypoints and create a joint constraint for each + for (size_t waypoint_index = 0; waypoint_index < trajectory->getWayPointCount(); ++waypoint_index) + { + moveit_msgs::msg::Constraints new_waypoint_constraint; + // Iterate through joints and copy waypoint data to joint constraint + for (size_t joint_index = 0; joint_index < joint_names.size(); ++joint_index) + { + moveit_msgs::msg::JointConstraint new_joint_constraint; + new_joint_constraint.joint_name = joint_names.at(joint_index); + new_joint_constraint.position = + trajectory->getWayPoint(waypoint_index).getVariablePosition(joint_names.at(joint_index)); + new_waypoint_constraint.joint_constraints.push_back(new_joint_constraint); + } + trajectory_constraints.push_back(new_waypoint_constraint); + } + return trajectory_constraints; +} +} // namespace + PlanningPipeline::PlanningPipeline(const moveit::core::RobotModelConstPtr& model, const std::shared_ptr& node, const std::string& parameter_namespace) : active_{ false } @@ -129,7 +163,8 @@ void PlanningPipeline::configure() // Check if planner is not NULL if (!planner_instance) { - throw std::runtime_error("Unable to initialize planning plugin " + planner_name + ". Planner instance is nullptr."); + throw std::runtime_error("Unable to initialize planning plugin " + planner_name + + ". Planner instance is nullptr."); } // Initialize planner @@ -261,6 +296,11 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& { if (res.error_code) { + // Update reference trajectory with latest solution (if available) + if (res.trajectory) + { + mutable_request.trajectory_constraints.constraints = getTrajectoryConstraints(res.trajectory); + } planning_interface::PlanningContextPtr context = planner->getPlanningContext(planning_scene, mutable_request, res.error_code); if (context) From c4e5485797a4cfff6d054312c3de12468186d9f0 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Wed, 18 Oct 2023 09:22:24 +0200 Subject: [PATCH 04/24] Update planner_plugin param to planner_plugins param --- .../planning_pipeline/res/planning_pipeline_parameters.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/moveit_ros/planning/planning_pipeline/res/planning_pipeline_parameters.yaml b/moveit_ros/planning/planning_pipeline/res/planning_pipeline_parameters.yaml index 29d81597cd..41ef31d187 100644 --- a/moveit_ros/planning/planning_pipeline/res/planning_pipeline_parameters.yaml +++ b/moveit_ros/planning/planning_pipeline/res/planning_pipeline_parameters.yaml @@ -1,9 +1,9 @@ planning_pipeline_parameters: - planning_plugin: { - type: string, + planning_plugins: { + type: string_array, description: "Name of the planner plugin used by the pipeline", read_only: true, - default_value: "UNKNOWN", + default_value: ["UNKNOWN"], } request_adapters: { type: string_array, From a7f9006e85aee1edc4d393cd4bf1913bfadf6c81 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 16 Nov 2023 15:38:15 +0100 Subject: [PATCH 05/24] Make compile --- .../planning_pipeline/planning_pipeline.h | 164 +++++++++--------- .../src/planning_pipeline.cpp | 6 +- .../test/planning_pipeline_tests.cpp | 12 +- 3 files changed, 92 insertions(+), 90 deletions(-) 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 8e36a55b46..ff923b5d7d 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 @@ -161,104 +161,104 @@ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline const bool /*display_computed_motion_plans*/) const { return false; - [[deprecated("Please use getPlannerPluginNames().")]] const std::string& getPlannerPluginName() const - { - return pipeline_parameters_.planning_plugins.at(0); - } - [[deprecated("Removed from API.")]] const planning_interface::PlannerManagerPtr& getPlannerManager() - { - return planner_vector_.at(0); - } - /* - END BLOCK OF DEPRECATED FUNCTIONS - */ + } + [[deprecated("Please use getPlannerPluginNames().")]] const std::string& getPlannerPluginName() const + { + return pipeline_parameters_.planning_plugins.at(0); + } + [[deprecated("Removed from API.")]] const planning_interface::PlannerManagerPtr& getPlannerManager() + { + return planner_vector_.at(0); + } + /* + END BLOCK OF DEPRECATED FUNCTIONS + */ - /** \brief Call the chain of planning request adapters, motion planner plugin, and planning response adapters in - sequence. \param planning_scene The planning scene where motion planning is to be done \param req The request for - motion planning \param res The motion planning response \param publish_received_requests Flag indicating whether - received requests should be published just before beginning processing (useful for debugging) - */ - [[nodiscard]] bool generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res, - const bool publish_received_requests = false) const; + /** \brief Call the chain of planning request adapters, motion planner plugin, and planning response adapters in + sequence. \param planning_scene The planning scene where motion planning is to be done \param req The request for + motion planning \param res The motion planning response \param publish_received_requests Flag indicating whether + received requests should be published just before beginning processing (useful for debugging) + */ + [[nodiscard]] bool generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res, + const bool publish_received_requests = false) const; - /** \brief Request termination, if a generatePlan() function is currently computing plans */ - void terminate() const; + /** \brief Request termination, if a generatePlan() function is currently computing plans */ + void terminate() const; - /** \brief Get the names of the planning plugins used */ - [[nodiscard]] const std::vector& getPlannerPluginNames() const - { - return pipeline_parameters_.planning_plugins; - } + /** \brief Get the names of the planning plugins used */ + [[nodiscard]] const std::vector& getPlannerPluginNames() const + { + return pipeline_parameters_.planning_plugins; + } - /** \brief Get the names of the planning request adapter plugins used */ - [[nodiscard]] const std::vector& getRequestAdapterPluginNames() const - { - return pipeline_parameters_.request_adapters; - } + /** \brief Get the names of the planning request adapter plugins used */ + [[nodiscard]] const std::vector& getRequestAdapterPluginNames() const + { + return pipeline_parameters_.request_adapters; + } - /** \brief Get the names of the planning response adapter plugins in the order they are processed. */ - [[nodiscard]] const std::vector& getResponseAdapterPluginNames() const - { - return pipeline_parameters_.response_adapters; - } + /** \brief Get the names of the planning response adapter plugins in the order they are processed. */ + [[nodiscard]] const std::vector& getResponseAdapterPluginNames() const + { + return pipeline_parameters_.response_adapters; + } - /** \brief Get the robot model that this pipeline is using */ - [[nodiscard]] const moveit::core::RobotModelConstPtr& getRobotModel() const - { - return robot_model_; - } + /** \brief Get the robot model that this pipeline is using */ + [[nodiscard]] const moveit::core::RobotModelConstPtr& getRobotModel() const + { + return robot_model_; + } - /** \brief Get current status of the planning pipeline */ - [[nodiscard]] bool isActive() const - { - return active_; - } + /** \brief Get current status of the planning pipeline */ + [[nodiscard]] bool isActive() const + { + return active_; + } - private: - /// \brief Helper function that is called by both constructors to configure and initialize a PlanningPipeline instance - void configure(); +private: + /// \brief Helper function that is called by both constructors to configure and initialize a PlanningPipeline instance + void configure(); - /** - * @brief Helper function to publish the planning pipeline state during the planning process - * - * @param req Current request to publish - * @param res Current pipeline result - * @param pipeline_stage Last pipeline stage that got invoked - */ - void publishPipelineState(moveit_msgs::msg::MotionPlanRequest req, - const planning_interface::MotionPlanResponse& res, const std::string& pipeline_stage) - const; + /** + * @brief Helper function to publish the planning pipeline state during the planning process + * + * @param req Current request to publish + * @param res Current pipeline result + * @param pipeline_stage Last pipeline stage that got invoked + */ + void publishPipelineState(moveit_msgs::msg::MotionPlanRequest req, const planning_interface::MotionPlanResponse& res, + const std::string& pipeline_stage) const; - // Flag that indicates whether or not the planning pipeline is currently solving a planning problem - mutable std::atomic active_; + // Flag that indicates whether or not the planning pipeline is currently solving a planning problem + mutable std::atomic active_; - // ROS node and parameters - std::shared_ptr node_; - const std::string parameter_namespace_; - planning_pipeline_parameters::Params pipeline_parameters_; + // ROS node and parameters + std::shared_ptr node_; + const std::string parameter_namespace_; + planning_pipeline_parameters::Params pipeline_parameters_; - // Planner plugin - std::unique_ptr> planner_plugin_loader_; - std::vector planner_vector_; + // Planner plugin + std::unique_ptr> planner_plugin_loader_; + std::vector planner_vector_; - // Plan request adapters - std::unique_ptr> request_adapter_plugin_loader_; - std::vector planning_request_adapter_vector_; + // Plan request adapters + std::unique_ptr> request_adapter_plugin_loader_; + std::vector planning_request_adapter_vector_; - // Plan response adapters - std::unique_ptr> response_adapter_plugin_loader_; - std::vector planning_response_adapter_vector_; + // Plan response adapters + std::unique_ptr> response_adapter_plugin_loader_; + std::vector planning_response_adapter_vector_; - // Robot model - moveit::core::RobotModelConstPtr robot_model_; + // Robot model + moveit::core::RobotModelConstPtr robot_model_; - /// Publish the planning pipeline progress - rclcpp::Publisher::SharedPtr progress_publisher_; + /// Publish the planning pipeline progress + rclcpp::Publisher::SharedPtr progress_publisher_; - rclcpp::Logger logger_; - }; + rclcpp::Logger logger_; +}; - MOVEIT_CLASS_FORWARD(PlanningPipeline); // Defines PlanningPipelinePtr, ConstPtr, WeakPtr... etc +MOVEIT_CLASS_FORWARD(PlanningPipeline); // Defines PlanningPipelinePtr, ConstPtr, WeakPtr... etc } // namespace planning_pipeline diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp index 45fcb5ba3c..05a971e71f 100644 --- a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp +++ b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp @@ -38,7 +38,7 @@ #include #include -namespace planning_pipeline +namespace { const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros_planning.planning_pipeline"); @@ -74,6 +74,8 @@ getTrajectoryConstraints(const robot_trajectory::RobotTrajectoryPtr& trajectory) } } // namespace +namespace planning_pipeline +{ PlanningPipeline::PlanningPipeline(const moveit::core::RobotModelConstPtr& model, const std::shared_ptr& node, const std::string& parameter_namespace) : active_{ false } @@ -90,7 +92,7 @@ PlanningPipeline::PlanningPipeline(const moveit::core::RobotModelConstPtr& model PlanningPipeline::PlanningPipeline(const moveit::core::RobotModelConstPtr& model, const std::shared_ptr& node, const std::string& parameter_namespace, - const std::string& planner_plugin_name, + const std::vector& planner_plugin_names, const std::vector& request_adapter_plugin_names, const std::vector& response_adapter_plugin_names) : active_{ false } diff --git a/moveit_ros/planning/planning_pipeline/test/planning_pipeline_tests.cpp b/moveit_ros/planning/planning_pipeline/test/planning_pipeline_tests.cpp index 010058554c..506c508e59 100644 --- a/moveit_ros/planning/planning_pipeline/test/planning_pipeline_tests.cpp +++ b/moveit_ros/planning/planning_pipeline/test/planning_pipeline_tests.cpp @@ -45,7 +45,8 @@ const std::vector REQUEST_ADAPTERS{ "planning_pipeline_test/AlwaysS "planning_pipeline_test/AlwaysSuccessRequestAdapter" }; const std::vector RESPONSE_ADAPTERS{ "planning_pipeline_test/AlwaysSuccessResponseAdapter", "planning_pipeline_test/AlwaysSuccessResponseAdapter" }; -const std::string PLANNER_PLUGIN = std::string("planning_pipeline_test/DummyPlannerManager"); +const std::vector PLANNER_PLUGINS{ "planning_pipeline_test/DummyPlannerManager", + "planning_pipeline_test/DummyPlannerManager" }; } // namespace class TestPlanningPipeline : public testing::Test { @@ -70,15 +71,13 @@ TEST_F(TestPlanningPipeline, HappyPath) // WHEN the planning pipeline is configured // THEN it is successful EXPECT_NO_THROW(pipeline_ptr_ = std::make_shared( - robot_model_, node_, "", PLANNER_PLUGIN, REQUEST_ADAPTERS, RESPONSE_ADAPTERS)); + robot_model_, node_, "", PLANNER_PLUGINS, REQUEST_ADAPTERS, RESPONSE_ADAPTERS)); // THEN a planning pipeline exists EXPECT_NE(pipeline_ptr_, nullptr); // THEN the pipeline is inactive EXPECT_FALSE(pipeline_ptr_->isActive()); // THEN the pipeline contains a valid robot model EXPECT_NE(pipeline_ptr_->getRobotModel(), nullptr); - // THEN a planner plugin is loaded - EXPECT_NE(pipeline_ptr_->getPlannerManager(), nullptr); // THEN the loaded request adapter names equal the adapter names input vector const auto loaded_req_adapters = pipeline_ptr_->getRequestAdapterPluginNames(); EXPECT_TRUE(std::equal(loaded_req_adapters.begin(), loaded_req_adapters.end(), REQUEST_ADAPTERS.begin())); @@ -86,7 +85,8 @@ TEST_F(TestPlanningPipeline, HappyPath) const auto loaded_res_adapters = pipeline_ptr_->getResponseAdapterPluginNames(); EXPECT_TRUE(std::equal(loaded_res_adapters.begin(), loaded_res_adapters.end(), RESPONSE_ADAPTERS.begin())); // THEN the loaded planner plugin name equals the planner name input argument - EXPECT_EQ(pipeline_ptr_->getPlannerPluginName(), PLANNER_PLUGIN); + const auto loaded_planners = pipeline_ptr_->getPlannerPluginNames(); + EXPECT_TRUE(std::equal(loaded_planners.begin(), loaded_planners.end(), PLANNER_PLUGINS.begin())); // WHEN generatePlan is called // THEN A successful response is created @@ -103,7 +103,7 @@ TEST_F(TestPlanningPipeline, NoPlannerPluginConfigured) // WHEN the pipeline is configured // THEN an exception is thrown EXPECT_THROW(pipeline_ptr_ = std::make_shared( - robot_model_, node_, "", "NoExistingPlannerPlugin", REQUEST_ADAPTERS, RESPONSE_ADAPTERS), + robot_model_, node_, "", std::vector(), REQUEST_ADAPTERS, RESPONSE_ADAPTERS), std::runtime_error); } From c02af9603e8b38bcd1a6f3699bbb605ac1f55bfb Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 16 Nov 2023 16:00:34 +0100 Subject: [PATCH 06/24] All planners must have a description! --- .../include/moveit/planning_interface/planning_interface.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h b/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h index c41c8505ab..0c5a139d54 100644 --- a/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h @@ -165,7 +165,7 @@ class PlannerManager const std::string& parameter_namespace); /// Get \brief a short string that identifies the planning interface - virtual std::string getDescription() const; + virtual std::string getDescription() const = 0; /// \brief Get the names of the known planning algorithms (values that can be filled as planner_id in the planning /// request) From 842c9801fc2ff88787cc90e6d101fe9783d0a85f Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 16 Nov 2023 16:00:47 +0100 Subject: [PATCH 07/24] Fix unittests --- .../planning/planning_pipeline/src/planning_pipeline.cpp | 9 +-------- .../test/planning_pipeline_test_plugins.cpp | 4 ++++ .../planning_pipeline/test/planning_pipeline_tests.cpp | 8 ++++++++ 3 files changed, 13 insertions(+), 8 deletions(-) diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp index 05a971e71f..ead92be84f 100644 --- a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp +++ b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp @@ -109,14 +109,6 @@ PlanningPipeline::PlanningPipeline(const moveit::core::RobotModelConstPtr& model void PlanningPipeline::configure() { - // Check if planning plugin name is available - if (pipeline_parameters_.planning_plugins.empty()) - { - const std::string classes_str = fmt::format("{}", fmt::join(planner_plugin_loader_->getDeclaredClasses(), ", ")); - throw std::runtime_error("Planning plugin name is empty. Please choose one of the available plugins: " + - classes_str); - } - // If progress topic parameter is not empty, initialize publisher if (!pipeline_parameters_.progress_topic.empty()) { @@ -307,6 +299,7 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& planner->getPlanningContext(planning_scene, mutable_request, res.error_code); if (context) { + RCLCPP_INFO(node_->get_logger(), "Calling Planner '%s'", planner->getDescription().c_str()); context->solve(res); publishPipelineState(mutable_request, res, planner->getDescription()); } diff --git a/moveit_ros/planning/planning_pipeline/test/planning_pipeline_test_plugins.cpp b/moveit_ros/planning/planning_pipeline/test/planning_pipeline_test_plugins.cpp index 1b2940f981..1b5d8986ac 100644 --- a/moveit_ros/planning/planning_pipeline/test/planning_pipeline_test_plugins.cpp +++ b/moveit_ros/planning/planning_pipeline/test/planning_pipeline_test_plugins.cpp @@ -122,6 +122,10 @@ class DummyPlannerManager : public planning_interface::PlannerManager { return true; } + std::string getDescription() const override + { + return std::string("DummyPlannerManager"); + } }; } // namespace planning_pipeline_test diff --git a/moveit_ros/planning/planning_pipeline/test/planning_pipeline_tests.cpp b/moveit_ros/planning/planning_pipeline/test/planning_pipeline_tests.cpp index 506c508e59..7100a69f09 100644 --- a/moveit_ros/planning/planning_pipeline/test/planning_pipeline_tests.cpp +++ b/moveit_ros/planning/planning_pipeline/test/planning_pipeline_tests.cpp @@ -105,6 +105,14 @@ TEST_F(TestPlanningPipeline, NoPlannerPluginConfigured) EXPECT_THROW(pipeline_ptr_ = std::make_shared( robot_model_, node_, "", std::vector(), REQUEST_ADAPTERS, RESPONSE_ADAPTERS), std::runtime_error); + + // GIVEN a configuration with planner plugin called UNKNOWN + // WHEN the pipeline is configured + // THEN an exception is thrown + EXPECT_THROW(pipeline_ptr_ = std::make_shared( + robot_model_, node_, "", std::vector({ "UNKNOWN" }), REQUEST_ADAPTERS, + RESPONSE_ADAPTERS), + std::runtime_error); } int main(int argc, char** argv) From ef3575fb8fb2ab4e766720ac08c78e423aa81c9a Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Fri, 17 Nov 2023 16:56:33 +0100 Subject: [PATCH 08/24] Fix small bug --- .../src/planning_pipeline.cpp | 48 +++++++++++-------- 1 file changed, 27 insertions(+), 21 deletions(-) diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp index ead92be84f..4f071e4602 100644 --- a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp +++ b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp @@ -288,28 +288,34 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& // Call planners for (const auto& planner : planner_vector_) { - if (res.error_code) + // Update reference trajectory with latest solution (if available) + if (res.trajectory) { - // Update reference trajectory with latest solution (if available) - if (res.trajectory) - { - mutable_request.trajectory_constraints.constraints = getTrajectoryConstraints(res.trajectory); - } - planning_interface::PlanningContextPtr context = - planner->getPlanningContext(planning_scene, mutable_request, res.error_code); - if (context) - { - RCLCPP_INFO(node_->get_logger(), "Calling Planner '%s'", planner->getDescription().c_str()); - context->solve(res); - publishPipelineState(mutable_request, res, planner->getDescription()); - } - else - { - RCLCPP_ERROR(node_->get_logger(), - "Failed to create PlanningContext for planner '%s'. Aborting planning pipeline.", - planner->getDescription().c_str()); - res.error_code = moveit::core::MoveItErrorCode::PLANNING_FAILED; - } + mutable_request.trajectory_constraints.constraints = getTrajectoryConstraints(res.trajectory); + } + + // Try creating a planning context + planning_interface::PlanningContextPtr context = + planner->getPlanningContext(planning_scene, mutable_request, res.error_code); + if (!context) + { + RCLCPP_ERROR(node_->get_logger(), + "Failed to create PlanningContext for planner '%s'. Aborting planning pipeline.", + planner->getDescription().c_str()); + res.error_code = moveit::core::MoveItErrorCode::PLANNING_FAILED; + break; + } + + // Run planner + RCLCPP_INFO(node_->get_logger(), "Calling Planner '%s'", planner->getDescription().c_str()); + context->solve(res); + publishPipelineState(mutable_request, res, planner->getDescription()); + + // If planner does not succeed, break chain and return false + if (!res.error_code) + { + RCLCPP_ERROR(node_->get_logger(), "Planner '%s' failed", planner->getDescription()); + break; } } From 6130b20a8b50fc54626957a402e7d05c43442d56 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Fri, 17 Nov 2023 17:24:30 +0100 Subject: [PATCH 09/24] Address compilattion error --- moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp index 4f071e4602..6ac9312652 100644 --- a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp +++ b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp @@ -314,7 +314,7 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& // If planner does not succeed, break chain and return false if (!res.error_code) { - RCLCPP_ERROR(node_->get_logger(), "Planner '%s' failed", planner->getDescription()); + RCLCPP_ERROR(node_->get_logger(), "Planner '%s' failed", planner->getDescription().c_str()); break; } } From 27f3623569c54dada2fbe723dce275095ab8cdf1 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Wed, 22 Nov 2023 12:32:11 +0100 Subject: [PATCH 10/24] Update migration guide + remove unused variables --- MIGRATION.md | 1 + moveit_configs_utils/default_configs/ompl_planning.yaml | 2 -- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/MIGRATION.md b/MIGRATION.md index 04baab23c5..beba48f995 100644 --- a/MIGRATION.md +++ b/MIGRATION.md @@ -3,6 +3,7 @@ API changes in MoveIt releases ## ROS Rolling +- planning_plugin: ompl_interface/OMPLPlanner - [10/2023] Planning request adapters are now separated into PlanRequest (preprocessing) and PlanResponse (postprocessing) adapters. The adapters are configured with ROS parameter vectors (vector order corresponds to execution order). Please update your pipeline configurations for example like this: ```diff - request_adapters: >- diff --git a/moveit_configs_utils/default_configs/ompl_planning.yaml b/moveit_configs_utils/default_configs/ompl_planning.yaml index 4588751d52..3bb0acfc8a 100644 --- a/moveit_configs_utils/default_configs/ompl_planning.yaml +++ b/moveit_configs_utils/default_configs/ompl_planning.yaml @@ -1,7 +1,5 @@ planning_plugins: - ompl_interface/OMPLPlanner -start_state_max_bounds_error: 0.1 -jiggle_fraction: 0.05 # The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline. request_adapters: - default_planning_request_adapters/ResolveConstraintFrames From 622db435259ef7f3ccd94f72cd3c756c4251c73c Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Wed, 22 Nov 2023 13:04:05 +0100 Subject: [PATCH 11/24] Address clang-tidy --- MIGRATION.md | 6 + .../benchmarks/src/BenchmarkExecutor.cpp | 38 +---- .../query_planners_service_capability.cpp | 160 +++++++++--------- .../move_group/src/move_group_context.cpp | 7 +- .../planning_pipeline/planning_pipeline.h | 6 + .../src/planning_pipeline_interfaces.cpp | 2 +- 6 files changed, 99 insertions(+), 120 deletions(-) diff --git a/MIGRATION.md b/MIGRATION.md index beba48f995..3d92437e69 100644 --- a/MIGRATION.md +++ b/MIGRATION.md @@ -3,7 +3,13 @@ API changes in MoveIt releases ## ROS Rolling +- [10/2023] The planning pipeline now has a vector of planner plugins rather than a single one. Please update the planner plugin parameter e.g. like this: +```diff - planning_plugin: ompl_interface/OMPLPlanner ++ planning_plugins: ++ - ompl_interface/OMPLPlanner +``` + - [10/2023] Planning request adapters are now separated into PlanRequest (preprocessing) and PlanResponse (postprocessing) adapters. The adapters are configured with ROS parameter vectors (vector order corresponds to execution order). Please update your pipeline configurations for example like this: ```diff - request_adapters: >- diff --git a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp index 5de41d7724..b7997d7591 100644 --- a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp @@ -114,7 +114,7 @@ BenchmarkExecutor::~BenchmarkExecutor() const auto& pipeline = moveit_cpp_->getPlanningPipelines().at(planning_pipeline_name); // Verify the pipeline has successfully initialized a planner - if (!pipeline->getPlannerManager()) + if (!pipeline) { RCLCPP_ERROR(getLogger(), "Failed to initialize planning pipeline '%s'", planning_pipeline_name.c_str()); continue; @@ -542,42 +542,6 @@ bool BenchmarkExecutor::plannerConfigurationsExist( return false; } } - - // Make sure planners exist within those pipelines - auto planning_pipelines = moveit_cpp_->getPlanningPipelines(); - for (const std::pair>& entry : pipeline_configurations) - { - planning_interface::PlannerManagerPtr pm = planning_pipelines[entry.first]->getPlannerManager(); - const planning_interface::PlannerConfigurationMap& config_map = pm->getPlannerConfigurations(); - - // if the planner is chomp or stomp skip this function and return true for checking planner configurations for the - // planning group otherwise an error occurs, because for OMPL a specific planning algorithm needs to be defined for - // a planning group, whereas with STOMP and CHOMP this is not necessary - if (pm->getDescription().compare("stomp") || pm->getDescription().compare("chomp")) - continue; - - for (std::size_t i = 0; i < entry.second.size(); ++i) - { - bool planner_exists = false; - for (const std::pair& config_entry : - config_map) - { - std::string planner_name = group_name + "[" + entry.second[i] + "]"; - planner_exists = (config_entry.second.group == group_name && config_entry.second.name == planner_name); - } - - if (!planner_exists) - { - RCLCPP_ERROR(getLogger(), "Planner '%s' does NOT exist for group '%s' in pipeline '%s'", - entry.second[i].c_str(), group_name.c_str(), entry.first.c_str()); - std::cout << "There are " << config_map.size() << " planner entries: " << '\n'; - for (const auto& config_map_entry : config_map) - std::cout << config_map_entry.second.name << '\n'; - return false; - } - } - } - return true; } diff --git a/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.cpp index f6751eb0f4..118b0241fb 100644 --- a/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.cpp @@ -73,94 +73,98 @@ void MoveGroupQueryPlannersService::initialize() bool MoveGroupQueryPlannersService::queryInterface( const std::shared_ptr& /* unused */, const std::shared_ptr& /*req*/, - const std::shared_ptr& res) + const std::shared_ptr& /* unused */) { - for (const auto& planning_pipelines : context_->moveit_cpp_->getPlanningPipelines()) - { - const auto& pipeline_id = planning_pipelines.first; - const auto& planning_pipeline = planning_pipelines.second; - const planning_interface::PlannerManagerPtr& planner_interface = planning_pipeline->getPlannerManager(); - if (planner_interface) - { - std::vector algs; - planner_interface->getPlanningAlgorithms(algs); - moveit_msgs::msg::PlannerInterfaceDescription pi_desc; - pi_desc.name = planner_interface->getDescription(); - pi_desc.pipeline_id = pipeline_id; - planner_interface->getPlanningAlgorithms(pi_desc.planner_ids); - res->planner_interfaces.push_back(pi_desc); - } - } - return true; + // TODO(sjahr): This is currently not working. Need to decide whether to fix or remove this. + // for (const auto& planning_pipelines : context_->moveit_cpp_->getPlanningPipelines()) + // { + // const auto& pipeline_id = planning_pipelines.first; + // const auto& planning_pipeline = planning_pipelines.second; + // const planning_interface::PlannerManagerPtr& planner_interface = planning_pipeline->getPlannerManager(); + // if (planner_interface) + // { + // std::vector algs; + // planner_interface->getPlanningAlgorithms(algs); + // moveit_msgs::msg::PlannerInterfaceDescription pi_desc; + // pi_desc.name = planner_interface->getDescription(); + // pi_desc.pipeline_id = pipeline_id; + // planner_interface->getPlanningAlgorithms(pi_desc.planner_ids); + // res->planner_interfaces.push_back(pi_desc); + // } + // } + return false; } -bool MoveGroupQueryPlannersService::getParams(const std::shared_ptr& /* unused */, - const std::shared_ptr& req, - const std::shared_ptr& res) +bool MoveGroupQueryPlannersService::getParams( + const std::shared_ptr& /* unused */, + const std::shared_ptr& /* unused */, + const std::shared_ptr& /* unused */) { - const planning_pipeline::PlanningPipelinePtr planning_pipeline = resolvePlanningPipeline(req->pipeline_id); - if (!planning_pipeline) - return false; - - const planning_interface::PlannerManagerPtr& planner_interface = planning_pipeline->getPlannerManager(); - if (planner_interface) - { - std::map config; - - const planning_interface::PlannerConfigurationMap& configs = planner_interface->getPlannerConfigurations(); - - planning_interface::PlannerConfigurationMap::const_iterator it = - configs.find(req->planner_config); // fetch default params first - if (it != configs.end()) - config.insert(it->second.config.begin(), it->second.config.end()); - - if (!req->group.empty()) - { // merge in group-specific params - it = configs.find(req->group + "[" + req->planner_config + "]"); - if (it != configs.end()) - config.insert(it->second.config.begin(), it->second.config.end()); - } - - for (const auto& key_value_pair : config) - { - res->params.keys.push_back(key_value_pair.first); - res->params.values.push_back(key_value_pair.second); - } - } - return true; + // TODO(sjahr): This is currently not working. Need to decide whether to fix or remove this. + // const planning_pipeline::PlanningPipelinePtr planning_pipeline = resolvePlanningPipeline(req->pipeline_id); + // if (!planning_pipeline) + // return false; + // + // const planning_interface::PlannerManagerPtr& planner_interface = planning_pipeline->getPlannerManager(); + // if (planner_interface) + //{ + // std::map config; + // + // const planning_interface::PlannerConfigurationMap& configs = planner_interface->getPlannerConfigurations(); + // + // planning_interface::PlannerConfigurationMap::const_iterator it = + // configs.find(req->planner_config); // fetch default params first + // if (it != configs.end()) + // config.insert(it->second.config.begin(), it->second.config.end()); + // + // if (!req->group.empty()) + // { // merge in group-specific params + // it = configs.find(req->group + "[" + req->planner_config + "]"); + // if (it != configs.end()) + // config.insert(it->second.config.begin(), it->second.config.end()); + // } + // + // for (const auto& key_value_pair : config) + // { + // res->params.keys.push_back(key_value_pair.first); + // res->params.values.push_back(key_value_pair.second); + // } + //} + return false; } bool MoveGroupQueryPlannersService::setParams( const std::shared_ptr& /* unused */, - const std::shared_ptr& req, + const std::shared_ptr& /* unused */, const std::shared_ptr& /*res*/) { - if (req->params.keys.size() != req->params.values.size()) - return false; - - const planning_pipeline::PlanningPipelinePtr planning_pipeline = resolvePlanningPipeline(req->pipeline_id); - if (!planning_pipeline) - return false; - - const planning_interface::PlannerManagerPtr& planner_interface = planning_pipeline->getPlannerManager(); - - if (planner_interface) - { - planning_interface::PlannerConfigurationMap configs = planner_interface->getPlannerConfigurations(); - const std::string config_name = - req->group.empty() ? req->planner_config : req->group + "[" + req->planner_config + "]"; - - planning_interface::PlannerConfigurationSettings& config = configs[config_name]; - config.group = req->group; - config.name = config_name; - if (req->replace) - config.config.clear(); - for (unsigned int i = 0, end = req->params.keys.size(); i < end; ++i) - config.config[req->params.keys[i]] = req->params.values[i]; - - planner_interface->setPlannerConfigurations(configs); - } - return true; + // TODO(sjahr): This is currently not working. Need to decide whether to fix or remove this. + // if (req->params.keys.size() != req->params.values.size()) + // return false; + // + // const planning_pipeline::PlanningPipelinePtr planning_pipeline = resolvePlanningPipeline(req->pipeline_id); + // if (!planning_pipeline) + // return false; + // + // const planning_interface::PlannerManagerPtr& planner_interface = planning_pipeline->getPlannerManager(); + // + // if (planner_interface) + //{ + // planning_interface::PlannerConfigurationMap configs = planner_interface->getPlannerConfigurations(); + // const std::string config_name = + // req->group.empty() ? req->planner_config : req->group + "[" + req->planner_config + "]"; + // + // planning_interface::PlannerConfigurationSettings& config = configs[config_name]; + // config.group = req->group; + // config.name = config_name; + // if (req->replace) + // config.config.clear(); + // for (unsigned int i = 0, end = req->params.keys.size(); i < end; ++i) + // config.config[req->params.keys[i]] = req->params.values[i]; + // + // planner_interface->setPlannerConfigurations(configs); + //} + return false; } } // namespace move_group diff --git a/moveit_ros/move_group/src/move_group_context.cpp b/moveit_ros/move_group/src/move_group_context.cpp index 616df10bee..98b3915a21 100644 --- a/moveit_ros/move_group/src/move_group_context.cpp +++ b/moveit_ros/move_group/src/move_group_context.cpp @@ -82,18 +82,17 @@ move_group::MoveGroupContext::~MoveGroupContext() bool move_group::MoveGroupContext::status() const { - const planning_interface::PlannerManagerPtr& planner_interface = planning_pipeline_->getPlannerManager(); - if (planner_interface) + if (planning_pipeline_) { RCLCPP_INFO_STREAM(moveit::getLogger(), - "MoveGroup context using planning plugin " << planning_pipeline_->getPlannerPluginName()); + "MoveGroup context using pipeline " << planning_pipeline_->getName().c_str()); RCLCPP_INFO_STREAM(moveit::getLogger(), "MoveGroup context initialization complete"); return true; } else { RCLCPP_WARN_STREAM(moveit::getLogger(), - "MoveGroup running was unable to load " << planning_pipeline_->getPlannerPluginName()); + "MoveGroup running was unable to load pipeline " << planning_pipeline_->getName().c_str()); return false; } } 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 ff923b5d7d..22653f73f6 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 @@ -217,6 +217,12 @@ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline return active_; } + /** \brief Return the parameter namespace as name of the planning pipeline. */ + [[nodiscard]] std::string getName() const + { + return parameter_namespace_; + } + private: /// \brief Helper function that is called by both constructors to configure and initialize a PlanningPipeline instance void configure(); 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 index ad9f8da382..316c18e794 100644 --- a/moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp +++ b/moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp @@ -182,7 +182,7 @@ createPlanningPipelineMap(const std::vector& pipeline_names, planning_pipeline::PlanningPipelinePtr pipeline = std::make_shared( robot_model, node, parameter_namespace + planning_pipeline_name); - if (!pipeline->getPlannerManager()) + if (!pipeline) { RCLCPP_ERROR(get_logger(), "Failed to initialize planning pipeline '%s'.", planning_pipeline_name.c_str()); continue; From 2775e5bc033331467c25c7676fc9d1b326c40130 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Wed, 22 Nov 2023 13:39:45 +0100 Subject: [PATCH 12/24] Satisfy clang-tidy part 2 --- moveit_ros/benchmarks/src/BenchmarkExecutor.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp index b7997d7591..94c24de0e3 100644 --- a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp @@ -132,8 +132,7 @@ BenchmarkExecutor::~BenchmarkExecutor() for (const std::pair& entry : moveit_cpp_->getPlanningPipelines()) { - RCLCPP_INFO_STREAM(getLogger(), - "Pipeline: " << entry.first << ", Planner: " << entry.second->getPlannerPluginName()); + RCLCPP_INFO_STREAM(getLogger(), entry.first); } } return true; From 1fc4bfec7a6b5ff969a48c91ae3113377c31ad64 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Wed, 22 Nov 2023 14:21:08 +0100 Subject: [PATCH 13/24] Remove unsused param --- .../benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h | 3 +-- moveit_ros/benchmarks/src/BenchmarkExecutor.cpp | 4 ++-- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h index 9820ba4791..a8259839e4 100644 --- a/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h +++ b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h @@ -170,8 +170,7 @@ class BenchmarkExecutor void shiftConstraintsByOffset(moveit_msgs::msg::Constraints& constraints, const std::vector& offset); /// Check that the desired planner plugins and algorithms exist for the given group - bool plannerConfigurationsExist(const std::map>& planners, - const std::string& group_name); + bool plannerConfigurationsExist(const std::map>& planners); /// Load the planning scene with the given name from the warehouse bool loadPlanningScene(const std::string& scene_name, moveit_msgs::msg::PlanningScene& scene_msg); diff --git a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp index 94c24de0e3..3c79bd9fa0 100644 --- a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp @@ -259,7 +259,7 @@ bool BenchmarkExecutor::initializeBenchmarks(const BenchmarkOptions& options, moveit_msgs::msg::PlanningScene& scene_msg, std::vector& requests) { - if (!plannerConfigurationsExist(options.planning_pipelines, options.group_name)) + if (!plannerConfigurationsExist(options.planning_pipelines)) { return false; } @@ -521,7 +521,7 @@ void BenchmarkExecutor::createRequestCombinations(const BenchmarkRequest& benchm } bool BenchmarkExecutor::plannerConfigurationsExist( - const std::map>& pipeline_configurations, const std::string& group_name) + const std::map>& pipeline_configurations) { // Make sure planner plugins exist for (const std::pair>& pipeline_config_entry : pipeline_configurations) From a34e5454aa6ee393a598a580018759b79b5cb69a Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 27 Nov 2023 12:10:03 +0100 Subject: [PATCH 14/24] Apply suggestions from code review Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> --- moveit_configs_utils/default_configs/chomp_planning.yaml | 2 +- .../include/moveit/planning_interface/planning_interface.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_configs_utils/default_configs/chomp_planning.yaml b/moveit_configs_utils/default_configs/chomp_planning.yaml index 93ee55c4a7..07c1f32c46 100644 --- a/moveit_configs_utils/default_configs/chomp_planning.yaml +++ b/moveit_configs_utils/default_configs/chomp_planning.yaml @@ -1,4 +1,4 @@ -planning_plugin: +planning_plugins: - chomp_interface/CHOMPPlanner enable_failure_recovery: true # The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline. diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h b/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h index 0c5a139d54..76bb51bb3e 100644 --- a/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h @@ -164,7 +164,7 @@ class PlannerManager virtual bool initialize(const moveit::core::RobotModelConstPtr& model, const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace); - /// Get \brief a short string that identifies the planning interface + /// \brief Get a short string that identifies the planning interface. virtual std::string getDescription() const = 0; /// \brief Get the names of the known planning algorithms (values that can be filled as planner_id in the planning From 1acfadca322795ba208de6d0640b09134b4810ed Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 27 Nov 2023 12:05:10 +0100 Subject: [PATCH 15/24] Update planning_plugin to planning_plugins --- ...ittest_pilz_industrial_motion_planner.test.py | 6 +++--- .../unittest_pilz_industrial_motion_planner.cpp | 10 +++++----- .../prbt_moveit_config/launch/demo.launch.py | 16 ++++++++++++---- .../src/moveit_planning_pipeline.cpp | 2 +- .../test/launch/hybrid_planning_common.py | 14 ++++++++++++-- .../planning_pipeline_interfaces.hpp | 3 --- 6 files changed, 33 insertions(+), 18 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_pilz_industrial_motion_planner.test.py b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_pilz_industrial_motion_planner.test.py index 225fcbc58e..aa2dd03b64 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_pilz_industrial_motion_planner.test.py +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_pilz_industrial_motion_planner.test.py @@ -18,8 +18,8 @@ def generate_test_description(): # Load the context test_config = load_moveit_config() - planning_plugin = { - "planning_plugin": "pilz_industrial_motion_planner/CommandPlanner" + planning_plugins = { + "planning_plugins": ["pilz_industrial_motion_planner/CommandPlanner"] } # run test @@ -29,7 +29,7 @@ def generate_test_description(): name="unittest_pilz_industrial_motion_planner", parameters=[ test_config.to_dict(), - planning_plugin, + planning_plugins, ], output="screen", ) diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner.cpp index aecedafed3..24493d2cdc 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner.cpp @@ -74,8 +74,8 @@ class CommandPlannerTest : public testing::Test ASSERT_TRUE(bool(robot_model_)) << "Failed to load robot model"; // Load planner name from node parameters - ASSERT_TRUE(node_->has_parameter("planning_plugin")) << "Could not find parameter 'planning_plugin'"; - node_->get_parameter("planning_plugin", planner_plugin_name_); + ASSERT_TRUE(node_->has_parameter("planning_plugins")) << "Could not find parameter 'planning_plugins'"; + node_->get_parameter>("planning_plugins", planner_plugin_names_); // Load the plugin try @@ -92,7 +92,7 @@ class CommandPlannerTest : public testing::Test // Create planner try { - planner_instance_.reset(planner_plugin_loader_->createUnmanagedInstance(planner_plugin_name_)); + planner_instance_.reset(planner_plugin_loader_->createUnmanagedInstance(planner_plugin_names_.at(0))); ASSERT_TRUE(planner_instance_->initialize(robot_model_, node_, "")) << "Initializing the planner instance failed."; } @@ -104,7 +104,7 @@ class CommandPlannerTest : public testing::Test void TearDown() override { - planner_plugin_loader_->unloadLibraryForClass(planner_plugin_name_); + planner_plugin_loader_->unloadLibraryForClass(planner_plugin_names_.at(0)); robot_model_.reset(); } @@ -114,7 +114,7 @@ class CommandPlannerTest : public testing::Test moveit::core::RobotModelConstPtr robot_model_; std::unique_ptr rm_loader_; - std::string planner_plugin_name_; + std::vector planner_plugin_names_; std::unique_ptr> planner_plugin_loader_; planning_interface::PlannerManagerPtr planner_instance_; }; diff --git a/moveit_planners/test_configs/prbt_moveit_config/launch/demo.launch.py b/moveit_planners/test_configs/prbt_moveit_config/launch/demo.launch.py index d9a053f1e8..a71023ecc5 100644 --- a/moveit_planners/test_configs/prbt_moveit_config/launch/demo.launch.py +++ b/moveit_planners/test_configs/prbt_moveit_config/launch/demo.launch.py @@ -81,12 +81,20 @@ def generate_launch_description(): "default_planning_pipeline": "ompl", "planning_pipelines": ["pilz", "ompl"], "pilz": { - "planning_plugin": "pilz_industrial_motion_planner/CommandPlanner", - "request_adapters": "", + "planning_plugins": ["pilz_industrial_motion_planner/CommandPlanner"], }, "ompl": { - "planning_plugin": "ompl_interface/OMPLPlanner", - "request_adapters": """default_planning_request_adapters/AddTimeOptimalParameterization default_planning_request_adapters/ValidateWorkspaceBounds default_planning_request_adapters/CheckStartStateBounds default_planning_request_adapters/CheckStartStateCollision default_planning_request_adapters/FixStartStatePathConstraints""", + "planning_plugins": ["ompl_interface/OMPLPlanner"], + "request_adapters": [ + "default_planning_request_adapters/ResolveConstraintFrames", + "default_planning_request_adapters/ValidateWorkspaceBounds", + "default_planning_request_adapters/CheckStartStateBounds", + "default_planning_request_adapters/CheckStartStateCollision", + ], + "response_adapters": [ + "default_planning_response_adapters/AddTimeOptimalParameterization", + "default_planning_response_adapters/ValidateSolution", + ], }, } ompl_planning_yaml = load_yaml( 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 5a487e8708..d93bfde272 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 @@ -60,7 +60,7 @@ bool MoveItPlanningPipeline::initialize(const rclcpp::Node::SharedPtr& node) node->declare_parameter(PLAN_REQUEST_PARAM_NS + "planning_time", 1.0); node->declare_parameter(PLAN_REQUEST_PARAM_NS + "max_velocity_scaling_factor", 1.0); node->declare_parameter(PLAN_REQUEST_PARAM_NS + "max_acceleration_scaling_factor", 1.0); - node->declare_parameter("ompl.planning_plugin", "ompl_interface/OMPLPlanner"); + node->declare_parameter>("ompl.planning_plugins", { "ompl_interface/OMPLPlanner" }); // Planning Scene options node->declare_parameter(PLANNING_SCENE_MONITOR_NS + "name", UNDEFINED); diff --git a/moveit_ros/hybrid_planning/test/launch/hybrid_planning_common.py b/moveit_ros/hybrid_planning/test/launch/hybrid_planning_common.py index 5beba7753b..f88681dbc3 100644 --- a/moveit_ros/hybrid_planning/test/launch/hybrid_planning_common.py +++ b/moveit_ros/hybrid_planning/test/launch/hybrid_planning_common.py @@ -70,8 +70,18 @@ def generate_common_hybrid_launch_description(): # The global planner uses the typical OMPL parameters planning_pipelines_config = { "ompl": { - "planning_plugin": "ompl_interface/OMPLPlanner", - "request_adapters": """default_planning_request_adapters/AddTimeOptimalParameterization default_planning_request_adapters/ValidateWorkspaceBounds default_planning_request_adapters/CheckStartStateBounds default_planning_request_adapters/CheckStartStateCollision""", + "planning_plugins": ["ompl_interface/OMPLPlanner"], + "request_adapters": [ + "default_planning_request_adapters/ResolveConstraintFrames", + "default_planning_request_adapters/ValidateWorkspaceBounds", + "default_planning_request_adapters/CheckStartStateBounds", + "default_planning_request_adapters/CheckStartStateCollision", + ], + "response_adapters": [ + "default_planning_response_adapters/AddTimeOptimalParameterization", + "default_planning_response_adapters/ValidateSolution", + "default_planning_response_adapters/DisplayMotionPath", + ], } } ompl_planning_yaml = load_yaml( 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 index 21e3763351..9c9760e2e4 100644 --- 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 @@ -103,9 +103,6 @@ const std::vector<::planning_interface::MotionPlanResponse> planWithParallelPipe * \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 From f1aa815403b3e71886e36d6ec32e7483939235ac Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 27 Nov 2023 16:44:42 +0100 Subject: [PATCH 16/24] Rename BenchmarkExecutor::plannerConfigurationsExist to pipelinesExist --- .../benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h | 4 ++-- moveit_ros/benchmarks/src/BenchmarkExecutor.cpp | 5 ++--- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h index a8259839e4..6a0be17731 100644 --- a/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h +++ b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h @@ -169,8 +169,8 @@ class BenchmarkExecutor void shiftConstraintsByOffset(moveit_msgs::msg::Constraints& constraints, const std::vector& offset); - /// Check that the desired planner plugins and algorithms exist for the given group - bool plannerConfigurationsExist(const std::map>& planners); + /// Check that the desired planning pipelines exist + bool pipelinesExist(const std::map>& planners); /// Load the planning scene with the given name from the warehouse bool loadPlanningScene(const std::string& scene_name, moveit_msgs::msg::PlanningScene& scene_msg); diff --git a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp index 3c79bd9fa0..23895659f9 100644 --- a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp @@ -259,7 +259,7 @@ bool BenchmarkExecutor::initializeBenchmarks(const BenchmarkOptions& options, moveit_msgs::msg::PlanningScene& scene_msg, std::vector& requests) { - if (!plannerConfigurationsExist(options.planning_pipelines)) + if (!pipelinesExist(options.planning_pipelines)) { return false; } @@ -520,8 +520,7 @@ void BenchmarkExecutor::createRequestCombinations(const BenchmarkRequest& benchm } } -bool BenchmarkExecutor::plannerConfigurationsExist( - const std::map>& pipeline_configurations) +bool BenchmarkExecutor::pipelinesExist(const std::map>& pipeline_configurations) { // Make sure planner plugins exist for (const std::pair>& pipeline_config_entry : pipeline_configurations) From c687de56f58fee3acdca00d43598a3bbfb66b32e Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 27 Nov 2023 16:45:11 +0100 Subject: [PATCH 17/24] Make planner vector a planner map --- .../moveit/planning_pipeline/planning_pipeline.h | 15 +++++++++++++-- .../planning_pipeline/src/planning_pipeline.cpp | 13 +++++++------ 2 files changed, 20 insertions(+), 8 deletions(-) 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 22653f73f6..e02b99ba0b 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 @@ -168,7 +168,7 @@ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline } [[deprecated("Removed from API.")]] const planning_interface::PlannerManagerPtr& getPlannerManager() { - return planner_vector_.at(0); + return planner_map_.at(pipeline_parameters_.planning_plugins.at(0)); } /* END BLOCK OF DEPRECATED FUNCTIONS @@ -223,6 +223,17 @@ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline return parameter_namespace_; } + /** \brief Get access to planner plugin */ + const planning_interface::PlannerManagerConstPtr getPlannerManager(const std::string& planner_name) + { + if (planner_map_.find(planner_name) == planner_map_.end()) + { + RCLCPP_ERROR(node_->get_logger(), "Cannot retrieve planner because '%s' does not exist.", planner_name.c_str()); + return nullptr; + } + return planner_map_.at(planner_name); + } + private: /// \brief Helper function that is called by both constructors to configure and initialize a PlanningPipeline instance void configure(); @@ -247,7 +258,7 @@ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline // Planner plugin std::unique_ptr> planner_plugin_loader_; - std::vector planner_vector_; + std::unordered_map planner_map_; // Plan request adapters std::unique_ptr> request_adapter_plugin_loader_; diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp index 6ac9312652..d5d64320fd 100644 --- a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp +++ b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp @@ -167,7 +167,7 @@ void PlanningPipeline::configure() throw std::runtime_error("Unable to initialize planning plugin " + planner_name); } RCLCPP_INFO(LOGGER, "Successfully loaded planner '%s'", planner_instance->getDescription().c_str()); - planner_vector_.push_back(std::move(planner_instance)); + planner_map_.insert(std::make_pair(planner_name, planner_instance)); } // Load the planner request adapters @@ -248,7 +248,7 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& planning_interface::MotionPlanResponse& res, const bool publish_received_requests) const { - assert(!planner_vector_.empty()); + assert(!planner_map_.empty()); // Set planning pipeline active active_ = true; @@ -286,8 +286,9 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& } // Call planners - for (const auto& planner : planner_vector_) + for (const auto& planner_name : pipeline_parameters_.planning_plugins) { + const auto& planner = planner_map_.at(planner_name); // Update reference trajectory with latest solution (if available) if (res.trajectory) { @@ -364,10 +365,10 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& void PlanningPipeline::terminate() const { - for (const auto& planner : planner_vector_) - if (planner) + for (const auto& planner_pair : planner_map_) + if (planner_pair.second) { - planner->terminate(); + planner_pair.second->terminate(); } } } // namespace planning_pipeline From 301acac2ca5a9747ea07d1f834c6f35693105fc7 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 27 Nov 2023 16:45:24 +0100 Subject: [PATCH 18/24] Re-enable query planner service --- .../query_planners_service_capability.cpp | 232 ++++++++++-------- .../query_planners_service_capability.h | 9 +- .../planning_pipeline/planning_pipeline.h | 2 +- 3 files changed, 136 insertions(+), 107 deletions(-) diff --git a/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.cpp index 118b0241fb..171457f73f 100644 --- a/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.cpp @@ -38,6 +38,7 @@ #include #include #include +#include namespace move_group { @@ -49,122 +50,153 @@ void MoveGroupQueryPlannersService::initialize() { query_service_ = context_->moveit_cpp_->getNode()->create_service( QUERY_PLANNERS_SERVICE_NAME, - [this](const std::shared_ptr& request_header, - const std::shared_ptr& req, + [this](const std::shared_ptr& req, const std::shared_ptr& res) { - return queryInterface(request_header, req, res); + queryInterface(req, res); }); get_service_ = context_->moveit_cpp_->getNode()->create_service( - GET_PLANNER_PARAMS_SERVICE_NAME, [this](const std::shared_ptr& request_header, - const std::shared_ptr& req, - const std::shared_ptr& res) { - return getParams(request_header, req, res); - }); + GET_PLANNER_PARAMS_SERVICE_NAME, + [this](const std::shared_ptr& req, + const std::shared_ptr& res) { getParams(req, res); }); set_service_ = context_->moveit_cpp_->getNode()->create_service( - SET_PLANNER_PARAMS_SERVICE_NAME, [this](const std::shared_ptr& request_header, - const std::shared_ptr& req, - const std::shared_ptr& res) { - return setParams(request_header, req, res); - }); + SET_PLANNER_PARAMS_SERVICE_NAME, + [this](const std::shared_ptr& req, + const std::shared_ptr& res) { setParams(req, res); }); } -bool MoveGroupQueryPlannersService::queryInterface( - const std::shared_ptr& /* unused */, - const std::shared_ptr& /*req*/, - const std::shared_ptr& /* unused */) +void MoveGroupQueryPlannersService::queryInterface( + const std::shared_ptr& /* unused */, + const std::shared_ptr& res) { - // TODO(sjahr): This is currently not working. Need to decide whether to fix or remove this. - // for (const auto& planning_pipelines : context_->moveit_cpp_->getPlanningPipelines()) - // { - // const auto& pipeline_id = planning_pipelines.first; - // const auto& planning_pipeline = planning_pipelines.second; - // const planning_interface::PlannerManagerPtr& planner_interface = planning_pipeline->getPlannerManager(); - // if (planner_interface) - // { - // std::vector algs; - // planner_interface->getPlanningAlgorithms(algs); - // moveit_msgs::msg::PlannerInterfaceDescription pi_desc; - // pi_desc.name = planner_interface->getDescription(); - // pi_desc.pipeline_id = pipeline_id; - // planner_interface->getPlanningAlgorithms(pi_desc.planner_ids); - // res->planner_interfaces.push_back(pi_desc); - // } - // } - return false; + for (const auto& planning_pipelines : context_->moveit_cpp_->getPlanningPipelines()) + { + const auto& planning_pipeline = planning_pipelines.second; + + // TODO(sjahr): Update for multiple planner plugins + const auto& planner_plugin_names = planning_pipeline->getPlannerPluginNames(); + if (planner_plugin_names.empty()) + { + RCLCPP_ERROR(moveit::getLogger(), "Pipeline '%s' does not have any planner plugins.", + planning_pipelines.first.c_str()); + return; + } + const auto planner_interface = planning_pipeline->getPlannerManager(planner_plugin_names.at(0)); + if (!planner_interface) + { + RCLCPP_ERROR(moveit::getLogger(), "Requesting planner '%s' from '%s' returned a null pointer.", + planner_plugin_names.at(0).c_str(), planning_pipelines.first.c_str()); + } + std::vector algs; + planner_interface->getPlanningAlgorithms(algs); + moveit_msgs::msg::PlannerInterfaceDescription pi_desc; + pi_desc.name = planner_interface->getDescription(); + pi_desc.pipeline_id = planning_pipelines.first; + planner_interface->getPlanningAlgorithms(pi_desc.planner_ids); + res->planner_interfaces.push_back(pi_desc); + } } -bool MoveGroupQueryPlannersService::getParams( - const std::shared_ptr& /* unused */, - const std::shared_ptr& /* unused */, - const std::shared_ptr& /* unused */) +void MoveGroupQueryPlannersService::getParams(const std::shared_ptr& req, + const std::shared_ptr& res) { - // TODO(sjahr): This is currently not working. Need to decide whether to fix or remove this. - // const planning_pipeline::PlanningPipelinePtr planning_pipeline = resolvePlanningPipeline(req->pipeline_id); - // if (!planning_pipeline) - // return false; - // - // const planning_interface::PlannerManagerPtr& planner_interface = planning_pipeline->getPlannerManager(); - // if (planner_interface) - //{ - // std::map config; - // - // const planning_interface::PlannerConfigurationMap& configs = planner_interface->getPlannerConfigurations(); - // - // planning_interface::PlannerConfigurationMap::const_iterator it = - // configs.find(req->planner_config); // fetch default params first - // if (it != configs.end()) - // config.insert(it->second.config.begin(), it->second.config.end()); - // - // if (!req->group.empty()) - // { // merge in group-specific params - // it = configs.find(req->group + "[" + req->planner_config + "]"); - // if (it != configs.end()) - // config.insert(it->second.config.begin(), it->second.config.end()); - // } - // - // for (const auto& key_value_pair : config) - // { - // res->params.keys.push_back(key_value_pair.first); - // res->params.values.push_back(key_value_pair.second); - // } - //} - return false; + const planning_pipeline::PlanningPipelinePtr planning_pipeline = resolvePlanningPipeline(req->pipeline_id); + if (!planning_pipeline) + { + RCLCPP_ERROR(moveit::getLogger(), "Pipeline '%s' does not exist.", req->pipeline_id.c_str()); + return; + } + + // TODO(sjahr): Update for multiple planner plugins + const auto& planner_plugin_names = planning_pipeline->getPlannerPluginNames(); + if (planner_plugin_names.empty()) + { + RCLCPP_ERROR(moveit::getLogger(), "Pipeline '%s' does not have any planner plugins.", req->pipeline_id.c_str()); + return; + } + const auto planner_interface = planning_pipeline->getPlannerManager(planner_plugin_names.at(0)); + if (!planner_interface) + { + RCLCPP_ERROR(moveit::getLogger(), "Requesting planner '%s' from '%s' returned a null pointer.", + planner_plugin_names.at(0).c_str(), req->pipeline_id.c_str()); + } + std::map config; + + const planning_interface::PlannerConfigurationMap& configs = planner_interface->getPlannerConfigurations(); + + planning_interface::PlannerConfigurationMap::const_iterator it = + configs.find(req->planner_config); // fetch default params first + if (it != configs.end()) + { + config.insert(it->second.config.begin(), it->second.config.end()); + } + + if (!req->group.empty()) + { // merge in group-specific params + it = configs.find(req->group + "[" + req->planner_config + "]"); + if (it != configs.end()) + { + config.insert(it->second.config.begin(), it->second.config.end()); + } + } + + for (const auto& key_value_pair : config) + { + res->params.keys.push_back(key_value_pair.first); + res->params.values.push_back(key_value_pair.second); + } } -bool MoveGroupQueryPlannersService::setParams( - const std::shared_ptr& /* unused */, - const std::shared_ptr& /* unused */, +void MoveGroupQueryPlannersService::setParams( + const std::shared_ptr& req, const std::shared_ptr& /*res*/) { - // TODO(sjahr): This is currently not working. Need to decide whether to fix or remove this. - // if (req->params.keys.size() != req->params.values.size()) - // return false; - // - // const planning_pipeline::PlanningPipelinePtr planning_pipeline = resolvePlanningPipeline(req->pipeline_id); - // if (!planning_pipeline) - // return false; - // - // const planning_interface::PlannerManagerPtr& planner_interface = planning_pipeline->getPlannerManager(); - // - // if (planner_interface) - //{ - // planning_interface::PlannerConfigurationMap configs = planner_interface->getPlannerConfigurations(); - // const std::string config_name = - // req->group.empty() ? req->planner_config : req->group + "[" + req->planner_config + "]"; - // - // planning_interface::PlannerConfigurationSettings& config = configs[config_name]; - // config.group = req->group; - // config.name = config_name; - // if (req->replace) - // config.config.clear(); - // for (unsigned int i = 0, end = req->params.keys.size(); i < end; ++i) - // config.config[req->params.keys[i]] = req->params.values[i]; - // - // planner_interface->setPlannerConfigurations(configs); - //} - return false; + if (req->params.keys.size() != req->params.values.size()) + { + RCLCPP_ERROR(moveit::getLogger(), "Number of parameter names does not match the number of parameters"); + return; + } + + const planning_pipeline::PlanningPipelinePtr planning_pipeline = resolvePlanningPipeline(req->pipeline_id); + if (!planning_pipeline) + { + RCLCPP_ERROR(moveit::getLogger(), "Pipeline '%s' does not exist.", req->pipeline_id.c_str()); + return; + } + + // TODO(sjahr): Update for multiple planner plugins + const auto& planner_plugin_names = planning_pipeline->getPlannerPluginNames(); + if (planner_plugin_names.empty()) + { + RCLCPP_ERROR(moveit::getLogger(), "Pipeline '%s' does not have any planner plugins.", req->pipeline_id.c_str()); + return; + } + auto planner_interface = planning_pipeline->getPlannerManager(planner_plugin_names.at(0)); + if (!planner_interface) + { + RCLCPP_ERROR(moveit::getLogger(), "Requesting planner '%s' from '%s' returned a null pointer.", + planner_plugin_names.at(0).c_str(), req->pipeline_id.c_str()); + return; + } + + planning_interface::PlannerConfigurationMap configs = planner_interface->getPlannerConfigurations(); + const std::string config_name = + req->group.empty() ? req->planner_config : req->group + "[" + req->planner_config + "]"; + + planning_interface::PlannerConfigurationSettings& config = configs[config_name]; + config.group = req->group; + config.name = config_name; + if (req->replace) + { + config.config.clear(); + } + for (unsigned int i = 0, end = req->params.keys.size(); i < end; ++i) + { + config.config[req->params.keys.at(i)] = req->params.values.at(i); + } + + planner_interface->setPlannerConfigurations(configs); } } // namespace move_group diff --git a/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.h b/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.h index 00103c17b5..8fa02ea1ef 100644 --- a/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.h @@ -51,15 +51,12 @@ class MoveGroupQueryPlannersService : public MoveGroupCapability void initialize() override; private: - bool queryInterface(const std::shared_ptr& request_header, - const std::shared_ptr& /*req*/, + void queryInterface(const std::shared_ptr& /*req*/, const std::shared_ptr& res); - bool getParams(const std::shared_ptr& request_header, - const std::shared_ptr& req, + void getParams(const std::shared_ptr& req, const std::shared_ptr& res); - bool setParams(const std::shared_ptr& request_header, - const std::shared_ptr& req, + void setParams(const std::shared_ptr& req, const std::shared_ptr& /*res*/); rclcpp::Service::SharedPtr query_service_; 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 e02b99ba0b..d8336e4ad9 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 @@ -224,7 +224,7 @@ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline } /** \brief Get access to planner plugin */ - const planning_interface::PlannerManagerConstPtr getPlannerManager(const std::string& planner_name) + const planning_interface::PlannerManagerPtr getPlannerManager(const std::string& planner_name) { if (planner_map_.find(planner_name) == planner_map_.end()) { From 6259bf6fefeedd19a3d9860e83320fd132e51df6 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 27 Nov 2023 18:26:13 +0100 Subject: [PATCH 19/24] Add more usefull deprecation warning --- .../include/moveit/planning_pipeline/planning_pipeline.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) 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 d8336e4ad9..89115bbdb6 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 @@ -166,7 +166,9 @@ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline { return pipeline_parameters_.planning_plugins.at(0); } - [[deprecated("Removed from API.")]] const planning_interface::PlannerManagerPtr& getPlannerManager() + [[deprecated( + "Please use 'getPlannerManager(const std::string& planner_name)'.")]] const planning_interface::PlannerManagerPtr& + getPlannerManager() { return planner_map_.at(pipeline_parameters_.planning_plugins.at(0)); } From 2f145081f5e83fcecfb1f8664c06ffdef9184374 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 27 Nov 2023 19:46:18 +0100 Subject: [PATCH 20/24] Clang tidy --- .../planning/planning_pipeline/src/planning_pipeline.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp index d5d64320fd..52413c5892 100644 --- a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp +++ b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp @@ -60,12 +60,11 @@ getTrajectoryConstraints(const robot_trajectory::RobotTrajectoryPtr& trajectory) { moveit_msgs::msg::Constraints new_waypoint_constraint; // Iterate through joints and copy waypoint data to joint constraint - for (size_t joint_index = 0; joint_index < joint_names.size(); ++joint_index) + for (const auto& joint_name : joint_names) { moveit_msgs::msg::JointConstraint new_joint_constraint; - new_joint_constraint.joint_name = joint_names.at(joint_index); - new_joint_constraint.position = - trajectory->getWayPoint(waypoint_index).getVariablePosition(joint_names.at(joint_index)); + new_joint_constraint.joint_name = joint_name; + new_joint_constraint.position = trajectory->getWayPoint(waypoint_index).getVariablePosition(joint_name); new_waypoint_constraint.joint_constraints.push_back(new_joint_constraint); } trajectory_constraints.push_back(new_waypoint_constraint); @@ -366,9 +365,11 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& void PlanningPipeline::terminate() const { for (const auto& planner_pair : planner_map_) + { if (planner_pair.second) { planner_pair.second->terminate(); } + } } } // namespace planning_pipeline From 0af995061736e96146baadb4117d8dba413ed557 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Tue, 28 Nov 2023 14:51:27 +0100 Subject: [PATCH 21/24] Update logger --- .../src/planning_pipeline_interfaces.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) 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 index 316c18e794..2deaccc6f9 100644 --- a/moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp +++ b/moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp @@ -44,7 +44,7 @@ namespace moveit namespace planning_pipeline_interfaces { -rclcpp::Logger get_logger() +rclcpp::Logger getLogger() { static rclcpp::Logger logger = moveit::makeChildLogger("planning_pipeline_interfaces"); return logger; @@ -59,7 +59,7 @@ planWithSinglePipeline(const ::planning_interface::MotionPlanRequest& motion_pla auto it = planning_pipelines.find(motion_plan_request.pipeline_id); if (it == planning_pipelines.end()) { - RCLCPP_ERROR(get_logger(), "No planning pipeline available for name '%s'", motion_plan_request.pipeline_id.c_str()); + RCLCPP_ERROR(getLogger(), "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; } @@ -88,7 +88,7 @@ const std::vector<::planning_interface::MotionPlanResponse> planWithParallelPipe const auto hardware_concurrency = std::thread::hardware_concurrency(); if (motion_plan_requests.size() > hardware_concurrency && hardware_concurrency != 0) { - RCLCPP_WARN(get_logger(), + RCLCPP_WARN(getLogger(), "More parallel planning problems defined ('%ld') than possible to solve concurrently with the " "hardware ('%d')", motion_plan_requests.size(), hardware_concurrency); @@ -106,7 +106,7 @@ const std::vector<::planning_interface::MotionPlanResponse> planWithParallelPipe } catch (const std::exception& e) { - RCLCPP_ERROR(get_logger(), "Planning pipeline '%s' threw exception '%s'", request.pipeline_id.c_str(), e.what()); + RCLCPP_ERROR(getLogger(), "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; } @@ -118,7 +118,7 @@ const std::vector<::planning_interface::MotionPlanResponse> planWithParallelPipe if (stopping_criterion_callback(plan_responses_container, motion_plan_requests)) { // Terminate planning pipelines - RCLCPP_INFO(get_logger(), "Stopping criterion met: Terminating planning pipelines that are still active"); + RCLCPP_INFO(getLogger(), "Stopping criterion met: Terminating planning pipelines that are still active"); for (const auto& request : motion_plan_requests) { try @@ -131,7 +131,7 @@ const std::vector<::planning_interface::MotionPlanResponse> planWithParallelPipe } catch (const std::out_of_range&) { - RCLCPP_WARN(get_logger(), "Cannot terminate pipeline '%s' because no pipeline with that name exists", + RCLCPP_WARN(getLogger(), "Cannot terminate pipeline '%s' because no pipeline with that name exists", request.pipeline_id.c_str()); } } @@ -174,7 +174,7 @@ createPlanningPipelineMap(const std::vector& pipeline_names, // Check if pipeline already exists if (planning_pipelines.count(planning_pipeline_name) > 0) { - RCLCPP_WARN(get_logger(), "Skipping duplicate entry for planning pipeline '%s'.", planning_pipeline_name.c_str()); + RCLCPP_WARN(getLogger(), "Skipping duplicate entry for planning pipeline '%s'.", planning_pipeline_name.c_str()); continue; } @@ -184,7 +184,7 @@ createPlanningPipelineMap(const std::vector& pipeline_names, if (!pipeline) { - RCLCPP_ERROR(get_logger(), "Failed to initialize planning pipeline '%s'.", planning_pipeline_name.c_str()); + RCLCPP_ERROR(getLogger(), "Failed to initialize planning pipeline '%s'.", planning_pipeline_name.c_str()); continue; } From 339496df165ff5763a393ff121bdd4c970c7bd46 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 30 Nov 2023 17:52:52 +0100 Subject: [PATCH 22/24] Remove warning that reference trajectory is ignored in stomp --- moveit_planners/stomp/src/stomp_moveit_planner_plugin.cpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/moveit_planners/stomp/src/stomp_moveit_planner_plugin.cpp b/moveit_planners/stomp/src/stomp_moveit_planner_plugin.cpp index e32528f2db..82b964065a 100644 --- a/moveit_planners/stomp/src/stomp_moveit_planner_plugin.cpp +++ b/moveit_planners/stomp/src/stomp_moveit_planner_plugin.cpp @@ -118,12 +118,6 @@ class StompPlannerManager : public PlannerManager RCLCPP_ERROR(LOGGER, "Invalid joint group '%s'", req.group_name.c_str()); return false; } - - if (!req.reference_trajectories.empty()) - { - RCLCPP_WARN(LOGGER, "Ignoring reference trajectories - not implemented!"); - } - return true; } From 10c554efcd17683580d50c29b6cf54e102b52a0e Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Wed, 6 Dec 2023 14:29:17 -0700 Subject: [PATCH 23/24] clang-format --- moveit_ros/move_group/src/move_group_context.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/moveit_ros/move_group/src/move_group_context.cpp b/moveit_ros/move_group/src/move_group_context.cpp index b94065e3a3..df35342e66 100644 --- a/moveit_ros/move_group/src/move_group_context.cpp +++ b/moveit_ros/move_group/src/move_group_context.cpp @@ -94,8 +94,7 @@ bool MoveGroupContext::status() const { if (planning_pipeline_) { - RCLCPP_INFO_STREAM(getLogger(), - "MoveGroup context using pipeline " << planning_pipeline_->getName().c_str()); + RCLCPP_INFO_STREAM(getLogger(), "MoveGroup context using pipeline " << planning_pipeline_->getName().c_str()); RCLCPP_INFO_STREAM(getLogger(), "MoveGroup context initialization complete"); return true; } From f59379cc15686abdf842ec7a2be9f75583a3e11c Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Wed, 6 Dec 2023 14:40:36 -0700 Subject: [PATCH 24/24] rebase logging api chainges --- .../query_planners_service_capability.cpp | 26 ++++++++++++------- 1 file changed, 16 insertions(+), 10 deletions(-) diff --git a/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.cpp index 10ae33e880..1bdc92881e 100644 --- a/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.cpp @@ -42,6 +42,13 @@ namespace move_group { +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("query_planners_service_capability"); +} +} // namespace MoveGroupQueryPlannersService::MoveGroupQueryPlannersService() : MoveGroupCapability("query_planners_service") { } @@ -78,14 +85,13 @@ void MoveGroupQueryPlannersService::queryInterface( const auto& planner_plugin_names = planning_pipeline->getPlannerPluginNames(); if (planner_plugin_names.empty()) { - RCLCPP_ERROR(moveit::getLogger(), "Pipeline '%s' does not have any planner plugins.", - planning_pipelines.first.c_str()); + RCLCPP_ERROR(getLogger(), "Pipeline '%s' does not have any planner plugins.", planning_pipelines.first.c_str()); return; } const auto planner_interface = planning_pipeline->getPlannerManager(planner_plugin_names.at(0)); if (!planner_interface) { - RCLCPP_ERROR(moveit::getLogger(), "Requesting planner '%s' from '%s' returned a null pointer.", + RCLCPP_ERROR(getLogger(), "Requesting planner '%s' from '%s' returned a null pointer.", planner_plugin_names.at(0).c_str(), planning_pipelines.first.c_str()); } std::vector algs; @@ -104,7 +110,7 @@ void MoveGroupQueryPlannersService::getParams(const std::shared_ptrpipeline_id); if (!planning_pipeline) { - RCLCPP_ERROR(moveit::getLogger(), "Pipeline '%s' does not exist.", req->pipeline_id.c_str()); + RCLCPP_ERROR(getLogger(), "Pipeline '%s' does not exist.", req->pipeline_id.c_str()); return; } @@ -112,13 +118,13 @@ void MoveGroupQueryPlannersService::getParams(const std::shared_ptrgetPlannerPluginNames(); if (planner_plugin_names.empty()) { - RCLCPP_ERROR(moveit::getLogger(), "Pipeline '%s' does not have any planner plugins.", req->pipeline_id.c_str()); + RCLCPP_ERROR(getLogger(), "Pipeline '%s' does not have any planner plugins.", req->pipeline_id.c_str()); return; } const auto planner_interface = planning_pipeline->getPlannerManager(planner_plugin_names.at(0)); if (!planner_interface) { - RCLCPP_ERROR(moveit::getLogger(), "Requesting planner '%s' from '%s' returned a null pointer.", + RCLCPP_ERROR(getLogger(), "Requesting planner '%s' from '%s' returned a null pointer.", planner_plugin_names.at(0).c_str(), req->pipeline_id.c_str()); } std::map config; @@ -154,14 +160,14 @@ void MoveGroupQueryPlannersService::setParams( { if (req->params.keys.size() != req->params.values.size()) { - RCLCPP_ERROR(moveit::getLogger(), "Number of parameter names does not match the number of parameters"); + RCLCPP_ERROR(getLogger(), "Number of parameter names does not match the number of parameters"); return; } const planning_pipeline::PlanningPipelinePtr planning_pipeline = resolvePlanningPipeline(req->pipeline_id); if (!planning_pipeline) { - RCLCPP_ERROR(moveit::getLogger(), "Pipeline '%s' does not exist.", req->pipeline_id.c_str()); + RCLCPP_ERROR(getLogger(), "Pipeline '%s' does not exist.", req->pipeline_id.c_str()); return; } @@ -169,13 +175,13 @@ void MoveGroupQueryPlannersService::setParams( const auto& planner_plugin_names = planning_pipeline->getPlannerPluginNames(); if (planner_plugin_names.empty()) { - RCLCPP_ERROR(moveit::getLogger(), "Pipeline '%s' does not have any planner plugins.", req->pipeline_id.c_str()); + RCLCPP_ERROR(getLogger(), "Pipeline '%s' does not have any planner plugins.", req->pipeline_id.c_str()); return; } auto planner_interface = planning_pipeline->getPlannerManager(planner_plugin_names.at(0)); if (!planner_interface) { - RCLCPP_ERROR(moveit::getLogger(), "Requesting planner '%s' from '%s' returned a null pointer.", + RCLCPP_ERROR(getLogger(), "Requesting planner '%s' from '%s' returned a null pointer.", planner_plugin_names.at(0).c_str(), req->pipeline_id.c_str()); return; }