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 collisions for a given robot state

In this tutorial we will show how a node called the environment server can consume collision maps and check whether particular robot states represent collisions, violations of joint limits, or violate Cartesian constraints. We will first create a short program that actually makes the call to the environment server to check a robot state for collision and then show the feedback that comes from a running environment server.

ROS Setup

In this tutorial, you will learn how to use the environment_server node to check collisions, joint limits and constraints for a given robot state. If you haven't already done so, the first thing we'll need to do is create a package for all the motion planning environment tutorials that we will work with. To do this we'll use the handy roscreate-pkg command where we want to create the package directory with a dependency on the pr2_arm_navigation_tutorials package as shown below:

roscreate-pkg arm_navigation_tutorials pr2_arm_navigation_tutorials

After this is done we'll need to roscd to the package we created, since we'll be using it as our workspace

roscd arm_navigation_tutorials

Also, make sure to set an environment variable called ROBOT to sim. E.g., in a bash shell,

 export ROBOT=sim

(Note: you need to do this only if you are running this tutorial on a simulator).

Creating the Node

Now that we have our package, we need to write the code that will call the query service. Fire up a text editor and paste the following into a file called src/get_state_validity.cpp. Don't worry if there are things you don't understand, we'll walk through the details of this file line-by-line shortly.

   1 #include <ros/ros.h>
   2 #include <planning_environment_msgs/GetStateValidity.h>
   3 
   4 int main(int argc, char **argv){
   5   ros::init (argc, argv, "get_state_validity_test");
   6   ros::NodeHandle rh;
   7 
   8   ros::service::waitForService("environment_server/get_state_validity");
   9   ros::ServiceClient check_state_validity_client_ = rh.serviceClient<planning_environment_msgs::GetStateValidity>("environment_server/get_state_validity");
  10 
  11   planning_environment_msgs::GetStateValidity::Request req;
  12   planning_environment_msgs::GetStateValidity::Response res;
  13 
  14   req.robot_state.joint_state.name.push_back("r_shoulder_pan_joint");
  15   req.robot_state.joint_state.name.push_back("r_shoulder_lift_joint");
  16   req.robot_state.joint_state.name.push_back("r_upper_arm_roll_joint");
  17   req.robot_state.joint_state.name.push_back("r_elbow_flex_joint");
  18   req.robot_state.joint_state.name.push_back("r_forearm_roll_joint");
  19   req.robot_state.joint_state.name.push_back("r_wrist_flex_joint");
  20   req.robot_state.joint_state.name.push_back("r_wrist_roll_joint");
  21   req.robot_state.joint_state.position.resize(7,0.0);
  22 
  23   //these set whatever non-zero joint angle values are desired
  24   req.robot_state.joint_state.position[0] = 0.0;
  25  
  26   req.robot_state.joint_state.header.stamp = ros::Time::now();
  27   req.check_collisions = true;
  28   if(check_state_validity_client_.call(req,res))
  29   {
  30     if(res.error_code.val == res.error_code.SUCCESS)
  31       ROS_INFO("Requested state is not in collision");
  32     else
  33       ROS_INFO("Requested state is in collision. Error code: %d",res.error_code.val);
  34   }
  35   else
  36   {
  37     ROS_ERROR("Service call to check state validity failed %s",check_state_validity_client_.getService().c_str());
  38     return false;
  39   }
  40   ros::shutdown();
  41 }

Creating the service request

The service request to the GetStateValidity service requires the following information:

• A set of joint positions that you would like to check
• The types of checks to be carried out

Filling up the joint state

  14   req.robot_state.joint_state.name.push_back("r_shoulder_pan_joint");
  15   req.robot_state.joint_state.name.push_back("r_shoulder_lift_joint");
  16   req.robot_state.joint_state.name.push_back("r_upper_arm_roll_joint");
  17   req.robot_state.joint_state.name.push_back("r_elbow_flex_joint");
  18   req.robot_state.joint_state.name.push_back("r_forearm_roll_joint");
  19   req.robot_state.joint_state.name.push_back("r_wrist_flex_joint");
  20   req.robot_state.joint_state.name.push_back("r_wrist_roll_joint");
  21   req.robot_state.joint_state.position.resize(7,0.0);
  22 
  23   //these set whatever non-zero joint angle values are desired
  24   req.robot_state.joint_state.position[0] = 0.0;

We first fill up the joint state with desired positions for the right arm. These are example positions, you can change and play around with them if you like.

Types of checks

The types of checks that can be performed on the input robot state are specified within the service request itself as individual flags.

  26   req.robot_state.joint_state.header.stamp = ros::Time::now();
  27   req.check_collisions = true;

There are currently 4 types of checks that you can ask to be carried out. Each type of check can be activated by setting the corresponding flag.

• collision checks: request.check_collisions = true;

• joint limits checks: request.check_joint_limits = true;

• goal constraints test: request.check_goal_constraints = true;

• path constraints test: request.check_path_constraints = true;

Building the node

