Voxel layer misbehaving.

[kulkarni-raunak]

Bug report

Required Info:

• Operating System:
  • Ubuntu 22.04
• Computer:
  • Intel® Core™ i7-7700K CPU @ 4.20GHz × 8
• ROS2 Version:
  • ROS2 Jazzy (source build)
• DDS implementation:
  • Default

Expected behavior

Local_costmaps and Global_costmaps are visualised in rviz2 without any problems.

Actual behavior

1. Voxel layers are missing.
2. Only Costmaps related to particular fixed_frames appears. (Global_costmap for map and local_costmap for odom)

Reproduction instructions

my_nav2_params.yaml

bt_navigator:
  ros__parameters:
    use_sim_time: False
    global_frame: map
    robot_base_frame: base_link #TODO: 
    odom_topic: /ad/odometry #TODO: 
    bt_loop_duration: 10
    default_server_timeout: 20
    enable_groot_monitoring: True
    groot_zmq_publisher_port: 1666
    groot_zmq_server_port: 1667
    navigators: ["navigate_to_pose", "navigate_through_poses"]
    navigate_to_pose:
      plugin: "nav2_bt_navigator::NavigateToPoseNavigator"
    navigate_through_poses:
      plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator"
    error_code_names:
      - compute_path_error_code
      - follow_path_error_code      

bt_navigator_rclcpp_node:
  ros__parameters:
    use_sim_time: False

controller_server:
  ros__parameters:
    use_sim_time: False
    controller_frequency: 5.0
    min_x_velocity_threshold: 0.1
    min_y_velocity_threshold: 0.1
    min_theta_velocity_threshold: 0.1
    failure_tolerance: 0.3
    progress_checker_plugins: ["progress_checker"]
    goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
    controller_plugins: ["FollowPath"]
    odom_topic: /ad/odometry #TODO: 
    use_realtime_priority: false
    # 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:
      stateful: True
      plugin: "nav2_controller::SimpleGoalChecker"
      # xy_goal_tolerance: 0.25
      # yaw_goal_tolerance: 0.25
      xy_goal_tolerance: 10.0
      yaw_goal_tolerance: 1.57
    FollowPath:
      plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
      desired_linear_vel: 0.5
      lookahead_dist: 10.0
      # min_lookahead_dist: 1.5
      # max_lookahead_dist: 2.5
      # lookahead_time: 1.5
      rotate_to_heading_angular_vel: 0.5
      transform_tolerance: 0.1
      use_velocity_scaled_lookahead_dist: false
      min_approach_linear_velocity: 0.02
      approach_velocity_scaling_dist: 20.0
      use_collision_detection: true
      max_allowed_time_to_collision_up_to_carrot: 30.0
      use_regulated_linear_velocity_scaling: false
      use_fixed_curvature_lookahead: false
      curvature_lookahead_dist: 2.0
      use_cost_regulated_linear_velocity_scaling: false
      regulated_linear_scaling_min_radius: 0.9
      regulated_linear_scaling_min_speed: 0.25
      use_rotate_to_heading: true
      allow_reversing: false
      rotate_to_heading_min_angle: 0.5
      max_angular_accel: 0.5
      max_robot_pose_search_dist: -1.0    

