Robotics StackExchange | Archived questions

Local costmap not updating in Nav2

Hi!

I am trying to map and localize my robot in its environment using RTABMAP with an RGBD camera + Nav2. I have no problem with the global costmap, as it works perfectly, however, my local costmap seems to not update properly, since sometimes the obstacles appear in the local costmap and sometimes not (comparing to the global costmap).

Here you can see an image of my problem:

image description

Here is my yaml configuration for Nav2:

bt_navigator:
  ros__parameters:
    use_sim_time: True
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odom/unfiltered
    enable_groot_monitoring: True
    groot_zmq_publisher_port: 1666
    groot_zmq_server_port: 1667
    default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
    plugin_lib_names:
    - nav2_compute_path_to_pose_action_bt_node
    - nav2_follow_path_action_bt_node
    - nav2_back_up_action_bt_node
    - nav2_spin_action_bt_node
    - nav2_wait_action_bt_node
    - nav2_clear_costmap_service_bt_node
    - nav2_is_stuck_condition_bt_node
    - nav2_goal_reached_condition_bt_node
    - nav2_goal_updated_condition_bt_node
    - nav2_initial_pose_received_condition_bt_node
    - nav2_reinitialize_global_localization_service_bt_node
    - nav2_rate_controller_bt_node
    - nav2_distance_controller_bt_node
    - nav2_speed_controller_bt_node
    - nav2_truncate_path_action_bt_node
    - nav2_goal_updater_node_bt_node
    - nav2_recovery_node_bt_node
    - nav2_pipeline_sequence_bt_node
    - nav2_round_robin_node_bt_node
    - nav2_transform_available_condition_bt_node
    - nav2_time_expired_condition_bt_node
    - nav2_distance_traveled_condition_bt_node
    - nav2_navigate_to_pose_action_bt_node

bt_navigator_rclcpp_node:
  ros__parameters:
    use_sim_time: True

controller_server:
  ros__parameters:
    use_sim_time: True
    odom_topic: /odom/unfiltered
    controller_frequency: 5.0
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.001
    min_theta_velocity_threshold: 0.001
    progress_checker_plugin: "progress_checker"
    goal_checker_plugin: "goal_checker"
    controller_plugins: ["FollowPath"]

    # Progress checker parameters
    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.2
      movement_time_allowance: 20.0
    # Goal checker parameters
    goal_checker:
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.25
      yaw_goal_tolerance: 0.25
      stateful: True
    # DWB parameters
    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"
      debug_trajectory_details: True
      publish_cost_grid_pc: True
      min_vel_x: 0.0
      min_vel_y: 0.0
      max_vel_x: 4.0
      max_vel_y: 4.0
      max_vel_theta: 4.5
      min_speed_xy: 0.4
      max_speed_xy: 4.0
      min_speed_theta: 0.0
      # Add high threshold velocity for turtlebot 3 issue.
      # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
      acc_lim_x: 2.5
      acc_lim_y: 0.0
      acc_lim_theta: 3.2
      decel_lim_x: -2.5
      decel_lim_y: 0.0
      decel_lim_theta: -3.2
      vx_samples: 20
      vy_samples: 10
      vtheta_samples: 20
      sim_time: 1.2
      linear_granularity: 0.05
      time_granularity: 1.0
      angular_granularity: 0.025
      transform_tolerance: 0.2
      xy_goal_tolerance: 0.25
      trans_stopped_velocity: 0.25
      short_circuit_trajectory_evaluation: True
      stateful: True
      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist", "ObstacleFootprint"]
      BaseObstacle.scale: 0.02
      ObstacleFootprint.scale: 32.0
      PathAlign.scale: 32.0
      PathAlign.forward_point_distance: 0.1
      GoalAlign.scale: 24.0
      GoalAlign.forward_point_distance: 0.1
      PathDist.scale: 32.0
      GoalDist.scale: 24.0
      RotateToGoal.scale: 32.0
      RotateToGoal.slowing_factor: 5.0
      RotateToGoal.lookahead_time: -1.0

