| 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 plannerTutorial Level: BEGINNER
Next Tutorial: Configuring a kinematics plugin
Contents
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!
