Hello , I’m having some trouble using the nvblox nav2 plugin.I laucnh nvblox with a zed x succesfully and the static_map_slice topic is working as well, but when i launch the nav2 i get this error message:
$ ros2 launch minithor_navigation nav.launch.py
[INFO] [launch]: All log files can be found below /home/jetson/.ros/log/2025-03-11-14-16-53-937641-jetson-desktop-127684
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [controller_server-1]: process started with pid [127774]
[INFO] [smoother_server-2]: process started with pid [127776]
[INFO] [planner_server-3]: process started with pid [127778]
[INFO] [behavior_server-4]: process started with pid [127782]
[INFO] [bt_navigator-5]: process started with pid [127784]
[INFO] [waypoint_follower-6]: process started with pid [127786]
[INFO] [velocity_smoother-7]: process started with pid [127788]
[INFO] [lifecycle_manager-8]: process started with pid [127790]
[lifecycle_manager-8] [INFO] [1741699014.748368390] [lifecycle_manager_navigation]: Creating
[waypoint_follower-6] [INFO] [1741699014.772683154] [waypoint_follower]:
[lifecycle_manager-8] [INFO] [1741699014.785800943] [lifecycle_manager_navigation]: Creating and initializing lifecycle service clients
[waypoint_follower-6] waypoint_follower lifecycle node launched.
[waypoint_follower-6] Waiting on external lifecycle transitions to activate
[waypoint_follower-6] See Managed nodes for more information.
[controller_server-1] [INFO] [1741699014.874789784] [controller_server]:
[controller_server-1] controller_server lifecycle node launched.
[controller_server-1] Waiting on external lifecycle transitions to activate
[controller_server-1] See Managed nodes for more information.
[controller_server-1] [INFO] [1741699014.908334691] [controller_server]: Creating controller server
[waypoint_follower-6] [INFO] [1741699014.920911732] [waypoint_follower]: Creating
[controller_server-1] [INFO] [1741699014.949121054] [local_costmap.local_costmap]:
[controller_server-1] local_costmap lifecycle node launched.
[controller_server-1] Waiting on external lifecycle transitions to activate
[controller_server-1] See Managed nodes for more information.
[controller_server-1] [INFO] [1741699014.964953085] [local_costmap.local_costmap]: Creating Costmap
[planner_server-3] [INFO] [1741699015.002656333] [planner_server]:
[planner_server-3] planner_server lifecycle node launched.
[planner_server-3] Waiting on external lifecycle transitions to activate
[planner_server-3] See Managed nodes for more information.
[planner_server-3] [INFO] [1741699015.008978342] [planner_server]: Creating
[behavior_server-4] [INFO] [1741699015.181535796] [behavior_server]:
[behavior_server-4] behavior_server lifecycle node launched.
[behavior_server-4] Waiting on external lifecycle transitions to activate
[behavior_server-4] See Managed nodes for more information.
[bt_navigator-5] [INFO] [1741699015.289244706] [bt_navigator]:
[bt_navigator-5] bt_navigator lifecycle node launched.
[bt_navigator-5] Waiting on external lifecycle transitions to activate
[bt_navigator-5] See Managed nodes for more information.
[bt_navigator-5] [INFO] [1741699015.294058294] [bt_navigator]: Creating
[smoother_server-2] [INFO] [1741699015.344325366] [smoother_server]:
[smoother_server-2] smoother_server lifecycle node launched.
[smoother_server-2] Waiting on external lifecycle transitions to activate
[smoother_server-2] See Managed nodes for more information.
[smoother_server-2] [INFO] [1741699015.356519389] [smoother_server]: Creating smoother server
[velocity_smoother-7] [INFO] [1741699015.534558960] [velocity_smoother]:
[velocity_smoother-7] velocity_smoother lifecycle node launched.
[velocity_smoother-7] Waiting on external lifecycle transitions to activate
[velocity_smoother-7] See Managed nodes for more information.
[planner_server-3] [INFO] [1741699015.586762463] [global_costmap.global_costmap]:
[planner_server-3] global_costmap lifecycle node launched.
[planner_server-3] Waiting on external lifecycle transitions to activate
[planner_server-3] See Managed nodes for more information.
[planner_server-3] [INFO] [1741699015.596037567] [global_costmap.global_costmap]: Creating Costmap
[lifecycle_manager-8] [INFO] [1741699016.420422342] [lifecycle_manager_navigation]: Starting managed nodes bringup…
[lifecycle_manager-8] [INFO] [1741699016.420577193] [lifecycle_manager_navigation]: Configuring controller_server
[controller_server-1] [INFO] [1741699016.421617411] [controller_server]: Configuring controller interface
[controller_server-1] [INFO] [1741699016.421929002] [controller_server]: getting goal checker plugins…
[controller_server-1] [INFO] [1741699016.422047789] [controller_server]: Controller frequency set to 20.0000Hz
[controller_server-1] [INFO] [1741699016.422120655] [local_costmap.local_costmap]: Configuring
[controller_server-1] [INFO] [1741699016.448433515] [local_costmap.local_costmap]: Using plugin “static_layer”
[controller_server-1] [INFO] [1741699016.497321066] [local_costmap.local_costmap]: Subscribing to the map topic (/map) with transient local durability
[controller_server-1] [INFO] [1741699016.506026205] [local_costmap.local_costmap]: Initialized plugin “static_layer”
[controller_server-1] [INFO] [1741699016.506105694] [local_costmap.local_costmap]: Using plugin “obstacle_layer”
[controller_server-1] [INFO] [1741699016.515900747] [local_costmap.local_costmap]: Subscribed to Topics:
[controller_server-1] [INFO] [1741699016.516098960] [local_costmap.local_costmap]: Initialized plugin “obstacle_layer”
[controller_server-1] [INFO] [1741699016.516132049] [local_costmap.local_costmap]: Using plugin “inflation_layer”
[controller_server-1] [INFO] [1741699016.519070264] [local_costmap.local_costmap]: Initialized plugin “inflation_layer”
[controller_server-1] [INFO] [1741699016.595818841] [controller_server]: Created progress_checker : progress_checker of type nav2_controller::SimpleProgressChecker
[controller_server-1] [INFO] [1741699016.598721023] [controller_server]: Created goal checker : general_goal_checker of type nav2_controller::SimpleGoalChecker
[controller_server-1] [INFO] [1741699016.599615924] [controller_server]: Controller Server has general_goal_checker goal checkers available.
[controller_server-1] [INFO] [1741699016.613820972] [controller_server]: Created controller : FollowPath of type dwb_core::DWBLocalPlanner
[controller_server-1] [INFO] [1741699016.617915567] [controller_server]: Setting transform_tolerance to 0.200000
[controller_server-1] [INFO] [1741699016.668199440] [controller_server]: Using critic “RotateToGoal” (dwb_critics::RotateToGoalCritic)
[controller_server-1] [INFO] [1741699016.668918657] [controller_server]: Critic plugin initialized
[controller_server-1] [INFO] [1741699016.669084197] [controller_server]: Using critic “Oscillation” (dwb_critics::OscillationCritic)
[controller_server-1] [INFO] [1741699016.669464974] [controller_server]: Critic plugin initialized
[controller_server-1] [INFO] [1741699016.669580369] [controller_server]: Using critic “BaseObstacle” (dwb_critics::BaseObstacleCritic)
[controller_server-1] [INFO] [1741699016.669765845] [controller_server]: Critic plugin initialized
[controller_server-1] [INFO] [1741699016.669884984] [controller_server]: Using critic “GoalAlign” (dwb_critics::GoalAlignCritic)
[controller_server-1] [INFO] [1741699016.670206464] [controller_server]: Critic plugin initialized
[controller_server-1] [INFO] [1741699016.670308323] [controller_server]: Using critic “PathAlign” (dwb_critics::PathAlignCritic)
[controller_server-1] [INFO] [1741699016.670536232] [controller_server]: Critic plugin initialized
[controller_server-1] [INFO] [1741699016.670629098] [controller_server]: Using critic “PathDist” (dwb_critics::PathDistCritic)
[controller_server-1] [INFO] [1741699016.670885873] [controller_server]: Critic plugin initialized
[controller_server-1] [INFO] [1741699016.671056149] [controller_server]: Using critic “GoalDist” (dwb_critics::GoalDistCritic)
[controller_server-1] [INFO] [1741699016.671360860] [controller_server]: Critic plugin initialized
[controller_server-1] [INFO] [1741699016.671398717] [controller_server]: Controller Server has FollowPath controllers available.
[lifecycle_manager-8] [INFO] [1741699016.699630856] [lifecycle_manager_navigation]: Configuring smoother_server
[smoother_server-2] [INFO] [1741699016.700112884] [smoother_server]: Configuring smoother server
[smoother_server-2] [INFO] [1741699016.746151789] [smoother_server]: Created smoother : simple_smoother of type nav2_smoother::SimpleSmoother
[smoother_server-2] [INFO] [1741699016.751713748] [smoother_server]: Smoother Server has simple_smoother smoothers available.
[lifecycle_manager-8] [INFO] [1741699016.787719611] [lifecycle_manager_navigation]: Configuring planner_server
[planner_server-3] [INFO] [1741699016.789667434] [planner_server]: Configuring
[planner_server-3] [INFO] [1741699016.789774892] [global_costmap.global_costmap]: Configuring
[planner_server-3] [INFO] [1741699016.814173243] [global_costmap.global_costmap]: Using plugin “static_layer”
[planner_server-3] [INFO] [1741699016.829005889] [global_costmap.global_costmap]: Subscribing to the map topic (/map) with transient local durability
[planner_server-3] [INFO] [1741699016.835696419] [global_costmap.global_costmap]: Initialized plugin “static_layer”
[planner_server-3] [INFO] [1741699016.835833447] [global_costmap.global_costmap]: Using plugin “obstacle_layer”
[planner_server-3] [INFO] [1741699016.846285955] [global_costmap.global_costmap]: Subscribed to Topics:
[planner_server-3] [INFO] [1741699016.846567274] [global_costmap.global_costmap]: Initialized plugin “obstacle_layer”
[planner_server-3] [INFO] [1741699016.846652492] [global_costmap.global_costmap]: Using plugin “inflation_layer”
[planner_server-3] [INFO] [1741699016.853059847] [global_costmap.global_costmap]: Initialized plugin “inflation_layer”
[planner_server-3] [INFO] [1741699016.927609907] [planner_server]: Created global planner plugin GridBased of type nav2_navfn_planner/NavfnPlanner
[planner_server-3] [INFO] [1741699016.928735438] [planner_server]: Configuring plugin GridBased of type NavfnPlanner
[planner_server-3] [INFO] [1741699016.933051766] [planner_server]: Planner Server has GridBased planners available.
[lifecycle_manager-8] [INFO] [1741699016.984883228] [lifecycle_manager_navigation]: Configuring behavior_server
[behavior_server-4] [INFO] [1741699016.986981583] [behavior_server]: Configuring
[behavior_server-4] [INFO] [1741699017.026365480] [behavior_server]: Creating behavior plugin spin of type nav2_behaviors/Spin
[behavior_server-4] [INFO] [1741699017.033753626] [behavior_server]: Configuring spin
[behavior_server-4] [INFO] [1741699017.092640107] [behavior_server]: Creating behavior plugin backup of type nav2_behaviors/BackUp
[behavior_server-4] [INFO] [1741699017.098525401] [behavior_server]: Configuring backup
[behavior_server-4] [INFO] [1741699017.158026585] [behavior_server]: Creating behavior plugin drive_on_heading of type nav2_behaviors/DriveOnHeading
[behavior_server-4] [INFO] [1741699017.164512245] [behavior_server]: Configuring drive_on_heading
[behavior_server-4] [INFO] [1741699017.229462457] [behavior_server]: Creating behavior plugin assisted_teleop of type nav2_behaviors/AssistedTeleop
[behavior_server-4] [INFO] [1741699017.251821845] [behavior_server]: Configuring assisted_teleop
[behavior_server-4] [INFO] [1741699017.308263307] [behavior_server]: Creating behavior plugin wait of type nav2_behaviors/Wait
[behavior_server-4] [INFO] [1741699017.314282556] [behavior_server]: Configuring wait
[lifecycle_manager-8] [INFO] [1741699017.353045446] [lifecycle_manager_navigation]: Configuring bt_navigator
[bt_navigator-5] [INFO] [1741699017.353855354] [bt_navigator]: Configuring
[lifecycle_manager-8] [INFO] [1741699017.856676189] [lifecycle_manager_navigation]: Configuring waypoint_follower
[waypoint_follower-6] [INFO] [1741699017.858775056] [waypoint_follower]: Configuring
[waypoint_follower-6] [INFO] [1741699017.949669862] [waypoint_follower]: Created waypoint_task_executor : wait_at_waypoint of type nav2_waypoint_follower::WaitAtWaypoint
[lifecycle_manager-8] [INFO] [1741699017.956455850] [lifecycle_manager_navigation]: Configuring velocity_smoother
[velocity_smoother-7] [INFO] [1741699017.957447586] [velocity_smoother]: Configuring velocity smoother
[lifecycle_manager-8] [INFO] [1741699017.970421340] [lifecycle_manager_navigation]: Activating controller_server
[controller_server-1] [INFO] [1741699017.970879975] [controller_server]: Activating
[controller_server-1] [INFO] [1741699017.970967401] [local_costmap.local_costmap]: Activating
[controller_server-1] [INFO] [1741699017.970996586] [local_costmap.local_costmap]: Checking transform
[controller_server-1] [INFO] [1741699017.971167822] [local_costmap.local_costmap]: start
[controller_server-1] [WARN] [1741699018.171644608] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[controller_server-1] [INFO] [1741699018.221409267] [controller_server]: Creating bond (controller_server) to lifecycle manager.
[lifecycle_manager-8] [INFO] [1741699018.342103707] [lifecycle_manager_navigation]: Server controller_server connected with bond.
[lifecycle_manager-8] [INFO] [1741699018.342227614] [lifecycle_manager_navigation]: Activating smoother_server
[smoother_server-2] [INFO] [1741699018.342819852] [smoother_server]: Activating
[smoother_server-2] [INFO] [1741699018.342893838] [smoother_server]: Creating bond (smoother_server) to lifecycle manager.
[controller_server-1] [WARN] [1741699018.371304573] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[lifecycle_manager-8] [INFO] [1741699018.457619237] [lifecycle_manager_navigation]: Server smoother_server connected with bond.
[lifecycle_manager-8] [INFO] [1741699018.457731720] [lifecycle_manager_navigation]: Activating planner_server
[planner_server-3] [INFO] [1741699018.458387736] [planner_server]: Activating
[planner_server-3] [INFO] [1741699018.458487706] [global_costmap.global_costmap]: Activating
[planner_server-3] [INFO] [1741699018.458515803] [global_costmap.global_costmap]: Checking transform
[planner_server-3] [INFO] [1741699018.458729632] [global_costmap.global_costmap]: start
[planner_server-3] [WARN] [1741699018.460082369] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[planner_server-3] [INFO] [1741699018.508900222] [planner_server]: Activating plugin GridBased of type NavfnPlanner
[planner_server-3] [INFO] [1741699018.510899726] [planner_server]: Creating bond (planner_server) to lifecycle manager.
[controller_server-1] [WARN] [1741699018.571346788] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[lifecycle_manager-8] [INFO] [1741699018.629174043] [lifecycle_manager_navigation]: Server planner_server connected with bond.
[lifecycle_manager-8] [INFO] [1741699018.629267997] [lifecycle_manager_navigation]: Activating behavior_server
[behavior_server-4] [INFO] [1741699018.629905773] [behavior_server]: Activating
[behavior_server-4] [INFO] [1741699018.629983247] [behavior_server]: Activating spin
[behavior_server-4] [INFO] [1741699018.630015599] [behavior_server]: Activating backup
[behavior_server-4] [INFO] [1741699018.630036912] [behavior_server]: Activating drive_on_heading
[behavior_server-4] [INFO] [1741699018.630054128] [behavior_server]: Activating assisted_teleop
[behavior_server-4] [INFO] [1741699018.630068625] [behavior_server]: Activating wait
[behavior_server-4] [INFO] [1741699018.630090577] [behavior_server]: Creating bond (behavior_server) to lifecycle manager.
[planner_server-3] [WARN] [1741699018.660121672] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[lifecycle_manager-8] [INFO] [1741699018.741393398] [lifecycle_manager_navigation]: Server behavior_server connected with bond.
[lifecycle_manager-8] [INFO] [1741699018.741491864] [lifecycle_manager_navigation]: Activating bt_navigator
[bt_navigator-5] [INFO] [1741699018.742473456] [bt_navigator]: Activating
[controller_server-1] [WARN] [1741699018.771779125] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[planner_server-3] [WARN] [1741699018.860232816] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[controller_server-1] [WARN] [1741699018.971340656] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[bt_navigator-5] [INFO] [1741699019.046104416] [bt_navigator]: Creating bond (bt_navigator) to lifecycle manager.
[planner_server-3] [WARN] [1741699019.060028753] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[lifecycle_manager-8] [INFO] [1741699019.163624859] [lifecycle_manager_navigation]: Server bt_navigator connected with bond.
[lifecycle_manager-8] [INFO] [1741699019.163777887] [lifecycle_manager_navigation]: Activating waypoint_follower
[waypoint_follower-6] [INFO] [1741699019.164449487] [waypoint_follower]: Activating
[waypoint_follower-6] [INFO] [1741699019.164531121] [waypoint_follower]: Creating bond (waypoint_follower) to lifecycle manager.
[controller_server-1] [WARN] [1741699019.171378679] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[planner_server-3] [WARN] [1741699019.260032919] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[lifecycle_manager-8] [INFO] [1741699019.280674154] [lifecycle_manager_navigation]: Server waypoint_follower connected with bond.
[lifecycle_manager-8] [INFO] [1741699019.280801645] [lifecycle_manager_navigation]: Activating velocity_smoother
[velocity_smoother-7] [INFO] [1741699019.281844807] [velocity_smoother]: Activating
[velocity_smoother-7] [INFO] [1741699019.282341843] [velocity_smoother]: Creating bond (velocity_smoother) to lifecycle manager.
[controller_server-1] [WARN] [1741699019.371389757] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[lifecycle_manager-8] [INFO] [1741699019.408972682] [lifecycle_manager_navigation]: Server velocity_smoother connected with bond.
[lifecycle_manager-8] [INFO] [1741699019.409108045] [lifecycle_manager_navigation]: Managed nodes are active
[lifecycle_manager-8] [INFO] [1741699019.409143118] [lifecycle_manager_navigation]: Creating bond timer…
[planner_server-3] [WARN] [1741699019.460029949] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[controller_server-1] [WARN] [1741699019.571334913] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[planner_server-3] [WARN] [1741699019.659976673] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[controller_server-1] [WARN] [1741699019.771344647] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[planner_server-3] [WARN] [1741699019.860071945] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[controller_server-1] [WARN] [1741699019.971346925] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[controller_server-1] [WARN] [1741699019.971424879] [local_costmap.local_costmap]: Can’t update static costmap layer, no map received
[planner_server-3] [WARN] [1741699020.060038990] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[controller_server-1] [WARN] [1741699020.171339475] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[planner_server-3] [WARN] [1741699020.260066677] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[planner_server-3] [WARN] [1741699020.260216952] [global_costmap.global_costmap]: Can’t update static costmap layer, no map received
[controller_server-1] [WARN] [1741699020.371508412] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[planner_server-3] [WARN] [1741699020.460022169] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[controller_server-1] [WARN] [1741699020.571386751] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[planner_server-3] [WARN] [1741699020.660019839] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[controller_server-1] [WARN] [1741699020.771349252] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[planner_server-3] [WARN] [1741699020.860084070] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[controller_server-1] [WARN] [1741699020.971475917] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[planner_server-3] [WARN] [1741699021.059988810] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[controller_server-1] [WARN] [1741699021.171340079] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[planner_server-3] [WARN] [1741699021.260005264] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[controller_server-1] [WARN] [1741699021.371291795] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[planner_server-3] [WARN] [1741699021.460269596] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[controller_server-1] [WARN] [1741699021.571337210] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[planner_server-3] [WARN] [1741699021.660304194] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[controller_server-1] [WARN] [1741699021.771516004] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[planner_server-3] [WARN] [1741699021.860088995] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[controller_server-1] [WARN] [1741699021.971345158] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[controller_server-1] [WARN] [1741699021.971412071] [local_costmap.local_costmap]: Can’t update static costmap layer, no map received
[planner_server-3] [WARN] [1741699022.060045831] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[controller_server-1] [WARN] [1741699022.171427085] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[planner_server-3] [WARN] [1741699022.259958219] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[planner_server-3] [WARN] [1741699022.260023276] [global_costmap.global_costmap]: Can’t update static costmap layer, no map received
[controller_server-1] [WARN] [1741699022.371333648] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[planner_server-3] [WARN] [1741699022.460068595] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[controller_server-1] [WARN] [1741699022.571346230] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[planner_server-3] [WARN] [1741699022.660041208] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
I am launching navigation.launch.py with this params file , i tried to implement the plugins like it is in nova carter navigation package but i cant seem to get it to work, I once had nvblox with nav working but that was back on isaac ros 2 and also in a different rig .
The params file (to be sure pasting the whole of it):
amcl:
ros__parameters:
use_sim_time: False
alpha1: 0.2
alpha2: 0.2
alpha3: 0.2
alpha4: 0.2
alpha5: 0.2
base_frame_id: “base_footprint”
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”
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
bt_navigator:
ros__parameters:
use_sim_time: False
global_frame: map
robot_base_frame: base_link
odom_topic: /odom
bt_loop_duration: 10
default_server_timeout: 20
wait_for_service_timeout: 1000
# ‘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_assisted_teleop_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_goal_updated_controller_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_assisted_teleop_cancel_bt_node
- nav2_drive_on_heading_cancel_bt_node
- nav2_is_battery_charging_condition_bt_nodebt_navigator_navigate_through_poses_rclcpp_node:
ros__parameters:
use_sim_time: Falsebt_navigator_navigate_to_pose_rclcpp_node:
ros__parameters:
use_sim_time: Falsecontroller_server:
ros__parameters:
use_sim_time: False
controller_frequency: 20.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_plugins: [“general_goal_checker”] # “precise_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 #precise_goal_checker: # plugin: "nav2_controller::SimpleGoalChecker" # xy_goal_tolerance: 0.25 # yaw_goal_tolerance: 0.25 # stateful: True general_goal_checker: stateful: True plugin: "nav2_controller::SimpleGoalChecker" xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 # DWB parameters FollowPath: plugin: "dwb_core::DWBLocalPlanner" debug_trajectory_details: True min_vel_x: 0.0 min_vel_y: 0.0 max_vel_x: 0.26 max_vel_y: 0.0 max_vel_theta: 1.0 min_speed_xy: 0.0 max_speed_xy: 0.26 min_speed_theta: 0.0 acc_lim_x: 2.5 acc_lim_y: 0.0 acc_lim_theta: 3.2 decel_lim_x: -2.5 decel_lim_y: 0.0 decel_lim_theta: -3.2 vx_samples: 20 vy_samples: 5 vtheta_samples: 20 sim_time: 1.7 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: 5.0 RotateToGoal.lookahead_time: -1.0 local_costmap: local_costmap: ros__parameters: update_frequency: 5.0 publish_frequency: 2.0 global_frame: odom robot_base_frame: base_link use_sim_time: False rolling_window: true width: 5 height: 5 resolution: 0.05 robot_radius: 0.27 always_send_full_costmap: True plugins: ["nvblox_layer", "inflation_layer"] nvblox_layer: plugin: "nvblox::nav2::NvbloxCostmapLayer" enabled: True nav2_costmap_global_frame: odom nvblox_map_slice_topic: "/nvblox_node/static_map_slice" convert_to_binary_costmap: True inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" enabled: True cost_scaling_factor: 3.0 inflation_radius: 0.35 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: False robot_radius: 0.27 resolution: 0.05 width: 50 height: 50 origin_x: -25.0 origin_y: -25.0 track_unknown_space: False mark_threshold: 2 always_send_full_costmap: True plugins: ["nvblox_layer", "inflation_layer"] static_map_layer: plugin: "nav2_costmap_2d::StaticLayer" map_subscribe_transient_local: True enabled: true nvblox_layer: plugin: "nvblox::nav2::NvbloxCostmapLayer" enabled: True nav2_costmap_global_frame: map nvblox_map_slice_topic: "/nvblox_node/static_map_slice" convert_to_binary_costmap: True inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" enabled: True cost_scaling_factor: 10.0 inflation_radius: 0.4
map_server:
ros__parameters:
use_sim_time: False
yaml_filename: “”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: Trueplanner_server:
ros__parameters:
expected_planner_frequency: 20.0
use_sim_time: False
planner_plugins: [“GridBased”]
GridBased:
plugin: “nav2_navfn_planner/NavfnPlanner”
tolerance: 0.5
use_astar: false
allow_unknown: truesmoother_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: Truebehavior_server:
ros__parameters:
costmap_topic: local_costmap/costmap_raw
footprint_topic: local_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”
global_frame: odom
robot_base_frame: base_link
transform_tolerance: 0.1
use_sim_time: False
simulate_ahead_time: 2.0
max_rotational_vel: 1.0
min_rotational_vel: 0.4
rotational_acc_lim: 3.2robot_state_publisher:
ros__parameters:
use_sim_time: Falsewaypoint_follower:
ros__parameters:
use_sim_time: False
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: 200velocity_smoother:
ros__parameters:
use_sim_time: False
smoothing_frequency: 20.0
scale_velocities: False
feedback: “OPEN_LOOP”
max_velocity: [0.3, 0.0, 1.0]
min_velocity: [-0.3, 0.0, -1.0]
max_accel: [2.5, 0.0, 3.2]
max_decel: [-2.5, 0.0, -3.2]
odom_topic: “odom”
odom_duration: 0.1
deadband_velocity: [0.0, 0.0, 0.0]
velocity_timeout: 1.0