Revert parameter initialization part of moveit#1138

vatanaksoytezer · Apr 6, 2022 · 104b923 · 104b923
1 parent 99939d3
commit 104b923
Showing 1 changed file with 12 additions and 5 deletions.
diff --git 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<const std::string, planning_interface::PlannerConfigurationSettings>& 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<const std::string, std::string>& 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());
     }
   }