ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Collision checking using MoveIt!

asked 2018-02-06 11:36:17 -0500

giobanny gravatar image

Hello everone,

I am using ROS Indigo on Xubuntu 14.04.5 LTS and having issues with collision detection using MoveIt (ros-indigo-moveit:amd64/trusty 0.7.13-0trusty-20180111-003649-0800).

When I start the demo.launch created by the setup assistant, I can check visually for collisions (parts appear in red) in rviz by changing the joint values and updating the new goal state as "current". (No collisions are indicated before updating)

After following several tutorials (Planning Scene Tutorial or ROS API Planning Scene Tutorial), reading through various Q&A (Collision checking for arbitrary poses using MoveIt! / PlanningSceneMonitor or Check collision between CollisonObject and robot with moveit ) and the google moveit-users group, I tried getting collision information from the same scene with red marked collisions using this code (inspired by):

#include <moveit/robot_state/conversions.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
#include <moveit_msgs/ExecuteKnownTrajectory.h>
#include <std_srvs/Empty.h>
#include <ros/ros.h>

int main(int argc, char** argv)
  // Init the ROS node
  ros::init(argc, argv, "collision_check");
    ros::NodeHandle nh_("~");

  std::string group_name_;

  group_name_ = "arm_chain";

  robot_model_loader::RobotModelLoaderPtr robot_model_loader_;
  robot_model_loader_.reset(new robot_model_loader::RobotModelLoader("robot_description"));
  planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
  planning_scene_monitor_.reset(new planning_scene_monitor::PlanningSceneMonitor(robot_model_loader_));

      ROS_INFO_STREAM("Context monitors started for " << nh_.getNamespace());
      ROS_ERROR_STREAM("Planning scene not configured for " << nh_.getNamespace());

        collision_detection::CollisionRequest collision_request;
        collision_detection::CollisionResult collision_result;
        planning_scene_monitor_->getPlanningScene()->checkSelfCollision(collision_request, collision_result);
        ROS_INFO_STREAM("Test 1: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision");

        // get the start state
    robot_state::RobotState start_state = planning_scene_monitor_->getPlanningScene()->getCurrentState();

    collision_detection::CollisionRequest creq;
    creq.group_name = group_name_;
    collision_detection::CollisionResult cres;
    planning_scene_monitor_->getPlanningScene()->checkCollision(creq, cres, start_state);
    if (cres.collision)
      // verbose mode
      collision_detection::CollisionRequest vcreq = creq;
      collision_detection::CollisionResult vcres;
      vcreq.verbose = true;
      planning_scene_monitor_->getPlanningScene()->checkCollision(vcreq, vcres, start_state);

      if (creq.group_name.empty())
        ROS_INFO("State appears to be in collision");
        ROS_INFO_STREAM("State appears to be in collision with respect to group " << creq.group_name);
      if (creq.group_name.empty())
        ROS_INFO_STREAM("Start state is valid");
        ROS_INFO_STREAM("Start state is valid with respect to group " << creq.group_name);
      return true;

  ros::Rate loop_rate(10);

  bool collision = false;

    //TODO Keep checking the scene for collisions


  return 0;

Running it I get this:

[ INFO] [1517934186.945159033]: Loading robot model 'youbot'...
[ INFO] [1517934187.098497064]: Loading robot model 'youbot'...
[ INFO] [1517934187.227919777]: Starting scene monitor
[ INFO] [1517934187.232268182]: Listening to '/planning_scene'
[ INFO] [1517934187.232319281]: Starting world geometry monitor
[ INFO] [1517934187.236487616]: Listening to '/collision_object'
[ INFO] [1517934187.240655616]: Listening to '/planning_scene_world' for planning scene world geometry
[ WARN] [1517934187.241435490]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead
[ WARN] [1517934187.241471283]: Target frame specified but no TF instance specified. No transforms will be applied to received data.
[ INFO] [1517934187.269569073]: Listening to '/attached_collision_object' for attached collision objects
[ INFO] [1517934187.269623563]: Context monitors started for /youbot_moveit_interface_collision_check
[ INFO] [1517934187.269923221]: Test 1: Current state is not in self collision
[ INFO] [1517934187.270076782]: Start state is valid with respect to group arm_chain
[ INFO] [1517934187.271758918]: Stopping world geometry ...
edit retag flag offensive close merge delete


I'm not sure what you're asking; just to clarify, you are expecting to see a collision when you call check collision, but you don't?

BryceWilley gravatar image BryceWilley  ( 2018-02-07 18:54:59 -0500 )edit

Exactly. I am trying to check for collision by first moving the simulated robot arm (from the robots moveit_config demo.launch) to different colliding poses, and then running the code. But it never indicates collisions.

giobanny gravatar image giobanny  ( 2018-02-08 01:36:55 -0500 )edit

So I think I know your problem, but I'm not sure how you're 'moving the simulated robot arm'. Are you dragging the arm around in RViz, or using the joint publisher?

BryceWilley gravatar image BryceWilley  ( 2018-02-08 11:18:29 -0500 )edit

I am using the joint state publisher, since the interactive markers don't work for some reason.

giobanny gravatar image giobanny  ( 2018-02-08 13:23:55 -0500 )edit

Hm. I'm not sure what could be the problem then, joint state publisher should change the current state of the robot. Things I'd suggest doing to debug:

  1. Confirm the current state is what you expect it to be (what joint state publisher says it is) with start_state.printStatePositions(std::cout);
BryceWilley gravatar image BryceWilley  ( 2018-02-08 16:48:35 -0500 )edit

'2'. Confirm the planning scene is up to date/ that it has all of the scene objects that it should with planning_scene_monitor_->getPlanningScene()->printKnownObjects(std::cout);

BryceWilley gravatar image BryceWilley  ( 2018-02-08 16:50:18 -0500 )edit

You are right!

  1. The current state obtained with start_state.printStatePositions(std::cout); is never changed by joint_state_publisher.

  2. Although I added a Box and a Cylinder object (visible in rviz), the planning scene does not print any "collision world objects" nor "attached bodies".

giobanny gravatar image giobanny  ( 2018-02-09 04:48:59 -0500 )edit

2 Answers

Sort by ยป oldest newest most voted

answered 2019-01-01 16:27:28 -0500

AndyZe gravatar image

Thanks for posting your example code, it really helped me even if you weren't 100% there. Here's some collision-checking code with PlanningSceneMonitor that works for me:

#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
    const robot_model::RobotModelPtr& kinematic_model = model_loader_ptr->getModel();
    planning_scene::PlanningScene planning_scene(kinematic_model);
    collision_detection::CollisionRequest collision_request;
    collision_request.group_name = parameters.move_group_name;
    collision_request.distance = true;
    collision_detection::CollisionResult collision_result;
    robot_state::RobotState& current_state = planning_scene.getCurrentStateNonConst();

    planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor;
    planning_scene_monitor.reset(new planning_scene_monitor::PlanningSceneMonitor(model_loader_ptr));

      ROS_ERROR_STREAM_NAMED(LOGNAME, "Error in setting up the PlanningSceneMonitor.");

    // Wait for initial messages
    ROS_INFO_NAMED(LOGNAME, "Waiting for first joint msg.");
    ROS_INFO_NAMED(LOGNAME, "Received first joint msg.");
    while (ros::ok())
      // ** Update robot joints. This could happen in a callback function**
      sensor_msgs::JointState jts = shared_variables.joints;

      for (std::size_t i = 0; i < jts.position.size(); ++i)
        current_state.setJointPositions([i], &jts.position[i]);

      planning_scene_monitor->getPlanningScene()->checkCollision(collision_request, collision_result, current_state);
edit flag offensive delete link more


Hi, could you maybe provide your full code for this? I'd like to implement this functionality but I've been struggling a lot to get it working.

umopapisdnquap gravatar image umopapisdnquap  ( 2019-10-19 06:24:08 -0500 )edit

answered 2018-02-08 17:51:57 -0500

I'd confirm that your arm is at the expected joint pose, you can do this in one of two ways:

a - Add a "RobotModel" display in rviz and it should show the robot a the real current position


b - Listen to the joint_states topics and verify that the joint position match the commanded values, in a sourced terminal run the following command:

rostopic echo -n 1 /joint_states

If you have namespaced your setup then you have to prepend that namespace to /joint_states

edit flag offensive delete link more


I checked both and obtain the desired Arm joint positions.

Following the other comments, is it possible that I am creating a new model in a new planning_scene that has no link to the one I am seeing in rviz?

giobanny gravatar image giobanny  ( 2018-02-09 05:03:08 -0500 )edit

In addition, getPlanningScene() has a warning in the documentation: Avoid this function! Returns an unsafe pointer to the current planning scene.

Can this be related?

giobanny gravatar image giobanny  ( 2018-02-09 05:05:58 -0500 )edit

I see that you are creating a PlanningSceneMonitor and then you are grabbing the planning scene from the monitor. This is scene is most likely an instance that is not in sync with the one hosted by the move group node.

jrgnicho gravatar image jrgnicho  ( 2018-02-09 09:05:46 -0500 )edit

You can call the /get_planning_scene service in order to get the planning scene in its current state.

jrgnicho gravatar image jrgnicho  ( 2018-02-09 09:06:29 -0500 )edit

I added the following:

ros::ServiceClient ps_client = nh_.serviceClient<moveit_msgs::GetPlanningScene>("get_planning_scene");

moveit_msgs::GetPlanningScene srv;

And before calling "checkCollision" I call the service:;

Is this what you meant?

giobanny gravatar image giobanny  ( 2018-02-12 06:48:26 -0500 )edit

By doing that it is still not working, the joints are still always the same.

giobanny gravatar image giobanny  ( 2018-02-12 06:50:07 -0500 )edit

Did you set the components bit flags on the request?

jrgnicho gravatar image jrgnicho  ( 2018-02-12 09:53:55 -0500 )edit

Just added them like this.

moveit_msgs::PlanningSceneComponents components_;

What should I do with the response of that call, moveit_msgs/PlanningScene scene ?

giobanny gravatar image giobanny  ( 2018-02-12 10:29:48 -0500 )edit

Question Tools



Asked: 2018-02-06 11:36:17 -0500

Seen: 4,698 times

Last updated: Jan 01 '19