Release 1.12.11 breaks local costmap - It doesn't always / fully update

[spmaniato]

On my Trusty/Indigo machine, (Debian package ros-indigo-navigation 1.12.8), the local costmap works as expected.

On my macOS/Indigo laptop (navigation 1.12.11-0.tar.gz built from source), I'm seeing weird local costmap behavior. In a nutshell, it's not updating properly in the presence of obstacles / laser scans within it's width and height. Please see screenshots below. (The global costmap looked fined, but I didn't pay close attention to be honest.)

There's a very slight chance this a macOS ROS installation artifact, although I doubt it See below

Relevant configuration (which is the same on both machines):

global_frame: odom # Problem also appears with map as the global frame

update_frequency:  5.0
publish_frequency: 2.0

static_map: false
rolling_window: true

width: 1.5
height: 1.5
resolution: 0.05

plugins:
  - {name: obstacle_layer,  type: "costmap_2d::ObstacleLayer"}
  - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

Please let me know if I should provide more information, any debug logs, etc. In the meantime, I'll try building 1.12.8 from source in macOS and 1.12.11 from source in Ubuntu to see what happens:

• Ubuntu
  • 1.12.11 from source: The exact same problem manifests ❌
  • 1.12.10 from source: TODO
  • 1.12.9 from source: TODO
  • 1.12.8 Debian package: Works as expected ✅
• macOS
  • 1.12.11 from source: This is where I happened to observe the problem ❌
  • 1.12.10 from source: Works as expected ✅
  • 1.12.9 from source: Works as expected ✅
  • 1.12.8 from source: Works as expected ✅

    Screenshots

[mikeferguson]

I'm not sure what is going on here -- we have adjusted some things in the inflation layer to fix several bugs that resulted in under-inflation. But we haven't seen anything like this -- and we have a huge number of robots running this code right now.

Could you create a video to show the temporal aspects of this?

[spmaniato]

Yes, I can. Stay tuned. (Btw, the screenshots above are in simulation: Stage / maze world)

Is there anything significantly different between your costmap configuration and mine? I'll play around with that a bit and see how and if it affects things.

I'll also narrow it down to the specific release that introduced this weird behavior.

[alexhenning]

Can you post the output of rosparam get /move_base. Additionally, are there any warnings printing out?

[spmaniato]

@mikeferguson Here's the video: https://youtu.be/mZmoK-ghe-I

@alexhenning No warnings at all. If I set move_base's ros.costmap_2d logger to debug, I see a bunch of things, but they don't look too suspicious (to me). They also appear during the phases when the local costmap seems to be doing OK.

[DEBUG] [1466719270.687160000, 538.800000000]: The point is too far away
[DEBUG] [1466719270.687197000, 538.800000000]: Updating area x: [172, 200] y: [82, 200]
[DEBUG] [1466719270.688120000, 538.800000000]: Map update time: 0.002691984
[DEBUG] [1466719270.689388000, 538.800000000]: The point is too far away
[...]
[DEBUG] [1466719058.689003000, 326.800000000]: Computing map coords failed
[DEBUG] [1466719058.689061000, 326.800000000]: The point is too far away
[DEBUG] [1466719058.689114000, 326.800000000]: Computing map coords failed
[DEBUG] [1466719058.689174000, 326.800000000]: The point is too far away
[DEBUG] [1466719270.889502000, 539.000000000]: Updating area x: [16, 39] y: [8, 40]
[DEBUG] [1466719270.889737000, 539.000000000]: Map update time: 0.004482031

And here are the ROS params:

$ rosparam get /move_base
GlobalPlanner: {allow_unknown: true, cost_factor: 3.0, default_tolerance: 0.1, lethal_cost: 253,
  neutral_cost: 50, orientation_mode: 1, planner_window_x: 0.0, planner_window_y: 0.0,
  publish_potential: true, use_dijkstra: true, use_grid_path: false, use_quadratic: true,
  visualize_potential: false}
