Local costmap not updating in Nav2

asked 2022-07-29 14:17:56 -0500

marpeja gravatar image

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 ...
(more)
edit retag flag offensive close merge delete

Comments

1

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

ljaniec gravatar image ljaniec  ( 2022-07-29 17:50:58 -0500 )edit

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.

marpeja gravatar image marpeja  ( 2022-07-29 18:01:36 -0500 )edit

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.

marpeja gravatar image marpeja  ( 2022-07-29 18:18:31 -0500 )edit

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?

ljaniec gravatar image ljaniec  ( 2022-07-29 18:20:57 -0500 )edit

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?

marpeja gravatar image marpeja  ( 2022-07-29 18:43:32 -0500 )edit

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

ljaniec gravatar image ljaniec  ( 2022-08-08 05:05:33 -0500 )edit

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!

marpeja gravatar image marpeja  ( 2022-08-09 10:49:58 -0500 )edit

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.

ljaniec gravatar image ljaniec  ( 2022-08-09 11:24:43 -0500 )edit