diff --git a/jetson/docker-compose.yml b/jetson/docker-compose.yml
index 8b5c531..e1c9d4a 100644
--- a/jetson/docker-compose.yml
+++ b/jetson/docker-compose.yml
@@ -14,44 +14,30 @@ services:
dockerfile: Dockerfile
container_name: saltybot-ros2
restart: unless-stopped
- runtime: nvidia # JetPack NVIDIA runtime
- privileged: false # use device passthrough instead
- network_mode: host # ROS2 DDS multicast needs host networking
-
+ runtime: nvidia
+ privileged: false
+ 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
- - stm32-bridge # needs /saltybot/imu, /saltybot/balance_state
- - csi-cameras # needs /camera/*/image_raw for compression
+ - saltybot-ros2
+ - stm32-bridge
+ - 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
diff --git a/jetson/ros2_ws/src/saltybot_outdoor/config/ekf_outdoor.yaml b/jetson/ros2_ws/src/saltybot_outdoor/config/ekf_outdoor.yaml
new file mode 100644
index 0000000..1be2a7a
--- /dev/null
+++ b/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
diff --git a/jetson/ros2_ws/src/saltybot_outdoor/config/geofence_vertices.yaml b/jetson/ros2_ws/src/saltybot_outdoor/config/geofence_vertices.yaml
new file mode 100644
index 0000000..ccf82d3
--- /dev/null
+++ b/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
diff --git a/jetson/ros2_ws/src/saltybot_outdoor/config/outdoor_nav_params.yaml b/jetson/ros2_ws/src/saltybot_outdoor/config/outdoor_nav_params.yaml
new file mode 100644
index 0000000..993b97d
--- /dev/null
+++ b/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
diff --git a/jetson/ros2_ws/src/saltybot_outdoor/launch/outdoor_nav.launch.py b/jetson/ros2_ws/src/saltybot_outdoor/launch/outdoor_nav.launch.py
new file mode 100644
index 0000000..236eea3
--- /dev/null
+++ b/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,
+ ])
diff --git a/jetson/ros2_ws/src/saltybot_outdoor/package.xml b/jetson/ros2_ws/src/saltybot_outdoor/package.xml
new file mode 100644
index 0000000..ff484f6
--- /dev/null
+++ b/jetson/ros2_ws/src/saltybot_outdoor/package.xml
@@ -0,0 +1,30 @@
+
+
+
+ saltybot_outdoor
+ 0.1.0
+ Outdoor navigation for SaltyBot — OSM routing, GPS waypoint following, geofence
+ seb
+ MIT
+
+ ament_python
+
+ rclpy
+ sensor_msgs
+ geometry_msgs
+ nav_msgs
+ std_msgs
+ std_srvs
+ nav2_msgs
+
+ robot_localization
+ nav2_bringup
+
+ ament_copyright
+ ament_flake8
+ ament_pep257
+
+
+ ament_python
+
+
diff --git a/jetson/ros2_ws/src/saltybot_outdoor/resource/saltybot_outdoor b/jetson/ros2_ws/src/saltybot_outdoor/resource/saltybot_outdoor
new file mode 100644
index 0000000..e69de29
diff --git a/jetson/ros2_ws/src/saltybot_outdoor/saltybot_outdoor/__init__.py b/jetson/ros2_ws/src/saltybot_outdoor/saltybot_outdoor/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/jetson/ros2_ws/src/saltybot_outdoor/saltybot_outdoor/geofence_node.py b/jetson/ros2_ws/src/saltybot_outdoor/saltybot_outdoor/geofence_node.py
new file mode 100644
index 0000000..b035151
--- /dev/null
+++ b/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:,'
+ /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()
diff --git a/jetson/ros2_ws/src/saltybot_outdoor/saltybot_outdoor/gps_waypoint_follower_node.py b/jetson/ros2_ws/src/saltybot_outdoor/saltybot_outdoor/gps_waypoint_follower_node.py
new file mode 100644
index 0000000..3503327
--- /dev/null
+++ b/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()
diff --git a/jetson/ros2_ws/src/saltybot_outdoor/saltybot_outdoor/osm_router_node.py b/jetson/ros2_ws/src/saltybot_outdoor/saltybot_outdoor/osm_router_node.py
new file mode 100644
index 0000000..67bd735
--- /dev/null
+++ b/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:'
+
+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()
diff --git a/jetson/ros2_ws/src/saltybot_outdoor/setup.cfg b/jetson/ros2_ws/src/saltybot_outdoor/setup.cfg
new file mode 100644
index 0000000..3f046da
--- /dev/null
+++ b/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
diff --git a/jetson/ros2_ws/src/saltybot_outdoor/setup.py b/jetson/ros2_ws/src/saltybot_outdoor/setup.py
new file mode 100644
index 0000000..dcffb49
--- /dev/null
+++ b/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',
+ ],
+ },
+)
diff --git a/projects/saltybot/SLAM-SETUP-PLAN.md b/projects/saltybot/SLAM-SETUP-PLAN.md
index 4d7e507..844011a 100644
--- a/projects/saltybot/SLAM-SETUP-PLAN.md
+++ b/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 |
---