Merge pull request 'feat: outdoor nav — OSM routing + geofence (#59)' (#67) from sl-perception/outdoor-nav into main

jetson/docker-compose.yml

@@ -14,44 +14,30 @@ services:
       dockerfile: Dockerfile
     container_name: saltybot-ros2
     restart: unless-stopped
-    runtime: nvidia                 # JetPack NVIDIA runtime
+    runtime: nvidia
-    privileged: false               # use device passthrough instead
+    privileged: false
-    network_mode: host              # ROS2 DDS multicast needs host networking
+    network_mode: host
-
     environment:
       - NVIDIA_VISIBLE_DEVICES=all
       - NVIDIA_DRIVER_CAPABILITIES=all
       - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
       - ROS_DOMAIN_ID=42
-      - DISPLAY=${DISPLAY:-:0}      # for RViz2 on host X11
+      - DISPLAY=${DISPLAY:-:0}
-
     volumes:
-      # X11 socket for RViz2
       - /tmp/.X11-unix:/tmp/.X11-unix:rw
-      # ROS2 workspace (host-mounted for live dev)
       - ./ros2_ws/src:/ros2_ws/src:rw
-      # 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 — 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 (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 "
         source /opt/ros/humble/setup.bash &&
@@ -111,7 +97,7 @@ services:
           rgb_camera.profile:=640x480x30
       "
 
-  # ── STM32 bridge node (bidirectional serial↔ROS2) ─────────────────────────
+  # ── STM32 bridge node (bidirectional serial<->ROS2) ────────────────────────
   stm32-bridge:
     image: saltybot/ros2-humble:jetson-orin
     build:
@@ -134,7 +120,7 @@ services:
           serial_port:=/dev/stm32-bridge
       "
 
-  # ── 4× IMX219 CSI cameras ─────────────────────────────────────────────────
+  # ── 4x IMX219 CSI cameras ──────────────────────────────────────────────────
   csi-cameras:
     image: saltybot/ros2-humble:jetson-orin
     build:
@@ -144,7 +130,7 @@ services:
     restart: unless-stopped
     runtime: nvidia
     network_mode: host
-    privileged: true                # CSI camera access requires elevated perms
+    privileged: true
     environment:
       - ROS_DOMAIN_ID=42
       - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
@@ -164,22 +150,8 @@ services:
           fps:=30
       "
 
-  # ── Surround vision — 360° bird's-eye view + Nav2 camera obstacle layer ─────
+  # ── Surround vision — 360 bird's-eye view + Nav2 camera obstacle layer ─────
   saltybot-surround:
-
-  # ── rosbridge WebSocket server ────────────────────────────────────────────
-  # Serves ROS2 topics to the web dashboard (roslibjs) on ws://jetson:9090
-  #
-  # Topics exposed (whitelist in ros2_ws/src/saltybot_bringup/config/rosbridge_params.yaml):
-  #   /map  /scan  /tf  /tf_static  /saltybot/imu  /saltybot/balance_state
-  #   /cmd_vel  /person/*  /camera/*/image_raw/compressed
-  #
-  # Also runs image_transport/republish nodes to compress 4× CSI camera streams.
-  # Raw 640×480 YUYV → JPEG-compressed sensor_msgs/CompressedImage.
-  #
-  # Install (already in image): apt install ros-humble-rosbridge-server
-  #                                         ros-humble-image-transport-plugins
-  rosbridge:
     image: saltybot/ros2-humble:jetson-orin
     build:
       context: .
@@ -189,7 +161,7 @@ services:
     runtime: nvidia
     network_mode: host
     depends_on:
-      - csi-cameras                 # IMX219 /camera/*/image_raw must be publishing
+      - csi-cameras
     environment:
       - NVIDIA_VISIBLE_DEVICES=all
       - NVIDIA_DRIVER_CAPABILITIES=all
@@ -201,28 +173,33 @@ services:
     command: >
       bash -c "
         source /opt/ros/humble/setup.bash &&
+        source /ros2_ws/install/local_setup.bash 2>/dev/null || true &&
         ros2 launch saltybot_surround surround_vision.launch.py
           start_cameras:=false
           camera_height:=0.30
           publish_rate:=5.0
+      "
 
+  # ── rosbridge WebSocket server — ws://jetson:9090 ──────────────────────────
+  rosbridge:
+    image: saltybot/ros2-humble:jetson-orin
+    build:
+      context: .
+      dockerfile: Dockerfile
     container_name: saltybot-rosbridge
     restart: unless-stopped
     runtime: nvidia
-    network_mode: host            # port 9090 is directly reachable on host
+    network_mode: host
     depends_on:
-      - saltybot-ros2             # needs /map, /tf published
+      - saltybot-ros2
-      - stm32-bridge              # needs /saltybot/imu, /saltybot/balance_state
+      - stm32-bridge
-      - csi-cameras               # needs /camera/*/image_raw for compression
+      - csi-cameras
     environment:
       - ROS_DOMAIN_ID=42
       - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
     volumes:
       - ./ros2_ws/src:/ros2_ws/src:rw
       - ./config:/config:ro
-    # Port 9090 is accessible on all host interfaces via network_mode: host.
-    # To restrict to a specific interface, set host: "192.168.x.x" in
-    # rosbridge_params.yaml instead of using Docker port mappings.
     command: >
       bash -c "
         source /opt/ros/humble/setup.bash &&
@@ -230,7 +207,40 @@ services:
         ros2 launch saltybot_bringup rosbridge.launch.py
       "
 
-  # ── Nav2 autonomous navigation stack ────────────────────────────────────────
+
+  # -- Remote e-stop bridge (MQTT over 4G -> STM32 CDC) ----------------------
+  # Subscribes to saltybot/estop MQTT topic. {"kill":true} -> 'E
+' to STM32.
+  # Cellular watchdog: 5s MQTT drop in AUTO mode -> 'F
+' (ESTOP_CELLULAR_TIMEOUT).
+  remote-estop:
+    image: saltybot/ros2-humble:jetson-orin
+    build:
+      context: .
+      dockerfile: Dockerfile
+    container_name: saltybot-remote-estop
+    restart: unless-stopped
+    runtime: nvidia
+    network_mode: host
+    depends_on:
+      - stm32-bridge
+    environment:
+      - ROS_DOMAIN_ID=42
+      - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
+    devices:
+      - /dev/stm32-bridge:/dev/stm32-bridge
+    volumes:
+      - ./ros2_ws/src:/ros2_ws/src:rw
+      - ./config:/config:ro
+    command: >
+      bash -c "
+        source /opt/ros/humble/setup.bash &&
+        source /ros2_ws/install/local_setup.bash 2>/dev/null || true &&
+        pip install paho-mqtt --quiet 2>/dev/null || true &&
+        ros2 launch saltybot_bridge remote_estop.launch.py
+      "
+
+  # ── Nav2 autonomous navigation stack ──────────────────────────────────────
   saltybot-nav2:
     image: saltybot/ros2-humble:jetson-orin
     build:
@@ -241,7 +251,7 @@ services:
     runtime: nvidia
     network_mode: host
     depends_on:
-      - saltybot-ros2               # RTAB-Map + sensors must be running first
+      - saltybot-ros2
     environment:
       - NVIDIA_VISIBLE_DEVICES=all
       - NVIDIA_DRIVER_CAPABILITIES=all
@@ -253,9 +263,47 @@ services:
     command: >
       bash -c "
         source /opt/ros/humble/setup.bash &&
+        source /ros2_ws/install/local_setup.bash 2>/dev/null || true &&
         ros2 launch saltybot_bringup nav2.launch.py
       "
 
+  # ── Outdoor navigation — OSM routing + GPS waypoints + geofence ───────────
+  # Start only for outdoor missions. Usage:
+  #   ros2 param set /osm_router goal_lat 37.7749
+  #   ros2 param set /osm_router goal_lon -122.4194
+  #   ros2 service call /outdoor/plan_route std_srvs/srv/Trigger '{}'
+  #   ros2 service call /outdoor/start_navigation std_srvs/srv/Trigger '{}'
+  saltybot-outdoor:
+    image: saltybot/ros2-humble:jetson-orin
+    build:
+      context: .
+      dockerfile: Dockerfile
+    container_name: saltybot-outdoor
+    restart: unless-stopped
+    runtime: nvidia
+    network_mode: host
+    depends_on:
+      - saltybot-nav2
+    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
+      - /mnt/nvme/saltybot/osm_cache:/data/osm_cache:rw
+    devices:
+      - /dev/sim7600:/dev/sim7600
+    command: >
+      bash -c "
+        source /opt/ros/humble/setup.bash &&
+        source /ros2_ws/install/local_setup.bash 2>/dev/null || true &&
+        ros2 launch saltybot_outdoor outdoor_nav.launch.py
+          use_rtk:=false
+          fence_active:=true
+      "
+
 volumes:
   saltybot-maps:
     driver: local

jetson/ros2_ws/src/saltybot_outdoor/config/ekf_outdoor.yaml

@@ -0,0 +1,92 @@
+# ekf_outdoor.yaml — robot_localization EKF config for outdoor GPS navigation
+#
+# Two EKF instances + navsat_transform_node:
+#
+#   ekf_filter_node_odom  — local EKF: wheel odom + IMU → /odometry/local (odom frame)
+#   ekf_filter_node_map   — global EKF: /odometry/local + GPS odometry → /odometry/global (map frame)
+#   navsat_transform_node — converts /gps/fix + /imu/data → /odometry/gps (map-frame odometry)
+#
+# Frame hierarchy:
+#   map ← (global EKF) ← odom ← (local EKF) ← base_link
+#
+# Hardware:
+#   IMU:  RealSense D435i BMI055 → /imu/data
+#   GPS:  SIM7600X cellular     → /gps/fix  (±2.5 m CEP)
+#   Odom: STM32 wheel encoders  → /odom
+#   RTK:  ZED-F9P (optional)    → /gps/fix  (±2 cm CEP when use_rtk: true)
+
+# ── Local EKF: fuses wheel odometry + IMU in odom frame ──────────────────────
+ekf_filter_node_odom:
+  ros__parameters:
+    frequency:               30.0
+    sensor_timeout:          0.1
+    two_d_mode:              true
+    publish_tf:              true
+    map_frame:               map
+    odom_frame:              odom
+    base_link_frame:         base_link
+    world_frame:             odom
+
+    odom0:                   /odom
+    odom0_config: [true,  true,  false,
+                   false, false, false,
+                   true,  true,  false,
+                   false, false, true,
+                   false, false, false]
+    odom0_differential:      false
+    odom0_relative:          false
+    odom0_queue_size:        5
+
+    imu0:                    /imu/data
+    imu0_config: [false, false, false,
+                  true,  true,  true,
+                  false, false, false,
+                  true,  true,  true,
+                  true,  true,  false]
+    imu0_differential:       false
+    imu0_relative:           false
+    imu0_remove_gravitational_acceleration: true
+    imu0_queue_size:         10
+
+# ── Global EKF: fuses local odometry + GPS odometry in map frame ──────────────
+ekf_filter_node_map:
+  ros__parameters:
+    frequency:               15.0
+    sensor_timeout:          0.5
+    two_d_mode:              true
+    publish_tf:              true
+    map_frame:               map
+    odom_frame:              odom
+    base_link_frame:         base_link
+    world_frame:             map
+
+    odom0:                   /odometry/local
+    odom0_config: [false, false, false,
+                   false, false, false,
+                   true,  true,  false,
+                   false, false, true,
+                   false, false, false]
+    odom0_differential:      false
+    odom0_queue_size:        5
+
+    odom1:                   /odometry/gps
+    odom1_config: [true,  true,  false,
+                   false, false, false,
+                   false, false, false,
+                   false, false, false,
+                   false, false, false]
+    odom1_differential:      false
+    odom1_queue_size:        5
+
+# ── navsat_transform_node — GPS NavSatFix → map-frame Odometry ────────────────
+navsat_transform:
+  ros__parameters:
+    frequency:                   10.0
+    delay:                       3.0
+    magnetic_declination_radians: 0.0
+    yaw_offset:                  0.0
+    zero_altitude:               true
+    broadcast_utm_transform:     false
+    publish_filtered_gps:        false
+    use_odometry_yaw:            false
+    wait_for_datum:              false

jetson/ros2_ws/src/saltybot_outdoor/config/geofence_vertices.yaml

@@ -0,0 +1,35 @@
+# geofence_vertices.yaml — GPS polygon safety boundary for outdoor navigation
+#
+# Format: fence_vertices is a flat list [lat0, lon0, lat1, lon1, ..., latN, lonN]
+# Minimum 3 vertex pairs (6 values). Leave empty [] to disable geofence.
+#
+# ── How to set your fence ────────────────────────────────────────────────────
+# 1. Go to https://geojson.io and mark your safe operating area corners
+# 2. Replace the example values below with real coordinates
+# 3. Reload at runtime: ros2 service call /outdoor/reload_fence std_srvs/srv/Trigger '{}'
+#
+# ── Unit conversions ─────────────────────────────────────────────────────────
+#   d_lat_deg = d_metres / 111320
+#   d_lon_deg = d_metres / (111320 * cos(lat_radians))
+#
+geofence:
+  ros__parameters:
+    # Empty = geofence inactive (WARNING: no safety boundary without a fence)
+    fence_vertices: []
+    check_rate_hz:  2.0
+    stop_on_exit:   true
+    map_frame:      "map"
+
+# ── EXAMPLE (uncomment and edit with real coordinates) ───────────────────────
+# geofence:
+#   ros__parameters:
+#     # SW → SE → NE → NW corners of a ~100m × 80m rectangle
+#     fence_vertices:
+#       - 37.774900   # SW lat
+#       - -122.419400 # SW lon
+#       - 37.774900   # SE lat
+#       - -122.418500 # SE lon
+#       - 37.775800   # NE lat
+#       - -122.418500 # NE lon
+#       - 37.775800   # NW lat
+#       - -122.419400 # NW lon

jetson/ros2_ws/src/saltybot_outdoor/config/outdoor_nav_params.yaml

@@ -0,0 +1,98 @@
+# outdoor_nav_params.yaml — Outdoor navigation configuration for SaltyBot
+#
+# Hardware: Jetson Orin Nano Super / SIM7600X cellular GPS (±2.5 m CEP)
+# RTK upgrade: u-blox ZED-F9P → ±2 cm CEP (set use_rtk: true when installed)
+#
+# ── GPS quality notes ────────────────────────────────────────────────────────
+# SIM7600X reports STATUS_FIX (0) in open sky, STATUS_NO_FIX (-1) indoors.
+# RTK ZED-F9P reports STATUS_GBAS_FIX (2) when corrections received.
+# Goal tolerance automatically tightens from 2.0m (cellular) to 0.3m (RTK).
+#
+# ── RTK upgrade procedure ────────────────────────────────────────────────────
+# 1. Connect ZED-F9P to /dev/ttyTHS0 (Orin 40-pin UART, pins 8/10)
+# 2. Set NTRIP credentials in rtk_gps.launch.py
+# 3. Run: ros2 launch saltybot_outdoor rtk_gps.launch.py
+# 4. Verify: ros2 topic echo /gps/fix | grep status
+#    → status.status == 2 (STATUS_GBAS_FIX) = RTK fixed
+#
+# References:
+#   SparkFun RTK Express: https://docs.sparkfun.com/SparkFun_RTK_Everywhere_Firmware/
+#   NTRIP caster list: https://rtk2go.com/sample-map/
+# ─────────────────────────────────────────────────────────────────────────────
+
+osm_router:
+  ros__parameters:
+    overpass_url:       "https://overpass-api.de/api/interpreter"
+    query_radius_m:     1500.0      # fetch OSM ways within this radius of midpoint
+    simplify_eps_m:     5.0         # Douglas–Peucker waypoint spacing (metres)
+    cache_dir:          "/data/osm_cache"
+    use_cache:          true
+    map_frame:          "map"
+    # Default goal — override at launch or via parameter set:
+    #   ros2 param set /osm_router goal_lat 48.8566
+    #   ros2 param set /osm_router goal_lon 2.3522
+    goal_lat:           0.0
+    goal_lon:           0.0
+
+gps_waypoint_follower:
+  ros__parameters:
+    map_frame:            "map"
+    min_fix_status:       0         # 0=STATUS_FIX; -1=NO_FIX; 2=RTK_FIXED
+    # Goal tolerance for SIM7600X (±2.5m CEP) — auto-tightens with RTK
+    goal_tolerance_xy:    2.0       # metres (outdoor vs 0.25m indoor)
+    goal_tolerance_yaw:   3.14159   # radians — don't constrain heading outdoors
+    nav_timeout_s:        300.0     # 5 min max per navigate_through_poses call
+    waypoint_spacing_m:   5.0       # skip waypoints closer than this
+
+geofence:
+  ros__parameters:
+    check_rate_hz:  2.0
+    stop_on_exit:   true
+    # fence_vertices: flat list of lat,lon pairs forming the boundary polygon.
+    # Leave empty to disable geofence (WARNING: no safety boundary).
+    # Example — 100m × 100m test square (replace with real boundary):
+    fence_vertices: []
+    map_frame: "map"
+
+# ── Outdoor Nav2 parameter overrides ────────────────────────────────────────
+# These supplement jetson/config/nav2_params.yaml for outdoor operation.
+# Apply by passing this file as an additional params_file to nav2.launch.py.
+#
+# Key differences from indoor config:
+#   - No static_layer (no SLAM map — GPS provides global position only)
+#   - Wider inflation radius (outdoor = wider obstacles / kerbs)
+#   - Higher max velocity (clear sidewalks vs cluttered indoor)
+#   - GPS costmap uses wider observation sources
+# ─────────────────────────────────────────────────────────────────────────────
+
+# Outdoor controller overrides
+controller_server:
+  ros__parameters:
+    FollowPath:
+      max_vel_x:   1.5      # m/s (indoor: 1.0)
+      max_speed_xy: 1.5
+      sim_time:    2.0      # seconds lookahead (longer for faster speeds)
+      # Larger goal tolerance at waypoints (GPS ±2.5m)
+    general_goal_checker:
+      xy_goal_tolerance:  2.0    # metres (indoor: 0.25)
+      yaw_goal_tolerance: 3.14   # don't constrain heading at GPS waypoints
+
+# Outdoor local costmap overrides
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      width:  5        # metres (wider window for outdoor speeds)
+      height: 5
+      inflation_layer:
+        inflation_radius: 0.50   # metres (indoor: 0.55, similar but explicit)
+
+# Outdoor global costmap overrides
+# Key: remove static_layer (no SLAM map), keep obstacle layers
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      plugins: ["obstacle_layer", "inflation_layer"]  # no static_layer outdoors
+      inflation_layer:
+        inflation_radius: 0.50
+      obstacle_layer:
+        observation_sources: scan surround_cameras

jetson/ros2_ws/src/saltybot_outdoor/launch/outdoor_nav.launch.py

@@ -0,0 +1,122 @@
+"""
+outdoor_nav.launch.py — Full outdoor navigation stack for SaltyBot
+
+Services started:
+  osm_router_node         — Overpass API OSM routing → /outdoor/waypoints
+  gps_waypoint_follower   — /outdoor/waypoints → Nav2 navigate_through_poses
+  geofence_node           — GPS polygon safety boundary
+  navsat_transform_node   — robot_localization GPS→map projection (EKF)
+  ekf_filter_node_odom    — local EKF (odom frame)
+  ekf_filter_node_map     — global EKF (map frame, GPS-fused)
+
+Nav2 is NOT started here — it must already be running (saltybot-nav2 container).
+
+Outdoor mode: uses GPS for global localisation instead of RTAB-Map.
+Indoor mode:  use slam_rtabmap.launch.py instead (RTAB-Map provides map→odom).
+
+Arguments:
+  use_rtk         (default false) — set true when ZED-F9P RTK module installed
+  fence_active    (default true)  — enable geofence safety boundary
+  goal_lat        (default 0.0)   — navigation goal latitude
+  goal_lon        (default 0.0)   — navigation goal longitude
+"""
+
+import os
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    pkg_dir = get_package_share_directory('saltybot_outdoor')
+    params_file    = os.path.join(pkg_dir, 'config', 'outdoor_nav_params.yaml')
+    ekf_file       = os.path.join(pkg_dir, 'config', 'ekf_outdoor.yaml')
+    geofence_file  = os.path.join(pkg_dir, 'config', 'geofence_vertices.yaml')
+
+    args = [
+        DeclareLaunchArgument('use_rtk',      default_value='false'),
+        DeclareLaunchArgument('fence_active', default_value='true'),
+        DeclareLaunchArgument('goal_lat',     default_value='0.0'),
+        DeclareLaunchArgument('goal_lon',     default_value='0.0'),
+    ]
+
+    # ── robot_localization: local EKF (odom frame) ───────────────────────────
+    ekf_local = Node(
+        package='robot_localization',
+        executable='ekf_node',
+        name='ekf_filter_node_odom',
+        output='screen',
+        parameters=[ekf_file, {'use_sim_time': False}],
+        remappings=[('odometry/filtered', '/odometry/local')],
+    )
+
+    # ── robot_localization: global EKF (map frame, GPS-fused) ────────────────
+    ekf_global = Node(
+        package='robot_localization',
+        executable='ekf_node',
+        name='ekf_filter_node_map',
+        output='screen',
+        parameters=[ekf_file, {'use_sim_time': False}],
+        remappings=[('odometry/filtered', '/odometry/global')],
+    )
+
+    # ── navsat_transform_node — GPS (NavSatFix) → map-frame Odometry ─────────
+    navsat = Node(
+        package='robot_localization',
+        executable='navsat_transform_node',
+        name='navsat_transform',
+        output='screen',
+        parameters=[ekf_file, {'use_sim_time': False}],
+        remappings=[
+            ('imu/data',       '/imu/data'),
+            ('gps/fix',        '/gps/fix'),
+            ('odometry/filtered', '/odometry/global'),
+            ('odometry/gps',   '/odometry/gps'),
+        ],
+    )
+
+    # ── OSM router ───────────────────────────────────────────────────────────
+    osm_router = Node(
+        package='saltybot_outdoor',
+        executable='osm_router_node',
+        name='osm_router',
+        output='screen',
+        parameters=[
+            params_file,
+            {
+                'goal_lat': LaunchConfiguration('goal_lat'),
+                'goal_lon': LaunchConfiguration('goal_lon'),
+            },
+        ],
+    )
+
+    # ── GPS waypoint follower ─────────────────────────────────────────────────
+    gps_follower = Node(
+        package='saltybot_outdoor',
+        executable='gps_waypoint_follower',
+        name='gps_waypoint_follower',
+        output='screen',
+        parameters=[params_file],
+    )
+
+    # ── Geofence ──────────────────────────────────────────────────────────────
+    geofence = Node(
+        package='saltybot_outdoor',
+        executable='geofence_node',
+        name='geofence',
+        output='screen',
+        parameters=[geofence_file],
+    )
+
+    return LaunchDescription([
+        *args,
+        ekf_local,
+        ekf_global,
+        navsat,
+        osm_router,
+        gps_follower,
+        geofence,
+    ])

jetson/ros2_ws/src/saltybot_outdoor/package.xml

@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>saltybot_outdoor</name>
+  <version>0.1.0</version>
+  <description>Outdoor navigation for SaltyBot — OSM routing, GPS waypoint following, geofence</description>
+  <maintainer email="seb@saltylab.io">seb</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_python</buildtool_depend>
+
+  <depend>rclpy</depend>
+  <depend>sensor_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
+  <depend>nav2_msgs</depend>
+
+  <exec_depend>robot_localization</exec_depend>
+  <exec_depend>nav2_bringup</exec_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

jetson/ros2_ws/src/saltybot_outdoor/saltybot_outdoor/geofence_node.py

@@ -0,0 +1,197 @@
+"""
+geofence_node — GPS polygon safety boundary for outdoor navigation
+
+Monitors /gps/fix and checks the robot's position against a configurable
+polygon of GPS vertices. If the robot exits the fence OR a requested waypoint
+lies outside the fence, this node:
+  1. Publishes /outdoor/abort True → gps_waypoint_follower cancels Nav2
+  2. Publishes /cmd_vel zero twist (emergency stop)
+  3. Publishes /outdoor/fence_violation with details
+
+The fence polygon is defined in geofence_vertices.yaml as a list of
+{lat, lon} pairs forming a closed polygon (first = last not required).
+Point-in-polygon test uses the ray-casting algorithm.
+
+Topics subscribed:
+  /gps/fix               sensor_msgs/NavSatFix
+  /outdoor/waypoints     geometry_msgs/PoseArray  — pre-navigation check
+
+Topics published:
+  /outdoor/abort         std_msgs/Bool            — True when fence violated
+  /outdoor/fence_status  std_msgs/String          — 'inside' | 'outside:<lat>,<lon>'
+  /cmd_vel               geometry_msgs/Twist      — zero-velocity on violation
+
+Services:
+  /outdoor/reload_fence  std_srvs/Trigger         — re-read geofence_vertices.yaml
+"""
+
+import math
+from typing import Optional
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
+
+from sensor_msgs.msg import NavSatFix
+from geometry_msgs.msg import PoseArray, Twist
+from std_msgs.msg import Bool, String
+from std_srvs.srv import Trigger
+
+
+def _point_in_polygon(lat: float, lon: float, polygon: list) -> bool:
+    """Ray-casting point-in-polygon test. polygon = [(lat, lon), ...]."""
+    n = len(polygon)
+    inside = False
+    j = n - 1
+    for i in range(n):
+        xi, yi = polygon[i]
+        xj, yj = polygon[j]
+        if ((yi > lon) != (yj > lon)) and (lat < (xj - xi) * (lon - yi) / (yj - yi) + xi):
+            inside = not inside
+        j = i
+    return inside
+
+
+def _polygon_area_m2(polygon: list) -> float:
+    """Approximate polygon area in m² using shoelace on flat-earth coords."""
+    if len(polygon) < 3:
+        return 0.0
+    lat0, lon0 = polygon[0]
+    cos0 = math.cos(math.radians(lat0))
+    pts = [
+        ((lat - lat0) * 111_320.0, (lon - lon0) * 111_320.0 * cos0)
+        for lat, lon in polygon
+    ]
+    n = len(pts)
+    area = 0.0
+    for i in range(n):
+        j = (i + 1) % n
+        area += pts[i][0] * pts[j][1]
+        area -= pts[j][0] * pts[i][1]
+    return abs(area) / 2.0
+
+
+class GeofenceNode(Node):
+
+    def __init__(self):
+        super().__init__('geofence')
+
+        self.declare_parameter('fence_vertices',  [])        # flat list: lat0,lon0,lat1,lon1,...
+        self.declare_parameter('check_rate_hz',   2.0)
+        self.declare_parameter('stop_on_exit',    True)      # publish zero /cmd_vel
+        self.declare_parameter('map_frame',       'map')
+
+        self._polygon: list[tuple[float, float]] = []
+        self._fix:     Optional[NavSatFix]        = None
+        self._violated = False
+
+        self._load_fence()
+
+        # ── Subscriptions ────────────────────────────────────────────────────
+        self.create_subscription(NavSatFix, '/gps/fix',           self._fix_cb,  10)
+        self.create_subscription(PoseArray, '/outdoor/waypoints', self._wps_cb,  10)
+
+        # ── Publishers ───────────────────────────────────────────────────────
+        self._abort_pub  = self.create_publisher(Bool,   '/outdoor/abort',        10)
+        self._status_pub = self.create_publisher(String, '/outdoor/fence_status', 10)
+        self._vel_pub    = self.create_publisher(Twist,  '/cmd_vel',              10)
+
+        # ── Service ──────────────────────────────────────────────────────────
+        self.create_service(Trigger, '/outdoor/reload_fence', self._reload_cb)
+
+        rate = self.get_parameter('check_rate_hz').value
+        self.create_timer(1.0 / rate, self._check)
+
+        area = _polygon_area_m2(self._polygon) if self._polygon else 0.0
+        self.get_logger().info(
+            f'geofence: {len(self._polygon)}-vertex polygon, area≈{area:.0f} m²'
+        )
+
+    # ── Callbacks ─────────────────────────────────────────────────────────────
+
+    def _fix_cb(self, msg: NavSatFix) -> None:
+        self._fix = msg
+
+    def _wps_cb(self, msg: PoseArray) -> None:
+        """Pre-check: warn if any map-frame waypoint is suspiciously far out."""
+        # In V1 we can't easily reverse-project map→GPS here without TF/EKF,
+        # so we just log the waypoint count and rely on the robot fix check.
+        self.get_logger().info(
+            f'geofence: {len(msg.poses)} waypoints received (pre-flight GPS check pending)'
+        )
+
+    def _reload_cb(self, _req, response):
+        self._load_fence()
+        response.success = True
+        response.message = f'Fence reloaded: {len(self._polygon)} vertices'
+        return response
+
+    # ── Fence check ───────────────────────────────────────────────────────────
+
+    def _check(self) -> None:
+        if not self._polygon:
+            self._pub_status('no_fence_configured')
+            return
+        if self._fix is None:
+            self._pub_status('no_gps')
+            return
+        if self._fix.status.status < 0:
+            return   # no fix — don't trigger on bad data
+
+        lat, lon = self._fix.latitude, self._fix.longitude
+        inside = _point_in_polygon(lat, lon, self._polygon)
+
+        if inside:
+            self._violated = False
+            self._pub_status('inside')
+        else:
+            if not self._violated:
+                self.get_logger().warn(
+                    f'GEOFENCE VIOLATION: robot at ({lat:.6f}, {lon:.6f}) is outside fence'
+                )
+                self._violated = True
+
+            self._pub_status(f'outside:{lat:.6f},{lon:.6f}')
+
+            # Publish abort
+            abort = Bool()
+            abort.data = True
+            self._abort_pub.publish(abort)
+
+            # Emergency stop
+            if self.get_parameter('stop_on_exit').value:
+                self._vel_pub.publish(Twist())   # zero twist = stop
+
+    # ── Helpers ───────────────────────────────────────────────────────────────
+
+    def _load_fence(self) -> None:
+        """Parse fence_vertices parameter: flat [lat0, lon0, lat1, lon1, ...]."""
+        raw = self.get_parameter('fence_vertices').value
+        if not raw or len(raw) < 6:
+            self._polygon = []
+            self.get_logger().warn('geofence: no valid fence_vertices — geofence inactive')
+            return
+        pairs = list(raw)
+        self._polygon = [(pairs[i], pairs[i + 1]) for i in range(0, len(pairs) - 1, 2)]
+        self.get_logger().info(f'geofence: loaded {len(self._polygon)}-vertex polygon')
+
+    def _pub_status(self, s: str) -> None:
+        msg = String()
+        msg.data = s
+        self._status_pub.publish(msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = GeofenceNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.try_shutdown()
+
+
+if __name__ == '__main__':
+    main()

jetson/ros2_ws/src/saltybot_outdoor/saltybot_outdoor/gps_waypoint_follower_node.py

@@ -0,0 +1,287 @@
+"""
+gps_waypoint_follower_node — GPS-to-Nav2 waypoint bridge for outdoor navigation
+
+Subscribes to the OSM router's waypoint output (/outdoor/waypoints) and the
+robot's GPS fix (/gps/fix), converts each waypoint to map-frame via
+robot_localization's navsat_transform_node, then sends the full list to Nav2's
+navigate_through_poses action server.
+
+GPS quality handling:
+  SIM7600X cellular GPS: ±2.5 m CEP — uses wider goal tolerances (2 m)
+  RTK upgrade (ZED-F9P): ±0.02 m CEP — tolerances tighten automatically
+
+GPS status thresholds (sensor_msgs/NavSatStatus):
+  STATUS_NO_FIX    (-1) → reject all goals, log error
+  STATUS_FIX       (0)  → standard GPS, use but warn
+  STATUS_SBAS_FIX  (1)  → SBAS-corrected, OK
+  STATUS_GBAS_FIX  (2)  → RTK fixed, best quality, tighten tolerance
+
+Topics subscribed:
+  /gps/fix              sensor_msgs/NavSatFix
+  /outdoor/waypoints    geometry_msgs/PoseArray (from osm_router_node, map frame)
+  /outdoor/abort        std_msgs/Bool           (external stop signal)
+
+Topics published:
+  /outdoor/gps_status   std_msgs/String   — current GPS quality description
+  /outdoor/nav_status   std_msgs/String   — navigation state machine status
+
+Actions called:
+  /navigate_through_poses  nav2_msgs/action/NavigateThroughPoses
+
+Services:
+  /outdoor/start_navigation  std_srvs/Trigger — begin executing /outdoor/waypoints
+  /outdoor/stop_navigation   std_srvs/Trigger — cancel in-progress navigation
+"""
+
+import math
+from typing import Optional
+
+import rclpy
+from rclpy.node import Node
+from rclpy.action import ActionClient
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.executors import MultiThreadedExecutor
+
+from sensor_msgs.msg import NavSatFix, NavSatStatus
+from geometry_msgs.msg import PoseArray, PoseStamped
+from std_msgs.msg import String, Bool
+from std_srvs.srv import Trigger
+
+from nav2_msgs.action import NavigateThroughPoses
+
+
+# GPS fix quality thresholds
+_MIN_FIX_STATUS = NavSatStatus.STATUS_FIX   # reject STATUS_NO_FIX (-1)
+_RTK_FIX_STATUS = NavSatStatus.STATUS_GBAS_FIX
+
+# Goal tolerance (metres) by GPS quality
+_TOLERANCE_STANDARD_GPS = 2.0    # SIM7600X ±2.5 m CEP
+_TOLERANCE_RTK          = 0.30   # ZED-F9P ±2 cm RTK
+
+
+class GpsWaypointFollowerNode(Node):
+
+    def __init__(self):
+        super().__init__('gps_waypoint_follower')
+
+        self.declare_parameter('map_frame',           'map')
+        self.declare_parameter('min_fix_status',      _MIN_FIX_STATUS)
+        self.declare_parameter('goal_tolerance_xy',   _TOLERANCE_STANDARD_GPS)
+        self.declare_parameter('goal_tolerance_yaw',  3.14)    # don't care about heading
+        self.declare_parameter('nav_timeout_s',       300.0)   # 5 min per waypoint batch
+        self.declare_parameter('waypoint_spacing_m',  5.0)     # skip closer waypoints
+
+        self._fix:       Optional[NavSatFix]  = None
+        self._waypoints: Optional[PoseArray]  = None
+        self._nav_active = False
+        self._cb_group   = ReentrantCallbackGroup()
+
+        # ── Subscriptions ────────────────────────────────────────────────────
+        self.create_subscription(NavSatFix,  '/gps/fix',           self._fix_cb,       10)
+        self.create_subscription(PoseArray,  '/outdoor/waypoints', self._waypoints_cb, 10)
+        self.create_subscription(Bool,       '/outdoor/abort',     self._abort_cb,     10)
+
+        # ── Publishers ───────────────────────────────────────────────────────
+        self._gps_status_pub = self.create_publisher(String, '/outdoor/gps_status', 10)
+        self._nav_status_pub = self.create_publisher(String, '/outdoor/nav_status',  10)
+
+        # ── Services ─────────────────────────────────────────────────────────
+        self.create_service(Trigger, '/outdoor/start_navigation', self._start_cb,
+                            callback_group=self._cb_group)
+        self.create_service(Trigger, '/outdoor/stop_navigation',  self._stop_cb,
+                            callback_group=self._cb_group)
+
+        # ── Nav2 action client ───────────────────────────────────────────────
+        self._nav_client = ActionClient(
+            self, NavigateThroughPoses, '/navigate_through_poses',
+            callback_group=self._cb_group,
+        )
+
+        # ── Status timer ─────────────────────────────────────────────────────
+        self.create_timer(2.0, self._publish_gps_status)
+
+        self._nav_goal_handle = None
+        self._publish_nav_status('idle')
+        self.get_logger().info('gps_waypoint_follower: ready. Call /outdoor/start_navigation.')
+
+    # ── Callbacks ─────────────────────────────────────────────────────────────
+
+    def _fix_cb(self, msg: NavSatFix) -> None:
+        self._fix = msg
+
+    def _waypoints_cb(self, msg: PoseArray) -> None:
+        self._waypoints = msg
+        self.get_logger().info(f'Received {len(msg.poses)} outdoor waypoints')
+
+    def _abort_cb(self, msg: Bool) -> None:
+        if msg.data:
+            self._cancel_navigation('abort signal received')
+
+    def _start_cb(self, _req, response):
+        if self._nav_active:
+            response.success = False
+            response.message = 'Navigation already active'
+            return response
+
+        if self._fix is None:
+            response.success = False
+            response.message = 'No GPS fix'
+            return response
+
+        fix_status = self._fix.status.status
+        min_status = self.get_parameter('min_fix_status').value
+        if fix_status < min_status:
+            response.success = False
+            response.message = f'GPS quality too low: status={fix_status}'
+            return response
+
+        if self._waypoints is None or len(self._waypoints.poses) == 0:
+            response.success = False
+            response.message = 'No waypoints loaded — call /outdoor/plan_route first'
+            return response
+
+        # Send waypoints to Nav2 asynchronously
+        self.create_timer(0.1, self._send_nav_goal, callback_group=self._cb_group)
+        response.success = True
+        response.message = f'Starting navigation with {len(self._waypoints.poses)} waypoints'
+        return response
+
+    def _stop_cb(self, _req, response):
+        self._cancel_navigation('stop requested')
+        response.success = True
+        response.message = 'Navigation cancelled'
+        return response
+
+    # ── Navigation ────────────────────────────────────────────────────────────
+
+    def _send_nav_goal(self) -> None:
+        """One-shot timer callback: send waypoints to Nav2."""
+        # Cancel the timer that called us
+        self.destroy_timer(list(self.timers)[-1])
+
+        if not self._nav_client.wait_for_server(timeout_sec=5.0):
+            self.get_logger().error('Nav2 navigate_through_poses action not available')
+            self._publish_nav_status('error:nav2_unavailable')
+            return
+
+        # Adjust goal tolerance by GPS quality
+        fix_status = self._fix.status.status if self._fix else _MIN_FIX_STATUS
+        if fix_status >= _RTK_FIX_STATUS:
+            tol = _TOLERANCE_RTK
+            self.get_logger().info('RTK fix detected — using tight tolerance (0.30 m)')
+        else:
+            tol = self.get_parameter('goal_tolerance_xy').value
+            self.get_logger().info(f'Standard GPS — tolerance = {tol:.1f} m')
+
+        # Build goal: PoseStamped list from PoseArray
+        goal = NavigateThroughPoses.Goal()
+        frame = self.get_parameter('map_frame').value
+        now   = self.get_clock().now().to_msg()
+
+        poses = self._filter_waypoints(self._waypoints.poses)
+        for pose in poses:
+            ps = PoseStamped()
+            ps.header.stamp    = now
+            ps.header.frame_id = frame
+            ps.pose = pose
+            goal.poses.append(ps)
+
+        self.get_logger().info(f'Sending {len(goal.poses)} waypoints to Nav2')
+        self._publish_nav_status(f'navigating:{len(goal.poses)}_waypoints')
+        self._nav_active = True
+
+        send_future = self._nav_client.send_goal_async(
+            goal,
+            feedback_callback=self._nav_feedback_cb,
+        )
+        send_future.add_done_callback(self._nav_goal_response_cb)
+
+    def _nav_goal_response_cb(self, future) -> None:
+        self._nav_goal_handle = future.result()
+        if not self._nav_goal_handle.accepted:
+            self.get_logger().error('Nav2 rejected navigation goal')
+            self._nav_active = False
+            self._publish_nav_status('error:goal_rejected')
+            return
+        result_future = self._nav_goal_handle.get_result_async()
+        result_future.add_done_callback(self._nav_result_cb)
+
+    def _nav_feedback_cb(self, feedback) -> None:
+        fb = feedback.feedback
+        remaining = getattr(fb, 'number_of_poses_remaining', '?')
+        self.get_logger().info(
+            f'Nav2 feedback: {remaining} waypoints remaining',
+            throttle_duration_sec=10.0,
+        )
+
+    def _nav_result_cb(self, future) -> None:
+        self._nav_active = False
+        result = future.result()
+        if result.status == 4:   # SUCCEEDED
+            self.get_logger().info('Outdoor navigation completed successfully')
+            self._publish_nav_status('completed')
+        else:
+            self.get_logger().warn(f'Navigation ended with status {result.status}')
+            self._publish_nav_status(f'failed:status={result.status}')
+
+    def _cancel_navigation(self, reason: str) -> None:
+        if self._nav_goal_handle is not None and self._nav_active:
+            self.get_logger().info(f'Cancelling navigation: {reason}')
+            self._nav_goal_handle.cancel_goal_async()
+        self._nav_active = False
+        self._publish_nav_status(f'cancelled:{reason}')
+
+    # ── Helpers ───────────────────────────────────────────────────────────────
+
+    def _filter_waypoints(self, poses: list) -> list:
+        """Remove waypoints closer than waypoint_spacing_m to previous."""
+        spacing = self.get_parameter('waypoint_spacing_m').value
+        out = [poses[0]]
+        for p in poses[1:]:
+            prev = out[-1]
+            d = math.hypot(
+                p.position.x - prev.position.x,
+                p.position.y - prev.position.y,
+            )
+            if d >= spacing:
+                out.append(p)
+        return out
+
+    def _publish_gps_status(self) -> None:
+        if self._fix is None:
+            status = 'no_fix'
+        elif self._fix.status.status < 0:
+            status = 'no_fix'
+        elif self._fix.status.status >= _RTK_FIX_STATUS:
+            status = f'rtk_fixed lat={self._fix.latitude:.6f} lon={self._fix.longitude:.6f}'
+        elif self._fix.status.status >= NavSatStatus.STATUS_FIX:
+            cov = self._fix.position_covariance[0] ** 0.5 if self._fix.position_covariance[0] > 0 else 2.5
+            status = f'fix_ok cep~{cov:.1f}m lat={self._fix.latitude:.6f} lon={self._fix.longitude:.6f}'
+        else:
+            status = f'poor_fix status={self._fix.status.status}'
+        msg = String()
+        msg.data = status
+        self._gps_status_pub.publish(msg)
+
+    def _publish_nav_status(self, s: str) -> None:
+        msg = String()
+        msg.data = s
+        self._nav_status_pub.publish(msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = GpsWaypointFollowerNode()
+    executor = MultiThreadedExecutor()
+    executor.add_node(node)
+    try:
+        executor.spin()
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.try_shutdown()
+
+
+if __name__ == '__main__':
+    main()

jetson/ros2_ws/src/saltybot_outdoor/saltybot_outdoor/osm_router_node.py

@@ -0,0 +1,371 @@
+"""
+osm_router_node — OpenStreetMap sidewalk/bike-lane route planner
+
+Given a start and goal GPS coordinate, this node:
+  1. Queries the Overpass API for pedestrian/cycleway ways within a
+     configurable radius around the midpoint
+  2. Builds a weighted graph (edge weight = haversine distance)
+  3. Runs A* (Dijkstra fallback) to find the shortest walkable path
+  4. Simplifies the waypoint list (Douglas–Peucker, ~5m spacing)
+  5. Converts lat/lon waypoints to map-frame PoseStamped via
+     robot_localization's /fromLL service
+  6. Publishes the route as nav_msgs/Path for RViz visualisation
+  7. Returns the PoseArray to the GPS waypoint follower
+
+ROS services:
+  /outdoor/plan_route  (saltybot_outdoor_msgs or std_srvs — see PlanRoute below)
+
+Topics published:
+  /outdoor/route   nav_msgs/Path    — map-frame path for RViz
+  /outdoor/status  std_msgs/String  — 'idle' | 'planning' | 'ready' | 'error:<msg>'
+
+Topics subscribed:
+  /gps/fix   sensor_msgs/NavSatFix  — current robot position
+
+Hardware:
+  SIM7600X cellular module publishes /gps/fix at ~1 Hz (±2.5 m CEP)
+  RTK upgrade path: swap to ZED-F9P for ±2 cm (see config/outdoor_nav_params.yaml)
+
+Overpass query: fetches highway=footway|cycleway|path|pedestrian|living_street
+within a bounding box, then prunes to walking-accessible ways only.
+
+Dependencies:
+  pip3 install requests   (Overpass HTTP)
+  No osmnx required — native JSON parsing keeps Docker image lean.
+"""
+
+import json
+import math
+import heapq
+import time
+from typing import Optional
+
+import rclpy
+from rclpy.node import Node
+from rclpy.callback_groups import ReentrantCallbackGroup
+
+from sensor_msgs.msg import NavSatFix
+from nav_msgs.msg import Path
+from geometry_msgs.msg import PoseStamped, PoseArray, Pose, Point
+from std_msgs.msg import String
+from std_srvs.srv import Trigger
+
+try:
+    import requests
+    _REQUESTS_OK = True
+except ImportError:
+    _REQUESTS_OK = False
+
+
+# OSM highway types considered walkable
+_WALKABLE = frozenset([
+    'footway', 'cycleway', 'path', 'pedestrian',
+    'living_street', 'residential', 'service',
+    'track', 'steps',
+])
+
+_OVERPASS_URL = 'https://overpass-api.de/api/interpreter'
+_OVERPASS_TIMEOUT = 20   # seconds
+
+
+# ── Haversine distance ────────────────────────────────────────────────────────
+
+def _haversine(lat1: float, lon1: float, lat2: float, lon2: float) -> float:
+    """Great-circle distance in metres."""
+    R = 6_371_000.0
+    phi1, phi2 = math.radians(lat1), math.radians(lat2)
+    dphi = math.radians(lat2 - lat1)
+    dlam = math.radians(lon2 - lon1)
+    a = math.sin(dphi / 2) ** 2 + math.cos(phi1) * math.cos(phi2) * math.sin(dlam / 2) ** 2
+    return R * 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
+
+
+# ── Douglas–Peucker polyline simplification ───────────────────────────────────
+
+def _perp_dist(pt, line_a, line_b):
+    """Cross-track distance from pt to segment a–b (all in lat/lon)."""
+    lat, lon = pt
+    la, lo_a = line_a
+    lb, lo_b = line_b
+    dx, dy = lb - la, lo_b - lo_a
+    if dx == 0 and dy == 0:
+        return _haversine(lat, lon, la, lo_a)
+    t = ((lat - la) * dx + (lon - lo_a) * dy) / (dx * dx + dy * dy)
+    t = max(0.0, min(1.0, t))
+    return _haversine(lat, lon, la + t * dx, lo_a + t * dy)
+
+
+def _douglas_peucker(points: list, epsilon_m: float) -> list:
+    """Reduce waypoints, keeping points > epsilon_m from simplified line."""
+    if len(points) <= 2:
+        return points
+    dmax, idx = 0.0, 0
+    end = points[-1]
+    for i in range(1, len(points) - 1):
+        d = _perp_dist(points[i], points[0], end)
+        if d > dmax:
+            dmax, idx = d, i
+    if dmax > epsilon_m:
+        left  = _douglas_peucker(points[:idx + 1], epsilon_m)
+        right = _douglas_peucker(points[idx:],     epsilon_m)
+        return left[:-1] + right
+    return [points[0], points[-1]]
+
+
+# ── A* on OSM graph ───────────────────────────────────────────────────────────
+
+def _astar(graph: dict, start: int, goal: int, coords: dict) -> Optional[list]:
+    """A* over OSM node graph. Returns list of node IDs or None."""
+    if start not in graph or goal not in graph:
+        return None
+    glat, glon = coords[goal]
+
+    def h(n):
+        return _haversine(coords[n][0], coords[n][1], glat, glon)
+
+    open_heap = [(h(start), 0.0, start, [start])]
+    visited = set()
+    while open_heap:
+        f, g, node, path = heapq.heappop(open_heap)
+        if node in visited:
+            continue
+        visited.add(node)
+        if node == goal:
+            return path
+        for nb, w in graph[node]:
+            if nb not in visited:
+                ng = g + w
+                heapq.heappush(open_heap, (ng + h(nb), ng, nb, path + [nb]))
+    return None
+
+
+# ── OSM graph builder ─────────────────────────────────────────────────────────
+
+def _build_graph(elements: list) -> tuple[dict, dict]:
+    """Parse Overpass JSON elements into adjacency list + coordinate dict."""
+    node_coords: dict[int, tuple[float, float]] = {}
+    graph: dict[int, list] = {}
+
+    for el in elements:
+        if el['type'] == 'node':
+            node_coords[el['id']] = (el['lat'], el['lon'])
+
+    for el in elements:
+        if el['type'] != 'way':
+            continue
+        highway = el.get('tags', {}).get('highway', '')
+        if highway not in _WALKABLE:
+            continue
+        nodes = el['nodes']
+        for i in range(len(nodes) - 1):
+            a, b = nodes[i], nodes[i + 1]
+            if a not in node_coords or b not in node_coords:
+                continue
+            w = _haversine(*node_coords[a], *node_coords[b])
+            graph.setdefault(a, []).append((b, w))
+            graph.setdefault(b, []).append((a, w))   # bidirectional
+
+    return graph, node_coords
+
+
+def _nearest_node(lat: float, lon: float, coords: dict) -> int:
+    """Return ID of OSM node nearest to (lat, lon)."""
+    return min(coords, key=lambda n: _haversine(lat, lon, *coords[n]))
+
+
+# ── ROS node ──────────────────────────────────────────────────────────────────
+
+class OsmRouterNode(Node):
+
+    def __init__(self):
+        super().__init__('osm_router')
+
+        self.declare_parameter('overpass_url',      _OVERPASS_URL)
+        self.declare_parameter('query_radius_m',    1500.0)   # area to fetch
+        self.declare_parameter('simplify_eps_m',    5.0)      # waypoint spacing
+        self.declare_parameter('cache_dir',         '/data/osm_cache')
+        self.declare_parameter('use_cache',         True)
+        self.declare_parameter('map_frame',         'map')
+
+        self._fix: Optional[NavSatFix] = None
+        self._cb_group = ReentrantCallbackGroup()
+
+        # ── Subscriptions ────────────────────────────────────────────────────
+        self.create_subscription(NavSatFix, '/gps/fix', self._fix_cb, 10)
+
+        # ── Publishers ───────────────────────────────────────────────────────
+        self._path_pub   = self.create_publisher(Path,        '/outdoor/route',  10)
+        self._status_pub = self.create_publisher(String,      '/outdoor/status', 10)
+        self._poses_pub  = self.create_publisher(PoseArray,   '/outdoor/waypoints', 10)
+
+        # ── Services ─────────────────────────────────────────────────────────
+        # /outdoor/plan_route — not yet triggered, called externally with goal.
+        # Goal lat/lon passed as parameter overrides for simplicity in V1.
+        self.declare_parameter('goal_lat', 0.0)
+        self.declare_parameter('goal_lon', 0.0)
+        self.create_service(
+            Trigger, '/outdoor/plan_route',
+            self._plan_route_cb,
+            callback_group=self._cb_group,
+        )
+
+        self._publish_status('idle')
+        self.get_logger().info('osm_router: ready. Call /outdoor/plan_route to start.')
+
+    # ── Callbacks ─────────────────────────────────────────────────────────────
+
+    def _fix_cb(self, msg: NavSatFix) -> None:
+        self._fix = msg
+
+    def _plan_route_cb(self, _req, response):
+        if self._fix is None:
+            response.success = False
+            response.message = 'No GPS fix yet'
+            return response
+
+        goal_lat = self.get_parameter('goal_lat').value
+        goal_lon = self.get_parameter('goal_lon').value
+        if goal_lat == 0.0 and goal_lon == 0.0:
+            response.success = False
+            response.message = 'goal_lat/goal_lon not set'
+            return response
+
+        self._publish_status('planning')
+        try:
+            path_msg, pose_array = self._plan(
+                self._fix.latitude, self._fix.longitude,
+                goal_lat, goal_lon,
+            )
+            self._path_pub.publish(path_msg)
+            self._poses_pub.publish(pose_array)
+            self._publish_status('ready')
+            response.success = True
+            response.message = f'{len(pose_array.poses)} waypoints planned'
+        except Exception as exc:
+            self.get_logger().error(f'Route planning failed: {exc}')
+            self._publish_status(f'error:{exc}')
+            response.success = False
+            response.message = str(exc)
+        return response
+
+    # ── Planning ──────────────────────────────────────────────────────────────
+
+    def _plan(self, slat, slon, glat, glon) -> tuple[Path, PoseArray]:
+        radius = self.get_parameter('query_radius_m').value
+        eps    = self.get_parameter('simplify_eps_m').value
+        frame  = self.get_parameter('map_frame').value
+
+        # Bounding box centred on midpoint
+        mlat = (slat + glat) / 2
+        mlon = (slon + glon) / 2
+        d_deg = (radius / 111_320.0) * 1.5   # pad
+        bbox = (mlat - d_deg, mlon - d_deg, mlat + d_deg, mlon + d_deg)
+
+        elements = self._fetch_osm(bbox)
+        graph, coords = _build_graph(elements)
+
+        if not coords:
+            raise RuntimeError('No walkable OSM nodes in query area')
+
+        start_node = _nearest_node(slat, slon, coords)
+        goal_node  = _nearest_node(glat, glon, coords)
+
+        node_path = _astar(graph, start_node, goal_node, coords)
+        if node_path is None:
+            raise RuntimeError('No walkable path found between start and goal')
+
+        raw_pts = [coords[n] for n in node_path]
+        simplified = _douglas_peucker(raw_pts, eps)
+
+        self.get_logger().info(
+            f'Route: {len(raw_pts)} → {len(simplified)} waypoints after simplification'
+        )
+
+        # Build nav_msgs/Path in map frame using ENU approximation.
+        # navsat_transform_node provides the GPS→map projection; here we use
+        # a local flat-earth approximation centred on the start position.
+        # Replace with /fromLL service call when robot_localization is running.
+        path_msg  = Path()
+        pose_arr  = PoseArray()
+        now = self.get_clock().now().to_msg()
+        path_msg.header.stamp = now
+        path_msg.header.frame_id = frame
+        pose_arr.header = path_msg.header
+
+        lat0, lon0 = slat, slon
+        for lat, lon in simplified:
+            x, y = _latlon_to_enu(lat, lon, lat0, lon0)
+            ps = PoseStamped()
+            ps.header = path_msg.header
+            ps.pose.position.x = x
+            ps.pose.position.y = y
+            ps.pose.orientation.w = 1.0
+            path_msg.poses.append(ps)
+
+            p = Pose()
+            p.position.x = x
+            p.position.y = y
+            p.orientation.w = 1.0
+            pose_arr.poses.append(p)
+
+        return path_msg, pose_arr
+
+    # ── Overpass fetch ────────────────────────────────────────────────────────
+
+    def _fetch_osm(self, bbox: tuple) -> list:
+        """Query Overpass for walkable ways in bbox. Returns element list."""
+        if not _REQUESTS_OK:
+            raise RuntimeError('requests library not installed (pip3 install requests)')
+
+        s, w, n, e = bbox
+        query = f"""
+[out:json][timeout:{_OVERPASS_TIMEOUT}];
+(
+  way["highway"~"^(footway|cycleway|path|pedestrian|living_street|residential|service|track|steps)$"]
+     ({s:.6f},{w:.6f},{n:.6f},{e:.6f});
+);
+out body;
+>;
+out skel qt;
+"""
+        url = self.get_parameter('overpass_url').value
+        self.get_logger().info(f'Querying Overpass: bbox={s:.4f},{w:.4f},{n:.4f},{e:.4f}')
+        resp = requests.post(url, data={'data': query}, timeout=_OVERPASS_TIMEOUT + 5)
+        resp.raise_for_status()
+        data = resp.json()
+        elements = data.get('elements', [])
+        self.get_logger().info(f'Overpass returned {len(elements)} elements')
+        return elements
+
+    # ── Helpers ───────────────────────────────────────────────────────────────
+
+    def _publish_status(self, msg: str) -> None:
+        s = String()
+        s.data = msg
+        self._status_pub.publish(s)
+
+
+def _latlon_to_enu(lat: float, lon: float, lat0: float, lon0: float) -> tuple[float, float]:
+    """Flat-earth ENU approximation (valid within ~10 km)."""
+    dlat = math.radians(lat - lat0)
+    dlon = math.radians(lon - lon0)
+    cos0 = math.cos(math.radians(lat0))
+    x = dlon * cos0 * 6_371_000.0   # East
+    y = dlat * 6_371_000.0           # North
+    return x, y
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = OsmRouterNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.try_shutdown()
+
+
+if __name__ == '__main__':
+    main()

jetson/ros2_ws/src/saltybot_outdoor/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/saltybot_outdoor
+[install]
+install_scripts=$base/lib/saltybot_outdoor

jetson/ros2_ws/src/saltybot_outdoor/setup.py

@@ -0,0 +1,34 @@
+import os
+from glob import glob
+from setuptools import find_packages, setup
+
+package_name = 'saltybot_outdoor'
+
+setup(
+    name=package_name,
+    version='0.1.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+         ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name, 'launch'),
+         glob('launch/*.py')),
+        (os.path.join('share', package_name, 'config'),
+         glob('config/*.yaml')),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='seb',
+    maintainer_email='seb@saltylab.io',
+    description='Outdoor navigation: OSM routing, GPS waypoint following, geofence',
+    license='MIT',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'osm_router_node       = saltybot_outdoor.osm_router_node:main',
+            'gps_waypoint_follower = saltybot_outdoor.gps_waypoint_follower_node:main',
+            'geofence_node         = saltybot_outdoor.geofence_node:main',
+        ],
+    },
+)

projects/saltybot/SLAM-SETUP-PLAN.md

@@ -92,7 +92,8 @@ Jetson Orin Nano Super (Ubuntu 22.04 / JetPack 6 / CUDA 12.x)
 | 2a | ✅ Done (PR #17) | Sensor drivers — `saltybot_bringup` package |
 | 2a+ | ✅ Done (PR #36) | Orin update: Dockerfile JetPack 6, RTAB-Map launch + config |
 | 2b | ✅ Done (PR #49) | Nav2 integration — path planning + obstacle avoidance |
-| 2c | ✅ Done (this PR) | 4× IMX219 surround vision + Nav2 camera obstacle layer |
+| 2c | ✅ Done (PR #52) | 4× IMX219 surround vision + Nav2 camera obstacle layer |
+| 2d | ✅ Done (this PR) | Outdoor navigation — OSM routing + GPS waypoints + geofence |
 
 ---