controller_server_rclcpp_node:
  ros__parameters:
    use_sim_time: True

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: world
      robot_base_frame: base_link
      use_sim_time: True
      rolling_window: true
      width: 3
      height: 3
      origin_x: -1.5
      origin_y: -1.5
      resolution: 0.05
      always_send_full_costmap: True
      robot_radius: 0.4
      plugins: ["obstacle_layer", "inflation_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.1
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: pointcloud
        pointcloud:
          topic: /camera/points
          max_obstacle_height: 10.0
          min_obstacle_height: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
          raytrace_max_range: 10.0
          raytrace_min_range: 0.0
          clearing: True
          marking: True
          data_type: "PointCloud2"
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: True
        publish_voxel_map: True
        origin_z: 0.0
        z_resolution: 0.05
        z_voxels: 16
        max_obstacle_height: 2.0
        mark_threshold: 0
        observation_sources: pointcloud
        pointcloud:
          topic: /camera/points
          max_obstacle_height: 10.0
          min_obstacle_height: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
          raytrace_max_range: 10.0
          raytrace_min_range: 0.0
          clearing: True
          marking: True
          data_type: "PointCloud2"
  local_costmap_client:
    ros__parameters:
      use_sim_time: True
  local_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: True

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      use_sim_time: True
      robot_radius: 0.4
      resolution: 0.05
      width: 3
      height: 3
      track_unknown_space: true
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: pointcloud
        pointcloud:
          topic: /camera/points
          max_obstacle_height: 2.0
          min_obstacle_height: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
          raytrace_max_range: 10.0
          raytrace_min_range: 0.0
          clearing: True
          marking: True
          data_type: "PointCloud2"
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.1
      always_send_full_costmap: True
  global_costmap_client:
    ros__parameters:
      use_sim_time: True
  global_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: True

planner_server:
  ros__parameters:
    expected_planner_frequency: 1.0
    use_sim_time: True
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"
      tolerance: 0.5
      use_astar: True
      allow_unknown: True

planner_server_rclcpp_node:
  ros__parameters:
    use_sim_time: True

recoveries_server:
  ros__parameters:
    costmap_topic: local_costmap/costmap_raw
    footprint_topic: local_costmap/published_footprint
    cycle_frequency: 10.0
    recovery_plugins: ["spin", "back_up", "wait"]
    spin:
      plugin: "nav2_recoveries/Spin"
    back_up:
      plugin: "nav2_recoveries/BackUp"
    wait:
      plugin: "nav2_recoveries/Wait"
    global_frame: world
    robot_base_frame: base_link
    transform_timeout: 0.1
    use_sim_time: true
    simulate_ahead_time: 2.0
    max_rotational_vel: 1.0
    min_rotational_vel: 0.4
    rotational_acc_lim: 3.2

robot_state_publisher:
  ros__parameters:
    use_sim_time: True

And here my parameters and remappings for RTABMAP:

parameters={
          'frame_id':'base_link',
          'use_sim_time':use_sim_time,
          'subscribe_depth':True,
          'use_action_for_goal':False,
          'qos_image':qos,
          'qos_imu':qos,
          'Reg/Force3DoF':'true',
          'Optimizer/GravitySigma':'0', # Disable imu constraints (we are already in 2D),
          'Grid/MaxGroundHeight':'0.1',
          'RGBD/LinearUpdate': '0.05'
    }

    remappings=[
          ('rgb/image', '/camera/image_raw'),
          ('rgb/camera_info', '/camera/camera_info'),
          ('depth/image', '/camera/depth/image_raw'),
          ('odom', '/odom/unfiltered')]

Thank you for your help!

Asked by marpeja on 2022-07-29 14:17:56 UTC

Comments

Can you write more about situations where you think you should have obstacles appearing? Why do you think they were incorrectly updated? I read your YAML configs and I didn't find any error or questionable parameter values

Asked by ljaniec on 2022-07-29 17:50:58 UTC

The thing is that the local costmap isn't matching the global costmap (if you see, the obstacle layer of the local costamp, which is brighter, doesn't allways cover the obstacle layer of the global costmap, which is darker).

When running in mapping mode, the global costmap keeps growing as so does the occupancygrid of the map being built, but the local costmap sometimes shows info about the walls, sometimes doesn't (The walls are always there).

I want the local costmap to be capturing the same info as the global costmap, but in a smaller plane that moves with the robot.

Asked by marpeja on 2022-07-29 18:01:36 UTC

