diff --git a/jetson/config/nav2_params.yaml b/jetson/config/nav2_params.yaml
new file mode 100644
index 0000000..dfda899
--- /dev/null
+++ b/jetson/config/nav2_params.yaml
@@ -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
diff --git a/jetson/docker-compose.yml b/jetson/docker-compose.yml
index 4c6c096..f6ef8d4 100644
--- a/jetson/docker-compose.yml
+++ b/jetson/docker-compose.yml
@@ -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
diff --git a/jetson/ros2_ws/src/saltybot_bringup/behavior_trees/navigate_to_pose_with_recovery.xml b/jetson/ros2_ws/src/saltybot_bringup/behavior_trees/navigate_to_pose_with_recovery.xml
new file mode 100644
index 0000000..369caf6
--- /dev/null
+++ b/jetson/ros2_ws/src/saltybot_bringup/behavior_trees/navigate_to_pose_with_recovery.xml
@@ -0,0 +1,75 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/jetson/ros2_ws/src/saltybot_bringup/launch/nav2.launch.py b/jetson/ros2_ws/src/saltybot_bringup/launch/nav2.launch.py
new file mode 100644
index 0000000..efb3433
--- /dev/null
+++ b/jetson/ros2_ws/src/saltybot_bringup/launch/nav2.launch.py
@@ -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])
diff --git a/jetson/ros2_ws/src/saltybot_bringup/package.xml b/jetson/ros2_ws/src/saltybot_bringup/package.xml
index d22c777..5083f62 100644
--- a/jetson/ros2_ws/src/saltybot_bringup/package.xml
+++ b/jetson/ros2_ws/src/saltybot_bringup/package.xml
@@ -11,6 +11,8 @@
realsense2_camera
realsense2_description
slam_toolbox
+ rtabmap_ros
+ nav2_bringup
robot_state_publisher
tf2_ros
diff --git a/jetson/ros2_ws/src/saltybot_bringup/setup.py b/jetson/ros2_ws/src/saltybot_bringup/setup.py
index 1b97221..507c836 100644
--- a/jetson/ros2_ws/src/saltybot_bringup/setup.py
+++ b/jetson/ros2_ws/src/saltybot_bringup/setup.py
@@ -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,
diff --git a/projects/saltybot/SLAM-SETUP-PLAN.md b/projects/saltybot/SLAM-SETUP-PLAN.md
index ddccb88..3a0d7b0 100644
--- a/projects/saltybot/SLAM-SETUP-PLAN.md
+++ b/projects/saltybot/SLAM-SETUP-PLAN.md
@@ -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) |
---