Nav2 Costmap Generation not accurate

Robotics & Edge Computing Robotics - Isaac Isaac ROS
1

Dear Community and Nvidia Developer, would greatly appreciate your help and advise!!

I’m currently facing accuracy issues with the global costmap (visualized in red, blue, and pink) generated by the NVBlox stack in the Isaac ROS pipeline. As shown in the attached image, the costmap does not correctly reflect the actual environment. Even when the floor is completely clear of obstacles, the global costmap continues to indicate false positives.

I suspect the issue might be due to unreliable depth sensing from the Intel RealSense D435i. Despite tuning efforts, the depth data still appears inaccurate. The RealSense is currently running firmware version 5.13.0.50. If anyone has recommendations for improving depth reliability or costmap fidelity in this configuration, I’d appreciate any suggestions.

image
image1852×1054 157 KB

image
image1842×942 135 KB

image
image1280×960 103 KB

image
image1280×960 112 KB

image
image1280×960 123 KB

Hardware Setup:

  • Robot: TurtleBot3 Burger
  • Compute: Jetson Orin NX 16GB
  • Depth Camera: Intel RealSense D435i (Firmware: 5.13.0.50)

NVblox + Nav2 Integration:

To integrate NVBlox with Nav2, I’ve included the following three configuration files:

  1. nav2_realsense.yaml
    Located at:
    src/isaac_ros_nvblox/nvblox_examples/nvblox_examples_bringup/config/nav2/nav2_realsense.yaml
    This defines behavior tree configuration, controller/planner settings, velocity smoothing, local/global costmap plugins using nvblox::nav2::NvbloxCostmapLayer, and parameters for the bt_navigator, controller_server, and other Nav2 nodes.
  2. nav2_realsense.launch.py
    Located at:
    src/isaac_ros_nvblox/nvblox_examples/nvblox_examples_bringup/launch/nav2/nav2_realsense.launch.py
    This launch file brings up the main Nav2 nodes using the above YAML configuration.
  3. realsense_nav2_example.launch.py
    Located at:
    src/isaac_ros_nvblox/nvblox_examples/nvblox_examples_bringup/launch/realsense_nav2_example.launch.py
    This is the top-level launch file that initializes the static transform from base_link to camera0_link, brings up the RealSense camera driver, VSLAM, NVBlox mapping, Nav2 stack, and RViz (optionally).

Questions / Clarification:

  • Are there any recommended filters or preprocessing steps for RealSense depth data to improve mapping accuracy with NVBlox? Or this is some other issue?
  • Is the use of odom as the global frame advisable in this configuration, or should I switch to map?
  • Do I need to fine-tune the max_obstacle_distance, inflation_distance, or camera mounting TF more precisely?

The 3 files:

  1. src/isaac_ros_nvblox/nvblox_examples/nvblox_examples_bringup/config/nav2/nav2_realsense.yaml
%YAML 1.2
---
bt_navigator:
  ros__parameters:
    use_sim_time: False
    global_frame: odom
    robot_base_frame: base_link
    odom_topic: /odom
    bt_loop_duration: 20
    default_server_timeout: 40
    # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
    # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
    # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
    # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
    plugin_lib_names:
    - nav2_compute_path_to_pose_action_bt_node
    - nav2_compute_path_through_poses_action_bt_node
    - nav2_smooth_path_action_bt_node
    - nav2_follow_path_action_bt_node
    - nav2_spin_action_bt_node
    - nav2_wait_action_bt_node
    - nav2_back_up_action_bt_node
    - nav2_drive_on_heading_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_globally_updated_goal_condition_bt_node
    - nav2_is_path_valid_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_truncate_path_local_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_path_expiring_timer_condition
    - nav2_distance_traveled_condition_bt_node
    - nav2_single_trigger_bt_node
    - nav2_is_battery_low_condition_bt_node
    - nav2_navigate_through_poses_action_bt_node
    - nav2_navigate_to_pose_action_bt_node
    - nav2_remove_passed_goals_action_bt_node
    - nav2_planner_selector_bt_node
    - nav2_controller_selector_bt_node
    - nav2_goal_checker_selector_bt_node
    - nav2_controller_cancel_bt_node
    - nav2_path_longer_on_approach_bt_node
    - nav2_wait_cancel_bt_node
    - nav2_spin_cancel_bt_node
    - nav2_back_up_cancel_bt_node
    - nav2_drive_on_heading_cancel_bt_node

bt_navigator_rclcpp_node:
  ros__parameters:
    use_sim_time: False

