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

Merge pull request 'feat: Nav2 path planning + obstacle avoidance (Phase 2b)' (#49) from sl-perception/nav2-integration into main

Browse Source
This commit is contained in:
seb 2026-02-28 22:58:49 -05:00
commit 3f627ac3c8
7 changed files with 525 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

351
jetson/config/nav2_params.yaml Normal file
View File

@ -0,0 +1,351 @@
# Nav2 parameters — SaltyBot (Jetson Orin Nano Super / ROS2 Humble)
#
# Robot: differential-drive self-balancing two-wheeler
# robot_radius: 0.15 m
# max_vel_x: 1.0 m/s
# max_vel_theta: 1.5 rad/s
#
# Localization: RTAB-Map (publishes /map + map→odom TF + /rtabmap/odom)
# → No AMCL, no map_server needed.
#
# Sensors:
# /scan — RPLIDAR A1M8 (obstacle layer)
# /camera/depth/color/points — RealSense D435i (voxel layer)
#
# Output: /cmd_vel (Twist) — STM32 bridge consumes this topic.
bt_navigator:
ros__parameters:
use_sim_time: false
global_frame: map
robot_base_frame: base_link
odom_topic: /rtabmap/odom
bt_loop_duration: 10
default_server_timeout: 20
wait_for_service_timeout: 1000
action_server_result_timeout: 900.0
navigators: ['navigate_to_pose', 'navigate_through_poses']
navigate_to_pose:
plugin: "nav2_bt_navigator/NavigateToPoseNavigator"
navigate_through_poses:
plugin: "nav2_bt_navigator/NavigateThroughPosesNavigator"
# default_bt_xml_filename is set at launch time via nav2.launch.py
error_code_names:
- compute_path_error_code
- follow_path_error_code
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_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
bt_navigator_navigate_through_poses_rclcpp_node:
ros__parameters:
use_sim_time: false
bt_navigator_navigate_to_pose_rclcpp_node:
ros__parameters:
use_sim_time: false
# ── Controller Server (DWB — local trajectory follower) ─────────────────────
controller_server:
ros__parameters:
use_sim_time: false
controller_frequency: 10.0 # Hz — Orin can handle 10Hz easily
min_x_velocity_threshold: 0.001
min_y_velocity_threshold: 0.5 # not holonomic — effectively disabled
min_theta_velocity_threshold: 0.001
failure_tolerance: 0.3
progress_checker_plugin: "progress_checker"
goal_checker_plugins: ["general_goal_checker"]
controller_plugins: ["FollowPath"]
progress_checker:
plugin: "nav2_controller::SimpleProgressChecker"
required_movement_radius: 0.5
movement_time_allowance: 10.0
general_goal_checker:
stateful: true
plugin: "nav2_controller::SimpleGoalChecker"
xy_goal_tolerance: 0.25
yaw_goal_tolerance: 0.25
FollowPath:
plugin: "dwb_core::DWBLocalPlanner"
debug_trajectory_details: false
# Velocity limits
min_vel_x: -0.25 # allow limited reverse
min_vel_y: 0.0
max_vel_x: 1.0
max_vel_y: 0.0
max_vel_theta: 1.5
min_speed_xy: 0.0
max_speed_xy: 1.0
min_speed_theta: 0.0
# Acceleration limits (differential drive)
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
# Trajectory sampling
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
# ── Planner Server (NavFn global path planner) ───────────────────────────────
planner_server:
ros__parameters:
use_sim_time: false
expected_planner_frequency: 20.0
planner_plugins: ["GridBased"]
GridBased:
plugin: "nav2_navfn_planner/NavfnPlanner"
tolerance: 0.5
use_astar: false
allow_unknown: true
# ── Smoother Server ──────────────────────────────────────────────────────────
smoother_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: true
# ── Behavior Server (recovery behaviors) ────────────────────────────────────
behavior_server:
ros__parameters:
use_sim_time: false
local_costmap_topic: local_costmap/costmap_raw
global_costmap_topic: global_costmap/costmap_raw
local_footprint_topic: local_costmap/published_footprint
global_footprint_topic: global_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"
local_frame: odom
global_frame: map
robot_base_frame: base_link
transform_tolerance: 0.1
simulate_ahead_time: 2.0
max_rotational_vel: 1.0
min_rotational_vel: 0.4
rotational_acc_lim: 3.2
# ── Waypoint Follower ────────────────────────────────────────────────────────
waypoint_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: 200
# ── Velocity Smoother ────────────────────────────────────────────────────────
velocity_smoother:
ros__parameters:
use_sim_time: false
smoothing_frequency: 20.0
scale_velocities: false
feedback: "OPEN_LOOP"
max_velocity: [1.0, 0.0, 1.5]
min_velocity: [-0.25, 0.0, -1.5]
max_accel: [2.5, 0.0, 3.2]
max_decel: [-2.5, 0.0, -3.2]
odom_topic: /rtabmap/odom
odom_duration: 0.1
deadband_velocity: [0.0, 0.0, 0.0]
velocity_timeout: 1.0
# ── Local Costmap ────────────────────────────────────────────────────────────
local_costmap:
local_costmap:
ros__parameters:
use_sim_time: false
update_frequency: 10.0 # Hz — Orin can sustain 10Hz
publish_frequency: 5.0
global_frame: odom
robot_base_frame: base_link
rolling_window: true
width: 3
height: 3
resolution: 0.05
robot_radius: 0.15
plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"]
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: true
observation_sources: scan
scan:
topic: /scan
max_obstacle_height: 0.80
clearing: true
marking: true
data_type: "LaserScan"
raytrace_max_range: 8.0
raytrace_min_range: 0.0
obstacle_max_range: 7.5
obstacle_min_range: 0.0
voxel_layer:
plugin: "nav2_costmap_2d::VoxelLayer"
enabled: true
publish_voxel_map: false
origin_z: 0.0
z_resolution: 0.05
z_voxels: 16
max_obstacle_height: 0.80
mark_threshold: 0
observation_sources: depth_camera
depth_camera:
topic: /camera/depth/color/points
min_obstacle_height: 0.05 # above floor level
max_obstacle_height: 0.80
marking: true
clearing: true
data_type: "PointCloud2"
raytrace_max_range: 4.0
raytrace_min_range: 0.0
obstacle_max_range: 3.5
obstacle_min_range: 0.0
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 3.0
inflation_radius: 0.55 # robot_radius (0.15) + 0.40m padding
always_send_full_costmap: false
# ── Global Costmap ───────────────────────────────────────────────────────────
global_costmap:
global_costmap:
ros__parameters:
use_sim_time: false
update_frequency: 5.0
publish_frequency: 1.0
global_frame: map
robot_base_frame: base_link
robot_radius: 0.15
resolution: 0.05
track_unknown_space: true
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
static_layer:
plugin: "nav2_costmap_2d::StaticLayer"
map_subscribe_transient_local: true # for RTAB-Map's transient-local /map
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: true
observation_sources: scan
scan:
topic: /scan
max_obstacle_height: 0.80
clearing: true
marking: true
data_type: "LaserScan"
raytrace_max_range: 8.0
raytrace_min_range: 0.0
obstacle_max_range: 7.5
obstacle_min_range: 0.0
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 3.0
inflation_radius: 0.55
always_send_full_costmap: false
# ── Map Server / Map Saver (not used — RTAB-Map provides /map) ───────────────
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: true

56
jetson/docker-compose.yml
View File

@ -1,8 +1,8 @@
version: "3.8"
# Jetson Nano — ROS2 Humble SLAM stack
# Jetson Orin Nano Super — ROS2 Humble SLAM stack
# Run with: docker compose up -d
# Requires: NVIDIA Container Runtime (nvidia-docker2) on host
# Requires: NVIDIA Container Toolkit (JetPack 6) on host
services:
@ -29,24 +29,28 @@ services:
# X11 socket for RViz2
- /tmp/.X11-unix:/tmp/.X11-unix:rw
# ROS2 workspace (host-mounted for live dev)
# Mount src/ subdirectory so host structure mirrors container /ros2_ws/src/
- ./ros2_ws/src:/ros2_ws/src:rw
# Persistent SLAM maps
- saltybot-maps:/maps
# Persistent SLAM maps on NVMe
- /mnt/nvme/saltybot/maps:/maps
# NVMe data volume
- /mnt/nvme/saltybot:/data:rw
# Config files
- ./config:/config:ro
devices:
# RPLIDAR A1M8 — typically /dev/ttyUSB0
- /dev/ttyUSB0:/dev/ttyUSB0
# STM32 UART bridge — typically /dev/ttyUSB1 or /dev/ttyACM0
- /dev/ttyUSB1:/dev/ttyUSB1
# RPLIDAR A1M8 — stable symlink via udev
- /dev/rplidar:/dev/rplidar
# STM32 USB CDC bridge — stable symlink via udev
- /dev/stm32-bridge:/dev/stm32-bridge
# RealSense D435i — USB3 device, needs udev rules
- /dev/bus/usb:/dev/bus/usb
# I2C bus (Jetson i2c-1 = pins 3/5)
- /dev/i2c-1:/dev/i2c-1
# GPIO (via Jetson.GPIO)
- /sys/class/gpio:/sys/class/gpio
# I2C bus (Orin Nano i2c-7 = 40-pin header pins 3/5)
- /dev/i2c-7:/dev/i2c-7
# CSI cameras via V4L2
- /dev/video0:/dev/video0
- /dev/video2:/dev/video2
- /dev/video4:/dev/video4
- /dev/video6:/dev/video6
command: >
bash -c "
@ -131,6 +135,32 @@ services:
-p baud_rate:=921600
"
# ── Nav2 autonomous navigation stack ────────────────────────────────────────
saltybot-nav2:
image: saltybot/ros2-humble:jetson-orin
build:
context: .
dockerfile: Dockerfile
container_name: saltybot-nav2
restart: unless-stopped
runtime: nvidia
network_mode: host
depends_on:
- saltybot-ros2 # RTAB-Map + sensors must be running first
environment:
- NVIDIA_VISIBLE_DEVICES=all
- NVIDIA_DRIVER_CAPABILITIES=all
- RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
- ROS_DOMAIN_ID=42
volumes:
- ./ros2_ws/src:/ros2_ws/src:rw
- ./config:/config:ro
command: >
bash -c "
source /opt/ros/humble/setup.bash &&
ros2 launch saltybot_bringup nav2.launch.py
"
volumes:
saltybot-maps:
driver: local

75
jetson/ros2_ws/src/saltybot_bringup/behavior_trees/navigate_to_pose_with_recovery.xml Normal file
View File

@ -0,0 +1,75 @@
<?xml version="1.0"?>
<!--
navigate_to_pose_with_recovery.xml — SaltyBot Nav2 behavior tree
Format: BehaviorTree.CPP v4 (ROS2 Humble)
Recovery sequence (outer RecoveryNode, 6 retries):
1. Clear local + global costmaps
2. Spin 90° (1.57 rad)
3. Wait 5 s
4. Back up 0.30 m
Each recovery runs round-robin until navigation succeeds or retries exhausted.
-->
<root BTCPP_format="4">
<BehaviorTree ID="NavigateToPoseWithRecovery">
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
<!-- ── Normal navigation: replan at 1 Hz, follow path ── -->
<PipelineSequence name="NavigateWithReplanning">
<RateController hz="1.0">
<RecoveryNode number_of_retries="1" name="ComputePathRecovery">
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
<ReactiveFallback name="ComputePathRecoveryFallback">
<GoalUpdated/>
<ClearEntireCostmap
name="ClearGlobalCostmap-Context"
service_name="global_costmap/clear_entirely_global_costmap"/>
</ReactiveFallback>
</RecoveryNode>
</RateController>
<RecoveryNode number_of_retries="1" name="FollowPathRecovery">
<FollowPath path="{path}" controller_id="FollowPath"/>
<ReactiveFallback name="FollowPathRecoveryFallback">
<GoalUpdated/>
<ClearEntireCostmap
name="ClearLocalCostmap-Context"
service_name="local_costmap/clear_entirely_local_costmap"/>
</ReactiveFallback>
</RecoveryNode>
</PipelineSequence>
<!-- ── Recovery behaviors (round-robin) ── -->
<ReactiveFallback name="RecoveryFallback">
<GoalUpdated/>
<RoundRobin name="RecoveryActions">
<!-- 1. Clear both costmaps -->
<Sequence name="ClearingActions">
<ClearEntireCostmap
name="ClearLocalCostmap-Subtask"
service_name="local_costmap/clear_entirely_local_costmap"/>
<ClearEntireCostmap
name="ClearGlobalCostmap-Subtask"
service_name="global_costmap/clear_entirely_global_costmap"/>
</Sequence>
<!-- 2. Spin 90° to detect surroundings -->
<Spin spin_dist="1.57"/>
<!-- 3. Wait 5 s (e.g. person clears path) -->
<Wait wait_duration="5"/>
<!-- 4. Back up 0.30 m at 0.15 m/s -->
<BackUp backup_dist="0.30" backup_speed="0.15"/>
</RoundRobin>
</ReactiveFallback>
</RecoveryNode>
</BehaviorTree>
</root>

50
jetson/ros2_ws/src/saltybot_bringup/launch/nav2.launch.py Normal file
View File

@ -0,0 +1,50 @@
"""
nav2.launch.py — Nav2 autonomous navigation stack for SaltyBot
Starts the full Nav2 navigation stack (controllers, planners, behavior server,
BT navigator, velocity smoother, lifecycle managers).
Localization is provided by RTAB-Map (slam_rtabmap.launch.py must be running):
/map — OccupancyGrid (static global costmap layer)
/rtabmap/odom — Odometry (velocity smoother + controller feedback)
TF map→odom — published by RTAB-Map
Output:
/cmd_vel — consumed by saltybot_bridge → STM32 over UART
Run sequence on Orin:
1. docker compose up saltybot-ros2 # RTAB-Map + sensors
2. docker compose up saltybot-nav2 # this launch file
"""
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
def generate_launch_description():
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
bringup_dir = get_package_share_directory('saltybot_bringup')
nav2_params_file = os.path.join(bringup_dir, 'config', 'nav2_params.yaml')
bt_xml_file = os.path.join(
bringup_dir, 'behavior_trees', 'navigate_to_pose_with_recovery.xml'
)
nav2_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(nav2_bringup_dir, 'launch', 'navigation_launch.py')
),
launch_arguments={
'use_sim_time': 'false',
'autostart': 'true',
'params_file': nav2_params_file,
'default_bt_xml_filename': bt_xml_file,
# RTAB-Map publishes /map with transient_local QoS — must match
'map_subscribe_transient_local': 'true',
}.items(),
)
return LaunchDescription([nav2_launch])

