How to retrieve the available motion planner configs?

asked 2018-02-22 12:11:28 -0500

Radu gravatar image

I am attempting to read the available planner-configs from the parameter server (PRMstarkConfigDefault, LBKPIECEkConfigDefault, BKPIECEkConfigDefault, etc.). If I read all parameters using:

//Retrieve all parameter names from the ROS server
std::vector<std::string> params;
nh_.getParamNames(params);
for(std::size_t i = 0; i < params.size(); ++i)
{
       ROS_INFO("Parameter : %s", params[i].c_str());
}

I get the following output (list is shortened to focus on what I'm interested in):

[ INFO] [1519316660.687755562]: Parameter : /move_group/planning_scene_monitor/publish_planning_scene_hz
[ INFO] [1519316660.687800561]: Parameter : /move_group/controller_list
[ INFO] [1519316660.687895226]: Parameter : /move_group/max_safe_path_cost
[ INFO] [1519316660.687980767]: Parameter : /move_group/planner_configs/PRMstarkConfigDefault/type
[ INFO] [1519316660.688084307]: Parameter : /move_group/planner_configs/LBKPIECEkConfigDefault/type
[ INFO] [1519316660.688183389]: Parameter : /move_group/planner_configs/BKPIECEkConfigDefault/type
[ INFO] [1519316660.688255304]: Parameter : /move_group/planner_configs/ESTkConfigDefault/type
[ INFO] [1519316660.688295804]: Parameter : /move_group/planner_configs/RRTstarkConfigDefault/type
[ INFO] [1519316660.688399635]: Parameter : /move_group/planner_configs/SBLkConfigDefault/type
[ INFO] [1519316660.688503217]: Parameter : /move_group/planner_configs/RRTkConfigDefault/type
[ INFO] [1519316660.688566175]: Parameter : /move_group/planner_configs/PRMkConfigDefault/type
[ INFO] [1519316660.688665340]: Parameter : /move_group/planner_configs/RRTConnectkConfigDefault/type
[ INFO] [1519316660.688728214]: Parameter : /move_group/planner_configs/KPIECEkConfigDefault/type
[ INFO] [1519316660.688768713]: Parameter : /move_group/planner_configs/TRRTkConfigDefault/type
[ INFO] [1519316660.688886045]: Parameter : /move_group/planning_plugin
[ INFO] [1519316660.688940086]: Parameter : /move_group/trajectory_execution/execution_velocity_scaling
[ INFO] [1519316660.688976085]: Parameter : /move_group/trajectory_execution/allowed_execution_duration_scaling
[ INFO] [1519316660.689021084]: Parameter : /move_group/trajectory_execution/execution_duration_monitoring
[ INFO] [1519316660.689124916]: Parameter : /move_group/allowed_execution_duration_scaling

How should I go about to get a vector of strings containing all planner configs? (i.e. {PRMstarkConfigDefault,LBKPIECEkConfigDefault,etc}

So far I have tried:

robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
robot_model::RobotModelPtr robot_model = robot_model_loader.getModel();
boost::scoped_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager>> planner_plugin_loader;
planning_interface::PlannerManagerPtr planner_instance;
std::string planner_plugin_name;

try
    {
      planner_plugin_loader.reset(new pluginlib::ClassLoader<planning_interface::PlannerManager>("moveit_core", "planning_interface::PlannerManager"));
    }
    catch (pluginlib::PluginlibException& ex)
    {
      ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what());
    }
try
{
    planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_plugin_name));
    if (!planner_instance->initialize(robot_model, nh_.getNamespace()))
        ROS_FATAL_STREAM("Could not initialize planner instance");
    ROS_INFO_STREAM("Using planning interface '" << planner_instance->getDescription() << "'. Planner pluggin name: " << planner_plugin_name.c_str());
}
catch(pluginlib::PluginlibException& ex)
{
    const std::vector<std::string> &classes = planner_plugin_loader->getDeclaredClasses();
    std::stringstream ss;
    for (std::size_t i = 0 ; i < classes.size() ; ++i)
        ss << classes[i] << " ";
    ROS_ERROR_STREAM("Exception while loading planner '" << planner_plugin_name << "': " << ex.what() << std::endl             << "Available plugins: " << ss.str());
}
std::vector<std::string> algs_names;
planner_instance->getPlanningAlgorithms(algs_names);
for(std::size_t i = 0; i < algs_names.size(); ++i)
{
    ROS_INFO("Algorithms names: %s", algs_names[i].c_str());
}

The output was:

[ INFO] [1519316660.766176631]: Using planning interface 'OMPL'. Planner pluggin name: ompl_interface/OMPLPlanner
[ INFO] [1519316660.766242380]: Algorithms names: arm
[ INFO] [1519316660.766354920]: Algorithms names: arm_with_torso
[ INFO] [1519316660.766548458]: Algorithms names: gripper

Any suggestion is welcomed...

edit retag flag offensive close merge delete

Comments

Why would you want to read those parameters? What is your goal? Have you solved this in the meantime?

fvd gravatar image fvd  ( 2020-02-29 06:48:26 -0500 )edit