NAV2 configured two observation sources but did not generate obstacles on the costmap. #5718
Description
Required Info:
- Operating System:
Ubuntu 22.04 - Computer:
Nvidia Jetson Orin Nx16GB - ROS2 Version:
Humble - Version or commit hash:
1.1.18-1jammy.20250729.003124 - DDS implementation:
rmw_cyclonedds_cpp - Nav2 Package:
MPPI
Tuning / Configuration Goal
I converted ultrasonic data to LaserScan data, intending to use it as an observation source for NAV2. The converted topic is named /ultrasonic_scan. The /ultrasonic_scan published by the ultrasonic conversion node works perfectly; the point cloud data from /ultrasonic_scan can be reliably viewed in rviz2.
However, when I added an observation source named [ultrasonic_scan] on top of the scan in the local_costmap and global_costmap parameters, it seems that NAV2 doesn't use the point cloud from /ultrasonic_scan when starting, nor does it generate obstacles based on /ultrasonic_scan.
ros2 topic echo /ultrasonic_scan:
---
header:
stamp:
sec: 1764055351
nanosec: 471156712
frame_id: ultrasonic_link
angle_min: -3.1415927410125732
angle_max: 3.1415927410125732
angle_increment: 0.0872664600610733
time_increment: 0.0
scan_time: 0.10000000149011612
range_min: 0.019999999552965164
range_max: 1.5
ranges:
- .inf
- 0.22100000083446503
- 0.22100000083446503
- 0.22100000083446503
- 0.22100000083446503
- 0.22100000083446503
- 0.22100000083446503
- 0.22100000083446503
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- 0.21899999678134918
- 0.21899999678134918
- 0.21899999678134918
- 0.21899999678134918
- 0.21899999678134918
- 0.21899999678134918
- 0.21899999678134918
- .inf
- .inf
intensities: []
---
Below is my NAV2 parameter file
param_tues_robot.yaml
If you cannot download:
amcl:
ros__parameters:
use_sim_time: False
alpha1: 0.4
alpha2: 0.2
alpha3: 0.2
alpha4: 0.4
alpha5: 0.2
base_frame_id: "base_footprint"
introspection_mode: "disabled"
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: 100.0
laser_min_range: -1.0
laser_model_type: "likelihood_field"
max_beams: 60
max_particles: 2000
min_particles: 500
odom_frame_id: "odom_combined"
pf_err: 0.05
pf_z: 0.99
recovery_alpha_fast: 0.0
recovery_alpha_slow: 0.0
resample_interval: 1
robot_model_type: "nav2_amcl::DifferentialMotionModel"
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
set_initial_pose: True
initial_pose.x: 0.0
initial_pose.y: 0.0
initial_pose.z: 0.0
initial_pose.yaw: 0.0
amcl_map_client:
ros__parameters:
use_sim_time: False
amcl_rclcpp_node:
ros__parameters:
use_sim_time: False
bt_navigator:
ros__parameters:
use_sim_time: False
transform_tolerance: 0.1
goal_reached_tol: 0.25
global_frame: map
robot_base_frame: base_footprint
odom_topic: /odom_combined
bt_loop_duration: 10
default_server_timeout: 20
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: 20.0
controller_plugins: ["FollowPath"]
odom_topic: /odom_combined
progress_checker_plugin: "progress_checker"
goal_checker_plugin: "goal_checker"
progress_checker:
plugin: "nav2_controller::SimpleProgressChecker"
required_movement_radius: 0.5
movement_time_allowance: 10.0
goal_checker:
plugin: "nav2_controller::SimpleGoalChecker"
xy_goal_tolerance: 0.15
yaw_goal_tolerance: 0.25
stateful: True
FollowPath:
plugin: "nav2_rotation_shim_controller::RotationShimController"
primary_controller: "nav2_mppi_controller::MPPIController"
time_steps: 56
model_dt: 0.05
batch_size: 2000
vx_std: 0.2
vy_std: 0.2
wz_std: 0.4
vx_max: 0.5
vx_min: -0.5
vy_max: 0.5
wz_max: 2.0
ax_max: 1.5
ax_min: -1.5
ay_max: 0.0
az_max: 1.5
iteration_count: 1
prune_distance: 1.5
enforce_path_inversion: false
transform_tolerance: 0.1
temperature: 0.3
gamma: 0.015
motion_model: "DiffDrive"
visualize: false
TrajectoryVisualizer:
trajectory_step: 5
time_step: 3
AckermannConstraints:
min_turning_r: 0.25
critics: ["ConstraintCritic", "ObstaclesCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"]
ConstraintCritic:
enabled: true
cost_power: 1
cost_weight: 4.0
GoalAngleCritic:
enabled: true
cost_power: 1
cost_weight: 3.0
threshold_to_consider: 0.5
GoalCritic:
enabled: true
cost_power: 1
cost_weight: 15.0
threshold_to_consider: 1.4
ObstaclesCritic:
enabled: true
cost_power: 1
repulsion_weight: 1.5
critical_weight: 30.0
consider_footprint: false
collision_cost: 10000.0
collision_margin_distance: 0.1
near_goal_distance: 0.4
cost_scaling_factor: 5.0
inflation_radius: 0.15
PreferForwardCritic:
enabled: true
cost_power: 1
cost_weight: 1.5
threshold_to_consider: 0.5
PathAlignCritic:
enabled: true
cost_power: 1
cost_weight: 25.0
max_path_occupancy_ratio: 0.07
trajectory_point_step: 4
threshold_to_consider: 0.5
offset_from_furthest: 20
use_path_orientations: false
PathAngleCritic:
enabled: true
cost_power: 1
cost_weight: 4.0
offset_from_furthest: 4
threshold_to_consider: 0.5
max_angle_to_furthest: 1.0
mode: 0
PathFollowCritic:
enabled: true
cost_power: 1
cost_weight: 25.0
offset_from_furthest: 5
threshold_to_consider: 1.4
controller_server_rclcpp_node:
ros__parameters:
use_sim_time: False
local_costmap:
local_costmap:
ros__parameters:
update_frequency: 5.0
publish_frequency: 2.0
global_frame: map
robot_base_frame: base_footprint
use_sim_time: False
rolling_window: true
width: 3
height: 3
resolution: 0.05
footprint: "[[-0.55, -0.3280], [-0.55, 0.3280], [0.144, 0.3280], [0.144, -0.3280]]"
plugins: ["voxel_layer","inflation_layer"]
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 3.0
inflation_radius: 0.45
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: scan ultrasonic_scan
combination_method: 2
scan:
topic: /scan
max_obstacle_height: 2.0
clearing: True
marking: True
data_type: "LaserScan"
raytrace_max_range: 10.0
raytrace_min_range: 0.0
obstacle_max_range: 2.5
obstacle_min_range: 0.0
ultrasonic_scan:
topic: /ultrasonic_scan
max_obstacle_height: 2.0
clearing: True
marking: True
data_type: "LaserScan"
raytrace_max_range: 3.0
raytrace_min_range: 0.0
obstacle_max_range: 2.5
obstacle_min_range: 0.0
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: 1.0
publish_frequency: 1.0
global_frame: map
robot_base_frame: base_footprint
use_sim_time: False
footprint: "[[-0.55, -0.3280], [-0.55, 0.3280], [0.144, 0.3280], [0.144, -0.3280]]"
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: scan ultrasonic_scan
scan:
topic: /scan
max_obstacle_height: 2.0
clearing: True
marking: True
data_type: "LaserScan"
raytrace_max_range: 10.0
raytrace_min_range: 0.0
obstacle_max_range: 2.5
obstacle_min_range: 0.0
ultrasonic_scan:
topic: /ultrasonic_scan
max_obstacle_height: 2.0
clearing: True
marking: True
data_type: "LaserScan"
raytrace_max_range: 3.0
raytrace_min_range: 0.0
obstacle_max_range: 2.5
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: 0.3
always_send_full_costmap: True
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: "WHEELTEC.yaml"
map_saver:
ros__parameters:
use_sim_time: False
save_map_timeout: 5.0
free_thresh_default: 0.25
occupied_thresh_default: 0.65
map_subscribe_transient_local: True
planner_server:
ros__parameters:
expected_planner_frequency: 2.0
use_sim_time: False
planner_plugins: ["GridBased"]
GridBased:
plugin: "nav2_smac_planner/SmacPlannerHybrid"
tolerance: 0.25
downsample_costmap: false
downsampling_factor: 1
allow_unknown: false
max_iterations: 1000000
max_on_approach_iterations: 1000
max_planning_time: 5.0
motion_model_for_search: "DUBIN"
cost_travel_multiplier: 2.0
angle_quantization_bins: 72
analytic_expansion_ratio: 3.5
analytic_expansion_max_cost: 200.0
minimum_turning_radius: 0.20
reverse_penalty: 1.2
change_penalty: 0.2
non_straight_penalty: 1.5
cost_penalty: 2.5
lookup_table_size: 20.0
cache_obstacle_heuristic: True
smooth_path: true
smoother:
max_iterations: 1000
w_smooth: 0.3
w_data: 0.2
tolerance: 1.0e-10
planner_server_rclcpp_node:
ros__parameters:
use_sim_time: False
smoother_server:
ros__parameters:
use_sim_time: True
smoother_plugins: ["simple_smoother"]
simple_smoother:
plugin: "nav2_smoother::SimpleSmoother"
tolerance: 0.1
max_its: 1000
do_refinement: True
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