I am trying to use OMPL planners directly through Motion Planning API, as [described in this tutorial](http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/motion_planning_api/motion_planning_api_tutorial.html) 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> 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("type", "geometric::RRTConnect"));
map settings_map;
settings_map.insert(pair(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 tolerance_pose(3, 0.01);
std::vector 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?
↧