Implement CRSF teleop

.github/workflows/build-docker-image.yaml

@@ -0,0 +1,52 @@
+---
+name: Build/Publish Docker Image
+
+on:
+  push:
+    branches:
+      - master
+  workflow_dispatch:
+    inputs:
+      tag:
+        description: tag that the image will be built with
+        required: true
+        default: humble
+      branch:
+        description: branch that will be used to build image
+        required: true
+        default: master
+
+jobs:
+  build_ros:
+    runs-on: ubuntu-22.04
+
+    steps:
+
+      - name: Checkout
+        uses: actions/checkout@v1
+        with:
+          ref: ${{ github.event.inputs.branch }}
+
+      - name: Set up QEMU
+        uses: docker/setup-qemu-action@v1
+
+      - name: Set up Docker Buildx
+        uses: docker/setup-buildx-action@v1
+        with:
+          version: latest
+
+      - name: Login to Docker Registry
+        uses: docker/login-action@v1
+        with:
+          registry: docker.io
+          username: ${{ secrets.DOCKERHUB_USERNAME }}
+          password: ${{ secrets.DOCKERHUB_TOKEN }}
+
+      - name: Build and push crsf_teleop image
+        uses: docker/build-push-action@v2
+        with:
+          context: .
+          file: ./Dockerfile
+          platforms: linux/arm64, linux/amd64
+          push: true
+          tags: husarion/crsf-teleop:${{ github.event.inputs.tag }}

.gitignore

@@ -0,0 +1 @@
+**/__pycache__

Dockerfile

@@ -0,0 +1,25 @@
+ARG ROS_DISTRO=humble
+FROM husarnet/ros:${ROS_DISTRO}-ros-core
+
+SHELL ["/bin/bash", "-c"]
+
+WORKDIR /ros2_ws
+
+COPY . src/husarion_ugv_crsf_teleop
+RUN apt-get update --fix-missing && \
+    apt upgrade -y && \
+    apt-get install -y ros-dev-tools && \
+    rm -rf /etc/ros/rosdep/sources.list.d/20-default.list && \
+    rosdep init && \
+    rosdep update --rosdistro $ROS_DISTRO && \
+    rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y && \
+    source /opt/ros/$ROS_DISTRO/setup.bash && \
+    colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release && \
+    rm -rf build log && \
+    export SUDO_FORCE_REMOVE=yes && \
+    apt-get remove -y ros-dev-tools && \
+    apt-get autoremove -y && \
+    apt-get clean && \
+    rm -rf /var/lib/apt/lists/*
+
+CMD ["ros2", "launch", "husarion_ugv_crsf_teleop", "teleop.launch.py"]

README.md

@@ -1 +1,43 @@
-# husarion_ugv_crsf_teleop
+# husarion_ugv_crsf_teleop
+
+This ROS 2 package allows you to control robots using a CRSF compatible remote control. A receiver should be connected to the robot's computer via USB-UART converter or be integrated as a hardware USB dongle. The CRSF protocol parser is implemented based on the following [specification](https://github.com/crsf-wg/crsf/wiki).
+
+## Launch Files
+
+- `teleop.launch.py`: Launches crsf_teleop_node node. Automatically respawns node if it exits. Node's namespace can be set using the `namespace` launch argument.
+
+## Configuration Files
+
+- [`crsf_teleop.yaml`](./config/crsf_teleop.yaml): Sets default parameter values for the crsf_teleop_node when `teleop.launch.py` is launched.
+
+## ROS Nodes
+
+### crsf_teleop_node
+
+Translates received CRSF commands to velocity commands for the robot.
+
+The following channels are used for controlling the robot via the TX16S remote control:
+- Channel 2 - Right gimbal up/down - forward/backward velocity
+- Channel 4 - Left gimbal left/right - turning (angular) velocity
+- Channel 5 - SF switch - emergency stop
+- Channel 7 - SA switch (down position) - silence `cmd_vel` messages, allows other nodes to control the robot while enabling e_stop functionality
+- Channel 11 - SG switch - tristate switch, selects robot speed
+
+#### Publishes
+
+- `cmd_vel` [*geometry_msgs/Twist*]: Publishes velocity commands to the robot.
+- `link_status` [*panther_crsf_teleop_msgs/LinkStatus*]: Describes radio link status between the remote control and the robot. Parameters are described in the [CRSF_FRAMETYPE_LINK_STATISTICS frame documentation](https://github.com/crsf-wg/crsf/wiki/CRSF_FRAMETYPE_LINK_STATISTICS).
+
+#### Service Clients
+
+- `hardware/e_stop_trigger` [*std_srvs/Trigger*]: Triggers an emergency stop.
+- `hardware/e_stop_reset` [*std_srvs/Trigger*]: Triggers an emergency stop reset.
+
+#### Parameters
+
+- `serial_port` [*string*, default: **/dev/ttyUSB0**]: Serial port to which the CRSF receiver is connected.
+- `baudrate` [*int*, default: **576000**]: Baudrate of the serial port.
+- `e_stop_republish` [*bool*, default: **False**]: Rebroadcasts asserted emergency stop signal once per second. Will override other emergency stop sources.
+- `enable_cmd_vel_silence_switch`[*bool*, default: **False**]: Enables remote to disable publishing `cmd_vel` messages on demand. Can be used as a remote emergency stop when using other nodes to control the robot.
+- `linear_speed_presets` [*double_array*, default: **[0.5, 1.0, 2.0]**]: Selectable robot maximum linear speed for `cmd_vel` topic.
+- `angular_speed_presets` [*double_array*, default: **[0.5, 1.0, 2.0]**]: Selectable robot maximum angular speed for the `cmd_vel` topic.

compose.yaml

@@ -0,0 +1,16 @@
+services:
+  husarion_ugv_crsf_teleop:
+    image: husarion/crsf-teleop:humble
+    network_mode: host
+    ipc: host
+    pid: host
+    restart: unless-stopped
+    privileged: true
+    environment:
+      - RMW_IMPLEMENTATION=${RMW_IMPLEMENTATION:-rmw_cyclonedds_cpp}
+      - ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-0}
+    devices:
+      - /dev/ttyUSBPAD
+    command: >
+      ros2 launch husarion_ugv_crsf_teleop teleop.launch.py
+      port:=/dev/ttyUSBPAD namespace:=panther

husarion_ugv_crsf_interfaces/CMakeLists.txt

@@ -0,0 +1,13 @@
+cmake_minimum_required(VERSION 3.8)
+project(husarion_ugv_crsf_interfaces)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME} "msg/LinkStatus.msg")
+
+ament_package()

husarion_ugv_crsf_interfaces/msg/LinkStatus.msg

@@ -0,0 +1,10 @@
+int16 rssi_1
+int16 rssi_2
+uint8 lq
+int8 uplink_snr
+uint8 used_antenna
+uint8 mode
+uint8 tx_power
+int16 downlink_rssi
+uint8 downlink_lq
+int8 downlink_snr

husarion_ugv_crsf_interfaces/package.xml

@@ -0,0 +1,25 @@
+<?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>husarion_ugv_crsf_interfaces</name>
+  <version>1.0.0</version>
+  <description>Custom messages for the CRSF teleop node</description>
+  <maintainer email="[email protected]">Husarion</maintainer>
+  <license>Apache License 2.0</license>
+
+  <url type="website">https://husarion.com/</url>
+  <url type="repository">https://github.com/husarion/husarion_ugv_crsf_teleop</url>
+  <url type="bugtracker">https://github.com/husarion/husarion_ugv_crsf_teleop/issues</url>
+
+  <author email="[email protected]">Milosz Lagan</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>rosidl_default_generators</build_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

husarion_ugv_crsf_teleop/config/crsf_teleop.yaml

