How to set up planner through PlannerManager

edit

I am trying to use OMPL planners directly through Motion Planning API, as described in this tutorial in Kinetic.

// RobotModelLoader
const std::string PLANNING_GROUP = "manipulator";
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
robot_model::RobotModelPtr robot_model = robot_model_loader.getModel();

robot_state::RobotStatePtr robot_state(new robot_state::RobotState(robot_model));
const robot_state::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP);

planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model));

boost::scoped_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager>> planner_plugin_loader;
// Loading a planner instance
planning_interface::PlannerManagerPtr planner_instance;
std::string planner_plugin_name = "ompl_interface/OMPLPlanner"; 
planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_plugin_name));
planner_instance->initialize(robot_model, node_handle.getNamespace());

I would like to change and configure the planner used, which I attempt to achieve by setting it in PlannerManagerPtr planner_instance.

planning_interface::PlannerConfigurationSettings settings;
settings.name = "experimental";
settings.group = "manipulator";
settings.config.insert(pair<string, string>("type", "geometric::RRTConnect"));
map<string, planning_interface::PlannerConfigurationSettings> settings_map;
settings_map.insert(pair<string, planning_interface::PlannerConfigurationSettings>(settings.name, settings));
planner_instance->setPlannerConfigurations(settings_map);

Then, I perform the planning request in PlanningContextPtr context. ID of the desired planner is a part of the planning request.

planning_interface::MotionPlanRequest req;
planning_interface::MotionPlanResponse res;
geometry_msgs::PoseStamped pose;
pose.header.frame_id = "base_link";
pose.pose.position.x = 0.3;
pose.pose.position.y = 0.0;
pose.pose.position.z = 0.75;
pose.pose.orientation.w = 1.0;

std::vector<double> tolerance_pose(3, 0.01);
std::vector<double> tolerance_angle(3, 0.01);

req.group_name = "manipulator";
req.allowed_planning_time = 1;
req.planner_id = "experimental";
moveit_msgs::Constraints pose_goal =
        kinematic_constraints::constructGoalConstraints("ee_link", pose, tolerance_pose, tolerance_angle);
req.goal_constraints.push_back(pose_goal);

planning_interface::PlanningContextPtr context =
        planner_instance->getPlanningContext(planning_scene, req, res.error_code_);

context->solve(res);

However, I get the following warning

[ WARN] [1534843451.389670926, 38.268000000]: Cannot find planning configuration for group 'manipulator' using planner 'experimental'. Will use defaults instead.

and the planner set in the planning request is not used, which is the problem I want to solve.

When I do

planner_instance->getPlanningAlgorithms(algs);

my planner "experimental" is listed.

What am I doing wrong?