Note: This tutorial assumes that you have completed the previous tutorials: Launching a joint space planner.
(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Calling the joint space planner

Description: Calling the joint space planner

Tutorial Level: BEGINNER

Next Tutorial: Configuring a kinematics plugin

In this tutorial, you will learn how to call the joint space planner and visualize the resulting path. For illustration, we will use the PR2 robot in simulation for this example. We will be sending a couple of joint space goals to the PR2 robot in simulation.

Calling the motion planner

You can call the motion planner by using the GetMotionPlan service request. Here's code for filling in the request and calling the service (which is named ompl_planning/plan_kinematic_path). More details about the fields of the request can be found in the move_arm tutorials.

Save this in a file with the name src/ompl_joint_goal.cpp in a ROS package named example_ompl_tutorials. Make sure it has a dependency on actionlib, motion_planning_msgs and planning_environment.

   1 #include <ros/ros.h>
   2 #include <actionlib/client/simple_action_client.h>
   3 #include <motion_planning_msgs/GetMotionPlan.h>
   4 
   5 #include <motion_planning_msgs/DisplayTrajectory.h>
   6 #include <planning_environment/monitors/joint_state_monitor.h>
   7 #include <boost/thread.hpp>
   8 
   9 void spinThread()
  10 {
  11   ros::spin();
  12 }
  13 
  14 int main(int argc, char **argv){
  15   ros::init (argc, argv, "ompl_joint_goal_test");
  16   boost::thread spin_thread(&spinThread);
  17 
  18   ros::NodeHandle nh;
  19 
  20   motion_planning_msgs::GetMotionPlan::Request request;
  21   motion_planning_msgs::GetMotionPlan::Response response;
  22 
  23   std::vector<std::string> names(7);
  24   names[0] = "r_shoulder_pan_joint";
  25   names[1] = "r_shoulder_lift_joint";
  26   names[2] = "r_upper_arm_roll_joint";
  27   names[3] = "r_elbow_flex_joint";
  28   names[4] = "r_forearm_roll_joint";
  29   names[5] = "r_wrist_flex_joint";
  30   names[6] = "r_wrist_roll_joint";
  31 
  32   request.motion_plan_request.group_name = "right_arm";
  33   request.motion_plan_request.num_planning_attempts = 1;
  34   request.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
  35 
  36   request.motion_plan_request.planner_id= std::string("");
  37   //  request.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
  38   request.motion_plan_request.goal_constraints.joint_constraints.resize(names.size());
  39 
  40   for (unsigned int i = 0 ; i < request.motion_plan_request.goal_constraints.joint_constraints.size(); ++i)
  41   {
  42     request.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = names[i];
  43     request.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0;
  44     request.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.1;
  45     request.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.1;
  46   }
  47     
  48   request.motion_plan_request.goal_constraints.joint_constraints[0].position = -2.0;
  49   request.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2;
  50   request.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.15;
  51 
  52   ros::ServiceClient service_client = nh.serviceClient<motion_planning_msgs::GetMotionPlan>("ompl_planning/plan_kinematic_path");
  53   service_client.call(request,response);
  54   if(response.error_code.val != response.error_code.SUCCESS)
  55   {
  56     ROS_ERROR("Motion planning failed");
  57   }
  58   else
  59   {
  60     ROS_INFO("Motion planning succeeded");
  61   }
  62 
  63 
  64   planning_environment::JointStateMonitor joint_state_monitor;
  65   ros::Publisher display_trajectory_publisher = nh.advertise<motion_planning_msgs::DisplayTrajectory>("joint_path_display", 1);
  66   while(display_trajectory_publisher.getNumSubscribers() < 1 && nh.ok())
  67   {
  68     ROS_INFO("Waiting for subscriber");
  69     ros::Duration(0.1).sleep();
  70   }
  71   motion_planning_msgs::DisplayTrajectory display_trajectory;
  72 
  73   display_trajectory.model_id = "pr2";
  74   display_trajectory.trajectory.joint_trajectory.header.frame_id = "base_footprint";
  75   display_trajectory.trajectory.joint_trajectory.header.stamp = ros::Time::now();
  76   display_trajectory.robot_state.joint_state =  joint_state_monitor.getJointStateRealJoints();
  77   display_trajectory.trajectory = response.trajectory;
  78   ROS_INFO("Publishing path for display");
  79   display_trajectory_publisher.publish(display_trajectory);
  80   joint_state_monitor.stop();
  81   ros::shutdown();
  82   spin_thread.join();
  83   return(0);
  84 }

Compile it

Add the following line to your CMakelists.txt

rosbuild_add_executable(ompl_joint_goal src/ompl_joint_goal.cpp)

Compile your code:

rosmake example_ompl_tutorials

Run it!

Start the simulator

roslaunch pr2_gazebo pr2_empty_world.launch

Start the motion planner

export ROBOT=sim
roslaunch pr2_3dnav right_arm_navigation.launch

Setup rviz for visualizing the plan

Follow the instructions at the bottom of this tutorial (from Point 7. Rviz Setup onwards). You will be setting up a planning model visualization that you can use to visualize the path generated by OMPL.

Run your request

rosrun example_ompl_tutorials ompl_joint_goal

You should be able to see the path visualized in rviz. Play around with the request a bit to see what more you can do!

Wiki: ompl_ros_interface/Tutorials/Calling the joint space planner (last edited 2011-05-24 12:02:52 by MartinGuenther)