Now that we have a package and a source file, we'll want to build and then try things out. The first step will be to add our src/get_state_validity.cpp file to our CMakeLists.txt file to get it to build. Open up CMakeLists.txt in your editor of choice and add the following line to the bottom of the file.

rosbuild_add_executable(get_state_validity src/get_state_validity.cpp)

Once this is done, we can build our executable by typing make.

rosmake

Launching the PR2 in simulation

We'll be using the PR2 to illustrate all the concepts in this tutorial, and thus the first step is to launch the PR2 in simulation. To launch the PR2 in simulation assuming that the pr2_arm_navigation_tutorials in your ROS_PACKAGE_PATH:

roslaunch pr2_arm_navigation_tutorials pr2_floorobj_world.launch

You should get something that looks like this configuration:

Launching rviz with preconfigured topics

In order to view the data topics we use in this tutorial run the following command to launch rviz with a preconfigured set of topics:

roslaunch pr2_arm_navigation_tutorials rviz_collision_tutorial_2.launch

Launch the laser perception pipeline

Next we need to launch the tilting laser to filtered collision map pipeline we created in the previous tutorial:

roslaunch pr2_arm_navigation_tutorials laser-perception.launch

The laser should begin nodding and if you enable the sixth topic in the rviz configuration (collision map) you should see a number of green squares representing the collision map:

Setup the environment server

Now, launch the environment server.

roslaunch pr2_arm_navigation_actions environment_server.launch

You should see the following output:

process[environment_server-1]: started with pid [16573]
[ INFO] 1417.882999973: Configuring action for 'right_arm'
[ INFO] 1417.882999973: Waiting for robot state ...
[ INFO] 1417.882999973: Waiting for joint state ...
[ INFO] 1417.882999973: Pose ignored
[ WARN] 1417.882999973: Message from [/collision_map_self_occ_node] has a non-fully-qualified frame_id [base_link]. Resolved locally to [/base_link].  This is will likely not work in multi-robot systems.  This message will only print once.
[ INFO] 1417.883999973: Ignoring joint 'r_gripper_joint'
[ INFO] 1417.883999973: Ignoring joint 'l_gripper_joint'
[ INFO] 1417.933999973: Robot state received!
[ INFO] 1417.934999973: Waiting for map ...
[ INFO] 1419.052999973: Map received!
[ INFO] 1419.070999973: Found collisions will be displayed as visualization markers
[ INFO] 1419.102999973: Environment monitor started

Run the code in a collision free configuration

Next enable the 9th rviz topic marked State Validity. This will cause another ghosted robot to appear in rviz floating near the existing robot. This ghosted robot will display the position we are checking for validity.

Then run the executable we just created

./bin/get_state_validity

If it succeeds, you should see the following output

[ INFO] 0.000000000: Requested state is not in collision

In rviz you should see something like the following:

The ghosted robot shows the position with all joint links at 0.0 given that the rest of the robot is in its current position; this state is not in collision with anything and is within all joint limits, and the environment server confirms that.

To see what happens if we ask for a state in collision, change the following line in get_state_validity.cpp:

  24   req.robot_state.joint_state.position[0] = 0.0;

to

       req.robot_state.joint_state.position[0] = -0.55;

This tells the environment server to check the state where the first joint ("r_shoulder_pan_link") has a value of -0.55, with all other values left at 0.0.

Before re-running get_state_validity make sure that the eighth rviz entry (Collision markers), is enabled. Recompile and run get_state_validity again.

If it succeeds, you should see the following output

[ INFO] 0.000000000: Requested state is in collision. Error code: -23

The error code corresponds to the error codes in the ArmNavigationErrorCodes message in the motion_planning_msgs package.

In rviz you see something like this:

The environment server has reported that the given state will be in collision. What it doesn't say is what collisions were present. In order to determine that we must consult rviz. When the environment server is asked to check a state that collides with something in the environment it broadcasts markers. If you had Collision Markers enabled when you ran your program you should have a number of small yellow spheres in the middle of the collision map hits where the pole is. It may help to disable the collision map to see them. These points represent some (but potentially not all) of the collisions that resulted from the state validity check. The spheres are centered when the collisions were detected. To see what collided with what at those points, choose Select from the rviz toolbar and select one or more points. In the rviz selection window you should see a number of markers with named "r_forearm_link+points". This indicates that at the requested arm position the right forearm link would hit the points from the collision map.

Colliding with objects in the environment is only one kind of collision the environment server can detect. To illustrate another kind of collision, again edit get_state_validity.cpp, editing the line to read:

       req.robot_state.joint_state.position[0] = 0.4;

Recompile and run get_state_validity again. You should again see:

[ INFO] 0.000000000: Requested state is in collision. Error code: -23

This time you should get yellow collision spheres by the robot's left arm, as shown in the this screenshot:

Selecting a collision should show one left arm link + one right arm link. This is a self-collision, where the robot will hit itself with the arms in the given position. These kind of collisions should generally also be avoided, and the environment server gives the ability to check for them.