| Note: Some of the files in here haven't been released yet and may not work. Sorry!. This tutorial assumes that you have completed the previous tutorials: Checking State Validity. |
 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. |
Adding Virtual Objects to the Planning Scene
Description: In this tutorial we show how to add virtual objects to the Planning Scene and to check state validity against the virtual objects.Keywords: planning_scene electric
Tutorial Level: INTERMEDIATE
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_virtual_object. In another terminal, run the following command:
rosrun pr2_arm_navigation_tutorials get_state_validity_virtual_object 0
You should see something like the following:
In this program we're still doing a state validity call, but now with the joint r_shoulder_lift_link set to a value of .9 . You'll also note that given the position of the tilting laser and the arms that we can only see part of the pole in the collision map even though it goes all the way down to the floor in the real world. This is a problem for checking validity, as you can see if you call the following command:
rosrun pr2_arm_navigation_tutorials get_state_validity_virtual_object 0 -.5
This gives you the following:
It's hard to see in a 2D view, but use Rviz's camera commands to convince yourself that the robot's hand is actually under the pole. The fact that this state is registering as green is a problem - if we were attempting to plan to get the robot's arm to its side it might attempt to go under the pole, knocking it over in the process. To avoid such collisions we might need to use some sort of inference algorithm to reason about space we don't have any information about - this kind of algorithm is outside the scope of this tutorial. Here we assume that we have reliable information that a pole that goes down to the floor exists in a particular space and we want to check validity based on that information. Note that there are two methods for adding an object to the planning scene - publishing on a topic or adding the object to the planning_scene_diff - in this tutorial we do the latter.
Quit the last command, and execute the following:
rosrun pr2_arm_navigation_tutorials get_state_validity_virtual_object 1 -.5
Changing the first argument to 1 tells the program to add the virtual object to the planning scene. Now you should get the following:
Note that when you add an object to the planning scene it masks out the parts of the collision map that lie near the object. The assumption is that if you are doing object recognition you have more reliable information about what's there than is contained in the collision map, and thus we filter out nearby points. With the virtual object in place the pose is now shown to be in collision, and a planning algorithm would need to plan around the entire pole.
The code
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 =
19 rh.serviceClient<arm_navigation_msgs::GetPlanningScene>(SET_PLANNING_SCENE_DIFF_NAME);
20
21 arm_navigation_msgs::GetPlanningScene::Request planning_scene_req;
22 arm_navigation_msgs::GetPlanningScene::Response planning_scene_res;
23
24 if(argc > 1) {
25
26 std::stringstream s(argv[1]);
27 bool add;
28 s >> add;
29
30 if(add) {
31 //add the cylinder into the collision space
32 arm_navigation_msgs::CollisionObject cylinder_object;
33 cylinder_object.id = "pole";
34 cylinder_object.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD;
35 cylinder_object.header.frame_id = "odom_combined";
36 cylinder_object.header.stamp = ros::Time::now();
37 arm_navigation_msgs::Shape object;
38 object.type = arm_navigation_msgs::Shape::CYLINDER;
39 object.dimensions.resize(2);
40 object.dimensions[0] = .1;
41 object.dimensions[1] = 1.0;
42 geometry_msgs::Pose pose;
43 pose.position.x = .6;
44 pose.position.y = -.6;
45 pose.position.z = .5;
46 pose.orientation.x = 0;
47 pose.orientation.y = 0;
48 pose.orientation.z = 0;
49 pose.orientation.w = 1;
50 cylinder_object.shapes.push_back(object);
51 cylinder_object.poses.push_back(pose);
52
53 planning_scene_req.planning_scene_diff.collision_objects.push_back(cylinder_object);
54 }
55 }
56
57 if(!get_planning_scene_client.call(planning_scene_req, planning_scene_res)) {
58 ROS_WARN("Can't get planning scene");
59 return -1;
60 }
61
62 planning_environment::CollisionModels collision_models("robot_description");
63 planning_models::KinematicState* state =
64 collision_models.setPlanningScene(planning_scene_res.planning_scene);
65
66 std::vector<std::string> arm_names =
67 collision_models.getKinematicModel()->getModelGroup("right_arm")->getUpdatedLinkModelNames();
68 std::vector<std::string> joint_names =
69 collision_models.getKinematicModel()->getModelGroup("right_arm")->getJointModelNames();
70
71 std::map<std::string, double> nvalues;
72 nvalues["r_shoulder_lift_joint"] = .9;
73
74 if(argc > 2) {
75 std::stringstream s(argv[2]);
76 double val;
77 s >> val;
78 nvalues["r_shoulder_pan_joint"] = val;
79 }
80 state->setKinematicState(nvalues);
81
82
83 std_msgs::ColorRGBA good_color, collision_color, joint_limits_color;
84 good_color.a = collision_color.a = joint_limits_color.a = .8;
85
86 good_color.g = 1.0;
87 collision_color.r = 1.0;
88 joint_limits_color.b = 1.0;
89
90 std_msgs::ColorRGBA point_markers;
91 point_markers.a = 1.0;
92 point_markers.r = 1.0;
93 point_markers.g = .8;
94
95 std_msgs::ColorRGBA color;
96 visualization_msgs::MarkerArray arr;
97 if(!state->areJointsWithinBounds(joint_names)) {
98 color = joint_limits_color;
99 } else if(collision_models.isKinematicStateInCollision(*state)) {
100 color = collision_color;
101 collision_models.getAllCollisionPointMarkers(*state,
102 arr,
103 point_markers,
104 ros::Duration(0.2));
105 } else {
106 color = good_color;
107 }
108
109 collision_models.getRobotMarkersGivenState(*state,
110 arr,
111 color,
112 "right_arm",
113 ros::Duration(0.2),
114 &arm_names);
115
116 while(ros::ok()) {
117 vis_marker_array_publisher_.publish(arr);
118 ros::spinOnce();
119 ros::Duration(.1).sleep();
120 }
121 collision_models.revertPlanningScene(state);
122 ros::shutdown();
123 }
Understanding the code
Most of this code is that same as in this tutorial, with a few exceptions. Most substantially, here is the code where we potentially add the virtual pole:
30 if(add) {
31 //add the cylinder into the collision space
32 arm_navigation_msgs::CollisionObject cylinder_object;
33 cylinder_object.id = "pole";
34 cylinder_object.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD;
35 cylinder_object.header.frame_id = "odom_combined";
36 cylinder_object.header.stamp = ros::Time::now();
37 arm_navigation_msgs::Shape object;
38 object.type = arm_navigation_msgs::Shape::CYLINDER;
39 object.dimensions.resize(2);
40 object.dimensions[0] = .1;
41 object.dimensions[1] = 1.0;
42 geometry_msgs::Pose pose;
43 pose.position.x = .6;
44 pose.position.y = -.6;
45 pose.position.z = .5;
46 pose.orientation.x = 0;
47 pose.orientation.y = 0;
48 pose.orientation.z = 0;
49 pose.orientation.w = 1;
50 cylinder_object.shapes.push_back(object);
51 cylinder_object.poses.push_back(pose);
52
53 planning_scene_req.planning_scene_diff.collision_objects.push_back(cylinder_object);
54 }
We give the object a name, describe the type, assign dimensions, and set a pose for the object. We also set the operation to ADD, meaning that it will be included in the planning scene. This goes into the planning_scene_diff, meaning that we recognize that this is a change from the current state of the world. The object will be added to the current state and pushed to all components, and also returned to us in the response. We can add any number of objects to the planning scene using this methodology. Adding objects in this manner does not change the current state of the world - if we don't include them with our next diff they will no longer be in the planning scene.
Finally, we potentially set the state for two joints instead of one in this block:
