From a4a2953326eac66c8b919b643daffcdeb3a8bde6 Mon Sep 17 00:00:00 2001 From: sl-jetson Date: Fri, 10 Apr 2026 20:42:36 -0400 Subject: [PATCH] =?UTF-8?q?feat:=20Here4=20DroneCAN=20GPS=20driver=20+=20N?= =?UTF-8?q?TRIP=20client=20(RTK=20ready)=20=E2=80=94=20Issue=20#725?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit New packages: - saltybot_dronecan_gps: DroneCAN driver for CubePilot Here4 RTK GPS - Subscribes uavcan.equipment.gnss.Fix2 (ID 1063) on can0 at 1Mbps - Publishes /gps/fix (NavSatFix), /gps/vel (TwistStamped), /gps/rtk_status - Maps DroneCAN fix_type 0-6 → sensor_msgs NavSatStatus + RTK label - Optional compass via uavcan.equipment.ahrs.MagneticFieldStrength - saltybot_ntrip_client: NTRIP RTCM3 → DroneCAN RTCMStream forwarding - Connects to rtk2go.com:2101 (configurable), auto-reconnects - Forwards corrections to Here4 via uavcan.equipment.gnss.RTCMStream - Publishes /ntrip/status (CONNECTED / DISCONNECTED / ERROR:) New launch file: - here4_gps.launch.py: launches both nodes with unified CAN + NTRIP params Updated: - can_setup.sh: adds 1Mbps DroneCAN mode (sudo ./can_setup.sh up dronecan) keeping 500kbps VESC default; CAN_BITRATE env var still overrides both - docker-compose.yml: adds here4-gps service with /dev/can0 device passthrough and NET_ADMIN cap; resolves leftover merge conflict markers - outdoor_nav_params.yaml: adds Here4 config section, updates RTK docs Co-Authored-By: Claude Sonnet 4.6 --- jetson/docker-compose.yml | 57 ++++- .../config/dronecan_gps_params.yaml | 9 + .../launch/here4_gps.launch.py | 110 ++++++++++ .../src/saltybot_dronecan_gps/package.xml | 34 +++ .../resource/saltybot_dronecan_gps | 1 + .../saltybot_dronecan_gps/__init__.py | 0 .../dronecan_gps_node.py | 204 +++++++++++++++++ .../src/saltybot_dronecan_gps/setup.cfg | 4 + .../src/saltybot_dronecan_gps/setup.py | 32 +++ .../config/ntrip_params.yaml | 18 ++ .../src/saltybot_ntrip_client/package.xml | 32 +++ .../resource/saltybot_ntrip_client | 1 + .../saltybot_ntrip_client/__init__.py | 0 .../ntrip_client_node.py | 207 ++++++++++++++++++ .../src/saltybot_ntrip_client/setup.cfg | 4 + .../src/saltybot_ntrip_client/setup.py | 32 +++ .../config/outdoor_nav_params.yaml | 48 +++- jetson/scripts/can_setup.sh | 36 ++- 18 files changed, 800 insertions(+), 29 deletions(-) create mode 100644 jetson/ros2_ws/src/saltybot_dronecan_gps/config/dronecan_gps_params.yaml create mode 100644 jetson/ros2_ws/src/saltybot_dronecan_gps/launch/here4_gps.launch.py create mode 100644 jetson/ros2_ws/src/saltybot_dronecan_gps/package.xml create mode 100644 jetson/ros2_ws/src/saltybot_dronecan_gps/resource/saltybot_dronecan_gps create mode 100644 jetson/ros2_ws/src/saltybot_dronecan_gps/saltybot_dronecan_gps/__init__.py create mode 100644 jetson/ros2_ws/src/saltybot_dronecan_gps/saltybot_dronecan_gps/dronecan_gps_node.py create mode 100644 jetson/ros2_ws/src/saltybot_dronecan_gps/setup.cfg create mode 100644 jetson/ros2_ws/src/saltybot_dronecan_gps/setup.py create mode 100644 jetson/ros2_ws/src/saltybot_ntrip_client/config/ntrip_params.yaml create mode 100644 jetson/ros2_ws/src/saltybot_ntrip_client/package.xml create mode 100644 jetson/ros2_ws/src/saltybot_ntrip_client/resource/saltybot_ntrip_client create mode 100644 jetson/ros2_ws/src/saltybot_ntrip_client/saltybot_ntrip_client/__init__.py create mode 100644 jetson/ros2_ws/src/saltybot_ntrip_client/saltybot_ntrip_client/ntrip_client_node.py create mode 100644 jetson/ros2_ws/src/saltybot_ntrip_client/setup.cfg create mode 100644 jetson/ros2_ws/src/saltybot_ntrip_client/setup.py diff --git a/jetson/docker-compose.yml b/jetson/docker-compose.yml index 97b108f..6f74f29 100644 --- a/jetson/docker-compose.yml +++ b/jetson/docker-compose.yml @@ -97,11 +97,7 @@ services: rgb_camera.profile:=640x480x30 " -<<<<<<< HEAD - # ── ESP32 bridge node (bidirectional serial<->ROS2) ──────────────────────── -======= - # ── ESP32-S3 bridge node (bidirectional serial<->ROS2) ──────────────────────── ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) + # ── ESP32-S3 bridge node (bidirectional serial<->ROS2) ─────────────────────── esp32-bridge: image: saltybot/ros2-humble:jetson-orin build: @@ -212,13 +208,8 @@ services: " -<<<<<<< HEAD - # -- Remote e-stop bridge (MQTT over 4G -> ESP32 CDC) ---------------------- - # Subscribes to saltybot/estop MQTT topic. {"kill":true} -> 'E\r\n' to ESP32 BALANCE. -======= - # -- Remote e-stop bridge (MQTT over 4G -> ESP32-S3 CDC) ---------------------- + # -- Remote e-stop bridge (MQTT over 4G -> ESP32-S3 CDC) --------------------- # Subscribes to saltybot/estop MQTT topic. {"kill":true} -> 'E\r\n' to ESP32-S3. ->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only) # Cellular watchdog: 5s MQTT drop in AUTO mode -> 'F\r\n' (ESTOP_CELLULAR_TIMEOUT). remote-estop: image: saltybot/ros2-humble:jetson-orin @@ -366,6 +357,50 @@ services: " + # ── Here4 DroneCAN GPS + NTRIP RTK client ──────────────────────────────── + # Issue #725 — CubePilot Here4 RTK GPS via DroneCAN (1Mbps, SocketCAN) + # Start: docker compose up -d here4-gps + # Monitor fix: docker compose exec here4-gps ros2 topic echo /gps/rtk_status + # Configure NTRIP: set NTRIP_MOUNT, NTRIP_USER, NTRIP_PASSWORD env vars + here4-gps: + image: saltybot/ros2-humble:jetson-orin + build: + context: . + dockerfile: Dockerfile + container_name: saltybot-here4-gps + restart: unless-stopped + runtime: nvidia + network_mode: host + depends_on: + - saltybot-nav2 + environment: + - ROS_DOMAIN_ID=42 + - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp + # NTRIP credentials — set these in your .env or override at runtime + - NTRIP_MOUNT=${NTRIP_MOUNT:-} + - NTRIP_USER=${NTRIP_USER:-} + - NTRIP_PASSWORD=${NTRIP_PASSWORD:-} + volumes: + - ./ros2_ws/src:/ros2_ws/src:rw + - ./config:/config:ro + devices: + - /dev/can0:/dev/can0 + cap_add: + - NET_ADMIN # needed for SocketCAN ip link set can0 up inside container + command: > + bash -c " + source /opt/ros/humble/setup.bash && + source /ros2_ws/install/local_setup.bash 2>/dev/null || true && + pip install python-dronecan --quiet 2>/dev/null || true && + ros2 launch saltybot_dronecan_gps here4_gps.launch.py + can_interface:=can0 + can_bitrate:=1000000 + ntrip_caster:=rtk2go.com + ntrip_mount:=${NTRIP_MOUNT:-} + ntrip_user:=${NTRIP_USER:-} + ntrip_password:=${NTRIP_PASSWORD:-} + " + volumes: saltybot-maps: driver: local diff --git a/jetson/ros2_ws/src/saltybot_dronecan_gps/config/dronecan_gps_params.yaml b/jetson/ros2_ws/src/saltybot_dronecan_gps/config/dronecan_gps_params.yaml new file mode 100644 index 0000000..ab40189 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_dronecan_gps/config/dronecan_gps_params.yaml @@ -0,0 +1,9 @@ +# dronecan_gps_params.yaml — CubePilot Here4 DroneCAN GPS driver defaults +# Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/725 + +dronecan_gps: + ros__parameters: + can_interface: "can0" + can_bitrate: 1000000 # Here4 default: 1Mbps DroneCAN + node_id: 127 # DroneCAN local node ID (GPS driver) + publish_compass: true # publish MagneticFieldStrength if available diff --git a/jetson/ros2_ws/src/saltybot_dronecan_gps/launch/here4_gps.launch.py b/jetson/ros2_ws/src/saltybot_dronecan_gps/launch/here4_gps.launch.py new file mode 100644 index 0000000..ae49ba0 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_dronecan_gps/launch/here4_gps.launch.py @@ -0,0 +1,110 @@ +""" +here4_gps.launch.py — CubePilot Here4 RTK GPS full stack +Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/725 + +Launches: + - dronecan_gps_node (saltybot_dronecan_gps) + - ntrip_client_node (saltybot_ntrip_client) + +Usage (minimal): + ros2 launch saltybot_dronecan_gps here4_gps.launch.py \\ + ntrip_mount:=RTCM3_GENERIC ntrip_user:=you@email.com + +Full options: + ros2 launch saltybot_dronecan_gps here4_gps.launch.py \\ + can_interface:=can0 \\ + can_bitrate:=1000000 \\ + ntrip_caster:=rtk2go.com \\ + ntrip_port:=2101 \\ + ntrip_mount:=MYBASE \\ + ntrip_user:=you@email.com \\ + ntrip_password:=secret +""" + +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() -> LaunchDescription: + gps_cfg = os.path.join( + get_package_share_directory('saltybot_dronecan_gps'), + 'config', 'dronecan_gps_params.yaml', + ) + ntrip_cfg = os.path.join( + get_package_share_directory('saltybot_ntrip_client'), + 'config', 'ntrip_params.yaml', + ) + + return LaunchDescription([ + # ── Shared CAN args ─────────────────────────────────────────────────── + DeclareLaunchArgument( + 'can_interface', default_value='can0', + description='SocketCAN interface name', + ), + DeclareLaunchArgument( + 'can_bitrate', default_value='1000000', + description='CAN bus bitrate — Here4 default is 1000000 (1 Mbps)', + ), + + # ── NTRIP args ──────────────────────────────────────────────────────── + DeclareLaunchArgument( + 'ntrip_caster', default_value='rtk2go.com', + description='NTRIP caster hostname', + ), + DeclareLaunchArgument( + 'ntrip_port', default_value='2101', + description='NTRIP caster port', + ), + DeclareLaunchArgument( + 'ntrip_mount', default_value='', + description='NTRIP mount point (REQUIRED)', + ), + DeclareLaunchArgument( + 'ntrip_user', default_value='', + description='NTRIP username (rtk2go.com requires email address)', + ), + DeclareLaunchArgument( + 'ntrip_password', default_value='', + description='NTRIP password', + ), + + # ── DroneCAN GPS node ───────────────────────────────────────────────── + Node( + package='saltybot_dronecan_gps', + executable='dronecan_gps_node', + name='dronecan_gps', + output='screen', + parameters=[ + gps_cfg, + { + 'can_interface': LaunchConfiguration('can_interface'), + 'can_bitrate': LaunchConfiguration('can_bitrate'), + }, + ], + ), + + # ── NTRIP client node ───────────────────────────────────────────────── + Node( + package='saltybot_ntrip_client', + executable='ntrip_client_node', + name='ntrip_client', + output='screen', + parameters=[ + ntrip_cfg, + { + 'can_interface': LaunchConfiguration('can_interface'), + 'can_bitrate': LaunchConfiguration('can_bitrate'), + 'ntrip_caster': LaunchConfiguration('ntrip_caster'), + 'ntrip_port': LaunchConfiguration('ntrip_port'), + 'ntrip_mount': LaunchConfiguration('ntrip_mount'), + 'ntrip_user': LaunchConfiguration('ntrip_user'), + 'ntrip_password': LaunchConfiguration('ntrip_password'), + }, + ], + ), + ]) diff --git a/jetson/ros2_ws/src/saltybot_dronecan_gps/package.xml b/jetson/ros2_ws/src/saltybot_dronecan_gps/package.xml new file mode 100644 index 0000000..c4fd972 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_dronecan_gps/package.xml @@ -0,0 +1,34 @@ + + + + saltybot_dronecan_gps + 0.1.0 + + DroneCAN GPS driver for CubePilot Here4 RTK. + Subscribes to uavcan.equipment.gnss.Fix2 (ID 1063) and + uavcan.equipment.ahrs.MagneticFieldStrength on CAN bus, + publishes /gps/fix (NavSatFix), /gps/vel (TwistStamped), + and /gps/rtk_status (String). + Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/725 + + Sebastien Vayrette + MIT + + ament_python + + rclpy + sensor_msgs + geometry_msgs + std_msgs + + python3-pip + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/jetson/ros2_ws/src/saltybot_dronecan_gps/resource/saltybot_dronecan_gps b/jetson/ros2_ws/src/saltybot_dronecan_gps/resource/saltybot_dronecan_gps new file mode 100644 index 0000000..e18544e --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_dronecan_gps/resource/saltybot_dronecan_gps @@ -0,0 +1 @@ +saltybot_dronecan_gps diff --git a/jetson/ros2_ws/src/saltybot_dronecan_gps/saltybot_dronecan_gps/__init__.py b/jetson/ros2_ws/src/saltybot_dronecan_gps/saltybot_dronecan_gps/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_dronecan_gps/saltybot_dronecan_gps/dronecan_gps_node.py b/jetson/ros2_ws/src/saltybot_dronecan_gps/saltybot_dronecan_gps/dronecan_gps_node.py new file mode 100644 index 0000000..35103ad --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_dronecan_gps/saltybot_dronecan_gps/dronecan_gps_node.py @@ -0,0 +1,204 @@ +#!/usr/bin/env python3 +""" +dronecan_gps_node.py — DroneCAN GPS driver for CubePilot Here4 RTK +Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/725 + +Subscribes to: + uavcan.equipment.gnss.Fix2 (msg ID 1063) — position + fix status + uavcan.equipment.ahrs.MagneticFieldStrength — compass (optional) + +Publishes: + /gps/fix sensor_msgs/NavSatFix + /gps/vel geometry_msgs/TwistStamped + /gps/rtk_status std_msgs/String + +DroneCAN fix_type → sensor_msgs status mapping: + 0 = NO_FIX → STATUS_NO_FIX (-1) + 1 = TIME_ONLY → STATUS_NO_FIX (-1) + 2 = 2D_FIX → STATUS_FIX (0) + 3 = 3D_FIX → STATUS_FIX (0) + 4 = DGPS → STATUS_SBAS_FIX (1) + 5 = RTK_FLOAT → STATUS_GBAS_FIX (2) + 6 = RTK_FIXED → STATUS_GBAS_FIX (2) +""" + +import math +import threading + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy + +from sensor_msgs.msg import NavSatFix, NavSatStatus +from geometry_msgs.msg import TwistStamped +from std_msgs.msg import String + +try: + import dronecan +except ImportError: + dronecan = None + +_SENSOR_QOS = QoSProfile( + reliability=ReliabilityPolicy.BEST_EFFORT, + history=HistoryPolicy.KEEP_LAST, + depth=5, +) + +# DroneCAN fix_type → (NavSatStatus.status, rtk_label) +_FIX_MAP = { + 0: (NavSatStatus.STATUS_NO_FIX, 'NO_FIX'), + 1: (NavSatStatus.STATUS_NO_FIX, 'TIME_ONLY'), + 2: (NavSatStatus.STATUS_FIX, '2D_FIX'), + 3: (NavSatStatus.STATUS_FIX, '3D_FIX'), + 4: (NavSatStatus.STATUS_SBAS_FIX, 'DGPS'), + 5: (NavSatStatus.STATUS_GBAS_FIX, 'RTK_FLOAT'), + 6: (NavSatStatus.STATUS_GBAS_FIX, 'RTK_FIXED'), +} + + +class DroneCanGpsNode(Node): + def __init__(self) -> None: + super().__init__('dronecan_gps') + + self.declare_parameter('can_interface', 'can0') + self.declare_parameter('can_bitrate', 1000000) + self.declare_parameter('node_id', 127) # DroneCAN local node ID + self.declare_parameter('publish_compass', True) + + self._iface = self.get_parameter('can_interface').value + self._bitrate = self.get_parameter('can_bitrate').value + self._node_id = self.get_parameter('node_id').value + self._publish_compass = self.get_parameter('publish_compass').value + + self._fix_pub = self.create_publisher(NavSatFix, '/gps/fix', _SENSOR_QOS) + self._vel_pub = self.create_publisher(TwistStamped, '/gps/vel', _SENSOR_QOS) + self._rtk_pub = self.create_publisher(String, '/gps/rtk_status', 10) + + if dronecan is None: + self.get_logger().error( + 'python-dronecan not installed. ' + 'Run: pip install python-dronecan' + ) + return + + self._lock = threading.Lock() + self._dc_node = None + self._spin_thread = threading.Thread( + target=self._dronecan_spin, daemon=True + ) + self._spin_thread.start() + self.get_logger().info( + f'DroneCanGpsNode started — interface={self._iface} ' + f'bitrate={self._bitrate}' + ) + + # ── DroneCAN spin (runs in background thread) ───────────────────────────── + + def _dronecan_spin(self) -> None: + try: + self._dc_node = dronecan.make_node( + self._iface, + node_id=self._node_id, + bitrate=self._bitrate, + ) + self._dc_node.add_handler( + dronecan.uavcan.equipment.gnss.Fix2, + self._on_fix2, + ) + if self._publish_compass: + self._dc_node.add_handler( + dronecan.uavcan.equipment.ahrs.MagneticFieldStrength, + self._on_mag, + ) + self.get_logger().info( + f'DroneCAN node online on {self._iface}' + ) + while rclpy.ok(): + self._dc_node.spin(timeout=0.1) + except Exception as exc: + self.get_logger().error(f'DroneCAN spin error: {exc}') + + # ── Message handlers ────────────────────────────────────────────────────── + + def _on_fix2(self, event) -> None: + msg = event.message + now = self.get_clock().now().to_msg() + + fix_type = int(msg.fix_type) + nav_status, rtk_label = _FIX_MAP.get( + fix_type, (NavSatStatus.STATUS_NO_FIX, 'UNKNOWN') + ) + + # NavSatFix + fix = NavSatFix() + fix.header.stamp = now + fix.header.frame_id = 'gps' + fix.status.status = nav_status + fix.status.service = NavSatStatus.SERVICE_GPS + + fix.latitude = math.degrees(msg.latitude_deg_1e8 * 1e-8) + fix.longitude = math.degrees(msg.longitude_deg_1e8 * 1e-8) + fix.altitude = msg.height_msl_mm * 1e-3 # mm → m + + # Covariance from position_covariance if available, else diagonal guess + if hasattr(msg, 'position_covariance') and len(msg.position_covariance) >= 9: + fix.position_covariance = list(msg.position_covariance) + fix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_FULL + else: + h_var = (msg.horizontal_pos_accuracy_m_1e2 * 1e-2) ** 2 \ + if hasattr(msg, 'horizontal_pos_accuracy_m_1e2') else 4.0 + v_var = (msg.vertical_pos_accuracy_m_1e2 * 1e-2) ** 2 \ + if hasattr(msg, 'vertical_pos_accuracy_m_1e2') else 4.0 + fix.position_covariance = [ + h_var, 0.0, 0.0, + 0.0, h_var, 0.0, + 0.0, 0.0, v_var, + ] + fix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_DIAGONAL_KNOWN + + self._fix_pub.publish(fix) + + # TwistStamped velocity + if hasattr(msg, 'ned_velocity'): + vel = TwistStamped() + vel.header.stamp = now + vel.header.frame_id = 'gps' + vel.twist.linear.x = float(msg.ned_velocity[0]) # North m/s + vel.twist.linear.y = float(msg.ned_velocity[1]) # East m/s + vel.twist.linear.z = float(msg.ned_velocity[2]) # Down m/s (ROS: up+) + self._vel_pub.publish(vel) + + # RTK status string + rtk_msg = String() + rtk_msg.data = rtk_label + self._rtk_pub.publish(rtk_msg) + + self.get_logger().debug( + f'Fix2: {fix.latitude:.6f},{fix.longitude:.6f} ' + f'alt={fix.altitude:.1f}m status={rtk_label}' + ) + + def _on_mag(self, event) -> None: + # Compass data logged; extend to publish /imu/mag if needed + msg = event.message + self.get_logger().debug( + f'Mag: {msg.magnetic_field_ga[0]:.3f} ' + f'{msg.magnetic_field_ga[1]:.3f} ' + f'{msg.magnetic_field_ga[2]:.3f} Ga' + ) + + +def main(args=None) -> None: + rclpy.init(args=args) + node = DroneCanGpsNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/jetson/ros2_ws/src/saltybot_dronecan_gps/setup.cfg b/jetson/ros2_ws/src/saltybot_dronecan_gps/setup.cfg new file mode 100644 index 0000000..965088c --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_dronecan_gps/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/saltybot_dronecan_gps +[install] +install_scripts=$base/lib/saltybot_dronecan_gps diff --git a/jetson/ros2_ws/src/saltybot_dronecan_gps/setup.py b/jetson/ros2_ws/src/saltybot_dronecan_gps/setup.py new file mode 100644 index 0000000..8c98732 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_dronecan_gps/setup.py @@ -0,0 +1,32 @@ +from setuptools import find_packages, setup +import os +from glob import glob + +package_name = 'saltybot_dronecan_gps' + +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='Sebastien Vayrette', + maintainer_email='seb@vayrette.com', + description='DroneCAN GPS driver for CubePilot Here4 RTK', + license='MIT', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'dronecan_gps_node = saltybot_dronecan_gps.dronecan_gps_node:main', + ], + }, +) diff --git a/jetson/ros2_ws/src/saltybot_ntrip_client/config/ntrip_params.yaml b/jetson/ros2_ws/src/saltybot_ntrip_client/config/ntrip_params.yaml new file mode 100644 index 0000000..1e4826e --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_ntrip_client/config/ntrip_params.yaml @@ -0,0 +1,18 @@ +# ntrip_params.yaml — NTRIP client configuration for Here4 RTK corrections +# Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/725 +# +# Override at launch: +# ros2 launch saltybot_dronecan_gps here4_gps.launch.py \ +# ntrip_mount:=MYBASE ntrip_user:=user ntrip_password:=pass + +ntrip_client: + ros__parameters: + ntrip_caster: "rtk2go.com" + ntrip_port: 2101 + ntrip_mount: "" # REQUIRED — set your mount point + ntrip_user: "" # empty = anonymous (rtk2go requires email) + ntrip_password: "" + can_interface: "can0" + can_bitrate: 1000000 + can_node_id: 126 # DroneCAN local node ID (NTRIP node) + reconnect_delay: 5.0 # seconds between reconnect attempts diff --git a/jetson/ros2_ws/src/saltybot_ntrip_client/package.xml b/jetson/ros2_ws/src/saltybot_ntrip_client/package.xml new file mode 100644 index 0000000..ada8a71 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_ntrip_client/package.xml @@ -0,0 +1,32 @@ + + + + saltybot_ntrip_client + 0.1.0 + + NTRIP client for RTK corrections. + Connects to an NTRIP caster (default: rtk2go.com:2101), receives RTCM3 + correction data, and forwards it to the Here4 via DroneCAN + (uavcan.equipment.gnss.RTCMStream) on the CAN bus. + Reconnects automatically on disconnect. + Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/725 + + Sebastien Vayrette + MIT + + ament_python + + rclpy + std_msgs + + python3-pip + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/jetson/ros2_ws/src/saltybot_ntrip_client/resource/saltybot_ntrip_client b/jetson/ros2_ws/src/saltybot_ntrip_client/resource/saltybot_ntrip_client new file mode 100644 index 0000000..b0e51e2 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_ntrip_client/resource/saltybot_ntrip_client @@ -0,0 +1 @@ +saltybot_ntrip_client diff --git a/jetson/ros2_ws/src/saltybot_ntrip_client/saltybot_ntrip_client/__init__.py b/jetson/ros2_ws/src/saltybot_ntrip_client/saltybot_ntrip_client/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_ntrip_client/saltybot_ntrip_client/ntrip_client_node.py b/jetson/ros2_ws/src/saltybot_ntrip_client/saltybot_ntrip_client/ntrip_client_node.py new file mode 100644 index 0000000..b187048 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_ntrip_client/saltybot_ntrip_client/ntrip_client_node.py @@ -0,0 +1,207 @@ +#!/usr/bin/env python3 +""" +ntrip_client_node.py — NTRIP client for Here4 RTK corrections +Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/725 + +Connects to an NTRIP caster (default: rtk2go.com:2101), streams RTCM3 +correction data, and forwards it to the Here4 GPS via DroneCAN +(uavcan.equipment.gnss.RTCMStream) on the CAN bus. + +Publishes: + /ntrip/status std_msgs/String — CONNECTED / DISCONNECTED / ERROR: + +Parameters: + ntrip_caster (str) rtk2go.com + ntrip_port (int) 2101 + ntrip_mount (str) '' — required, e.g. 'RTCM3_GENERIC' + ntrip_user (str) '' — leave empty for anonymous casters + ntrip_password (str) '' + can_interface (str) can0 + can_bitrate (int) 1000000 + can_node_id (int) 126 — DroneCAN local node ID for NTRIP node + reconnect_delay (float) 5.0 — seconds between reconnect attempts +""" + +import base64 +import socket +import threading +import time + +import rclpy +from rclpy.node import Node +from std_msgs.msg import String + +try: + import dronecan +except ImportError: + dronecan = None + +_NTRIP_AGENT = 'saltybot-ntrip/1.0' +_RTCM_CHUNK = 512 # bytes per DroneCAN RTCMStream message (max 128 per frame) +_RECV_BUF = 4096 + + +class NtripClientNode(Node): + def __init__(self) -> None: + super().__init__('ntrip_client') + + self.declare_parameter('ntrip_caster', 'rtk2go.com') + self.declare_parameter('ntrip_port', 2101) + self.declare_parameter('ntrip_mount', '') + self.declare_parameter('ntrip_user', '') + self.declare_parameter('ntrip_password', '') + self.declare_parameter('can_interface', 'can0') + self.declare_parameter('can_bitrate', 1000000) + self.declare_parameter('can_node_id', 126) + self.declare_parameter('reconnect_delay', 5.0) + + self._caster = self.get_parameter('ntrip_caster').value + self._port = self.get_parameter('ntrip_port').value + self._mount = self.get_parameter('ntrip_mount').value + self._user = self.get_parameter('ntrip_user').value + self._password = self.get_parameter('ntrip_password').value + self._iface = self.get_parameter('can_interface').value + self._bitrate = self.get_parameter('can_bitrate').value + self._node_id = self.get_parameter('can_node_id').value + self._reconnect_delay = self.get_parameter('reconnect_delay').value + + self._status_pub = self.create_publisher(String, '/ntrip/status', 10) + + if dronecan is None: + self.get_logger().error( + 'python-dronecan not installed. Run: pip install python-dronecan' + ) + self._publish_status('ERROR:dronecan_not_installed') + return + + if not self._mount: + self.get_logger().error( + 'ntrip_mount parameter is required (e.g. RTCM3_GENERIC)' + ) + self._publish_status('ERROR:no_mount_point') + return + + self._dc_node = dronecan.make_node( + self._iface, + node_id=self._node_id, + bitrate=self._bitrate, + ) + + self._stop_event = threading.Event() + self._thread = threading.Thread( + target=self._ntrip_loop, daemon=True + ) + self._thread.start() + + self.get_logger().info( + f'NTRIP client started — ' + f'{self._caster}:{self._port}/{self._mount}' + ) + + # ── NTRIP loop (reconnects on any error) ────────────────────────────────── + + def _ntrip_loop(self) -> None: + while not self._stop_event.is_set() and rclpy.ok(): + try: + self._connect_and_stream() + except Exception as exc: + self.get_logger().warn(f'NTRIP disconnected: {exc}') + self._publish_status(f'DISCONNECTED') + if not self._stop_event.is_set() and rclpy.ok(): + self.get_logger().info( + f'Reconnecting in {self._reconnect_delay}s…' + ) + time.sleep(self._reconnect_delay) + + def _connect_and_stream(self) -> None: + sock = socket.create_connection( + (self._caster, self._port), timeout=10.0 + ) + sock.settimeout(30.0) + + # Send NTRIP HTTP/1.1 GET request + request = self._build_request() + sock.sendall(request.encode('ascii')) + + # Read response header + header = b'' + while b'\r\n\r\n' not in header: + chunk = sock.recv(256) + if not chunk: + raise ConnectionError('Connection closed during header read') + header += chunk + + header_str = header.split(b'\r\n\r\n')[0].decode('ascii', errors='replace') + if 'ICY 200 OK' not in header_str and '200 OK' not in header_str: + raise ConnectionError(f'NTRIP rejected: {header_str[:120]}') + + self.get_logger().info( + f'NTRIP connected to {self._caster}:{self._port}/{self._mount}' + ) + self._publish_status('CONNECTED') + + # Any trailing bytes after header are RTCM data + leftover = header.split(b'\r\n\r\n', 1)[1] if b'\r\n\r\n' in header else b'' + if leftover: + self._forward_rtcm(leftover) + + while not self._stop_event.is_set() and rclpy.ok(): + data = sock.recv(_RECV_BUF) + if not data: + raise ConnectionError('NTRIP stream closed by server') + self._forward_rtcm(data) + + def _build_request(self) -> str: + lines = [ + f'GET /{self._mount} HTTP/1.1', + f'Host: {self._caster}:{self._port}', + f'User-Agent: {_NTRIP_AGENT}', + 'Ntrip-Version: Ntrip/2.0', + 'Connection: close', + ] + if self._user: + creds = base64.b64encode( + f'{self._user}:{self._password}'.encode() + ).decode() + lines.append(f'Authorization: Basic {creds}') + lines += ['', ''] + return '\r\n'.join(lines) + + # ── DroneCAN RTCMStream forwarding ──────────────────────────────────────── + + def _forward_rtcm(self, data: bytes) -> None: + """Chunk RTCM data into DroneCAN RTCMStream messages.""" + for i in range(0, len(data), _RTCM_CHUNK): + chunk = data[i:i + _RTCM_CHUNK] + try: + msg = dronecan.uavcan.equipment.gnss.RTCMStream() + msg.data = list(chunk) + self._dc_node.broadcast(msg) + self._dc_node.spin(timeout=0.0) + except Exception as exc: + self.get_logger().warn(f'DroneCAN send error: {exc}') + + def _publish_status(self, status: str) -> None: + msg = String() + msg.data = status + self._status_pub.publish(msg) + + def destroy_node(self) -> None: + self._stop_event.set() + super().destroy_node() + + +def main(args=None) -> None: + rclpy.init(args=args) + node = NtripClientNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/jetson/ros2_ws/src/saltybot_ntrip_client/setup.cfg b/jetson/ros2_ws/src/saltybot_ntrip_client/setup.cfg new file mode 100644 index 0000000..7d8e41a --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_ntrip_client/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/saltybot_ntrip_client +[install] +install_scripts=$base/lib/saltybot_ntrip_client diff --git a/jetson/ros2_ws/src/saltybot_ntrip_client/setup.py b/jetson/ros2_ws/src/saltybot_ntrip_client/setup.py new file mode 100644 index 0000000..7e494bd --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_ntrip_client/setup.py @@ -0,0 +1,32 @@ +from setuptools import find_packages, setup +import os +from glob import glob + +package_name = 'saltybot_ntrip_client' + +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='Sebastien Vayrette', + maintainer_email='seb@vayrette.com', + description='NTRIP client — RTCM3 corrections via DroneCAN to Here4', + license='MIT', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'ntrip_client_node = saltybot_ntrip_client.ntrip_client_node:main', + ], + }, +) 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 index 993b97d..3e1ee9f 100644 --- a/jetson/ros2_ws/src/saltybot_outdoor/config/outdoor_nav_params.yaml +++ b/jetson/ros2_ws/src/saltybot_outdoor/config/outdoor_nav_params.yaml @@ -1,22 +1,29 @@ # 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) +# RTK (active): CubePilot Here4 → ±2 cm CEP via DroneCAN (Issue #725) +# - Connected via CANable 2.0 (SocketCAN can0) at 1 Mbps +# - RTK corrections from NTRIP caster via saltybot_ntrip_client +# - Launch: docker compose up -d here4-gps +# (set NTRIP_MOUNT / NTRIP_USER / NTRIP_PASSWORD env vars) # # ── 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. +# Here4 DroneCAN fix_type mapping (saltybot_dronecan_gps): +# 5 = RTK_FLOAT → STATUS_GBAS_FIX (2) — sub-metre +# 6 = RTK_FIXED → STATUS_GBAS_FIX (2) — ±2 cm (best) +# Monitor: ros2 topic echo /gps/rtk_status # 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 +# ── Here4 setup procedure ─────────────────────────────────────────────────── +# 1. can_setup.sh up dronecan # bring up can0 at 1Mbps +# 2. Set NTRIP_MOUNT, NTRIP_USER, NTRIP_PASSWORD in .env +# 3. docker compose up -d here4-gps +# 4. Verify: ros2 topic echo /gps/rtk_status → RTK_FIXED +# 5. saltybot-outdoor: set use_rtk:=true in docker-compose.yml # # References: -# SparkFun RTK Express: https://docs.sparkfun.com/SparkFun_RTK_Everywhere_Firmware/ +# Here4 manual: https://docs.cubepilot.org/user-guides/here-4/here-4-manual # NTRIP caster list: https://rtk2go.com/sample-map/ # ───────────────────────────────────────────────────────────────────────────── @@ -96,3 +103,26 @@ global_costmap: inflation_radius: 0.50 obstacle_layer: observation_sources: scan surround_cameras + +# ── Here4 RTK GPS configuration (Issue #725) ──────────────────────────────── +# Active when here4-gps docker service is running. +# The dronecan_gps node publishes on the same /gps/fix topic as SIM7600X, +# so gps_waypoint_follower picks up RTK automatically. +# Set use_rtk:=true in saltybot-outdoor docker command to tighten tolerances. +here4_gps: + ros__parameters: + # CAN bus — must match can_setup.sh dronecan mode and docker-compose device + can_interface: "can0" + can_bitrate: 1000000 # 1 Mbps — Here4 DroneCAN default + # DroneCAN node IDs (must be unique on bus; VESCs use 61 and 79) + gps_node_id: 127 + ntrip_node_id: 126 + # NTRIP — override via env vars NTRIP_MOUNT / NTRIP_USER / NTRIP_PASSWORD + ntrip_caster: "rtk2go.com" + ntrip_port: 2101 + ntrip_mount: "" # set your mount point + ntrip_user: "" # rtk2go.com requires a valid email address + ntrip_password: "" + reconnect_delay: 5.0 # seconds between NTRIP reconnect attempts + # RTK goal tolerance — applied by gps_waypoint_follower when use_rtk:=true + goal_tolerance_xy_rtk: 0.3 # metres (vs 2.0m cellular) diff --git a/jetson/scripts/can_setup.sh b/jetson/scripts/can_setup.sh index d5db2fe..ea8a1b0 100755 --- a/jetson/scripts/can_setup.sh +++ b/jetson/scripts/can_setup.sh @@ -1,22 +1,36 @@ #!/usr/bin/env bash -# can_setup.sh — Bring up CANable 2.0 (gs_usb) as can0 at 500 kbps +# can_setup.sh — Bring up CANable 2.0 (gs_usb) as can0 # Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/643 # # Usage: -# sudo ./can_setup.sh # bring up -# sudo ./can_setup.sh down # bring down -# sudo ./can_setup.sh verify # candump one-shot check (Ctrl-C to stop) +# sudo ./can_setup.sh # bring up (default: 500kbps for VESC) +# sudo ./can_setup.sh up dronecan # bring up at 1Mbps for Here4 DroneCAN +# sudo ./can_setup.sh down +# sudo ./can_setup.sh verify # candump one-shot check (Ctrl-C to stop) # -# VESCs on bus: CAN ID 61 (0x3D) and CAN ID 79 (0x4F), 500 kbps +# Bitrate modes: +# default / vesc — 500 kbps (VESC IDs 0x3D/0x4F) +# dronecan / here4 — 1000 kbps (CubePilot Here4 RTK GPS, DroneCAN default) +# +# CAN_BITRATE env var overrides both modes. set -euo pipefail IFACE="${CAN_IFACE:-can0}" -BITRATE="${CAN_BITRATE:-500000}" log() { echo "[can_setup] $*"; } die() { echo "[can_setup] ERROR: $*" >&2; exit 1; } cmd="${1:-up}" +mode="${2:-vesc}" + +# Resolve bitrate: env var wins, then mode keyword, then 500k default +if [[ -n "${CAN_BITRATE:-}" ]]; then + BITRATE="$CAN_BITRATE" +elif [[ "$mode" == "dronecan" || "$mode" == "here4" ]]; then + BITRATE=1000000 +else + BITRATE=500000 +fi case "$cmd" in up) @@ -25,7 +39,7 @@ case "$cmd" in die "$IFACE not found — is CANable 2.0 plugged in and gs_usb loaded?" fi - log "Bringing up $IFACE at ${BITRATE} bps..." + log "Bringing up $IFACE at ${BITRATE} bps (mode: ${mode})..." ip link set "$IFACE" down 2>/dev/null || true ip link set "$IFACE" up type can bitrate "$BITRATE" ip link set "$IFACE" up @@ -40,13 +54,17 @@ case "$cmd" in ;; verify) - log "Listening on $IFACE — expecting frames from VESC IDs 0x3D (61) and 0x4F (79)" + if [[ "$mode" == "dronecan" || "$mode" == "here4" ]]; then + log "Listening on $IFACE — expecting DroneCAN frames from Here4 GPS (1Mbps)" + else + log "Listening on $IFACE — expecting frames from VESC IDs 0x3D (61) and 0x4F (79)" + fi log "Press Ctrl-C to stop." exec candump "$IFACE" ;; *) - echo "Usage: $0 [up|down|verify]" + echo "Usage: $0 [up [vesc|dronecan]|down|verify [dronecan]]" exit 1 ;; esac