MoveIT! Occupancy Map truncating inside robot space.

edit

Hello there,

I'm currently trying to configure the occupancy map on my MoveIT package. This is what I have accomplished so far:

1- Installed MoveIT and properly do planning etc. 2- Create a custom robot with links, joints etc.

3- Installed Intel Realsense ROS package with pointCloud2 data streaming

4- Configure Occupancy map following MoveIT tutorial. I see the octomap geometry in the scene.

The following is how the robot looks like(I cannot upload files here, it warns me about needing 5 points or more):

https://www.dropbox.com/s/g272u1g7qo0...

Notice that this robot has a Cage, which is part of the base link.

Then, I set a transform which is the base for the Octomap streaming data:

https://www.dropbox.com/s/giqksdsq6io...

This data looks fine, and with other robots it even avoids collision.

The problem is when I try to position the transform inside the Robot cage... It basically truncates the octomap and does not allow the scanned geometry to be positioned inside the robot cage, which is the main goal since I want it to avoid anything obstructing the arm movements.

https://www.dropbox.com/s/qv96h65ouor...

I have tried many things, specifically, I played with the Sensors configuration and the self_mask property, but nothing seems to fix the outcome.

The following is my sensor file:

sensors:
  - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
    point_cloud_topic: /camera/depth/color/points
    max_range: 0.4
    point_subsample: 1
    padding_offset: 0.01
    padding_scale: 1.0
    max_update_rate: 0
    filtered_cloud_topic: filtered_cloud
    self_mask:
      min_sensor_dist: .5
      self_see_default_padding: 0.03
      self_see_default_scale: 1.0
      self_see_links:
        - name: base_link
          padding: 0.06

I position the octomap using a static transform in the demo.launch file:

<node pkg="tf2_ros" type="static_transform_publisher" name="to_temp_link" args="0.18 0.33 -0.2 1.6 -1.6 0 camera_depth_optical_frame base_link" />

I would really appreciate any help or direction on what the problem really is or any documentation to configure such use case.

System information:

Operating System: Ubuntu 18.04.3 LTS

Kernel version: 4.15.0-50-generic

ROS Version: Melodic

MoveIT package version: Melodic Stable