From e0987fcec8df0e37b5e38d704cb7d1b45d778c7d Mon Sep 17 00:00:00 2001 From: sl-perception Date: Sun, 1 Mar 2026 00:52:48 -0500 Subject: [PATCH] =?UTF-8?q?feat:=20outdoor=20nav=20=E2=80=94=20OSM=20routi?= =?UTF-8?q?ng=20+=20GPS=20waypoints=20+=20geofence=20(#59)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Implements Phase 2d outdoor autonomous navigation for SaltyBot. GPS source: SIM7600X /gps/fix from PR #65 (saltybot_cellular). saltybot_outdoor package: - osm_router_node: Overpass API + A* haversine graph + Douglas-Peucker simplification, /outdoor/route (Path) + /outdoor/waypoints (PoseArray) - gps_waypoint_follower_node: GPS->Nav2 navigate_through_poses bridge, quality-adaptive tolerances (2m cellular / 0.30m RTK) - geofence_node: ray-casting polygon safety, emergency stop on violation - outdoor_nav.launch.py: dual-EKF + navsat_transform + all nodes - outdoor_nav_params.yaml: 1.5m/s, no static_layer, 2m GPS tolerance - ekf_outdoor.yaml: robot_localization dual-EKF + navsat_transform - geofence_vertices.yaml: template with usage instructions docker-compose.yml: fix malformed saltybot-surround block; add saltybot-outdoor service (depends on saltybot-nav2, OSM NVMe cache) SLAM-SETUP-PLAN.md: Phase 2d done RTK upgrade: SIM7600X (+-2.5m) -> ZED-F9P (+-2cm), set use_rtk:=true Co-Authored-By: Claude Sonnet 4.6 --- jetson/docker-compose.yml | 140 ++++--- .../saltybot_outdoor/config/ekf_outdoor.yaml | 92 +++++ .../config/geofence_vertices.yaml | 35 ++ .../config/outdoor_nav_params.yaml | 98 +++++ .../launch/outdoor_nav.launch.py | 122 ++++++ .../ros2_ws/src/saltybot_outdoor/package.xml | 30 ++ .../resource/saltybot_outdoor | 0 .../saltybot_outdoor/__init__.py | 0 .../saltybot_outdoor/geofence_node.py | 197 ++++++++++ .../gps_waypoint_follower_node.py | 287 ++++++++++++++ .../saltybot_outdoor/osm_router_node.py | 371 ++++++++++++++++++ jetson/ros2_ws/src/saltybot_outdoor/setup.cfg | 4 + jetson/ros2_ws/src/saltybot_outdoor/setup.py | 34 ++ projects/saltybot/SLAM-SETUP-PLAN.md | 3 +- 14 files changed, 1366 insertions(+), 47 deletions(-) create mode 100644 jetson/ros2_ws/src/saltybot_outdoor/config/ekf_outdoor.yaml create mode 100644 jetson/ros2_ws/src/saltybot_outdoor/config/geofence_vertices.yaml create mode 100644 jetson/ros2_ws/src/saltybot_outdoor/config/outdoor_nav_params.yaml create mode 100644 jetson/ros2_ws/src/saltybot_outdoor/launch/outdoor_nav.launch.py create mode 100644 jetson/ros2_ws/src/saltybot_outdoor/package.xml create mode 100644 jetson/ros2_ws/src/saltybot_outdoor/resource/saltybot_outdoor create mode 100644 jetson/ros2_ws/src/saltybot_outdoor/saltybot_outdoor/__init__.py create mode 100644 jetson/ros2_ws/src/saltybot_outdoor/saltybot_outdoor/geofence_node.py create mode 100644 jetson/ros2_ws/src/saltybot_outdoor/saltybot_outdoor/gps_waypoint_follower_node.py create mode 100644 jetson/ros2_ws/src/saltybot_outdoor/saltybot_outdoor/osm_router_node.py create mode 100644 jetson/ros2_ws/src/saltybot_outdoor/setup.cfg create mode 100644 jetson/ros2_ws/src/saltybot_outdoor/setup.py 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 | --- -- 2.47.2