TrajectoryPlannerROS: {acc_lim_theta: 2.0, acc_lim_x: 1.2, acc_lim_y: 1.2, angular_sim_granularity: 0.017,
  dwa: false, escape_reset_dist: 0.1, escape_reset_theta: 1.57079632679, escape_vel: -0.15,
  gdist_scale: 0.8, heading_lookahead: 0.325, heading_scoring: false, heading_scoring_timestep: 0.1,
  holonomic_robot: false, latch_xy_goal_tolerance: true, max_vel_theta: 0.8, max_vel_x: 0.25,
  meter_scoring: true, min_in_place_vel_theta: 0.3, min_vel_theta: -0.8, min_vel_x: 0.0,
  occdist_scale: 0.01, oscillation_reset_dist: 0.05, pdist_scale: 0.6, penalize_negative_x: false,
  publish_cost_grid_pc: true, restore_defaults: false, sim_granularity: 0.025, sim_time: 1.0,
  simple_attractor: false, vtheta_samples: 20, vx_samples: 5, xy_goal_tolerance: 0.1,
  y_vels: '-0.3,-0.1,0.1,-0.3', yaw_goal_tolerance: 0.1}
base_global_planner: global_planner/GlobalPlanner
base_local_planner: base_local_planner/TrajectoryPlannerROS
clearing_rotation_allowed: true
conservative_reset_dist: 3.0
controller_frequency: 5.0
controller_patience: 15.0
global_costmap:
  footprint: '[[0.229,0.1315],[-0.1685,0.1315],[-0.1685,-0.1315],[0.229,-0.1315]]'
  footprint_padding: 0.01
  global_frame: map
  height: 10
  inflation_layer: {circumscribed_radius: 0.264, cost_scaling_factor: 5.0, enabled: true,
    inflation_radius: 0.4, inscribed_radius: 0.1315}
  obstacle_layer:
    combination_method: 1
    enabled: true
    footprint_clearing_enabled: true
    lidar: {clearing: true, data_type: LaserScan, marking: true, max_obstacle_height: 0.5,
      min_obstacle_height: 0.0, obstacle_range: 2.5, raytrace_range: 3.0, sensor_frame: base_laser_link,
      topic: scan}
    max_obstacle_height: 2.0
    observation_sources: lidar
    track_unknown_space: true
  origin_x: 0.0
  origin_y: 0.0
  plugins:
  - {name: static_layer, type: 'costmap_2d::StaticLayer'}
  - {name: obstacle_layer, type: 'costmap_2d::ObstacleLayer'}
  - {name: inflation_layer, type: 'costmap_2d::InflationLayer'}
  pose_update_frequency: 10.0
  publish_frequency: 2.0
  resolution: 0.05
  robot_base_frame: base_link
  robot_radius: 0.46
  static_layer: {enabled: true}
  static_map: true
  transform_tolerance: 1.0
  update_frequency: 2.0
  width: 10
local_costmap:
  footprint: '[[0.229,0.1315],[-0.1685,0.1315],[-0.1685,-0.1315],[0.229,-0.1315]]'
  footprint_padding: 0.01
  global_frame: odom
  height: 2
  inflation_layer: {circumscribed_radius: 0.264, cost_scaling_factor: 5.0, enabled: true,
    inflation_radius: 0.4, inscribed_radius: 0.1315}
  obstacle_layer:
    combination_method: 1
    enabled: true
    footprint_clearing_enabled: true
    lidar: {clearing: true, data_type: LaserScan, marking: true, max_obstacle_height: 0.5,
      min_obstacle_height: 0.0, obstacle_range: 2.5, raytrace_range: 3.0, sensor_frame: base_laser_link,
      topic: scan}
    max_obstacle_height: 2.0
    observation_sources: lidar
    track_unknown_space: true
  origin_x: 0.0
  origin_y: 0.0
  plugins:
  - {name: obstacle_layer, type: 'costmap_2d::ObstacleLayer'}
  - {name: inflation_layer, type: 'costmap_2d::InflationLayer'}
  pose_update_frequency: 10.0
  publish_frequency: 2.0
  resolution: 0.05
  robot_base_frame: base_link
  robot_radius: 0.46
  rolling_window: true
  static_layer: {enabled: true}
  static_map: false
  transform_tolerance: 1.0
  update_frequency: 5.0
  width: 2
