Octomap updates for task planning

[pawelir]

Description

When attempting a pick action, the collision object representing the target item is successfully added to the planning scene. However, the octomap voxels that collide with this object are not cleared instantaneously, causing the planning request to fail due to persistent self-collision. I tried few config variations, my observation is that the higher octomap's resolution is the longer it takes the fully clear colliding voxels.

Questions

1. Does anybody encountered similar behavior?
2. I thought about calling clear_octomap service just after adding collision object. Here, the question would be if during task planning, the octomap will be already updated with new points and how to ensure this without adding sleep in the code.
3. What would be a suggested way to deal with it, play with task logic within MTC, or debug moveit's octomap?

I'd appreciate your feedback 🙏🏼

Setup

Env

• ROS 2 Humble
• Ubuntu 22.04

Config

octomap_resolution: 0.015

sensors:
  - robotic_platform_camera
robotic_platform_camera:
  sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
  point_cloud_topic: depth/points
  max_range: 2.0
  point_subsample: 5
  padding_offset: 0.04
  padding_scale: 1.0
  max_update_rate: 30.0
  filtered_cloud_topic: moveit_perception_pipeline/points_filtered

• I don't see any major delays between point_cloud_topic and filtered_cloud_topic. Both are around 200 ms delay.

Task sequence

1. Add collision object

planning_scene_interface_->applyCollisionObject(collision_object);

2. Create pick task

    auto builder = TaskBuilder(this->shared_from_this(), "pick", config);

    builder.AddCurrentState("current state")
        .AddConnectMotion("move to pick", 5.0)
        .BeginSerialContainer(kPickObject.data())
        .AddRelativeMotion("approach", approach_offset)
        .AddGenerateGrasps(kGraspCandidates.data(), object_id, grasp_candidates,
                           params_.inverse_kinematics.max_solutions)
        .AddToggleCollision("allow collision", object_id, true)
        .AddGripperMotion("close gripper", params_.predefined_poses.hand.closed)
        .AddAttachObject("attach object", object_id)
        .AddRelativeMotion("retreat", retreat_offset)
        .EndSerialContainer();

    auto task = builder.Build();

3. Plan and execute task

Attachements

Image shows voxel debris inside collision object

Automatically add collision object during task planning

1_automatically_add_collision_object_during_task_planning.webm

• 2 attempts to plan pick task
• First attempt fails due to collisions between object and octomap
• Second attempt succeeded since all voxels within object volume were removed after couple of seconds

Manually add collision object

2_manually_add_collision_object.webm

• It's clearly visible that not all colliding voxels are removed right away
• One voxel remains for long time until I ultimately call clear_octomap service

[rhaschke]

Looks like this is a bug with MoveIt's octomap. Moving it there.