Check collision between CollisonObject and robot with moveit

edit

Hi,

I'm try to check the collision between collisionObject and robot.

I added the collision object following the tutorial http://docs.ros.org/kinetic/api/movei... , and the collision object is shown in Rviz. However, when I try to check the collision, there is some problem.

 moveit_msgs::CollisionObject cylinder;

cylinder.id = "seven_dof_arm_cylinder";
cylinder.header.frame_id = "base_link";

shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.CYLINDER;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.6;
primitive.dimensions[1] = 0.2;

geometry_msgs::Pose pose;
pose.orientation.w = 1.0;
pose.position.x =  0.0;
pose.position.y = -0.4;
pose.position.z =  0.4;

cylinder.primitives.push_back(primitive);
cylinder.primitive_poses.push_back(pose);
cylinder.operation = cylinder.ADD;

std::vector<moveit_msgs::CollisionObject> collision_objects;
collision_objects.push_back(cylinder);

current_scene.addCollisionObjects(collision_objects);

Then, I check the collision with

std::vector<double> joint_values; const robot_model::JointModelGroup* joint_model_group = current_state.getJointModelGroup("arm"); current_state.copyJointGroupPositions(joint_model_group, joint_values);

joint_values[0] = -1.57; //hard-coded since we know collisions will happen here
joint_values[1] = 0.5;

current_state.setJointGroupPositions(joint_model_group, joint_values);

collision_result.clear();
planning_scene.checkSelfCollision(collision_request, collision_result);

ROS_INFO_STREAM("collision Test: "<< (collision_result.collision ? "in" : "not in")
                              << " self collision");

actually, the robot and the collision object is in collision at this position, but the result shows not in collision.

So, what's wrong with my codes. please help me out.

------------------------------------updated 2017-03-29------------------------------------------------ my entire codes

#include <moveit/move_group_interface/move_group.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <dual_arm_robot.hpp>
#include <sensor_msgs/JointState.h>
#include <ros/ros.h>
#include <geometry_msgs/Pose.h>
#include <moveit_msgs/PlanningScene.h>
#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/GetStateValidity.h>
#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/ApplyPlanningScene.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit/robot_state/conversions.h>

#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>

int main(int argc, char **argv) {

ros::init(argc, argv, "planning_scene_ros_api_tutorial");
ros::AsyncSpinner spinner(1);
spinner.start();

ros::NodeHandle node_handle;
ros::Duration sleep_time(10.0);
sleep_time.sleep();
sleep_time.sleep();


ros::Publisher planning_scene_diff_publisher = node_handle.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);

ros::Publisher pub = node_handle.advertise<sensor_msgs::JointState>("/joint_states", 10);

while (planning_scene_diff_publisher.getNumSubscribers() < 1)
{
    ros::WallDuration sleep_t(0.5);
    sleep_t.sleep();
}


moveit_msgs::CollisionObject collsion_object;
collsion_object.header.frame_id = "world";
collsion_object.id = "box_1";

shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.15;
primitive.dimensions[1] = 0.15;
primitive.dimensions[2] = 0.4;

geometry_msgs::Pose box_pose;
box_pose.orientation.w = 1.0;
box_pose.position.x = 0.3;
box_pose.position.y = 0;
box_pose.position.z = -0.35;

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

ROS_INFO("Adding the object into the world at the location of the right wrist.");
moveit_msgs::PlanningScene planning_scene;
planning_scene.world.collision_objects.push_back(collsion_object);
planning_scene.is_diff = true;
planning_scene_diff_publisher.publish(planning_scene);
sleep_time.sleep();

ros::ServiceClient planning_scene_diff_client =
        node_handle.serviceClient<moveit_msgs::ApplyPlanningScene>("apply_planning_scene");
planning_scene_diff_client.waitForExistence();

moveit_msgs::ApplyPlanningScene srv;
srv.request.scene = planning_scene;
planning_scene_diff_client.call(srv);

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

planning_scene::PlanningScene planning_sence(kinematic_model);

robot_state::RobotState &check_collision_state = planning_sence.getCurrentStateNonConst();

collision_detection::CollisionRequest collision_request_right;   
collision_detection::CollisionRequest collision_request_left;    

collision_detection::CollisionResult collision_result_left;      
collision_detection::CollisionResult collision_result_right;      

collision_detection::AllowedCollisionMatrix ...