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
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
possible answers at https://answers.ros.org/question/384944/navigation-2-sensor-origin-out-of-map-bounds-the-costmap-cant-raytrace-for-it/
Asked by dcconner on 2023-08-03 13:55:44 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