diff --git a/MIGRATION.md b/MIGRATION.md index 04baab23c5..3d92437e69 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 b632d65c05..07c1f32c46 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 9c02c44bdd..3bb0acfc8a 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 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_core/planning_interface/include/moveit/planning_interface/planning_interface.h b/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h index c41c8505ab..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,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_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/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; } 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/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h index 9820ba4791..6a0be17731 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 09c811d2c9..f108fec0db 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 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/move_group/src/default_capabilities/query_planners_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.cpp index 76cf27ef1e..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 @@ -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 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/move_group/src/move_group_context.cpp b/moveit_ros/move_group/src/move_group_context.cpp index cacc9c86df..df35342e66 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 99f17b42fe..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 @@ -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 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, diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp index 87bd34b979..bac47ee51a 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 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 010058554c..7100a69f09 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 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 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 23d6bd89f8..73abd5fe4c 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;