controller_server:
  ros__parameters:
    use_sim_time: False
    controller_frequency: 10.0
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.5
    min_theta_velocity_threshold: 0.001
    failure_tolerance: 0.3
    progress_checker_plugin: "progress_checker"
    goal_checker_plugin: ["general_goal_checker"]
    controller_plugins: ["FollowPath"]

    # Progress checker parameters
    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.5
      movement_time_allowance: 10.0
    # Goal checker parameters
    general_goal_checker:
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.25
      yaw_goal_tolerance: 3.14
      stateful: True
    # DWB parameters
    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"
      debug_trajectory_details: True
      min_vel_x: 0.0
      min_vel_y: 0.0
      max_vel_x: 2.0
      max_vel_y: 0.0
      max_vel_theta: 1.0
      min_speed_xy: 0.0
      max_speed_xy: 2.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: 1.0
      acc_lim_y: 0.0
      acc_lim_theta: 1.0
      decel_lim_x: -1.0
      decel_lim_y: 0.0
      decel_lim_theta: -1.0
      vx_samples: 30
      vy_samples: 10
      vtheta_samples: 20
      sim_time: 1.4
      linear_granularity: 0.05
      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"]
      BaseObstacle.scale: 0.02
      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: 1.0
      RotateToGoal.lookahead_time: -1.0

# Update velocity smoother default params to match our controller server config
# https://navigation.ros.org/configuration/packages/configuring-velocity-smoother.html
velocity_smoother:
  ros__parameters:
    smoothing_frequency: 20.0
    scale_velocities: false
    feedback: "OPEN_LOOP"
    max_velocity: [1.0, 0.0, 2.0]
    min_velocity: [0.0, 0.0, -2.0]
    deadband_velocity: [0.0, 0.0, 0.0]
    velocity_timeout: 1.0
    max_accel: [1.0, 0.0, 2.0]
    max_decel: [-1.0, 0.0, -2.0]
    odom_topic: "odom"
    odom_duration: 0.1

controller_server_rclcpp_node:
  ros__parameters:
    use_sim_time: False

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 10.0
      publish_frequency: 10.0
      global_frame: odom
      robot_base_frame: base_link
      use_sim_time: False
      rolling_window: True
      width: 10
      height: 10
      resolution: 0.05
      robot_radius: 0.1
      plugins: ["nvblox_base_layer", "nvblox_human_layer"]
      nvblox_base_layer:
        plugin: "nvblox::nav2::NvbloxCostmapLayer"
        enabled: True
        max_obstacle_distance: 1.0
        inflation_distance: 0.4
        nvblox_map_slice_topic: "/nvblox_node/static_map_slice"
      nvblox_human_layer:
        plugin: "nvblox::nav2::NvbloxCostmapLayer"
        enabled: True
        max_obstacle_distance: 1.0
        inflation_distance: 0.4
        nvblox_map_slice_topic: "/nvblox_human_node/human_map_slice"
      always_send_full_costmap: True
  local_costmap_client:
    ros__parameters:
      use_sim_time: False
  local_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: False

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 10.0
      publish_frequency: 10.0
      global_frame: odom
      robot_base_frame: base_link
      use_sim_time: False
      rolling_window: True
      width: 200
      height: 200
      robot_radius: 0.1
      resolution: 0.1
      origin_x: -100.0
      origin_y: -100.0
      plugins: ["nvblox_base_layer", "nvblox_human_layer"]
      nvblox_base_layer:
        plugin: "nvblox::nav2::NvbloxCostmapLayer"
        enabled: True
        max_obstacle_distance: 1.0
        inflation_distance: 0.4
        nvblox_map_slice_topic: "/nvblox_node/static_map_slice"
      nvblox_human_layer:
        plugin: "nvblox::nav2::NvbloxCostmapLayer" 
        enabled: True
        max_obstacle_distance: 1.0
        inflation_distance: 0.4
        nvblox_map_slice_topic: "/nvblox_human_node/human_map_slice"
      always_send_full_costmap: True
  global_costmap_client:
    ros__parameters:
      use_sim_time: False
  global_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: False

planner_server:
  ros__parameters:
    expected_planner_frequency: 10.0
    use_sim_time: False
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"
      tolerance: 0.5
      use_astar: false
      allow_unknown: true

planner_server_rclcpp_node:
  ros__parameters:
    use_sim_time: False

smoother_server:
  ros__parameters:
    use_sim_time: False
    smoother_plugins: ["simple_smoother"]
    simple_smoother:
      plugin: "nav2_smoother::SimpleSmoother"
      tolerance: 1.0e-10
      max_its: 1000
      do_refinement: True

behavior_server:
  ros__parameters:
    costmap_topic: local_costmap/costmap_raw
    footprint_topic: local_costmap/published_footprint
    cycle_frequency: 5.0
    behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"]
    spin:
      plugin: "nav2_behaviors/Spin"
    backup:
      plugin: "nav2_behaviors/BackUp"
    drive_on_heading:
      plugin: "nav2_behaviors/DriveOnHeading"
    wait:
      plugin: "nav2_behaviors/Wait"
    global_frame: odom
    robot_base_frame: base_link
    transform_tolerance: 0.2
    use_sim_time: False
    simulate_ahead_time: 2.0
    max_rotational_vel: 1.0
    min_rotational_vel: 0.4
    rotational_acc_lim: 1.0

robot_state_publisher:
  ros__parameters:
    use_sim_time: False

waypoint_follower:
  ros__parameters:
    loop_rate: 20
    stop_on_failure: false
    waypoint_task_executor_plugin: "wait_at_waypoint"
    wait_at_waypoint:
      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
      enabled: True
      waypoint_pause_duration: 200
  1. src/isaac_ros_nvblox/nvblox_examples/nvblox_examples_bringup/launch/nav2/nav2_realsense.launch.py
