From 104b923d729889b59d7cb926f2dc27498308959d Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Wed, 6 Apr 2022 20:39:41 +0300 Subject: [PATCH] Revert parameter initialization part of #1138 --- .../ompl/ompl_interface/src/ompl_interface.cpp | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp index f18477e3c35..699286d831a 100644 --- a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp +++ b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp @@ -197,7 +197,14 @@ void OMPLInterface::loadPlannerConfigurations() << rclcpp::to_string(parameter.get_type()) << "]"); continue; } - specific_group_params[name] = parameter.value_to_string(); + if (parameter.get_type() == rclcpp::ParameterType::PARAMETER_STRING) + specific_group_params[name] = parameter.as_string(); + else if (parameter.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE) + specific_group_params[name] = moveit::core::toString(parameter.as_double()); + else if (parameter.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER) + specific_group_params[name] = std::to_string(parameter.as_int()); + else if (parameter.get_type() == rclcpp::ParameterType::PARAMETER_BOOL) + specific_group_params[name] = std::to_string(parameter.as_bool()); } } @@ -237,13 +244,13 @@ void OMPLInterface::loadPlannerConfigurations() } } - for (const auto& [name, config_settings] : pconfig) + for (const std::pair& config : pconfig) { - RCLCPP_DEBUG(LOGGER, "Parameters for configuration '%s'", name.c_str()); + RCLCPP_DEBUG(LOGGER, "Parameters for configuration '%s'", config.first.c_str()); - for (const auto& [param_name, param_value] : config_settings.config) + for (const std::pair& parameters : config.second.config) { - RCLCPP_DEBUG_STREAM(LOGGER, " - " << param_name << " = " << param_value); + RCLCPP_DEBUG(LOGGER, " - %s = %s", parameters.first.c_str(), parameters.second.c_str()); } }