diff --git a/.github/workflows/tutorial_docker.yaml b/.github/workflows/tutorial_docker.yaml index 4eb21563975..9aa63ddfd2b 100644 --- a/.github/workflows/tutorial_docker.yaml +++ b/.github/workflows/tutorial_docker.yaml @@ -50,7 +50,7 @@ jobs: path: .ccache key: docker-tutorial-ccache-${{ matrix.ROS_DISTRO }}-${{ hashFiles( '.docker/tutorial-source/Dockerfile' ) }} - name: inject ccache into docker - uses: reproducible-containers/buildkit-cache-dance@v2.1.2 + uses: reproducible-containers/buildkit-cache-dance@v2.1.3 with: cache-source: .ccache cache-target: /root/.ccache/ diff --git a/MIGRATION.md b/MIGRATION.md index 04baab23c5c..3d92437e699 100644 --- a/MIGRATION.md +++ b/MIGRATION.md @@ -3,6 +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_configs_utils/default_configs/chomp_planning.yaml b/moveit_configs_utils/default_configs/chomp_planning.yaml index b632d65c05c..07c1f32c46c 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_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. request_adapters: diff --git a/moveit_configs_utils/default_configs/ompl_planning.yaml b/moveit_configs_utils/default_configs/ompl_planning.yaml index 9c02c44bdd1..3bb0acfc8a1 100644 --- a/moveit_configs_utils/default_configs/ompl_planning.yaml +++ b/moveit_configs_utils/default_configs/ompl_planning.yaml @@ -1,4 +1,5 @@ -planning_plugin: ompl_interface/OMPLPlanner +planning_plugins: + - ompl_interface/OMPLPlanner # 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 9925a81a8bb..d0a07ce4c29 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 38120524cb9..e811ac9e914 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_core/collision_detection/include/moveit/collision_detection/collision_common.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h index 1b6ff542b0a..db246518fd0 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h @@ -141,41 +141,7 @@ struct CostSource } }; -/** \brief Representation of a collision checking result */ -struct CollisionResult -{ - using ContactMap = std::map, std::vector >; - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - /** \brief Clear a previously stored result */ - void clear() - { - collision = false; - distance = std::numeric_limits::max(); - contact_count = 0; - contacts.clear(); - cost_sources.clear(); - } - - /** \brief Throttled warning printing the first collision pair, if any. All collisions are logged at DEBUG level */ - void print() const; - - /** \brief True if collision was found, false otherwise */ - bool collision = false; - - /** \brief Closest distance between two bodies */ - double distance = std::numeric_limits::max(); - - /** \brief Number of contacts returned */ - std::size_t contact_count = 0; - - /** \brief A map returning the pairs of body ids in contact, plus their contact details */ - ContactMap contacts; - - /** \brief These are the individual cost sources when costs are computed */ - std::set cost_sources; -}; +struct CollisionResult; /** \brief Representation of a collision checking request */ struct CollisionRequest @@ -187,6 +153,9 @@ struct CollisionRequest /** \brief If true, compute proximity distance */ bool distance = false; + /** \brief If true, return detailed distance information. Distance must be set to true as well */ + bool detailed_distance = false; + /** \brief If true, a collision cost is computed */ bool cost = false; @@ -354,4 +323,43 @@ struct DistanceResult distances.clear(); } }; + +/** \brief Representation of a collision checking result */ +struct CollisionResult +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** \brief Clear a previously stored result */ + void clear() + { + collision = false; + distance = std::numeric_limits::max(); + distance_result.clear(); + contact_count = 0; + contacts.clear(); + cost_sources.clear(); + } + + /** \brief Throttled warning printing the first collision pair, if any. All collisions are logged at DEBUG level */ + void print() const; + + /** \brief True if collision was found, false otherwise */ + bool collision = false; + + /** \brief Closest distance between two bodies */ + double distance = std::numeric_limits::max(); + + /** \brief Distance data for each link */ + DistanceResult distance_result; + + /** \brief Number of contacts returned */ + std::size_t contact_count = 0; + + /** \brief A map returning the pairs of body ids in contact, plus their contact details */ + using ContactMap = std::map, std::vector >; + ContactMap contacts; + + /** \brief These are the individual cost sources when costs are computed */ + std::set cost_sources; +}; } // namespace collision_detection diff --git a/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp index e5b01de84f7..e1406dc4993 100644 --- a/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp @@ -290,6 +290,10 @@ void CollisionEnvFCL::checkSelfCollisionHelper(const CollisionRequest& req, Coll dreq.enableGroup(getRobotModel()); distanceSelf(dreq, dres, state); res.distance = dres.minimum_distance.distance; + if (req.detailed_distance) + { + res.distance_result = dres; + } } } @@ -343,6 +347,10 @@ void CollisionEnvFCL::checkRobotCollisionHelper(const CollisionRequest& req, Col dreq.enableGroup(getRobotModel()); distanceRobot(dreq, dres, state); res.distance = dres.minimum_distance.distance; + if (req.detailed_distance) + { + res.distance_result = dres; + } } } 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 c41c8505ab8..76bb51bb3ea 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,8 +164,8 @@ 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 - virtual std::string getDescription() const; + /// \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 /// request) diff --git a/moveit_core/utils/CMakeLists.txt b/moveit_core/utils/CMakeLists.txt index a5d0a884f04..3bd09ef841d 100644 --- a/moveit_core/utils/CMakeLists.txt +++ b/moveit_core/utils/CMakeLists.txt @@ -8,7 +8,8 @@ target_include_directories(moveit_utils PUBLIC $ $ ) -ament_target_dependencies(moveit_utils Boost moveit_msgs rclcpp rsl fmt) +ament_target_dependencies(moveit_utils Boost moveit_msgs rclcpp fmt) +target_link_libraries(moveit_utils rsl::rsl) set_target_properties(moveit_utils PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") install(DIRECTORY include/ DESTINATION include/moveit_core) @@ -20,7 +21,7 @@ target_include_directories(moveit_test_utils PUBLIC $ $ ) -target_link_libraries(moveit_test_utils moveit_robot_model moveit_kinematics_base) +target_link_libraries(moveit_test_utils moveit_robot_model moveit_kinematics_base rsl::rsl) ament_target_dependencies(moveit_test_utils ament_index_cpp Boost @@ -30,7 +31,6 @@ ament_target_dependencies(moveit_test_utils urdfdom urdfdom_headers rclcpp - rsl fmt ) set_target_properties(moveit_test_utils PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") 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 225fcbc58e6..aa2dd03b649 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 aecedafed39..24493d2cdcc 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/stomp/src/stomp_moveit_planner_plugin.cpp b/moveit_planners/stomp/src/stomp_moveit_planner_plugin.cpp index e32528f2db6..82b964065ab 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; } 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 d9a053f1e86..a71023ecc5d 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_py/moveit/policies/__init__.py b/moveit_py/moveit/policies/__init__.py new file mode 100644 index 00000000000..53e7dff01be --- /dev/null +++ b/moveit_py/moveit/policies/__init__.py @@ -0,0 +1 @@ +from .policy import Policy diff --git a/moveit_py/moveit/policies/policy.py b/moveit_py/moveit/policies/policy.py new file mode 100644 index 00000000000..24e1f70a5a1 --- /dev/null +++ b/moveit_py/moveit/policies/policy.py @@ -0,0 +1,151 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2023, Peter David Fagan +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Author: Peter David Fagan + +""" +Definition of an abstract base class for policy deployment. +For now, this policy only supports moveit_servo command interfaces and Image sensors. +""" + +from abc import ABC, abstractmethod + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile + +from control_msgs.msg import JointJog +from geometry_msgs.msg import PoseStamped, Twist +from sensor_msgs.msg import Image + +from std_srvs.srv import SetBool + +import message_filters + + +class Policy(ABC, Node): + """An abstract base class for deploying learnt policies.""" + + def __init__(self, params, node_name="policy_node"): + """Initialise the policy.""" + super().__init__(node_name) + self.logger = self.get_logger() + + # parse parameters + self.param_listener = params.ParamListener(self) + self.params = self.param_listener.get_params() + + # set policy to inactive by default + self._is_active = False + self.activate_policy = self.create_service( + SetBool, + "activate_policy", + self.activate_policy, + ) + + # register sensor topics + self.register_sensors() + + # register servo command topic + self.register_command() + + @property + def is_active(self): + """Returns True if the policy is active.""" + return self._is_active + + @is_active.setter + def active(self, value): + """Sets the policy to active state via Python API.""" + self._is_active = value + + def activate_policy(self, request, response): + """Sets the policy to active state via ROS interface.""" + self._is_active = request.data + return response + + def get_sensor_msg_type(self, msg_type): + """Returns the ROS 2 message type for a given sensor type.""" + if msg_type == "sensor_msgs/Image": + return Image + else: + raise ValueError(f"Sensor type {sensor_type} not supported.") + + def get_command_msg_type(self, msg_type): + """Returns the ROS 2 message type for a given command type.""" + if msg_type == "geometry_msgs/PoseStamped": + return PoseStamped + elif msg_type == "geometry_msgs/Twist": + return Twist + elif msg_type == "control_msgs/JointJog": + return JointJog + else: + raise ValueError(f"Command type {msg_type} not supported.") + + def register_sensors(self): + """Register the topics to listen to for sensor data.""" + self.sensor_subs = [] + # TODO: refactor this section + # Related Issue: https://github.com/PickNikRobotics/generate_parameter_library/issues/155 + for sensor_idx in range(self.params.num_sensors): + sensor_params = self.get_parameters_by_prefix(f"sensor{sensor_idx + 1}") + self.sensor_subs.append( + message_filters.Subscriber( + self, + self.get_sensor_msg_type(sensor_params["type"].value), + str(sensor_params["topic"].value), + qos_profile=QoSProfile(depth=sensor_params["qos"].value), + ) + ) + + # create a synchronizer for the sensor topics + self.sensor_sync = message_filters.ApproximateTimeSynchronizer( + self.sensor_subs, + self.params.sensor_queue, + self.params.sensor_slop, + ) + + # register model forward pass as callback + self.sensor_sync.registerCallback(self.forward) + + def register_command(self): + """Register the topic to publish actions to.""" + self.command_pub = self.create_publisher( + self.get_command_msg_type(self.params.command.type), + self.params.command.topic, + QoSProfile(depth=self.params.command.qos), + ) + + @abstractmethod + def forward(): + """Perform a forward pass of the policy.""" + pass diff --git a/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h index 9820ba47916..6a0be17731e 100644 --- a/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h +++ b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h @@ -169,9 +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, - const std::string& group_name); + /// 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 09c811d2c93..f108fec0db6 100644 --- a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp @@ -121,7 +121,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; @@ -139,8 +139,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; @@ -267,7 +266,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 (!pipelinesExist(options.planning_pipelines)) { return false; } @@ -528,8 +527,7 @@ void BenchmarkExecutor::createRequestCombinations(const BenchmarkRequest& benchm } } -bool BenchmarkExecutor::plannerConfigurationsExist( - const std::map>& pipeline_configurations, const std::string& group_name) +bool BenchmarkExecutor::pipelinesExist(const std::map>& pipeline_configurations) { // Make sure planner plugins exist for (const std::pair>& pipeline_config_entry : pipeline_configurations) @@ -549,42 +547,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/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 5a487e8708a..d93bfde2726 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 5beba7753b5..f88681dbc39 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/move_group/src/default_capabilities/query_planners_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.cpp index 76cf27ef1e6..1bdc92881ed 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,9 +38,17 @@ #include #include #include +#include namespace move_group { +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("query_planners_service_capability"); +} +} // namespace MoveGroupQueryPlannersService::MoveGroupQueryPlannersService() : MoveGroupCapability("query_planners_service") { } @@ -49,118 +57,152 @@ 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*/, +void MoveGroupQueryPlannersService::queryInterface( + const std::shared_ptr& /* unused */, const std::shared_ptr& res) { 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) + + // TODO(sjahr): Update for multiple planner plugins + const auto& planner_plugin_names = planning_pipeline->getPlannerPluginNames(); + if (planner_plugin_names.empty()) + { + 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) { - 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); + 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; + 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); } - return true; } -bool MoveGroupQueryPlannersService::getParams(const std::shared_ptr& /* unused */, - const std::shared_ptr& req, +void MoveGroupQueryPlannersService::getParams(const std::shared_ptr& req, const std::shared_ptr& res) { const planning_pipeline::PlanningPipelinePtr planning_pipeline = resolvePlanningPipeline(req->pipeline_id); if (!planning_pipeline) - return false; + { + RCLCPP_ERROR(getLogger(), "Pipeline '%s' does not exist.", req->pipeline_id.c_str()); + return; + } - const planning_interface::PlannerManagerPtr& planner_interface = planning_pipeline->getPlannerManager(); - if (planner_interface) + // TODO(sjahr): Update for multiple planner plugins + const auto& planner_plugin_names = planning_pipeline->getPlannerPluginNames(); + if (planner_plugin_names.empty()) + { + 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) { - std::map config; + 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; - const planning_interface::PlannerConfigurationMap& configs = planner_interface->getPlannerConfigurations(); + const planning_interface::PlannerConfigurationMap& configs = planner_interface->getPlannerConfigurations(); - planning_interface::PlannerConfigurationMap::const_iterator it = - configs.find(req->planner_config); // fetch default params first + 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()); - - 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); - } + 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; } -bool MoveGroupQueryPlannersService::setParams( - const std::shared_ptr& /* unused */, +void MoveGroupQueryPlannersService::setParams( const std::shared_ptr& req, const std::shared_ptr& /*res*/) { if (req->params.keys.size() != req->params.values.size()) - return false; + { + 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) - return false; + { + RCLCPP_ERROR(getLogger(), "Pipeline '%s' does not exist.", req->pipeline_id.c_str()); + return; + } - const planning_interface::PlannerManagerPtr& planner_interface = planning_pipeline->getPlannerManager(); + // TODO(sjahr): Update for multiple planner plugins + const auto& planner_plugin_names = planning_pipeline->getPlannerPluginNames(); + if (planner_plugin_names.empty()) + { + 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(getLogger(), "Requesting planner '%s' from '%s' returned a null pointer.", + planner_plugin_names.at(0).c_str(), req->pipeline_id.c_str()); + return; + } - 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) { - 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); + config.config[req->params.keys.at(i)] = req->params.values.at(i); } - return true; + + 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 00103c17b56..8fa02ea1efd 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/move_group/src/move_group_context.cpp b/moveit_ros/move_group/src/move_group_context.cpp index cacc9c86dfb..df35342e66f 100644 --- a/moveit_ros/move_group/src/move_group_context.cpp +++ b/moveit_ros/move_group/src/move_group_context.cpp @@ -92,18 +92,16 @@ MoveGroupContext::~MoveGroupContext() bool MoveGroupContext::status() const { - const planning_interface::PlannerManagerPtr& planner_interface = planning_pipeline_->getPlannerManager(); - if (planner_interface) + if (planning_pipeline_) { - RCLCPP_INFO_STREAM(getLogger(), - "MoveGroup context using planning plugin " << planning_pipeline_->getPlannerPluginName()); + RCLCPP_INFO_STREAM(getLogger(), "MoveGroup context using pipeline " << planning_pipeline_->getName().c_str()); RCLCPP_INFO_STREAM(getLogger(), "MoveGroup context initialization complete"); return true; } else { RCLCPP_WARN_STREAM(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 99f17b42fe2..89115bbdb68 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()); @@ -163,6 +162,16 @@ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline { return false; } + [[deprecated("Please use getPlannerPluginNames().")]] const std::string& getPlannerPluginName() const + { + return pipeline_parameters_.planning_plugins.at(0); + } + [[deprecated( + "Please use 'getPlannerManager(const std::string& planner_name)'.")]] const planning_interface::PlannerManagerPtr& + getPlannerManager() + { + return planner_map_.at(pipeline_parameters_.planning_plugins.at(0)); + } /* END BLOCK OF DEPRECATED FUNCTIONS */ @@ -180,10 +189,10 @@ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline /** \brief Request termination, if a generatePlan() function is currently computing plans */ void terminate() const; - /** \brief Get the name of the planning plugin used */ - [[nodiscard]] const std::string& getPlannerPluginName() const + /** \brief Get the names of the planning plugins used */ + [[nodiscard]] const std::vector& getPlannerPluginNames() const { - return pipeline_parameters_.planning_plugin; + return pipeline_parameters_.planning_plugins; } /** \brief Get the names of the planning request adapter plugins used */ @@ -198,12 +207,6 @@ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline return pipeline_parameters_.response_adapters; } - /** \brief Get the planner manager for the loaded planning plugin */ - [[nodiscard]] const planning_interface::PlannerManagerPtr& getPlannerManager() - { - return planner_instance_; - } - /** \brief Get the robot model that this pipeline is using */ [[nodiscard]] const moveit::core::RobotModelConstPtr& getRobotModel() const { @@ -216,6 +219,23 @@ 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_; + } + + /** \brief Get access to planner plugin */ + const planning_interface::PlannerManagerPtr 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(); @@ -240,7 +260,7 @@ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline // Planner plugin std::unique_ptr> planner_plugin_loader_; - planning_interface::PlannerManagerPtr planner_instance_; + std::unordered_map planner_map_; // Plan request adapters std::unique_ptr> request_adapter_plugin_loader_; 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 29d81597cd9..41ef31d1873 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, diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp index 87bd34b9796..bac47ee51ab 100644 --- a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp +++ b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp @@ -38,6 +38,41 @@ #include #include +namespace +{ +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 (const auto& joint_name : joint_names) + { + moveit_msgs::msg::JointConstraint new_joint_constraint; + 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); + } + return trajectory_constraints; +} +} // namespace + namespace planning_pipeline { PlanningPipeline::PlanningPipeline(const moveit::core::RobotModelConstPtr& model, @@ -56,7 +91,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 } @@ -65,7 +100,7 @@ PlanningPipeline::PlanningPipeline(const moveit::core::RobotModelConstPtr& model , robot_model_(model) , logger_(moveit::getLogger("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(); @@ -73,14 +108,6 @@ PlanningPipeline::PlanningPipeline(const moveit::core::RobotModelConstPtr& model void PlanningPipeline::configure() { - // Check if planning plugin name is available - if (pipeline_parameters_.planning_plugin.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()) { @@ -100,30 +127,46 @@ 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 + + ". Planner instance is nullptr."); + } + + // 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_map_.insert(std::make_pair(planner_name, planner_instance)); } // Load the planner request adapters @@ -204,7 +247,7 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& planning_interface::MotionPlanResponse& res, const bool publish_received_requests) const { - assert(planner_instance_ != nullptr); + assert(!planner_map_.empty()); // Set planning pipeline active active_ = true; @@ -241,22 +284,38 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& } } - // Call planner - if (res.error_code) + // Call planners + for (const auto& planner_name : pipeline_parameters_.planning_plugins) { - planning_interface::PlanningContextPtr context = - planner_instance_->getPlanningContext(planning_scene, mutable_request, res.error_code); - if (context) + const auto& planner = planner_map_.at(planner_name); + // Update reference trajectory with latest solution (if available) + if (res.trajectory) { - context->solve(res); - publishPipelineState(mutable_request, res, planner_instance_->getDescription()); + mutable_request.trajectory_constraints.constraints = getTrajectoryConstraints(res.trajectory); } - else + + // 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_instance_->getDescription().c_str()); + 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().c_str()); + break; } } @@ -305,9 +364,12 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& void PlanningPipeline::terminate() const { - if (planner_instance_) + for (const auto& planner_pair : planner_map_) { - planner_instance_->terminate(); + if (planner_pair.second) + { + planner_pair.second->terminate(); + } } } } // namespace planning_pipeline 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 1b2940f981a..1b5d8986ace 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 010058554cf..7100a69f09f 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,15 @@ 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); + + // 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); } 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 21e37633513..9c9760e2e4b 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 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 23d6bd89f87..73abd5fe4cd 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 @@ -181,7 +181,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(getLogger(), "Failed to initialize planning pipeline '%s'.", planning_pipeline_name.c_str()); continue;