sl-perception
9d6c72bd24
Implements saltybot_dronecan_gps ROS2 package — DroneCAN/UAVCAN v0 bridge that publishes Here4 GPS, IMU, magnetometer, and barometer data to ROS2. CANable2 freed from ESP32 BALANCE comms (bd-wim1) now runs Here4 at 1 Mbps DroneCAN. Key features: - dronecan_parser.py: pure-Python DSDL converters (Fix2, RawIMU, Mag, StaticPressure, StaticTemperature, NodeStatus, RTCMStream chunks), testable without dronecan library or CAN hardware - here4_node.py: ROS2 node, auto-discovers Here4 node ID on first Fix2, publishes /gps/fix + /gps/velocity for navsat_transform EKF fusion, HDOP-based NavSatStatus upgrade (RTK/SBAS), RTCM injection via /rtcm (ByteMultiArray) or /rtcm_hex (String hex fallback) - 39 unit tests, all passing - bring_up_can:=true parameter to configure SocketCAN at launch Resolves bd-p47c Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
110 lines
3.7 KiB
Python
110 lines
3.7 KiB
Python
"""here4.launch.py — Launch the Here4 GPS DroneCAN bridge on CANable2.
|
|
|
|
bd-p47c: CANable2 freed by bd-wim1 (ESP32 moved to UART/USB) now used for
|
|
Here4 GPS at 1 Mbps DroneCAN/UAVCAN v0.
|
|
|
|
CAN setup (one-time, survives reboot via systemd-networkd or udev)
|
|
------------------------------------------------------------------
|
|
# Option A — manual (quick test):
|
|
sudo ip link set can0 down
|
|
sudo ip link set can0 type can bitrate 1000000
|
|
sudo ip link set can0 up
|
|
|
|
# Option B — let the node do it (requires CAP_NET_ADMIN):
|
|
ros2 launch saltybot_dronecan_gps here4.launch.py bring_up_can:=true
|
|
|
|
# Option C — systemd-networkd (/etc/systemd/network/80-can0.network):
|
|
[Match]
|
|
Name=can0
|
|
[CAN]
|
|
BitRate=1M
|
|
|
|
Published topics
|
|
----------------
|
|
/gps/fix sensor_msgs/NavSatFix → navsat_transform_node (EKF)
|
|
/gps/velocity geometry_msgs/TwistWithCovarianceStamped
|
|
/here4/fix sensor_msgs/NavSatFix (alias)
|
|
/here4/imu sensor_msgs/Imu
|
|
/here4/mag sensor_msgs/MagneticField
|
|
/here4/baro sensor_msgs/FluidPressure
|
|
/here4/status std_msgs/String JSON
|
|
/here4/node_id std_msgs/Int32
|
|
|
|
Subscribed topics
|
|
-----------------
|
|
/rtcm std_msgs/ByteMultiArray RTCM corrections (NTRIP client)
|
|
/rtcm_hex std_msgs/String hex-encoded RTCM (fallback)
|
|
|
|
Usage
|
|
-----
|
|
# Default (interface already up):
|
|
ros2 launch saltybot_dronecan_gps here4.launch.py
|
|
|
|
# Bring up interface automatically (CAP_NET_ADMIN required):
|
|
ros2 launch saltybot_dronecan_gps here4.launch.py bring_up_can:=true
|
|
|
|
# Override interface (e.g. second CAN adapter):
|
|
ros2 launch saltybot_dronecan_gps here4.launch.py can_interface:=can1
|
|
|
|
# Pin to specific Here4 node ID (skip auto-detect):
|
|
ros2 launch saltybot_dronecan_gps here4.launch.py node_id_filter:=10
|
|
|
|
System dependency
|
|
-----------------
|
|
pip install dronecan # DroneCAN/UAVCAN v0 Python library
|
|
apt install can-utils # optional: candump, cansend for debugging
|
|
"""
|
|
|
|
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:
|
|
pkg_share = get_package_share_directory("saltybot_dronecan_gps")
|
|
params_file = os.path.join(pkg_share, "config", "here4_params.yaml")
|
|
|
|
args = [
|
|
DeclareLaunchArgument(
|
|
"can_interface",
|
|
default_value="can0",
|
|
description="SocketCAN interface (CANable2 @ 1 Mbps)",
|
|
),
|
|
DeclareLaunchArgument(
|
|
"bring_up_can",
|
|
default_value="false",
|
|
description="Bring up can_interface via ip link (requires CAP_NET_ADMIN)",
|
|
),
|
|
DeclareLaunchArgument(
|
|
"node_id_filter",
|
|
default_value="0",
|
|
description="DroneCAN node ID of Here4 (0=auto-detect from first Fix2)",
|
|
),
|
|
DeclareLaunchArgument(
|
|
"local_node_id",
|
|
default_value="126",
|
|
description="DroneCAN node ID for this Orin (must be unique on bus)",
|
|
),
|
|
]
|
|
|
|
node = Node(
|
|
package="saltybot_dronecan_gps",
|
|
executable="here4_node",
|
|
name="here4_can_node",
|
|
output="screen",
|
|
parameters=[
|
|
params_file,
|
|
{
|
|
"can_interface": LaunchConfiguration("can_interface"),
|
|
"bring_up_can": LaunchConfiguration("bring_up_can"),
|
|
"node_id_filter": LaunchConfiguration("node_id_filter"),
|
|
"local_node_id": LaunchConfiguration("local_node_id"),
|
|
},
|
|
],
|
|
)
|
|
|
|
return LaunchDescription([*args, node])
|