[Documentation] [TitleIndex] [WordIndex

Note: This tutorial assumes that you have completed the previous tutorials: Configuring a kinematics plugin.
(!) 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.

Specifying pose goals

Description: Specifying pose goals

Tutorial Level: INTERMEDIATE

In this tutorial, you will learn how to send a pose goal for a planner for a manipulator configured with a kinematics plugin (see Configuring a kinematics plugin). We will use the PR2 robot as an example since its already configured in ROS diamondback.

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 code in a file with the name ompl_pose_goal.cpp in the ROS package named example_ompl_tutorials you created earlier in this tutorial.

   1 #include <ros/ros.h>
   2 #include <motion_planning_msgs/GetMotionPlan.h>
   3 
   4 #include <motion_planning_msgs/DisplayTrajectory.h>
   5 #include <planning_environment/monitors/joint_state_monitor.h>
   6 #include <boost/thread.hpp>
   7 
   8 void spinThread()
   9 {
  10   ros::spin();
  11 }
  12 
  13 int main(int argc, char **argv)
  14 {
  15   ros::init (argc, argv, "ompl_pose_goal");
  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   request.motion_plan_request.group_name = "right_arm";
  24   request.motion_plan_request.num_planning_attempts = 1;
  25   request.motion_plan_request.planner_id = std::string("");
  26   request.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
  27   
  28   request.motion_plan_request.goal_constraints.position_constraints.resize(1);
  29   request.motion_plan_request.goal_constraints.position_constraints[0].header.stamp = ros::Time::now();
  30   request.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id = "torso_lift_link";
  31     
  32   request.motion_plan_request.goal_constraints.position_constraints[0].link_name = "r_wrist_roll_link";
  33   request.motion_plan_request.goal_constraints.position_constraints[0].position.x = 0.75;
  34   request.motion_plan_request.goal_constraints.position_constraints[0].position.y = -0.188;
  35   request.motion_plan_request.goal_constraints.position_constraints[0].position.z = 0;
  36     
  37   request.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.type = geometric_shapes_msgs::Shape::BOX;
  38   request.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
  39   request.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
  40   request.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
  41 
  42   request.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.w = 1.0;
  43   request.motion_plan_request.goal_constraints.position_constraints[0].weight = 1.0;
  44 
  45   request.motion_plan_request.goal_constraints.orientation_constraints.resize(1);
  46   request.motion_plan_request.goal_constraints.orientation_constraints[0].header.stamp = ros::Time::now();
  47   request.motion_plan_request.goal_constraints.orientation_constraints[0].header.frame_id = "torso_lift_link";    
  48   request.motion_plan_request.goal_constraints.orientation_constraints[0].link_name = "r_wrist_roll_link";
  49   request.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.x = 0.0;
  50   request.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.y = 0.0;
  51   request.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.z = 0.0;
  52   request.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.w = 1.0;
  53     
  54   request.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_roll_tolerance = 0.04;
  55   request.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_pitch_tolerance = 0.04;
  56   request.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_yaw_tolerance = 0.04;
  57 
  58   request.motion_plan_request.goal_constraints.orientation_constraints[0].weight = 1.0;
  59 
  60 
  61   ros::ServiceClient service_client = nh.serviceClient<motion_planning_msgs::GetMotionPlan>("ompl_planning/plan_kinematic_path");
  62   service_client.call(request,response);
  63   if(response.error_code.val != response.error_code.SUCCESS)
  64   {
  65     ROS_ERROR("Motion planning failed");
  66   }
  67   else
  68   {
  69     ROS_INFO("Motion planning succeeded");
  70   }
  71 
  72 
  73   planning_environment::JointStateMonitor joint_state_monitor;
  74   ros::Publisher display_trajectory_publisher = nh.advertise<motion_planning_msgs::DisplayTrajectory>("joint_path_display", 1);
  75   while(display_trajectory_publisher.getNumSubscribers() < 1 && nh.ok())
  76   {
  77     ROS_INFO("Waiting for subscriber");
  78     ros::Duration(0.1).sleep();
  79   }
  80   motion_planning_msgs::DisplayTrajectory display_trajectory;
  81 
  82   display_trajectory.model_id = "pr2";
  83   display_trajectory.trajectory.joint_trajectory.header.frame_id = "base_footprint";
  84   display_trajectory.trajectory.joint_trajectory.header.stamp = ros::Time::now();
  85   display_trajectory.robot_state.joint_state =  joint_state_monitor.getJointStateRealJoints();
  86   display_trajectory.trajectory = response.trajectory;
  87   ROS_INFO("Publishing path for display");
  88   display_trajectory_publisher.publish(display_trajectory);
  89   joint_state_monitor.stop();
  90   ros::shutdown();
  91   spin_thread.join();
  92   return(0);
  93 
  94 }

Compile it

Add the following line to your CMakelists.txt

rosbuild_add_executable(ompl_pose_goal src/ompl_pose_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_pose_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!

Specifying multiple goals (Optional)

You could, if you wanted, specify multiple pose goals to the planner. You can do this by adding more position and orientation constraints to the request. The planner will plan to one of the goals in your list. This could be useful if, for example, you want to plan to one of the multiple grasps available for grasping an object.


2019-11-16 13:05