The octomap is updated live from your Xtion sensor data. The collision
objects are added by you. They are used to add collision objects that
you know the position of; the most common way is to have some sort of
object recognition node that recognizes, for example, tabletop objects,
and adds these objects as collision objects to the PlanningSceneWorld.
(If you look at moveit_msgs/PlanningSceneWorld, you can see that it has
one octomap and a list of collision objects).
You can add objects by either publishing to the "/collision_object"
topic, or by using a convenience function from the move_group C++
interface.
Once a collision object is added to the PlanningScene, the voxels that
overlap with it are automatically filtered out of the octomap. You do
not need to call excludeWorldObjectFromOctree() yourself (that's
probably why it's protected).
By default, collision checking is done between all robot links and the
octomap, and between all robot links and each collision object.
However, you can disable collision checks between certain robot links
and certain collision objects, and that's exactly what you should do:
disable collision checking between the fingers and the object you're
trying to pick up. There are basically two ways to do it:
Disable the collision checks before calling the move_group action.
All calls to the move_group action after that will ignore those
collisions. You'll have to remember to re-enable the collisions
afterwards.
Specify the allowed collisions for that specific move_group call
only, as a parameter of that action call. This is somewhat nicer in
my opinion.
For (1), there are a couple of options:
1a. Using the PlanningSceneMonitor. From C++, I think this is better
than the solution (1b) below. It should go something like this
(warning, completely untested!):
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
planning_scene_monitor::PlanningSceneMonitorPtr psm = planning_scene_monitor::PlanningSceneMonitorPtr(
new planning_scene_monitor::PlanningSceneMonitor("robot_description", tf, "name"));
planning_scene_monitor::LockedPlanningSceneRW planning_scene = planning_scene_monitor::LockedPlanningSceneRW(psm);
collision_detection::AllowedCollisionMatrix acm = planning_scene->getAllowedCollisionMatrix();
// TODO: modify acm here as explained in the PlanningScene tutorial: http://docs.ros.org/api/pr2_moveit_tutorials/html/planning/src/doc/planning_scene_tutorial.html
As an inspiration, I found digging through this code helpful: https://github.com/ros-planning/moveit_ros/tree/hydro-devel/visualization/planning_scene_rviz_plugin
1b. Publishing to the "/planning_scene" topic. From Python, it looks
like this:
self._pubPlanningScene = rospy.Publisher('planning_scene', PlanningScene)
rospy.wait_for_service('/get_planning_scene', 10.0)
get_planning_scene = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene)
request = PlanningSceneComponents(components=PlanningSceneComponents.ALLOWED_COLLISION_MATRIX)
response = get_planning_scene(request)
acm = response.scene.allowed_collision_matrix
if not 'button' in acm.default_entry_names:
# add button to allowed collision matrix
acm.default_entry_names += ['button']
acm.default_entry_values += [True]
planning_scene_diff = PlanningScene(
is_diff=True,
allowed_collision_matrix=acm)
self._pubPlanningScene.publish(planning_scene_diff)
rospy.sleep(1.0)
For (2), you can use the same planning_scene_diff that I computed above
by putting it into your MoveGroupGoal.planning_options.planning_scene_diff.
It's important that you really get the current allowed collision matrix first
and then modify it, as shown above.
Note that I added the collision object to the default_entry_names,
which means that collisions between that object and all robot links are
allowed, not just the fingers ... (more)
