Check collision between robot and environment using MoveIt! checkCollision

edit

Hi, I am trying to detect a collision between a robot and its environment. I have created an object and published it so that it is visible in RViz. "checkSelfCollision" works as intended, and check the robot according to "joint_state_publisher", but "checkCollision" does not detect any collision between robot and wall.

What am I missing?

Running; Ros Kinetic, Ubuntu 16.04

#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/planning_interface/planning_interface.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
#include <moveit/kinematic_constraints/utils.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/PlanningScene.h>
#include <moveit_visual_tools/moveit_visual_tools.h>

#include <boost/scoped_ptr.hpp>

// Environment stuff
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>


int main(int argc, char** argv)
{
    const std::string node_name = "dfr1500_moveit_motion_planning";
    ros::init(argc, argv, node_name);
    ros::NodeHandle nh;
    ros::AsyncSpinner spinner(1);
    spinner.start();



    const std::string PLANNING_GROUP = "dfr1500_full";
    robot_model_loader::RobotModelLoaderPtr robot_model_loader
                (new robot_model_loader::RobotModelLoader("robot_description"));
    robot_model::RobotModelPtr robot_model = robot_model_loader->getModel();

    moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

    const robot_state::JointModelGroup* joint_model_group =   
             move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);

    ////////////////////////////////////////////////////////////
    // Adding object
    ////////////////////////////////////////////////////////////

    // Define a collision object ROS message.
    moveit_msgs::CollisionObject collision_object;
    std::vector<moveit_msgs::CollisionObject> collision_objects;

    // Create Wall
    collision_object.header.frame_id = move_group.getPlanningFrame();
    collision_object.id = "wall";

    shape_msgs::SolidPrimitive primitive;
    primitive.type = primitive.BOX;

    primitive.dimensions.resize(3);
    primitive.dimensions[0] = 6;
    primitive.dimensions[1] = 0.3;
    primitive.dimensions[2] = 4;

    geometry_msgs::Pose box_pose;
    box_pose.position.x = 2.95;
    box_pose.position.y = -0.9;
    box_pose.position.z = 1.95;

    collision_object.primitives.push_back(primitive);
    collision_object.primitive_poses.push_back(box_pose);
    collision_object.operation = collision_object.ADD;

    collision_objects.push_back(collision_object);

    // Add the collision object into the world
    ROS_INFO_NAMED("tutorial", "Add an object into the world");
    planning_scene_interface.addCollisionObjects(collision_objects);


    ////////////////////////////////////////////////////////////
    // Check collisions
    ////////////////////////////////////////////////////////////

    /* Create a RobotState and JointModelGroup to keep track of the current robot pose and planning group*/
    robot_state::RobotStatePtr robot_state(new robot_state::RobotState(robot_model));

    const std::vector<std::string>& joint_names = joint_model_group->getVariableNames();
    std::vector<double> joint_values;

    //ROS_INFO_STREAM("Fetching joint values, num = " << joint_model_group.getJointModel("R1"));

    // Using the :moveit_core:`RobotModel`, we can construct a
    // :planning_scene:`PlanningScene` that maintains the state of
    // the world (including the robot).
    planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model));

    //planning_scene.addCollisionObjects(collision_objects);
    // With the planning scene we create a planing scene monitor that
    // monitors planning scene diffs and applys them to the planning scene
    planning_scene_monitor::PlanningSceneMonitorPtr psm(new planning_scene_monitor::PlanningSceneMonitor(planning_scene, robot_model_loader));
    psm->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE);
    psm->startStateMonitor();
    psm->startSceneMonitor();

    sleep(1); // Need to slepp to update links

    // Update joints
    robot_state::RobotState r = psm->getPlanningScene()->getCurrentState();
    r.copyJointGroupPositions(joint_model_group, joint_values); // Update joint positions

    ROS_INFO_STREAM("Fetching joint values, num = " << joint_names.size());
    // Print out current joint values
    for (std::size_t i = 0; i < joint_names.size(); ++i){
        ROS_INFO("Joint %s: %f", joint_names[i].c_str(), joint_values[i]);
    }

    // Check self collision
    collision_detection::CollisionRequest collision_request;
    collision_detection::CollisionResult collision_result;

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

    // Check environment collision
    collision_result.clear();
    planning_scene->checkCollision(collision_request, collision_result);
    ROS_INFO_STREAM("Test 2: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision");

    ros::shutdown();
  return 0;
}