Moreover, I transformed the info provided by my depth camera sensor into a laser scan using depthimage_to_laserscan (and changed the corresponding observation sources in the layers of my costmap) and I am still getting the same behaviour. When the laser detects an obstacle, sometimes it gets printed in the obstacle layer of the local costmap and sometimes it doesn't.

Asked by marpeja on 2022-07-29 18:18:31 UTC

Maybe you can try with toying with observation_persistence/inf_is_valid obstacle parameters? Maybe you can have smaller inflate in the inflation layer too? Do you use this tuning tutorial?

Asked by ljaniec on 2022-07-29 18:20:57 UTC

I tried both of them but still having the same problem... I followed the tuning tutorial too and inflated the costmap very little (as I am moving in a cave with very little space).

The thing is that the global costmap is working perfectly fine and updates the obstacles when it should in the mapping mode, but the local costmap, with almost the same config doesn't.

Other thing that I have detected is that when going to the wall of the right after being close to the left one or viceversa, the local costmap keeps drawing obstacles on the previous wall as if it had some kind of delay... Could the problem be something related to it?

Asked by marpeja on 2022-07-29 18:43:32 UTC

I think this part about inflation potential fields contradicts a bit your approach to a little inflation in the cost map. Local map updates in real time with new information from the RGB-D camera, maybe the "clearing" factor can be adjusted a bit? So that only 1-2 scans would clear the obstacles in the local cost map? It can be these artifacts are your ghost obstacles

Asked by ljaniec on 2022-08-08 05:05:33 UTC

How do you change the clearing factor? Although the problem is that neither the global nor the local costmap detect the obstacle in navigation mode when placed it after the mapping mode, but when running the mapping mode using RTABMap the map is able to detect that obstacle if it is placed... I really don't know why is this... Does it have to do with the layers used?

I will correct my inflation layer too! Thank you mate!

Asked by marpeja on 2022-08-09 10:49:58 UTC

I understand parameters like lethal_cost_threshold set higher, same with update frequencies, and in others: <voxel layer>.footprint_clearing_enabled, <voxel layer>. <data source>.clearing and similar - there is no single "clearing factor", of course.

Asked by ljaniec on 2022-08-09 11:24:43 UTC

@marpeja

Although the problem is that neither the global nor the local costmap detect the obstacle in navigation mode when placed it >after the mapping mode, but when running the mapping mode using RTABMap the map is able to detect that obstacle if it is >placed...

Did you already find a solution to it? I have exactly the same issue. When working with SLAM(mapping) the dynamic obstacles are detected. But when working with AMCL (localization) no obstacle is detected in the global and local costmap.

Asked by Nobel on 2022-08-30 01:53:44 UTC

Unfortunately no... I am stil trying to find a solution to it. However, if you find it earlier please let me know!

Asked by marpeja on 2022-08-30 05:07:01 UTC

@marpeja

I think this is problem see bug 3014.

I determined root cause of the costmap collision checker issue. Since the obstacle layer isn't getting its callbacks as @gezp reported, the local costmap is empty so it shows always collision free. We use the local costmap in the behavior server, not the global costmap, which does not have the static map set by default in nav2_params.yaml.

When a static map is set or the global costmap/footprint/frame are used, things work fine!

So once we solve the issue described in ros2/rmw_fastrtps#613 / short term patch in #3018 then this should all be good

Just don't know how yet to implement it. Any tips?

Asked by Nobel on 2022-08-30 07:01:53 UTC

I really don't know as I started not too long ago with Nav2... However I found this looking in the thread you shared:

So I thought that problem can be in obstacle layer and I took a look in navigation2->nav2_costmap_2d->plugins->obstacle_layer.cpp. I realized, that in line 226, we unsubscribe the laser scan subscriber, which is subscribed above in lines 224,225. I comment line 226 and local costmap is working now fine for me and is indicated in RViz normally

I am going to see if I am able to do this and see if it works. I will tell you the result whenever I have it done!

However, if you find another solution please let me know!

Asked by marpeja on 2022-08-30 09:16:40 UTC

@Nobel

I did it, but nothing changed... the local costmap isn't updating with the obstacles...

Did you manage to get anything?

Asked by marpeja on 2022-08-30 14:32:08 UTC

Answers