oscillation_distance: 0.05
oscillation_reset_dist: 0.05
oscillation_timeout: 5.0
planner_frequency: 0.0
planner_patience: 5.0
recovery_behavior_enabled: true
recovery_behaviors:
- {name: conservative_reset, reset_distance: 3.0, type: clear_costmap_recovery/ClearCostmapRecovery}
- {frequency: 20.0, name: rotate_recovery, sim_granularity: 0.017, type: rotate_recovery/RotateRecovery}
- {name: aggressive_reset, reset_distance: 0.5, type: clear_costmap_recovery/ClearCostmapRecovery}
restore_defaults: false
shutdown_costmaps: false

PS. All of this is with 1.12.11 built from source

[spmaniato]

I tested all releases between 1.12.8 and 1.12.11 (also see updated bullet points in original post) The problem was introduced in 1.12.11 💡

[alexhenning]

All changes with the exception of https://github.com/ros-planning/navigation/commit/0a686c90c6f188a2731f6f88c2c08610f0ec907e were to the inflation layer. However, even if that was buggy, it shouldn't stop the lethal obstacles (yellow) from showing up. One difference I notice is that you're using costmap_2d::ObstacleLayer, whereas we use a layer based on costmap_2d::VoxelLayer. You could try switching to it and see what happens. You could also try setting track_unknown_space: true or combination_method: 0 to see if it has something to do with unknown space.

[alexhenning]

Another thought is that you set overwrite the max_obstacle_height to 0.5. If your URDF rounded the orientation for the laser it could chop it off. There should be no downside to increasing the value.

    lidar: {clearing: true, data_type: LaserScan, marking: true, max_obstacle_height: 0.5,
      min_obstacle_height: 0.0, obstacle_range: 2.5, raytrace_range: 3.0, sensor_frame: base_laser_link,
      topic: scan}
    max_obstacle_height: 2.0

[spmaniato]

Thanks for looking into it @alexhenning No luck unfortunately:

1. Switching to the VoxelLayer had no effect (however, I did keep the previous parameters)
2. Trying all 4 combinations of track_unknown_space and combination_method had no effect.
3. Changing the max_obstacle_height from 0.5 to 2.0 had no effect. (The LiDAR is at a height of 0.141 [m] from the ground)

[alexhenning]

@spmaniato since most the changes were inflation related, can you disable the inflation layer and make sure that you are getting yellow pixels where the lasers are?

[spmaniato]

since most the changes were inflation related, can you disable the inflation layer and make sure that you are getting yellow pixels where the lasers are?

Good call @alexhenning The obstacle layer (yellow) does indeed show up properly after disabling the inflation layer!

[mikeferguson]

Pretty sure I've found the cause of this -- and I also can can see why it didn't come up in our real world testing, but came up in a simulated environment with a single outer wall. @alexhenning -- please take a look at the commit I referenced above -- we need to get a patch out for this tomorrow.

We should also look at adding some sort of test to make sure that each layer only increases the bounds (in this case, the inflation layer actually decreases the bounds).

[alexhenning]

@mikeferguson Yeah, that's bad... I've got it fixed locally. @spmaniato I'll have a PR after some local testing.

[alexhenning]

@spmaniato Can you try https://github.com/ros-planning/navigation/pull/486 and verify things works for you?

[spmaniato]

Can you try #486 and verify things works for you?

@alexhenning Yep, e88e04b6e0f615598b5724bc1a8d6b23a4ac5387 fixes the issue I've been seeing 👍

[alexhenning]

@spmaniato Awesome, thanks for helping narrow this down!