import os
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    config_path = os.path.join(
        '/workspaces/isaac_ros-dev/src/isaac_ros_nvblox/nvblox_examples/nvblox_examples_bringup/config/nav2',
        'nav2_realsense.yaml'
    )

    return LaunchDescription([

        Node(
            package='nav2_controller',
            executable='controller_server',
            name='controller_server',
            output='screen',
            parameters=[config_path]
        ),

        Node(
            package='nav2_planner',
            executable='planner_server',
            name='planner_server',
            output='screen',
            parameters=[config_path]
        ),

        Node(
            package='nav2_bt_navigator',
            executable='bt_navigator',
            name='bt_navigator',
            output='screen',
            parameters=[config_path]
        ),

        Node(
            package='nav2_behaviors',
            executable='behavior_server',
            name='behavior_server',
            output='screen',
            parameters=[config_path]
        ),

        Node(
            package='nav2_waypoint_follower',
            executable='waypoint_follower',
            name='waypoint_follower',
            output='screen',
            parameters=[config_path]
        ),

        Node(
            package='nav2_lifecycle_manager',
            executable='lifecycle_manager',
            name='lifecycle_manager_navigation',
            output='screen',
            parameters=[{
                'use_sim_time': False,
                'autostart': True,
                'node_names': [
                    'controller_server',
                    'planner_server',
                    'bt_navigator',
                    'behavior_server',
                    'waypoint_follower'
                ]
            }]
        ),
    ])
  1. src/isaac_ros_nvblox/nvblox_examples/nvblox_examples_bringup/launch/realsense_nav2_example.launch.py
from isaac_ros_launch_utils.all_types import *
import isaac_ros_launch_utils as lu

from nvblox_ros_python_utils.nvblox_launch_utils import NvbloxMode, NvbloxCamera
from nvblox_ros_python_utils.nvblox_constants import NVBLOX_CONTAINER_NAME


def generate_launch_description() -> LaunchDescription:
    args = lu.ArgumentContainer()
    args.add_arg('log_level', 'info')
    args.add_arg('num_cameras', 1)
    args.add_arg('mode', default=NvbloxMode.static, cli=True)
    args.add_arg('run_rviz', default='True')
    args.add_arg('use_foxglove_whitelist', True)
    args.add_arg('container_name', NVBLOX_CONTAINER_NAME)
    args.add_arg('global_frame', default='odom')

    actions = args.get_launch_actions()

    actions.append(
        Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        name='camera_mount_tf',
        arguments=['0.015', '0', '0.122', '0', '0', '0', '1', 'base_link', 'camera0_link'],
        )
        
    )

    # Start container early so composables can be loaded
    actions.append(
        lu.component_container(args.container_name, log_level=args.log_level)
    )

    # Launch Realsense camera
    actions.append(
        lu.include(
            'nvblox_examples_bringup',
            'launch/sensors/realsense.launch.py',
            launch_arguments={
                'container_name': args.container_name,
                'num_cameras': args.num_cameras
            }
        )
    )

    # Launch Visual SLAM
    actions.append(
        lu.include(
            'nvblox_examples_bringup',
            'launch/perception/vslam.launch.py',
            launch_arguments={
                'container_name': args.container_name,
                'camera': str(NvbloxCamera.realsense),
                'output_odom_frame_name': args.global_frame
            }
        )
    )

    # Launch NVBlox Mapping
    actions.append(
        lu.include(
            'nvblox_examples_bringup',
            'launch/perception/nvblox.launch.py',
            launch_arguments={
                'container_name': args.container_name,
                'mode': args.mode,
                'camera': str(NvbloxCamera.realsense),
                'num_cameras': args.num_cameras
            }
        )
    )

    # Launch Nav2
    actions.append(
        lu.include(
            'nvblox_examples_bringup',
            'launch/nav2/nav2_realsense.launch.py'
        )
    )

    # Optional: RViz
    actions.append(
        lu.include(
            'nvblox_examples_bringup',
            'launch/visualization/visualization.launch.py',
            launch_arguments={
                'mode': args.mode,
                'camera': str(NvbloxCamera.realsense),
                'use_foxglove_whitelist': args.use_foxglove_whitelist
            },
            condition=IfCondition(args.run_rviz)
        )
    )

    return LaunchDescription(actions)
2

Hi @m230060

Thank you for your messages.

Examining the posted images, it appears that the ground is infiltrating the costmap.

Try to tune the esdf_slice_min_height (and depending on robot height, the esdf_slice_max_height ) parameter following the documentation ROS Parameters — isaac_ros_docs documentation.

The slice bounds can be visualized with the ~/map_slice_bounds topic ROS Topics and Services — isaac_ros_docs documentation.

Everything between the two visualized planes will be included as an obstacle in the costmap.
The min/lower plane should be above the ground (but low enough to include low lying objects that need to be detected as obstacles).

Best,
Raffaello

1 Like