Merge remote-tracking branch 'origin/sl-jetson/here4-dronecan-driver'
# Conflicts: # jetson/ros2_ws/src/saltybot_dronecan_gps/package.xml # jetson/ros2_ws/src/saltybot_dronecan_gps/setup.cfg # jetson/ros2_ws/src/saltybot_dronecan_gps/setup.py
This commit is contained in:
commit
da64277e8d
@ -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
|
||||
|
||||
@ -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
|
||||
@ -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'),
|
||||
},
|
||||
],
|
||||
),
|
||||
])
|
||||
@ -0,0 +1 @@
|
||||
saltybot_dronecan_gps
|
||||
@ -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()
|
||||
@ -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
|
||||
32
jetson/ros2_ws/src/saltybot_ntrip_client/package.xml
Normal file
32
jetson/ros2_ws/src/saltybot_ntrip_client/package.xml
Normal file
@ -0,0 +1,32 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>saltybot_ntrip_client</name>
|
||||
<version>0.1.0</version>
|
||||
<description>
|
||||
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
|
||||
</description>
|
||||
<maintainer email="seb@vayrette.com">Sebastien Vayrette</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<buildtool_depend>ament_python</buildtool_depend>
|
||||
|
||||
<depend>rclpy</depend>
|
||||
<depend>std_msgs</depend>
|
||||
|
||||
<exec_depend>python3-pip</exec_depend>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@ -0,0 +1 @@
|
||||
saltybot_ntrip_client
|
||||
@ -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:<reason>
|
||||
|
||||
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()
|
||||
4
jetson/ros2_ws/src/saltybot_ntrip_client/setup.cfg
Normal file
4
jetson/ros2_ws/src/saltybot_ntrip_client/setup.cfg
Normal file
@ -0,0 +1,4 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/saltybot_ntrip_client
|
||||
[install]
|
||||
install_scripts=$base/lib/saltybot_ntrip_client
|
||||
32
jetson/ros2_ws/src/saltybot_ntrip_client/setup.py
Normal file
32
jetson/ros2_ws/src/saltybot_ntrip_client/setup.py
Normal file
@ -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',
|
||||
],
|
||||
},
|
||||
)
|
||||
@ -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)
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user