Robotics StackExchange | Archived questions

Nav2 [local_costmap.local_costmap]: Sensor origin: out of map bounds. The costmap can't raytrace for it.

Hello, I recently started getting this warning when I launch the Nav2

[localcostmap.localcostmap]: Sensor origin: (1.60, 0.00, 0.96), out of map bounds. The costmap can't raytrace for it

I have never saw this warning and of course I can't remember what changes I made. But regardless, I really have no clue how to address this. I don't even know where to start from.
Of course I googled a little and found something but I'm not sure it's related to my issue: https://answers.ros.org/question/380985/sensor-origin-out-of-map-bounds-the-costmap-cant-raytrace-it/

My Nav2 params are:

amcl:
  ros__parameters:
    use_sim_time: True
    alpha1: 0.2
    alpha2: 0.2
    alpha3: 0.2
    alpha4: 0.2
    alpha5: 0.2
    base_frame_id: "base_link"
    beam_skip_distance: 0.5
    beam_skip_error_threshold: 0.9
    beam_skip_threshold: 0.3
    do_beamskip: false
    global_frame_id: "map"
    lambda_short: 0.1
    laser_likelihood_max_dist: 2.0
    laser_max_range: 20.0
    laser_min_range: 0.1
    laser_model_type: "likelihood_field"
    max_beams: 60
    max_particles: 2000
    min_particles: 500
    odom_frame_id: "odom"
    pf_err: 0.05
    pf_z: 0.99
    recovery_alpha_fast: 0.0
    recovery_alpha_slow: 0.0
    resample_interval: 1
    robot_model_type: "differential"
    save_pose_rate: 0.5
    sigma_hit: 0.2
    tf_broadcast: true
    transform_tolerance: 1.0
    update_min_a: 0.2
    update_min_d: 0.25
    z_hit: 0.5
    z_max: 0.05
    z_rand: 0.5
    z_short: 0.05
    scan_topic: scan

amcl_map_client:
  ros__parameters:
    use_sim_time: True

amcl_rclcpp_node:
  ros__parameters:
    use_sim_time: True

bt_navigator:
  ros__parameters:
    use_sim_time: True
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odometry/filtered
    enable_groot_monitoring: True
    groot_zmq_publisher_port: 1666
    groot_zmq_server_port: 1667
    default_bt_xml_filename: "navigate_w_replanning_and_round_robin_recovery"
    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

bt_navigator_rclcpp_node:
  ros__parameters:
    use_sim_time: True

controller_server:
  ros__parameters:
    use_sim_time: True
    controller_frequency: 10.0
    min_x_velocity_threshold: 0.005
    min_y_velocity_threshold: 0.005
    min_theta_velocity_threshold: 0.01
    failure_tolerance: 0.3
    odom_topic: /odometry/filtered
    progress_checker_plugin: "progress_checker"
    goal_checker_plugins: ["goal_checker"]
    controller_plugins: ["FollowPath"]

    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.05
      movement_time_allowance: 10.0
    goal_checker:
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.2 # 0.25
      yaw_goal_tolerance: 0.1 #0.25
      stateful: True
    # TEB parameters
    # http://wiki.ros.org/teb_local_planner
    FollowPath:
      plugin: "teb_local_planner::TebLocalPlannerROS"
      odom_topic: odom
      map_frame: map

      # footprint_model.type: circular
      # footprint_model.radius: 2.0
      footprint_model.type: polygon
      footprint_model.vertices: "[ [1.54, 0.405], [1.54, -0.405], [-0.2, -0.405], [-0.2, 0.405] ]"

      min_turning_radius: 2.0
      wheelbase: 1.35
      cmd_angle_instead_rotvel: true
      min_obstacle_dist: 5.0
      inflation_dist: 1.0
      costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH"
      costmap_converter_spin_thread: True
      costmap_converter_rate: 15
      enable_homotopy_class_planning: True
      enable_multithreading: True
      optimization_verbose: False
      teb_autoresize: True
      min_samples: 3
      max_samples: 20
      max_global_plan_lookahead_dist: 10.0
      visualize_hc_graph: True
      max_vel_x: 0.75
      max_vel_x_backwards: 0.2
      max_vel_y: 0.0 # should be zero for non-holonomic robots
      acc_lim_y: 0.05
      max_vel_theta: 0.5
      acc_lim_x: 0.5
      acc_lim_theta: 0.1
      yaw_goal_tolerance: 0.2
      xy_goal_tolerance: 0.1
      include_costmap_obstacles: True
      publish_feedback: True