controller_server_rclcpp_node:
  ros__parameters:
    use_sim_time: False

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 5.0
      global_frame: odom
      robot_base_frame: base_link #TODO: 
      use_sim_time: False
      rolling_window: true
      width: 210
      height: 210
      resolution: 1.2
      robot_radius: 3.0
      plugins: ["voxel_layer", "inflation_layer"]
      filters: ["keepout_filter"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 3.0
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: True
        publish_voxel_map: True
        origin_z: 0.0
        z_resolution: 1.25
        z_voxels: 16
        max_obstacle_height: 2.0
        mark_threshold: 0
        observation_sources: point_cloud2
        point_cloud2:
          topic: /ad/lidar/velodyne_points #TODO: 
          clearing: True
          marking: True
          expected_update_rate: 0.0
          data_type: "PointCloud2"
          sensor_frame: velodyne #TODO: 
          max_obstacle_height: 2.0
          min_obstacle_height: 0.2
          # raytrace_max_range: 3.0
          raytrace_max_range: 200.0
          raytrace_min_range: 0.0
          # obstacle_max_range: 2.5
          obstacle_max_range: 150.0
          obstacle_min_range: 0.0
      keepout_filter:
        plugin: "nav2_costmap_2d::KeepoutFilter"
        enabled: True
        filter_info_topic: "/costmap_filter_info"
        transform_tolerance: 0.1                      
      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: 5.0
      publish_frequency: 5.0
      global_frame: map
      robot_base_frame: base_link
      width: 2330
      height: 1670            
      origin_x: -1165.0
      origin_y: -835.0
      use_sim_time: False
      robot_radius: 3.0
      resolution: 1.2
      track_unknown_space: true
      plugins: ["static_layer", "voxel_layer", "inflation_layer"]
      filters: ["keepout_filter"]
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: True
        publish_voxel_map: true
        origin_z: 0.0
        z_resolution: 1.25
        z_voxels: 16
        max_obstacle_height: 2.0
        mark_threshold: 0
        observation_sources: point_cloudd2
        point_cloud2:
          topic: /ad/lidar/velodyne_points #TODO:
          clearing: True
          marking: True
          expected_update_rate: 0.0
          data_type: "PointCloud2"
          sensor_frame: velodyne #TODO:
          max_obstacle_height: 2.0
          min_obstacle_height: 0.2
          # raytrace_max_range: 3.0
          raytrace_max_range: 200.0
          raytrace_min_range: 0.0
          # obstacle_max_range: 2.5
          obstacle_max_range: 150.0
          obstacle_min_range: 0.0              
      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: 3.0
      always_send_full_costmap: True
      keepout_filter:
        plugin: "nav2_costmap_2d::KeepoutFilter"
        enabled: True
        filter_info_topic: "/costmap_filter_info"
        transform_tolerance: 0.1   
  global_costmap_client:
    ros__parameters:
      use_sim_time: False
  global_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: False

map_server:
  ros__parameters:
    use_sim_time: False
    # yaml_filename: "/opt/ros/jazzy/share/nav2_bringup/maps/marum_yard/marum_yard.yaml" #TODO: 

map_saver:
  ros__parameters:
    use_sim_time: False
    save_map_timeout: 1.0
    free_thresh_default: 0.4
    occupied_thresh_default: 0.5
    map_subscribe_transient_local: True

costmap_filter_info_server:
  ros__parameters:
    type: 0
    filter_info_topic: "/costmap_filter_info"
    mask_topic: "/filter_mask"
    base: 0.0
    multiplier: 1.0

filter_mask_server: 
  ros__parameters:
    frame_id: "map"
    topic_name: "/filter_mask"
    yaml_filename: "/opt/ros/jazzy/share/nav2_bringup/maps/marum_yard/marum_yard_avoid_region_00.yaml"

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

planner_server_rclcpp_node:
  ros__parameters:
    use_sim_time: False

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

robot_state_publisher:
  ros__parameters:
    use_sim_time: False

behavior_server:
  ros__parameters:
    local_costmap_topic: local_costmap/costmap_raw
    global_costmap_topic: global_costmap/costmap_raw
    local_footprint_topic: local_costmap/published_footprint
    global_footprint_topic: global_costmap/published_footprint
    cycle_frequency: 10.0
    behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
    spin:
      plugin: "nav2_behaviors::Spin"
    backup:
      plugin: "nav2_behaviors::BackUp"
    drive_on_heading:
      plugin: "nav2_behaviors::DriveOnHeading"
    wait:
      plugin: "nav2_behaviors::Wait"
    assisted_teleop:
      plugin: "nav2_behaviors::AssistedTeleop"
    local_frame: odom
    global_frame: map
    robot_base_frame: base_link #TODO: 
    transform_tolerance: 0.1
    simulate_ahead_time: 2.0
    max_rotational_vel: 1.0
    min_rotational_vel: 0.4
    rotational_acc_lim: 3.2    

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

collision_monitor:
  ros__parameters:
    base_frame_id: "base_link" #TODO: 
    odom_frame_id: "odom"
    cmd_vel_in_topic: "cmd_vel_smoothed"
    cmd_vel_out_topic: "cmd_vel"
    state_topic: "collision_monitor_state"
    transform_tolerance: 0.2
    source_timeout: 1.0
    base_shift_correction: True
    stop_pub_timeout: 2.0
    # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types,
    # and robot footprint for "approach" action type.
    polygons: ["FootprintApproach"]
    FootprintApproach:
      type: "polygon"
      action_type: "approach"
      footprint_topic: "/local_costmap/published_footprint"
      time_before_collision: 1.2
      simulation_time_step: 0.1
      min_points: 6
      visualize: False
      enabled: True
    observation_sources: ["pointcloud"]
    pointcloud:
      type: "pointcloud"
      topic: "/ad/lidar/velodyne_points"
      min_height: 0.2
      max_height: 2.0
      enabled: True           

The subsequent costmaps appear in rviz2 only when selecting appropriate frames but no Voxel layer is added see

Now changing

• DDS implementation:
  • rmw_zenoh_cpp

The subsequent costmaps appear in rviz2 only when again selecting appropriate frames and this time with Voxel layers as well. See

Additional information

Using a custom localization node.

[SteveMacenski]

What is your expected behavior, I don't understand. Are you saying that you're missing the costmap occupancy grids of the 3D visualization of the voxel grid? I don't see that you have the voxel grid topic in your rviz display panel, nor is it published by default since that publication and processing method is expensive (but can be enabled with the cloud voxel grid publisher node in costmap 2d).

I'd appreciate if you updated your title and description to include more specific detail about what it is that you're looking for.

For the occupancy grid, that I see in the rviz config: Did you set the QoS correctly for the occupancy grids from local/global costmap? Can you reproduce this with Nav2's included rviz configuration?

Are you sure your topics are correct and frames exist? Any error logs?