checkCollision in MoveIt doesn't show collision between robot and scene

edit

Hi everyone

I have problem - function checkCollision in MoveIt doesn't show collision between robot and scene.

The purpose is to move arm to random valid poses for testing planning algorithms.

My code:

int main( int argc, char** argv )
{
ros::init(argc, argv, "moveit");
ros::AsyncSpinner spinner(1);
spinner.start();

Planner planner("left_arm", "right_arm");

addBox(planner);

for (int i=0; i< 1000; ++i)
{
    ros::Time begin = ros::Time::now();
    if (planner.moveRandomPos(1))
    {
        cout << ros::Time::now()-begin << endl;
    }
}
ros::shutdown();
return 0;
}

function addBox for left arm:

bool addBox(Planner& planner)
{
moveit_msgs::CollisionObject collision_object;
collision_object.header.frame_id = planner.move_group1->getPlanningFrame();
collision_object.id = "box";

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

geometry_msgs::Pose stlPose;
stlPose.orientation.w = 1.0;
stlPose.position.x = 0.3; //left
stlPose.position.y = -0.2; //nazad
stlPose.position.z = 1.5;

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

planner.node_handle.advertise<moveit_msgs::CollisionObject>("collision_object", 0);

std::vector<moveit_msgs::CollisionObject> collision_objects;
collision_objects.push_back(collision_object);
ROS_INFO_NAMED("tutorial", "Add an object into the world");
planner.planning_scene_interface.addCollisionObjects(collision_objects);

ros::Duration(1.0).sleep();

moveit_msgs::PlanningScene planning_scene;
ros::Publisher planning_scene_diff_publisher = planner.node_handle.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
planning_scene.world.collision_objects.push_back(collision_object);
planning_scene.is_diff = true;
planning_scene_diff_publisher.publish(planning_scene);
sleep(2.0);
}

Structure Planner with constructor, moveRandomPos and isRobotStateCollision functions

struct Planner {
ros::NodeHandle node_handle;
string packPath, planGroupName1, planGroupName2, planner_plugin_name;
std::vector<std::string> joint_names1, joint_names2, link_names1, link_names2;

moveit::planning_interface::MoveGroupInterface *move_group1, *move_group2;
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
planning_scene::PlanningScenePtr planning_scene;
planning_scene_monitor::PlanningSceneMonitor *planning_scene_monitor;

robot_model_loader::RobotModelLoader *robot_model_loader;
robot_model::RobotModelPtr kinematic_model;

robot_state::RobotStatePtr kinematic_state;
robot_state::JointModelGroup *joint_model_group1, *joint_model_group2;

Planner(string planGroup1,string planGroup2)
{
    planGroupName1 = planGroup1;
    planGroupName2 = planGroup2;
    packPath = ros::package::getPath("moveit")+"/";

    move_group1 = new moveit::planning_interface::MoveGroupInterface(planGroupName1);
    move_group2 = new moveit::planning_interface::MoveGroupInterface(planGroupName2);
    robot_model_loader = new robot_model_loader::RobotModelLoader("robot_description");

    //new
    planning_scene_monitor = new planning_scene_monitor::PlanningSceneMonitor("robot_description");
    planning_scene = planning_scene_monitor->getPlanningScene();

    kinematic_model = robot_model_loader->getModel();
    kinematic_state = robot_state::RobotStatePtr(new robot_state::RobotState(kinematic_model));
    joint_model_group1 = kinematic_model->getJointModelGroup(planGroupName1);
    joint_model_group2 = kinematic_model->getJointModelGroup(planGroupName2);

    joint_names1 = joint_model_group1->getJointModelNames();
    joint_names2 = joint_model_group2->getJointModelNames();
    link_names1 = joint_model_group1->getLinkModelNames();
    link_names2 = joint_model_group2->getLinkModelNames();
}

 bool moveRandomPos(u_int armNumber)
 {
    moveit::planning_interface::MoveGroupInterface *move_group;
    robot_state::JointModelGroup *joint_model_group;

    switch (armNumber) {
    case 1:
        move_group = move_group1;
        joint_model_group = joint_model_group1;
        break;
    case 2:
        move_group = move_group2;
        joint_model_group = joint_model_group2;
        break;
    default:
        cout << "Bad armNumber" << endl;
        return false;
    }

    move_group->setStartStateToCurrentState();
    robot_state::RobotState goal_state = planning_scene->getCurrentState();

    for (u_int i=0; i<100; ++i)
    {
        goal_state.setToRandomPositions(joint_model_group);

        if (!isRobotStateCollision(goal_state))
        {
            break;
        }

        if (i==99)
        {
            cout << "Can't gen random pos" << endl;
            return false;
        }
    }

    Eigen::Affine3d end_effector_state = goal_state.getGlobalLinkTransform(joint_model_group->getLinkModelNames().back());

    geometry_msgs::Pose idle_pose;
    tf::poseEigenToMsg(end_effector_state, idle_pose);
    move_group->setPoseTarget(idle_pose);

    moveit::planning_interface::MoveGroupInterface::Plan my_plan;
    move_group->setPlanningTime(10.0);

    if (move_group->plan(my_plan))
        return true;
    else
        return false;
 }

 bool isRobotStateCollision(robot_state::RobotState& robSt)
 {
    collision_detection::CollisionRequest req;
    collision_detection::CollisionResult res;
    collision_detection::AllowedCollisionMatrix acm = planning_scene->getAllowedCollisionMatrix();

    acm.print(cout);
    planning_scene->checkCollision(req, res, robSt, acm);

    return res.collision;
 }
}

Planning is nice, robot avoids "box" without collisions. But I have troubles with moveRandomPos and checkCollision function. I want to ... (more)