costmap_converter:
  ros__parameters:
    use_sim_time: True

controller_server_rclcpp_node:
  ros__parameters:
    use_sim_time: True

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: odom
      robot_base_frame: base_link
      use_sim_time: True
      rolling_window: true
      width: 10
      height: 10
      origin_x: -2.0 # Relative to width
      # origin_y: 0.0 # Relative to height
      resolution: 0.05
      # Length 1.74 m
      # Width 0.81 m
      footprint: "[ [1.54, 0.405], [1.54, -0.405], [-0.2, -0.405], [-0.2, 0.405] ]"
      # robot_radius: 0.22
      footprint: "[ [1.5, 0.4], [1.5, -0.4], [-0.2, -0.4], [-0.2, 0.4] ]"
      plugins: ["voxel_layer", "inflation_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 10.0
        inflation_radius: 2.0 #0.55
      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: front_pointcloud
        front_pointcloud:
          topic: /scan
          max_obstacle_height: 2.0
          min_obstacle_height: 0.2
          obstacle_max_range: 10.0
          obstacle_min_range: 0.2
          raytrace_max_range: 3.0
          raytrace_min_range: 1.0
          clearing: True
          marking: True
          # data_type: "PointCloud2"
          data_type: "LaserScan"
      static_layer:
        map_subscribe_transient_local: True
      always_send_full_costmap: True
  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: 1.0
      # Length 1.74 m
      # Width 0.81 m
      footprint: "[ [1.54, 0.405], [1.54, -0.405], [-0.2, -0.405], [-0.2, 0.405] ]"
      footprint_padding: 0.5
      resolution: 0.05
      track_unknown_space: true
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: zed_front_pointcloud #??
        zed_front_pointcloud: #??
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          obstacle_max_range: 10.0
          obstacle_min_range: 0.2
          raytrace_max_range: 3.0
          raytrace_min_range: 1.0
          # data_type: "PointCloud2"
          data_type: "LaserScan"
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
        # map_topic: map
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 10.0
        inflation_radius: 4.0
      always_send_full_costmap: True
  global_costmap_client:
    ros__parameters:
      use_sim_time: True
  global_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: True

map_saver:
  ros__parameters:
    use_sim_time: True
    save_map_timeout: 5000
    free_thresh_default: 0.25
    occupied_thresh_default: 0.65
    map_subscribe_transient_local: False

planner_server:
  ros__parameters:
    planner_plugins: ["GridBased"]
    use_sim_time: True

    GridBased:
      plugin: "smac_planner/SmacPlanner"
      tolerance: 0.5                    # tolerance for planning if unable to reach exact pose, in meters, for 2D node
      downsample_costmap: false         # whether or not to downsample the map
      downsampling_factor: 1            # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm)
      allow_unknown: true # false              # allow traveling in unknown space
      max_iterations: -1                # maximum total iterations to search for before failing
      max_on_approach_iterations: 1000  # maximum number of iterations to attempt to reach goal once in tolerance, 2D only
      max_planning_time_ms: 2000.0      # max time in ms for planner to plan, smooth, and upsample. Will scale maximum smoothing and upsampling times based on remaining time after planning.
      smooth_path: false                # Whether to smooth searched path
      motion_model_for_search: "REEDS_SHEPP"  # 2D Moore, Von Neumann; SE2 Dubin, Redds-Shepp - Options for SE2: DUBIN, REEDS_SHEPP. 2D: MOORE, VON_NEUMANN
      angle_quantization_bins: 72       # For SE2 node: Number of angle bins for search, must be 1 for 2D node (no angle search)
      minimum_turning_radius: 2.0      # For SE2 node & smoother: minimum turning radius in m of path / vehicle
      reverse_penalty: 1.2              # For Reeds-Shepp model: penalty to apply if motion is reversing, must be => 1
      change_penalty: 0.20              # For SE2 node: penalty to apply if motion is changing directions, must be >= 0
      non_straight_penalty: 1.05        # For SE2 node: penalty to apply if motion is non-straight, must be => 1
      cost_penalty: 1.3                 # For SE2 node: penalty to apply to higher cost zones

      smoother:
        smoother:
          w_curve: 30.0                 # weight to minimize curvature of path
          w_dist: 0.0                   # weight to bind path to original as optional replacement for cost weight
          w_smooth: 30000.0             # weight to maximize smoothness of path
          w_cost: 0.025                 # weight to steer robot away from collision and cost
          cost_scaling_factor: 10.0     # this should match the inflation layer's parameter

        # I do not recommend users mess with this unless they're doing production tuning
        optimizer:
          max_time: 0.10                # maximum compute time for smoother
          max_iterations: 500           # max iterations of smoother
          debug_optimizer: false        # print debug info
          gradient_tol: 1.0e-10
          fn_tol: 1.0e-20
          param_tol: 1.0e-15
          advanced:
            min_line_search_step_size: 1.0e-20
            max_num_line_search_step_size_iterations: 50
            line_search_sufficient_function_decrease: 1.0e-20
            max_num_line_search_direction_restarts: 10
            max_line_search_step_expansion: 50

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: odom
    robot_base_frame: base_link
    transform_timeout: 0.1
    use_sim_time: true
    simulate_ahead_time: 2.0
    max_rotational_vel: 0.5
    min_rotational_vel: 0.4
    rotational_acc_lim: 0.5

robot_state_publisher:
  ros__parameters:
    use_sim_time: True

Asked by guidout on 2023-06-16 11:30:22 UTC

Comments

Please post your params otherwise there is nothing that can be said with certainty. What is the size of your local costmap and what is its origin? Modifying either of these will ensure your sensor origin is definitely within the bounds of the costmap.

Asked by adityatandon on 2023-06-16 11:57:12 UTC

Sometimes, depending on localization method, you need to set the initial robot pose via RViz. Have you verified the robot to odom to map pose ? Can you see the robot model and costmap in RViz?

Asked by dcconner on 2023-06-16 13:49:21 UTC

I posted my nav2 params. My local cost map is 10x10. How do I check its origin?

Asked by guidout on 2023-06-16 14:49:04 UTC

@dcconner, yes odom to map looks correct and I both model and costmap in rviz

Asked by guidout on 2023-06-16 14:49:55 UTC

Answers

I'm just going to be really honest that that issue can come up with really subtle issues with your robot's URDF configuration or costmap configuration. I can't guess all the ways it might be wrong but that should help narrow it down. Also:

  width: 10
  height: 10
  origin_x: -2.0 # Relative to width
  # origin_y: 0.0 # Relative to height

The origin should always be half the width / height - unless you know precisely what you're doing. That's probably a good amount of your problem

Asked by stevemacenski on 2023-06-19 14:07:23 UTC

Comments

@stevemacenski thanks for the input. I removed that origin_x offset, but that didn't fix the issue (as you probably expected). Can you expand on the URDF born issues you mentioned? I'm really unsure about what you mean by that. About the costmap configuration...well...yes, the problem might be there by everything kinda makes sense to me so I can't really pinpoint the potential issue.

Asked by guidout on 2023-06-20 06:51:46 UTC