We have seen how to detect collision in RViz, but what do we have to do if we want to get collision information in our ROS node. In this section, we will discuss how to get the collision information of our robot in an ROS code. This example can check self collision and environment collision, and also tell which links were collided. The example called check_collision.cpp is placed in the seven_dof_arm_test/src folder. This code is a modified version of the collision checking example of PR2 MoveIt! robot tutorials (https://github.com/ros-planning/moveit_pr2/tree/indigo-devel/pr2_moveit_tutorials).

In this code, the following snippet loads the kinematic model of the robot to the planning scene:

robot_model_loader::RobotModelLoader
 robot_model_loader("robot_description");
robot_model::RobotModelPtr kinematic_model =
 robot_model_loader.getModel();
planning_scene::PlanningScene planning_scene(kinematic_model);

To test self collision in the robot's current state, we can create two instances of class collision_detection::CollisionRequest andcollision_detection::CollisionResult, which have the name of collision_request and collision_result. After creating these objects, pass it MoveIt! collision checking function,  planning_scene.checkSelfCollision(), which can give the collision result in collision_result object and we can print the details, which are shown next:

planning_scene.checkSelfCollision(collision_request,
 collision_result);
ROS_INFO_STREAM("1. Self collision Test: "<<
 (collision_result.collision ? "in" : "not in")
<< " self collision");

If we want to test collision in a particular group, we can do that by mentioning group_name as shown next. Here group_name is arm:

collision_request.group_name = "arm";
current_state.setToRandomPositions();
//Previous results should be cleared
collision_result.clear();
planning_scene.checkSelfCollision(collision_request,
 collision_result);
ROS_INFO_STREAM("3. Self collision Test(In a group): "<<
 (collision_result.collision ? "in" : "not in"));

For performing a full collision check, we have to use the following function called planning_scene.checkCollision(). We need to mention the current robot state and the ACM matrix in this function.

The following is the code snippet to perform full collision checking using this function:

collision_detection::AllowedCollisionMatrix acm =
 planning_scene.getAllowedCollisionMatrix();
robot_state::RobotState copied_state =
 planning_scene.getCurrentState();
planning_scene.checkCollision(collision_request, collision_result,
 copied_state, acm);
ROS_INFO_STREAM("6. Full collision Test: "<<
 (collision_result.collision ? "in" : "not in")
<< " collision");

We can launch the demo of motion planning and run this node using the following command:

$ roslaunch seven_dof_arm_config demo.launch

Run the collision checking node:

$ roslaunch seven_dof_arm_test check_collision

You will get a report such as the one shown in the following image. The robot is now not in collision; if it is in collision, it will send a report of it: