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_));


    if(planning_scene_monitor_->getPlanningScene())
    {
      planning_scene_monitor_->startSceneMonitor("/planning_scene");
      planning_scene_monitor_->startWorldGeometryMonitor();
      planning_scene_monitor_->startStateMonitor();
      ROS_INFO_STREAM("Context monitors started for " << nh_.getNamespace());
    }
    else
    {
      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");
      else
        ROS_INFO_STREAM("State appears to be in collision with respect to group " << creq.group_name);
    } 
    else
    {
      if (creq.group_name.empty())
        ROS_INFO_STREAM("Start state is valid");
      else
        ROS_INFO_STREAM("Start state is valid with respect to group " << creq.group_name);
      return true;
    }

  ros::Rate loop_rate(10);

  bool collision = false;

  while(ros::ok())
  {
    //TODO Keep checking the scene for collisions

    //ros::spinOnce();

    loop_rate.sleep();
  }
  ros::shutdown();
  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 monitor
[ INFO] [1517934187.273524271]: Stopping scene monitor

So it is not detecting the above illustrated situation.

Obviously I am doing something wrong. Can anyone help?

I checked rostopic echo /movegroup/monitoredplanningscene to see if I get some information, but what I get here is empty collisionobjects and allowedcollisionmatrix. Is there something wrong with my setup? What does (noname)+ mean?

name: (noname)+
robot_state: 
  joint_state: 
    header: 
      seq: 0
      stamp: 
        secs: 0
        nsecs:         0
      frame_id: /base_link
    name: ['arm_joint_1', 'arm_joint_2', 'arm_joint_3', 'arm_joint_4', 'arm_joint_5', 'gripper_finger_joint_l', 'gripper_finger_joint_r']
    position: [2.94960643587, 2.6847968661377517, -2.5078715675795964, 0.258253982587798, 0.0, 0.005999999999999999, 0.005999999999999999]
    velocity: []
    effort: []
  multi_dof_joint_state: 
    header: 
      seq: 0
      stamp: 
        secs: 0
        nsecs:         0
      frame_id: /base_link
    joint_names: []
    transforms: []
    twist: []
    wrench: []
  attached_collision_objects: []
  is_diff: False
robot_model_name: youbot
fixed_frame_transforms: []
allowed_collision_matrix: 
  entry_names: []
  entry_values: []
  default_entry_names: []
  default_entry_values: []
link_padding: []
link_scale: []
object_colors: []
world: 
  collision_objects: []
  octomap: 
    header: 
      seq: 0
      stamp: 
        secs: 0
        nsecs:         0
      frame_id: ''
    origin: 
      position: 
        x: 0.0
        y: 0.0
        z: 0.0
      orientation: 
        x: 0.0
        y: 0.0
        z: 0.0
        w: 0.0
    octomap: 
      header: 
        seq: 0
        stamp: 
          secs: 0
          nsecs:         0
        frame_id: ''
      binary: False
      id: ''
      resolution: 0.0
      data: []
is_diff: True
---

So, how is the collision detection intended to be used?

My goal is to check for collisions while giving velocity commands to the manipulator (like JOG-Mode with collision detection). Self collisions as well as some defined world objects should be detected. So maybe I need to check for distance to objects, in order to slow down or stop. But first I think I should be able to get some collision feedback.

Thanks for any help in advance!

rostopic echo -n 1 /joint_states

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

moveit_msgs::GetPlanningScene srv;

ps_client.call(srv);

moveit_msgs::PlanningSceneComponents components_;
components_.components=511;
srv.request.components=components_;
ps_client.call(srv);

srv.request.components = srv.request.ROBOT_STATE | srv.request.WORLD_OBJECT_GEOMETRY| ...

[ INFO] [1518514475.000357309]: Known objects in planning scene:
Collision World Objects:

Attached Bodies: [ INFO] [1518514475.003499526]: Found a contact between 'arm_link_3' (type 'Robot link') and 'arm_link_4' (type 'Robot link'), which constitutes a collision. Contact information is not stored

#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));
    planning_scene_monitor->startSceneMonitor();
    planning_scene_monitor->startStateMonitor();

    if(planning_scene_monitor->getPlanningScene())
    {
      planning_scene_monitor->startSceneMonitor("/planning_scene");
      planning_scene_monitor->startWorldGeometryMonitor();
      planning_scene_monitor->startStateMonitor();
    }
    else
    {
      ROS_ERROR_STREAM_NAMED(LOGNAME, "Error in setting up the PlanningSceneMonitor.");
      exit(EXIT_FAILURE);
    }

    // Wait for initial messages
    ROS_INFO_NAMED(LOGNAME, "Waiting for first joint msg.");
    ros::topic::waitForMessage<sensor_msgs::JointState>(parameters.joint_topic);
    ROS_INFO_NAMED(LOGNAME, "Received first joint msg.");
...
    while (ros::ok())
    {
      // ** Update robot joints. This could happen in a callback function**
      pthread_mutex_lock(&shared_variables.joints_mutex);
      sensor_msgs::JointState jts = shared_variables.joints;
      pthread_mutex_unlock(&shared_variables.joints_mutex);

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

      collision_result.clear();
      planning_scene_monitor->getPlanningScene()->checkCollision(collision_request, collision_result, current_state);