What would be the right way of dealing with different Z geometry in costmap?

[flplv]

Hello there!

Assume I have a robot that is T shaped, which is, in it's top end it is considerably wider than on it's lower end, and that I want to navigate in a map with narrow low Z corridors, where only the bottom part of the robot fits, but the top part would not collide since the obstacles are low in Z.

How would you recommend me to configure the costmap layers to deal with different Z footprints? Should I create multiple overlays with different Z ranges and footprints?

Best!

[at-wat]

@flplv I think using two footprint layers with different footprint polygon and publish two OccupancyGrid for each layer realizes such use case.
Parameters would be something like:

# costmap_cspace/costmap_3d node
costmap_3d:
  ...
  layers:
    - name: bottom_footprint  # this layer subscribes /bottom_footprint as 2D obstacle map of this layer
      type: Costmap3dLayerFootprint
      linear_expand: 0.15
      linear_spread: 0.3
      footprint: [[0.1, -0.2], [0.1, 0.2], [-0.1, 0.2], [-0.1, -0.2]]
      ...
    - name: top_footprint  # this layer subscribes /top_footprint as 2D obstacle map of this layer
      type: Costmap3dLayerFootprint
      linear_expand: 0.15
      linear_spread: 0.3
      ...
      footprint: [[0.2, -0.2], [0.2, 0.2], [0.1, 0.2], [0.1, -0.2]]

# costmap_cspace/pointcloud2_to_map node
#  for bottom layer (remap OccupancyGrid output to /bottom_footprint)
pointcloud2_to_map_bottom:
  z_min: 0.1
  z_max: 0.4
  ...

# costmap_cspace/pointcloud2_to_map node
#  for top layer (remap OccupancyGrid output to /top_footprint)
pointcloud2_to_map_top:
  z_min: 0.3
  z_max: 0.6
  ...

(not tested)

[flplv]

Hummmmmm interesting. Thank you my friend. How would you set the layer merging in the proposed setup? Max or something different?

ps: happy 2021

[at-wat]

How would you set the layer merging in the proposed setup? Max or something different?

Cost values of the layers are merged by max by default. (possible modes: max, overwrite)

[sualeh-rgt]

Hello!
Having a circular robot I intend to reduce the angular resolutuion to optimize the resources. Costmap is generated fine but planner_3d crashes. lets say i set the parameter costmap_3d: ang_resolution: to 1. Also at lower resolutions the local path becomes more sharp. How to allow planner_3d to follow the gradient in the distance map, without strict angular constraints?

[at-wat]

@sualeh-rgt planning without angular constraints isn't considered in this package. I think changes are required to support such robots. PR is always welcomed!