@@ -0,0 +1,9 @@
+/**:
+  crsf_teleop_node:
+    ros__parameters:
+      port: /dev/ttyUSB0
+      baud: 576000
+      e_stop_republish: false
+      enable_cmd_vel_silence_switch: false
+      linear_speed_presets: [0.5, 1.0, 2.0]
+      angular_speed_presets: [0.5, 1.0, 2.0]

husarion_ugv_crsf_teleop/husarion_ugv_crsf_teleop/crsf/message.py

@@ -0,0 +1,137 @@
+# Copyright 2024 Husarion sp. z o.o.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from dataclasses import dataclass, field
+from enum import IntEnum
+from typing import List
+
+CRSF_SYNC = 0xC8
+CRSF_SYNC_EDGETX = 0xEE
+CRSF_MSG_EXTENDED = 0x28
+CRSF_RC_CHANNELS_PACKED_LEN = 22
+CRSF_RC_CHANNELS_LEN = 16
+
+
+class PacketType(IntEnum):
+    GPS = 0x02
+    VARIO = 0x07
+    BATTERY_SENSOR = 0x08
+    BARO_ALTITUDE = 0x09
+    HEARTBEAT = 0x0B
+    VIDEO_TRANSMITTER = 0x0F
+    LINK_STATISTICS = 0x14
+    RC_CHANNELS_PACKED = 0x16
+    RC_CHANNELS_SUBSET = 0x17
+    LINK_RX_ID = 0x1C
+    LINK_TX_ID = 0x1D
+    ATTITUDE = 0x1E
+    FLIGHT_MODE = 0x21
+    DEVICE_PING = 0x28
+    DEVICE_INFO = 0x29
+    PARAM_ENTRY = 0x2B
+    PARAM_READ = 0x2C
+    PARAM_WRITE = 0x2D
+    ELRS_STATUS = 0x2E
+    COMMAND = 0x32
+    RADIO_ID = 0x3A
+    KISS_REQUEST = 0x78
+    KISS_RESPONSE = 0x79
+    MSP_REQUEST = 0x7A
+    MSP_RESPONSE = 0x7B
+    MSP_WRITE = 0x7C
+    DISPLAYPORT_CMD = 0x7D
+    CUSTOM_TELEMETRY = 0x80
+
+
+@dataclass
+class CRSFMessage:
+    msg_type: PacketType = 0
+    payload: bytearray = field(default_factory=bytearray)
+    crc: int = 0
+
+    destination: int = 0
+    source: int = 0
+
+    def is_extended(self) -> bool:
+        if self.msg_type not in PacketType:
+            raise ValueError("Invalid message type")
+
+        # All extended messages begin from the 0x28 address
+        if self.msg_type >= CRSF_MSG_EXTENDED:
+            return True
+
+    def calculate_crc(self, assign_to_self: bool = True) -> int:
+        crc = self._crc8_dvb_s2(0, self.msg_type)
+
+        if self.is_extended():
+            crc = self._crc8_dvb_s2(crc, self.destination)
+            crc = self._crc8_dvb_s2(crc, self.source)
+
+        for byte in self.payload:
+            crc = self._crc8_dvb_s2(crc, byte)
+
+        if assign_to_self:
+            self.crc = crc
+
+        return crc
+
+    def encode(self) -> bytearray:
+        data = bytearray()
+
+        if self.msg_type not in PacketType:
+            raise ValueError("Invalid message type")
+
+        data.append(CRSF_SYNC)
+        data.append(len(self.payload))
+        data.append(self.msg_type)
+
+        if self.is_extended():
+            data.append(self.destination)
+            data.append(self.source)
+
+        data.extend(self.payload)
+        data.append(self.calculate_crc())
+
+        return data
+
+    def _crc8_dvb_s2(self, crc, a) -> int:
+        crc = crc ^ a
+        for ii in range(8):
+            if crc & 0x80:
+                crc = (crc << 1) ^ 0xD5
+            else:
+                crc = crc << 1
+        return crc & 0xFF
+
+
+def unpack_channels(packed: bytearray) -> List[int]:
+    if len(packed) != CRSF_RC_CHANNELS_PACKED_LEN:
+        raise ValueError("Input data must be 22 bytes long")
+
+    channels = []
+    bit_buffer = int.from_bytes(packed, byteorder="little")
+
+    for _ in range(CRSF_RC_CHANNELS_LEN):
+        channels.append(bit_buffer & 0x7FF)
+        bit_buffer >>= 11
+
+    return channels
+
+
+# Normalize channel values from CRSF range [172, 1812] to [-1, 1]
+def normalize_channel_values(channels: List[int]) -> List[float]:
+    if len(channels) != CRSF_RC_CHANNELS_LEN:
+        raise ValueError("Input data must contain 16 channels")
+
+    return [(channel - 992) / 820.0 for channel in channels]