Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[ompl] Small code refactor #1138

Merged
merged 2 commits into from
Mar 31, 2022
Merged

[ompl] Small code refactor #1138

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
a9d6d7d
[ompl] Small code refactor
galou Mar 25, 2022
6648552
Merge branch 'main' into ompl_code_refactor
galou Mar 30, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
27 changes: 10 additions & 17 deletions moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -182,29 +182,22 @@ void OMPLInterface::loadPlannerConfigurations()

// get parameters specific for the robot planning group
std::map<std::string, std::string> 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();
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@henningkayser I just saw this, I think this will cause a bug(or throw an exception), the change to double/bool needs to be reverted, the value_to_string interpret the boolean as "true"/"false" but boost::lexical_cast<bool> will fail for those values, for the double case std::to_string uses the locale which may use a different delimiter see and not be desired for our case see

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Should I do it?

}
}

Expand Down Expand Up @@ -244,13 +237,13 @@ void OMPLInterface::loadPlannerConfigurations()
}
}

for (const std::pair<const std::string, planning_interface::PlannerConfigurationSettings>& 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<const std::string, std::string>& 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);
}
}

Expand Down
2 changes: 1 addition & 1 deletion moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -86,7 +86,7 @@ class OMPLPlannerManager : public planning_interface::PlannerManager
const std::string& parameter_namespace) override
{
ompl_interface_ = std::make_unique<OMPLInterface>(model, node, parameter_namespace);
config_settings_ = ompl_interface_->getPlannerConfigurations();
setPlannerConfigurations(ompl_interface_->getPlannerConfigurations());
return true;
}

Expand Down