| Note: This tutorial only works in Electric. |
 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. |
Checking a state for validity
Description: In this tutorial we show how you can use the planning scene architecture with the environment monitor, as well as code in the planning_environment stack to check state validity - whether or not a given state is within the joint limits, or in collision with the robot's other links or the environment. You'll also see how to publish markers based on this information.Keywords: arm_navigation electric planning_environment
Tutorial Level: INTERMEDIATE
Next Tutorial: Checking Trajectory Validity
ROS Setup
To use this tutorial make sure that you've run the following:
sudo apt-get install ros-electric-pr2-arm-navigation
Also make sure that in each terminal you use you've run:
export ROBOT=sim
Setting up the tutorial
In one terminal run the following:
roslaunch pr2_arm_navigation_tutorials pr2_floorobj_world.launch
In another terminal run:
roslaunch pr2_arm_navigation_tutorials planning_scene_architecture.launch
This will bring up an Rviz window.
Running the tutorial
This tutorial uses the executable in pr2_arm_navigation_tutorials called get_state_validity. In another terminal, run the following command:
rosrun pr2_arm_navigation_tutorials get_state_validity
You should see something like the following in the Rviz window:
What we've done is checked the current robot state for validity. The fact that the arm is shown in green means that the system thinks the state is valid - it's between the joint limits and is not in contact with the robot or the environment.
We can make things more interesting by giving an argument to the get_state_validity executable - this argument specifies a position for the r_shoulder_pan_joint of the robot in radians. Press <Ctrl-C> to quit the program and now run the following:
rosrun pr2_arm_navigation_tutorials get_state_validity .5
You should see something like the following:
In this case, the position we've tested with a value of .5 assigned to the r_shoulder_pan_joint has put us into contact with the left_arm. You should see yellow spheres - you can select them and look at the name to see the bodies that have come into contact.
Next quit again and run this:
rosrun pr2_arm_navigation_tutorials get_state_validity -.5
You should see something like this:
When we send the arm the other way it comes into contact with the collision map constructed from the laser data, which makes the state invalid.
Finally, quit and try this:
rosrun pr2_arm_navigation_tutorials get_state_validity -2.5
You should get this:
In this case the commanded position for the joint is past the joint limit of around -2.1, and it's shown in blue to indicate that the state is invalid due to a violation of joint limits.
The code
You can find this code in the package pr2_arm_navigation_tutorials in the directory src/get_state_validity.cpp.
1 #include <ros/ros.h>
2 #include <arm_navigation_msgs/GetPlanningScene.h>
3 #include <planning_environment/models/collision_models.h>
4
5 static const std::string SET_PLANNING_SCENE_DIFF_NAME = "/environment_server/set_planning_scene_diff";
6
7 int main(int argc, char **argv){
8 ros::init (argc, argv, "get_state_validity_test");
9 ros::NodeHandle rh;
10
11 ros::Publisher vis_marker_publisher_;
12 ros::Publisher vis_marker_array_publisher_;
13
14 vis_marker_publisher_ = rh.advertise<visualization_msgs::Marker>("state_validity_markers", 128);
15 vis_marker_array_publisher_ = rh.advertise<visualization_msgs::MarkerArray>("state_validity_markers_array", 128);
16
17 ros::service::waitForService(SET_PLANNING_SCENE_DIFF_NAME);
18 ros::ServiceClient get_planning_scene_client = rh.serviceClient<arm_navigation_msgs::GetPlanningScene>(SET_PLANNING_SCENE_DIFF_NAME);
19
20 arm_navigation_msgs::GetPlanningScene::Request planning_scene_req;
21 arm_navigation_msgs::GetPlanningScene::Response planning_scene_res;
22
23 if(!get_planning_scene_client.call(planning_scene_req, planning_scene_res)) {
24 ROS_WARN("Can't get planning scene");
25 return -1;
26 }
27
28 planning_environment::CollisionModels collision_models("robot_description");
29 planning_models::KinematicState* state = collision_models.setPlanningScene(planning_scene_res.planning_scene);
30
31 std::vector<std::string> arm_names = collision_models.getKinematicModel()->getModelGroup("right_arm")->getUpdatedLinkModelNames();
32 std::vector<std::string> joint_names = collision_models.getKinematicModel()->getModelGroup("right_arm")->getJointModelNames();
33
34 if(argc > 1) {
35 std::stringstream s(argv[1]);
36 double val;
37 s >> val;
38 std::map<std::string, double> nvalues;
39 nvalues["r_shoulder_pan_joint"] = val;
40
41 state->setKinematicState(nvalues);
42 }
43
44 std_msgs::ColorRGBA good_color, collision_color, joint_limits_color;
45 good_color.a = collision_color.a = joint_limits_color.a = .8;
46
47 good_color.g = 1.0;
48 collision_color.r = 1.0;
49 joint_limits_color.b = 1.0;
50
51 std_msgs::ColorRGBA point_markers;
52 point_markers.a = 1.0;
53 point_markers.r = 1.0;
54 point_markers.g = .8;
55
56 std_msgs::ColorRGBA color;
57 visualization_msgs::MarkerArray arr;
58 if(!state->areJointsWithinBounds(joint_names)) {
59 color = joint_limits_color;
60 } else if(collision_models.isKinematicStateInCollision(*state)) {
61 color = collision_color;
62 collision_models.getAllCollisionPointMarkers(*state,
63 arr,
64 point_markers,
65 ros::Duration(0.2));
66 } else {
67 color = good_color;
68 }
69
70 collision_models.getRobotMarkersGivenState(*state,
71 arr,
72 color,
73 "right_arm",
74 ros::Duration(0.2),
75 &arm_names);
76
77 while(ros::ok()) {
78 vis_marker_array_publisher_.publish(arr);
79 ros::spinOnce();
80 ros::Duration(.1).sleep();
81 }
82 collision_models.revertPlanningScene(state);
83 ros::shutdown();
84 }
Understanding the code
This example for checking validity differs substantially from the approach discussed in this Diamondback tutorial. In Diamondback, we called out to the environment server, with a particular state, and it gave us a response based on the current environment. In this tutorial, we call out to the environment server in order to get a copy of the current planning scene. We then do the work of checking states given that Planning Scene ourselves using the C++ API for the planning_environment::CollisionModels class, even producing and publishing markers using the API.
First, we get a copy of the current planning scene using this code:
17 ros::service::waitForService(SET_PLANNING_SCENE_DIFF_NAME);
18 ros::ServiceClient get_planning_scene_client = rh.serviceClient<arm_navigation_msgs::GetPlanningScene>(SET_PLANNING_SCENE_DIFF_NAME);
19
20 arm_navigation_msgs::GetPlanningScene::Request planning_scene_req;
21 arm_navigation_msgs::GetPlanningScene::Response planning_scene_res;
22
23 if(!get_planning_scene_client.call(planning_scene_req, planning_scene_res)) {
24 ROS_WARN("Can't get planning scene");
25 return -1;
26 }
The environment server will give us the current planning scene. The most important aspects of that scene for this tutorial are the position of the robot and the collision map, which we can see in the Rviz screenshots.
Next we create a CollisionModels class, seeding it from the parameter server. Then we take the planning scene and set the collision_models class given it. This will set all pertinent information within our CollisionModels - we should get an exact reconstruction of the world state as it existed within the environment_server at the moment we got the planning scene. The latter call returns a KinematicState object which will be set to the current robot state.
Given that our planning_scene_state contains the current state of the robot, we can change it very easily by just supplying a new value for one or more of the joints using the KinematicState API. In this case, we assign the value from the command line in a map, and then call KinematicState::SetKinematicState, which will set the joint positions in the map and then call forward kinematics.
In terms of checking whether the new state is valid, we first check the joint limits for the arm we care about using this code:
58 if(!state->areJointsWithinBounds(joint_names)) {
Next we check whether the state is collision, and get all the markers corresponding to collisions if so:
Then we get the markers corresponding to the robot state for the joints of the arm given the color we configured, and publish to our marker topic until the <Ctrl-C> comes in:
Finally, we revert the Planning Scene in the collision_models, which destroys our KinematicState in the process:
82 collision_models.revertPlanningScene(state);
