Is it possible to use multiple inflation layers in a local costmap, that is, one inflation per sensor?

asked 2020-06-03 06:26:17 -0500

Rinzler316 gravatar image

I have been searching all over but I can't seem to find an answer for this question. I have a costmap setup with two sensor, a LIDAR and a mmWave RADAR and I would like to use different inflation coefficients to each sensor. I am using ROS Kinetic with debian 9, Kernel 4.18.0 and architecture x86.

Below is the current layer setup:

- {name: obstacle_layer,      type: "costmap_2d::ObstacleLayer"} 
- {name: obstacle_layer_3d,   type: "costmap_2d::ObstacleLayer"}
- {name: inflation_layer,     type: "costmap_2d::InflationLayer"}
- {name: constraint_layer,    type: "my_constraint_layer::MyConstraintLayer"}

I tried the following and the cost seems to be applied as expected although only the top most inflation layer "inflation_layer_3d" parameters are taken into account.

- {name: obstacle_layer,      type: "costmap_2d::ObstacleLayer"} 
- {name: inflation_layer,     type: "costmap_2d::InflationLayer"}
- {name: obstacle_layer_3d,   type: "costmap_2d::ObstacleLayer"}
- {name: inflation_layer_3d,  type: "costmap_2d::InflationLayer"}
- {name: constraint_layer,    type: "my_constraint_layer::MyConstraintLayer"}

When the layers are ordered as seen below, move_base doesn't even start.

- {name: obstacle_layer_3d,   type: "costmap_2d::ObstacleLayer"}
- {name: inflation_layer_3d,  type: "costmap_2d::InflationLayer"}
- {name: obstacle_layer,      type: "costmap_2d::ObstacleLayer"} 
- {name: inflation_layer,     type: "costmap_2d::InflationLayer"}
- {name: constraint_layer,    type: "my_constraint_layer::MyConstraintLayer"}

Is it possible to apply different inflation costs to different observation sources and my implementation is wrong or this is not a feature of a layered costmap? The full local costmap parameters can be seen below with only one inflation layer:

# Coordinate frame and tf parameters
global_frame: odom

# Map management parameters
update_frequency: 15.0
rolling_window: true
width: 15.0
height: 15.0
resolution: 0.05
origin_x: 0.0
origin_y: 0.0

robot_radius: 0.60
footprint_padding: 0.0

obstacle_layer:
  # Obstacle costmap plugin
  track_unknown_space: true           # default false. Allows to label a pixel as unknown.
  footprint_clearing_enabled: true    # default true

  # Sensor management parameters
  observation_sources: base_scan
  base_scan:
    topic: /base_scan
    sensor_frame: /base_laser_link
    observation_persistence: 0.0    # default 0.0
    expected_update_rate: 0.0       # default 0.0
    data_type: LaserScan            # default PointCloud
    clearing: true                  # default false
    marking: true                   # default true
    max_obstacle_height: 0.35       # default 2.0
    min_obstacle_height: 0.0        # default 0.0
    obstacle_range: 45              # default 2.5
    raytrace_range: 45.5            # default 3.0
    inf_is_valid: true              # default false

obstacle_layer_3d:
  # Obstacle costmap plugin
  track_unknown_space: true           # default false. Allows to label a pixel as unknown.
  footprint_clearing_enabled: false   # default true

  # Sensor management parameters
  observation_sources: radar_scan
  radar_scan:
    topic: /radar_scan/laser # Converted to LaserScan from PointCloud2
    sensor_frame: /base_link
    observation_persistence: 4.0    # default 0.0
    expected_update_rate: 0.0       # default 0.0
    data_type: LaserScan            # default PointCloud
    clearing: true                  # default false
    marking: true                   # default true
    max_obstacle_height: 2.0        # default 2.0
    min_obstacle_height: 0.0        # default 0.0
    obstacle_range: 4               # default 2.5
    raytrace_range: 4.5             # default 3.0
    inf_is_valid: true              # default false

inflation_layer:
  cost_scaling_factor: 1.5    # a factor value of <10 will reduce the cost in a slower pace.
  inflation_radius: 2.0       # default 0.5. The radius in meters to which the map inflates obstacle cost values.

constraint_layer:
  enabled: false              # (must also enable layer in DWA planner config)
  lethal_cost: 253
  inflation_cost: 252
  inflation_radius: 0.2

Thanks in advance.

edit retag flag offensive close merge delete

Comments

Just to be clear, since I don't know what your constraint layer does, what happens when you have this: - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} - {name: obstacle_layer_3d, type: "costmap_2d::ObstacleLayer"} - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

David Lu gravatar image David Lu  ( 2020-06-04 17:02:10 -0500 )edit

Sorry for the late reply, thanks for the comment. My constraint layer doesn't actually do anything at the moment, it can be disabled with no change in the outcome.

Rinzler316 gravatar image Rinzler316  ( 2020-06-23 05:28:54 -0500 )edit

Were you able to get this working in the end?

jorgemia gravatar image jorgemia  ( 2022-11-11 08:44:12 -0500 )edit

Not the question exactly, sorry. In the end I created a new layer with the properties I wanted and the inflation was no longer an issue.

Rinzler316 gravatar image Rinzler316  ( 2022-11-29 06:11:13 -0500 )edit

Hello @Rinzler316, how did you archive the goal, can you give me some hints?

harvin gravatar image harvin  ( 2023-05-22 21:09:50 -0500 )edit

Hey, I followed the ROS tutorials: http://wiki.ros.org/costmap_2d/Tutori.... You can also look at the implementation of the standard costmap_2d layers for inspiration: https://github.com/ros-planning/navig...

Basically, your layer needs to have an updateCosts and an updateBoundaries method which are called by the costmap_2d package. What you do with those methods is then entirely up to you.

Rinzler316 gravatar image Rinzler316  ( 2023-05-25 13:01:43 -0500 )edit

Also, for future viewers, in the end, my conclusion is that it wasn't possible with the base plugins. It is possible, however, to see the state of the costmap and create layers that inflate the costmap based on specific values.

You can for example create an obstacle layer that marks objects with a value 100 and another that marks with a value 200. Then, two separate inflation layers could handle these separate values differently.

Rinzler316 gravatar image Rinzler316  ( 2023-05-25 13:11:12 -0500 )edit