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

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
+      # Persistent SLAM maps on NVMe
-      - saltybot-maps:/maps
+      - /mnt/nvme/saltybot/maps:/maps
+      # NVMe data volume
+      - /mnt/nvme/saltybot:/data:rw
       # Config files
       - ./config:/config:ro
 
     devices:
-      # RPLIDAR A1M8 — typically /dev/ttyUSB0
+      # RPLIDAR A1M8 — stable symlink via udev
-      - /dev/ttyUSB0:/dev/ttyUSB0
+      - /dev/rplidar:/dev/rplidar
-      # STM32 UART bridge — typically /dev/ttyUSB1 or /dev/ttyACM0
+      # STM32 USB CDC bridge — stable symlink via udev
-      - /dev/ttyUSB1:/dev/ttyUSB1
+      - /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)
+      # I2C bus (Orin Nano i2c-7 = 40-pin header pins 3/5)
-      - /dev/i2c-1:/dev/i2c-1
+      - /dev/i2c-7:/dev/i2c-7
-      # GPIO (via Jetson.GPIO)
+      # CSI cameras via V4L2
-      - /sys/class/gpio:/sys/class/gpio
+      - /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

jetson/ros2_ws/src/saltybot_bringup/behavior_trees/navigate_to_pose_with_recovery.xml

@@ -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>

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])

jetson/ros2_ws/src/saltybot_bringup/package.xml

@@ -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>
 

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,

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 |
+| 2a+ | ✅ Done (PR #36) | Orin update: Dockerfile JetPack 6, RTAB-Map launch + config |
-| 2b | Pending | Nav2 integration (separate bead, after Phase 1 balance) |
+| 2b | ✅ Done (this PR) | Nav2 integration — path planning + obstacle avoidance |
 | 2c | Pending | 4× IMX219 surround vision (new bead, after hardware arrives) |
 
 ---