seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests 5 Actions Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'origin/sl-perception/issue-478-costmaps'

Browse Source 
# Conflicts:
#	jetson/ros2_ws/src/saltybot_bringup/config/nav2_params.yaml
This commit is contained in:
sl-jetson 2026-03-06 11:43:11 -05:00
commit a3d3ea1471
1 changed files with 30 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

81
jetson/ros2_ws/src/saltybot_bringup/config/nav2_params.yaml
View File

@ -1,18 +1,18 @@
# Nav2 parameters — SaltyBot (Jetson Orin Nano Super / ROS2 Humble) # Nav2 parameters — SaltyBot (Jetson Orin Nano Super / ROS2 Humble)
# #
# Robot: differential-drive self-balancing two-wheeler # Robot: differential-drive self-balancing two-wheeler
# robot_radius: 0.22 m (0.4m x 0.4m footprint) # robot_radius: 0.15 m (~0.2m with margin)
# footprint: 0.4 x 0.4 m # footprint: 0.4 x 0.4 m
# max_vel_x: 0.3 m/s (conservative for FC + hoverboard ESC, Issue #475) # max_vel_x: 1.0 m/s
# max_vel_theta: 0.5 rad/s (conservative for FC + hoverboard ESC, Issue #475) # max_vel_theta: 1.5 rad/s
# #
# Localization: RTAB-Map (publishes /map + map→odom TF + /rtabmap/odom) # Localization: RTAB-Map (publishes /map + map→odom TF + /rtabmap/odom)
# → No AMCL, no map_server needed. # → No AMCL, no map_server needed.
# #
# Sensors (Issue #478 — Costmap configuration): # Sensors (Issue #478 — Costmap configuration):
# /scan — RPLIDAR A1M8 360° LaserScan (obstacle layer) # /scan — RPLIDAR A1M8 360° LaserScan (obstacle)
# /depth_scan — RealSense D435i depth_to_laserscan (obstacle layer) # /depth_scan — RealSense D435i depth_to_laserscan (obstacle)
# /camera/depth/color/points — RealSense D435i PointCloud2 (voxel layer, local only) # /camera/depth/color/points — RealSense D435i PointCloud2 (voxel)
# #
# Inflation: # Inflation:
# inflation_radius: 0.3m (robot_radius 0.15m + 0.15m padding) # inflation_radius: 0.3m (robot_radius 0.15m + 0.15m padding)
@ -108,8 +108,8 @@ controller_server:
progress_checker: progress_checker:
plugin: "nav2_controller::SimpleProgressChecker" plugin: "nav2_controller::SimpleProgressChecker"
required_movement_radius: 0.2 # Issue #479: Minimum 20cm movement to detect progress required_movement_radius: 0.5
movement_time_allowance: 10.0 # 10 second window to detect movement, then attempt recovery movement_time_allowance: 10.0
general_goal_checker: general_goal_checker:
stateful: true stateful: true
@ -120,14 +120,14 @@ controller_server:
FollowPath: FollowPath:
plugin: "dwb_core::DWBLocalPlanner" plugin: "dwb_core::DWBLocalPlanner"
debug_trajectory_details: false debug_trajectory_details: false
# Velocity limits (conservative for FC + hoverboard ESC, Issue #475) # Velocity limits
min_vel_x: -0.15 # allow limited reverse (half of max forward) min_vel_x: -0.25 # allow limited reverse
min_vel_y: 0.0 min_vel_y: 0.0
max_vel_x: 0.3 # conservative: 0.3 m/s linear max_vel_x: 1.0
max_vel_y: 0.0 max_vel_y: 0.0
max_vel_theta: 0.5 # conservative: 0.5 rad/s angular max_vel_theta: 1.5
min_speed_xy: 0.0 min_speed_xy: 0.0
max_speed_xy: 0.3 # match max_vel_x max_speed_xy: 1.0
min_speed_theta: 0.0 min_speed_theta: 0.0
# Acceleration limits (differential drive) # Acceleration limits (differential drive)
acc_lim_x: 2.5 acc_lim_x: 2.5
@ -185,8 +185,6 @@ smoother_server:
do_refinement: true do_refinement: true
# ── Behavior Server (recovery behaviors) ──────────────────────────────────── # ── Behavior Server (recovery behaviors) ────────────────────────────────────
# Issue #479: Recovery behaviors with conservative speed/distance limits for FC+Hoverboard
# ESC priority: E-stop (Issue #459) takes priority over any recovery behavior.
behavior_server: behavior_server:
ros__parameters: ros__parameters:
use_sim_time: false use_sim_time: false
@ -195,40 +193,25 @@ behavior_server:
local_footprint_topic: local_costmap/published_footprint local_footprint_topic: local_costmap/published_footprint
global_footprint_topic: global_costmap/published_footprint global_footprint_topic: global_costmap/published_footprint
cycle_frequency: 10.0 cycle_frequency: 10.0
behavior_plugins: ["spin", "backup", "wait"] behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
# Spin recovery: In-place 90° rotation to escape local deadlock
spin: spin:
plugin: "nav2_behaviors/Spin" plugin: "nav2_behaviors/Spin"
spin_dist: 1.5708 # 90 degrees (π/2 radians)
max_rotational_vel: 0.5 # Conservative: 0.5 rad/s for self-balancer stability
min_rotational_vel: 0.25 # Minimum angular velocity
rotational_acc_lim: 1.6 # Half of max (3.2) for smooth accel on self-balancer
time_allowance: 10.0 # max 10 seconds to complete 90° rotation
# Backup recovery: Reverse 0.3m at 0.1 m/s to escape obstacles
# Conservative for FC (Flux Capacitor) + Hoverboard ESC drivetrain
backup: backup:
plugin: "nav2_behaviors/BackUp" plugin: "nav2_behaviors/BackUp"
backup_dist: 0.3 # Reverse 0.3 meters (safe distance for deadlock escape) drive_on_heading:
backup_speed: 0.1 # Very conservative: 0.1 m/s reverse plugin: "nav2_behaviors/DriveOnHeading"
max_backup_vel: 0.15 # Absolute max reverse velocity
min_backup_vel: 0.05 # Minimum backup velocity threshold
time_allowance: 5.0 # max 5 seconds (0.3m at 0.1m/s = 3s normal)
# Wait recovery: Pause 5 seconds to let obstacles move away
wait: wait:
plugin: "nav2_behaviors/Wait" plugin: "nav2_behaviors/Wait"
wait_duration: 5.0 # 5 second pause assisted_teleop:
plugin: "nav2_behaviors/AssistedTeleop"
local_frame: odom local_frame: odom
global_frame: map global_frame: map
robot_base_frame: base_link robot_base_frame: base_link
transform_tolerance: 0.1 transform_tolerance: 0.1
simulate_ahead_time: 2.0 simulate_ahead_time: 2.0
max_rotational_vel: 0.5 # Conservative: self-balancer stability limit max_rotational_vel: 1.0
min_rotational_vel: 0.25 min_rotational_vel: 0.4
rotational_acc_lim: 1.6 # Half of main limit for recovery behaviors rotational_acc_lim: 3.2
# ── Waypoint Follower ──────────────────────────────────────────────────────── # ── Waypoint Follower ────────────────────────────────────────────────────────
waypoint_follower: waypoint_follower:
@ -243,15 +226,14 @@ waypoint_follower:
waypoint_pause_duration: 200 waypoint_pause_duration: 200
# ── Velocity Smoother ──────────────────────────────────────────────────────── # ── Velocity Smoother ────────────────────────────────────────────────────────
# Conservative speeds for FC + hoverboard ESC (Issue #475)
velocity_smoother: velocity_smoother:
ros__parameters: ros__parameters:
use_sim_time: false use_sim_time: false
smoothing_frequency: 20.0 smoothing_frequency: 20.0
scale_velocities: false scale_velocities: false
feedback: "OPEN_LOOP" feedback: "OPEN_LOOP"
max_velocity: [0.3, 0.0, 0.5] # conservative: 0.3 m/s linear, 0.5 rad/s angular max_velocity: [1.0, 0.0, 1.5]
min_velocity: [-0.15, 0.0, -0.5] min_velocity: [-0.25, 0.0, -1.5]
max_accel: [2.5, 0.0, 3.2] max_accel: [2.5, 0.0, 3.2]
max_decel: [-2.5, 0.0, -3.2] max_decel: [-2.5, 0.0, -3.2]
odom_topic: /rtabmap/odom odom_topic: /rtabmap/odom
@ -269,13 +251,11 @@ local_costmap:
global_frame: odom global_frame: odom
robot_base_frame: base_link robot_base_frame: base_link
rolling_window: true rolling_window: true
width: 3 width: 3 # 3m x 3m rolling window
height: 3 height: 3
resolution: 0.05 resolution: 0.05
robot_radius: 0.22 robot_radius: 0.15
# Footprint: [x, y] in base_link frame, in counterclockwise order footprint: "[[-0.2, -0.2], [-0.2, 0.2], [0.2, 0.2], [0.2, -0.2]]" # 0.4m x 0.4m
# Robot footprint ~0.4m x 0.4m, with 2m lookahead buffer for controller stability
footprint: "[[-0.2, -0.2], [-0.2, 0.2], [0.2, 0.2], [0.2, -0.2]]"
plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"] plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"]
obstacle_layer: obstacle_layer:
@ -330,7 +310,7 @@ local_costmap:
inflation_layer: inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer" plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 3.0 cost_scaling_factor: 3.0
inflation_radius: 0.3 # robot_radius (0.15) + 0.15m padding for safety inflation_radius: 0.3 # robot_radius (0.15) + 0.15m padding
always_send_full_costmap: false always_send_full_costmap: false
@ -343,9 +323,8 @@ global_costmap:
publish_frequency: 1.0 publish_frequency: 1.0
global_frame: map global_frame: map
robot_base_frame: base_link robot_base_frame: base_link
robot_radius: 0.22 robot_radius: 0.15
# Footprint: [x, y] in base_link frame, in counterclockwise order footprint: "[[-0.2, -0.2], [-0.2, 0.2], [0.2, 0.2], [0.2, -0.2]]" # 0.4m x 0.4m
footprint: "[[-0.2, -0.2], [-0.2, 0.2], [0.2, 0.2], [0.2, -0.2]]"
resolution: 0.05 resolution: 0.05
track_unknown_space: true track_unknown_space: true
plugins: ["static_layer", "obstacle_layer", "inflation_layer"] plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
@ -395,7 +374,7 @@ global_costmap:
inflation_layer: inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer" plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 3.0 cost_scaling_factor: 3.0
inflation_radius: 0.3 # robot_radius (0.15) + 0.15m padding for safety inflation_radius: 0.3 # robot_radius (0.15) + 0.15m padding
always_send_full_costmap: false always_send_full_costmap: false