Problem with using planning_scene::PlanningScene::checkCollision();

[唐威]

Hi, all:

I am trying out using the planning_scene::PlanningScene::checkCollision() for collision checking as I learned from this moveit tutorial http://docs.ros.org/indigo/api/pr2_moveit_tutorials/html/planning/src/doc/planning_scene_tutorial.html. 

I will copy some of the key lines here for illustration:

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

planning_scene.checkCollision(collision_request, collision_result, copied_state, acm);

ROS_INFO_STREAM("Test 6: Current state is "
                << (collision_result.collision ? "in" : "not in")
                << " self collision");

As far as I can see, the planning_scene here hasn't introduced any collision objects to the world. Does anyone has any experience about how to introduce collision objects in this scenario? (Either do it within this process or receive a message from other process).

Also, as I read through the list, I found that most of the questions concerning collision_check is about avoiding collision during planning. My question, however, is how to check collision when things are still in their current state.

Any comment is welcome. Thank you very much!

[12bee...@seecs.edu.pk]

The code in the tutorial effectively only checks for self-collisions. To check for environment collisions, you'll have to use the PlanningSceneMonitor class. Basically you initialize it, and get a pointer to the PlanningScene from it like this:

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

Then you start the scene monitor like this:

    planning_scene_monitor->startWorldGeometryMonitor();
    planning_scene_monitor->startSceneMonitor();
    planning_scene_monitor->startStateMonitor();

Now, the monitor is subscribed to the "/planning_scene" topic. All you have to do is publish environment objects/obstacles (I think the sample code for that is given in the tutorial) on this topic and the monitor will get hold of them automatically. Provided that there's no issue with your namespaces, when you call checkCollision() from the pointer obtained from the PlanningSceneMonitor, collisions with obstacles should be reported. Hope that helps.