2
jetson/ros2_ws/src/saltybot_bringup/package.xml
View File

@ -11,6 +11,8 @@
<exec_depend>realsense2_camera</exec_depend>
<exec_depend>realsense2_description</exec_depend>
<exec_depend>slam_toolbox</exec_depend>
<exec_depend>rtabmap_ros</exec_depend>
<exec_depend>nav2_bringup</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>
<exec_depend>tf2_ros</exec_depend>

2
jetson/ros2_ws/src/saltybot_bringup/setup.py
View File

@ -16,6 +16,8 @@ setup(
glob('launch/*.launch.py')),
(os.path.join('share', package_name, 'config'),
glob('config/*.yaml')),
(os.path.join('share', package_name, 'behavior_trees'),
glob('behavior_trees/*.xml')),
],
install_requires=['setuptools'],
zip_safe=True,

4
projects/saltybot/SLAM-SETUP-PLAN.md
View File

@ -90,8 +90,8 @@ Jetson Orin Nano Super (Ubuntu 22.04 / JetPack 6 / CUDA 12.x)
| Phase | Status | Description |
|-------|--------|-------------|
| 2a | ✅ Done (PR #17) | Sensor drivers — `saltybot_bringup` package |
| 2a+ | This PR | Orin update: Dockerfile JetPack 6, RTAB-Map launch + config |
| 2b | Pending | Nav2 integration (separate bead, after Phase 1 balance) |
| 2a+ | ✅ Done (PR #36) | Orin update: Dockerfile JetPack 6, RTAB-Map launch + config |
| 2b | ✅ Done (this PR) | Nav2 integration — path planning + obstacle avoidance |
| 2c | Pending | 4× IMX219 surround vision (new bead, after hardware arrives) |
---