Collision checking using MoveIt!

edit

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 ...