diff --git a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp index 9677fae93a..f18477e3c3 100644 --- a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp +++ b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp @@ -182,29 +182,22 @@ void OMPLInterface::loadPlannerConfigurations() // get parameters specific for the robot planning group std::map specific_group_params; - for (const auto& k : KNOWN_GROUP_PARAMS) + for (const auto& [name, type] : KNOWN_GROUP_PARAMS) { std::string param_name{ group_name_param }; param_name += "."; - param_name += k.first; + param_name += name; if (node_->has_parameter(param_name)) { const rclcpp::Parameter parameter = node_->get_parameter(param_name); - if (parameter.get_type() != k.second) + if (parameter.get_type() != type) { - RCLCPP_ERROR_STREAM(LOGGER, "Invalid type for parameter '" << k.first << "' expected [" - << rclcpp::to_string(k.second) << "] got [" + RCLCPP_ERROR_STREAM(LOGGER, "Invalid type for parameter '" << name << "' expected [" + << rclcpp::to_string(type) << "] got [" << rclcpp::to_string(parameter.get_type()) << "]"); continue; } - if (parameter.get_type() == rclcpp::ParameterType::PARAMETER_STRING) - specific_group_params[k.first] = parameter.as_string(); - else if (parameter.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE) - specific_group_params[k.first] = moveit::core::toString(parameter.as_double()); - else if (parameter.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER) - specific_group_params[k.first] = std::to_string(parameter.as_int()); - else if (parameter.get_type() == rclcpp::ParameterType::PARAMETER_BOOL) - specific_group_params[k.first] = std::to_string(parameter.as_bool()); + specific_group_params[name] = parameter.value_to_string(); } } @@ -244,13 +237,13 @@ void OMPLInterface::loadPlannerConfigurations() } } - for (const std::pair& config : pconfig) + for (const auto& [name, config_settings] : pconfig) { - RCLCPP_DEBUG(LOGGER, "Parameters for configuration '%s'", config.first.c_str()); + RCLCPP_DEBUG(LOGGER, "Parameters for configuration '%s'", name.c_str()); - for (const std::pair& parameters : config.second.config) + for (const auto& [param_name, param_value] : config_settings.config) { - RCLCPP_DEBUG(LOGGER, " - %s = %s", parameters.first.c_str(), parameters.second.c_str()); + RCLCPP_DEBUG_STREAM(LOGGER, " - " << param_name << " = " << param_value); } } diff --git a/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp b/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp index 00b45a240c..ba06cf3d43 100644 --- a/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp @@ -86,7 +86,7 @@ class OMPLPlannerManager : public planning_interface::PlannerManager const std::string& parameter_namespace) override { ompl_interface_ = std::make_unique(model, node, parameter_namespace); - config_settings_ = ompl_interface_->getPlannerConfigurations(); + setPlannerConfigurations(ompl_interface_->getPlannerConfigurations()); return true; }