diff --git a/.github/workflows/build-docker-image.yaml b/.github/workflows/build-docker-image.yaml new file mode 100644 index 0000000..761130b --- /dev/null +++ b/.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 }} diff --git a/.github/workflows/pre-commit.yaml b/.github/workflows/pre-commit.yaml new file mode 100644 index 0000000..08f7311 --- /dev/null +++ b/.github/workflows/pre-commit.yaml @@ -0,0 +1,11 @@ +--- +name: Pre-Commit + +on: + push: + +jobs: + pre-commit: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master + with: + ros_distro: humble diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..eeb8a6e --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +**/__pycache__ diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 0000000..8f8266e --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,99 @@ +repos: + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v4.6.0 + hooks: + - id: check-added-large-files + # mesh files has to be taken into account + args: ["--maxkb=3000"] + - id: check-ast + - id: check-json + # vscode .json files do not follow the standard JSON format + exclude: ^.vscode/ + - id: check-merge-conflict + - id: check-symlinks + - id: check-xml + - id: check-yaml + - id: debug-statements + - id: destroyed-symlinks + - id: detect-private-key + - id: end-of-file-fixer + - id: fix-byte-order-marker + - id: name-tests-test + - id: mixed-line-ending + - id: trailing-whitespace + + - repo: https://github.com/PyCQA/isort + rev: 5.13.2 + hooks: + - id: isort + args: ["--profile", "black"] + + - repo: https://github.com/cheshirekow/cmake-format-precommit + rev: v0.6.13 + hooks: + - id: cmake-format + + - repo: https://github.com/pre-commit/mirrors-clang-format + rev: v18.1.8 + hooks: + - id: clang-format + + - repo: https://github.com/codespell-project/codespell + rev: v2.3.0 + hooks: + - id: codespell + name: codespell + description: Checks for common misspellings in text files. + entry: codespell + args: + [ + "--ignore-words-list", + "ned" # north, east, down (NED) + ] + exclude_types: [rst, svg] + language: python + types: [text] + + - repo: https://github.com/jumanjihouse/pre-commit-hook-yamlfmt + rev: 0.2.3 + hooks: + - id: yamlfmt + files: ^.github|./\.yaml + args: [--mapping, '2', --sequence, '4', --offset, '2', --width, '100'] + + - repo: https://github.com/psf/black + rev: 24.8.0 + hooks: + - id: black + args: ["--line-length=99"] + + - repo: https://github.com/PyCQA/flake8 + rev: 7.1.1 + hooks: + - id: flake8 + args: + ["--ignore=E501,W503"] # ignore too long line and line break before binary operator, + # black checks it + + - repo: local + hooks: + - id: ament_copyright + name: ament_copyright + description: Check if copyright notice is available in all files. + stages: [commit] + entry: ament_copyright + language: system + + # Docs - RestructuredText hooks + - repo: https://github.com/PyCQA/doc8 + rev: v1.1.1 + hooks: + - id: doc8 + args: ["--max-line-length=100", "--ignore=D001"] + exclude: ^.*\/CHANGELOG\.rst/.*$ + + - repo: https://github.com/tier4/pre-commit-hooks-ros + rev: v0.10.0 + hooks: + - id: prettier-package-xml + - id: sort-package-xml diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 0000000..37e7aac --- /dev/null +++ b/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"] diff --git a/README.md b/README.md index 4dfc521..0349562 100644 --- a/README.md +++ b/README.md @@ -1 +1,43 @@ -# husarion_ugv_crsf_teleop \ No newline at end of file +# 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. diff --git a/compose.yaml b/compose.yaml new file mode 100644 index 0000000..e771de5 --- /dev/null +++ b/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 diff --git a/husarion_ugv_crsf_interfaces/CMakeLists.txt b/husarion_ugv_crsf_interfaces/CMakeLists.txt new file mode 100644 index 0000000..dc5f234 --- /dev/null +++ b/husarion_ugv_crsf_interfaces/CMakeLists.txt @@ -0,0 +1,15 @@ +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) +find_package(std_msgs REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} "msg/LinkStatus.msg" DEPENDENCIES + rosidl_default_generators std_msgs) + +ament_package() diff --git a/husarion_ugv_crsf_interfaces/msg/LinkStatus.msg b/husarion_ugv_crsf_interfaces/msg/LinkStatus.msg new file mode 100644 index 0000000..7c16d1e --- /dev/null +++ b/husarion_ugv_crsf_interfaces/msg/LinkStatus.msg @@ -0,0 +1,11 @@ +std_msgs/Header header +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 diff --git a/husarion_ugv_crsf_interfaces/package.xml b/husarion_ugv_crsf_interfaces/package.xml new file mode 100644 index 0000000..cd25594 --- /dev/null +++ b/husarion_ugv_crsf_interfaces/package.xml @@ -0,0 +1,27 @@ + + + + husarion_ugv_crsf_interfaces + 1.0.0 + Custom messages for the CRSF teleop node + Husarion + Apache License 2.0 + + https://husarion.com/ + https://github.com/husarion/husarion_ugv_crsf_teleop + https://github.com/husarion/husarion_ugv_crsf_teleop/issues + + Milosz Lagan + + ament_cmake + + std_msgs + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + + diff --git a/husarion_ugv_crsf_teleop/config/crsf_teleop.yaml b/husarion_ugv_crsf_teleop/config/crsf_teleop.yaml new file mode 100644 index 0000000..fad5034 --- /dev/null +++ b/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] diff --git a/husarion_ugv_crsf_teleop/husarion_ugv_crsf_teleop/__init__.py b/husarion_ugv_crsf_teleop/husarion_ugv_crsf_teleop/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/husarion_ugv_crsf_teleop/husarion_ugv_crsf_teleop/crsf/__init__.py b/husarion_ugv_crsf_teleop/husarion_ugv_crsf_teleop/crsf/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/husarion_ugv_crsf_teleop/husarion_ugv_crsf_teleop/crsf/message.py b/husarion_ugv_crsf_teleop/husarion_ugv_crsf_teleop/crsf/message.py new file mode 100644 index 0000000..aee8fa7 --- /dev/null +++ b/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] diff --git a/husarion_ugv_crsf_teleop/husarion_ugv_crsf_teleop/crsf/parser.py b/husarion_ugv_crsf_teleop/husarion_ugv_crsf_teleop/crsf/parser.py new file mode 100644 index 0000000..355315b --- /dev/null +++ b/husarion_ugv_crsf_teleop/husarion_ugv_crsf_teleop/crsf/parser.py @@ -0,0 +1,145 @@ +# 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. +# +# Based on https://github.com/crsf-wg/crsf/wiki/Python-Parser by crsf-wg + +from enum import Enum +from typing import Callable, Tuple + +from .message import CRSF_SYNC, CRSF_SYNC_EDGETX, CRSFMessage, PacketType + + +class CRSFParser: + class State(Enum): + SEEK_SYNC = (0,) + MSG_LEN = (1,) + MSG_TYPE = (2,) + MSG_EXT_DEST = (3,) + MSG_EXT_SRC = (4,) + MSG_PAYLOAD = (5,) + MSG_CRC = 6 + + class Result(Enum): + PACKET_VALID = (0,) + IN_PROGRESS = (1,) + PACKET_INVALID = 2 + + _msg: CRSFMessage + _buffer: bytearray = bytearray() + state: State = State.SEEK_SYNC + on_message: Callable[[CRSFMessage], None] + + def __init__(self): + pass + + def parse(self, data: bytearray): + self._buffer.extend(data) + + i = 0 + while len(self._buffer) - i > 0: + byte = self._buffer[i] + (result, length) = self.parse_byte(byte) + + if result == self.Result.PACKET_VALID: + # Process message + if self.on_message: + self.on_message(self._msg) + + # Remove raw message from the buffer + self._buffer = self._buffer[length:] + i = 0 + + elif result == self.Result.PACKET_INVALID: + # Remove invalid message/byte from the buffer + self._buffer = self._buffer[length:] + i = 0 + + elif result == self.Result.IN_PROGRESS: + i += length + + # Parses a single byte of a CRSF message + # Returns a tuple of the result and the number of bytes consumed + # + # To allow iterative parsing input data buffer first element/s should + # only be discarded if the result is PACKET_VALID or PACKET_INVALID. + # IN_PROGRESS indicates that the byte was read and parsed but the + # message parsing could fail in the future. + def parse_byte(self, byte: int) -> Tuple[Result, int]: + INVALID_DISCARD_BYTE = (self.Result.PACKET_INVALID, 1) + IN_PROGRESS = (self.Result.IN_PROGRESS, 1) + + if self.state == self.State.SEEK_SYNC: + if byte == CRSF_SYNC or byte == CRSF_SYNC_EDGETX: + self._msg = CRSFMessage() + self._msg.payload.clear() + + self.state = self.State.MSG_LEN + + return IN_PROGRESS + else: + return INVALID_DISCARD_BYTE + + elif self.state == self.State.MSG_LEN: + if byte > 64 - 2: + self.state = self.State.SEEK_SYNC + return INVALID_DISCARD_BYTE + + # Len includes 2 byte packet type and crc fields + self._msg.length = byte - 2 + self.state = self.State.MSG_TYPE + return IN_PROGRESS + + elif self.state == self.state.MSG_TYPE: + try: + self._msg.msg_type = PacketType(byte) + except ValueError: + self.state = self.State.SEEK_SYNC + return INVALID_DISCARD_BYTE + + if self._msg.is_extended(): + self.state = self.State.MSG_EXT_DEST + self._msg.length -= 2 + else: + self.state = self.State.MSG_PAYLOAD + return IN_PROGRESS + + elif self.state == self.State.MSG_EXT_DEST: + self._msg.destination = byte + self.state = self.State.MSG_EXT_SRC + return IN_PROGRESS + + elif self.state == self.State.MSG_EXT_SRC: + self._msg.source = byte + self.state = self.State.MSG_PAYLOAD + return IN_PROGRESS + + elif self.state == self.State.MSG_PAYLOAD: + self._msg.payload.append(byte) + if len(self._msg.payload) == self._msg.length: + self.state = self.State.MSG_CRC + return IN_PROGRESS + + elif self.state == self.State.MSG_CRC: + if self._msg.calculate_crc() == byte: + # Reset parser + self.state = self.State.SEEK_SYNC + + # Packet len consists of payload length + 4 bytes for sync, len, type and crc + # + 2 bytes if packet has an extended format + return ( + self.Result.PACKET_VALID, + len(self._msg.payload) + 4 + (2 if self._msg.is_extended() else 0), + ) + else: + return INVALID_DISCARD_BYTE diff --git a/husarion_ugv_crsf_teleop/husarion_ugv_crsf_teleop/crsf_teleop_node.py b/husarion_ugv_crsf_teleop/husarion_ugv_crsf_teleop/crsf_teleop_node.py new file mode 100644 index 0000000..4dea357 --- /dev/null +++ b/husarion_ugv_crsf_teleop/husarion_ugv_crsf_teleop/crsf_teleop_node.py @@ -0,0 +1,262 @@ +# 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 enum import IntEnum + +import rclpy +import serial +from geometry_msgs.msg import Twist +from rcl_interfaces.msg import FloatingPointRange, ParameterDescriptor +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy, QoSProfile, QoSReliabilityPolicy +from std_srvs.srv import Trigger + +from husarion_ugv_crsf_interfaces.msg import LinkStatus + +from .crsf.message import ( + CRSFMessage, + PacketType, + normalize_channel_values, + unpack_channels, +) +from .crsf.parser import CRSFParser + +REQUESTED_E_STOP_THRESHOLD = 0.5 +SEND_CMD_VEL_THRESHOLD = -0.5 + +LINK_QUALITY_VERY_LOW_THRESHOLD = 15 +LINK_QUALITY_LOW_THRESHOLD = 30 + + +# RC remote to channel id mapping +class Switch(IntEnum): + RIGHT_HORIZONTAL = 1 + LEFT_VERTICAL = 3 + SF = 4 + SA = 6 + SG = 10 + + +class CRSFInterface(Node): + def __init__(self): + super().__init__("crsf_interface") + + self.cmd_vel_publisher = self.create_publisher( + Twist, + "cmd_vel", + QoSProfile( + reliability=QoSReliabilityPolicy.RELIABLE, + durability=QoSDurabilityPolicy.VOLATILE, + depth=1, + ), + ) + + self.link_status_publisher = self.create_publisher( + LinkStatus, + "link_status", + QoSProfile( + reliability=QoSReliabilityPolicy.RELIABLE, + durability=QoSDurabilityPolicy.VOLATILE, + depth=1, + ), + ) + + self.e_stop_trigger = self.create_client( + Trigger, + "hardware/e_stop_trigger", + ) + + self.e_stop_reset = self.create_client( + Trigger, + "hardware/e_stop_reset", + ) + + self.link_status = LinkStatus() + + self.declare_node_parameters() + + [ + port, + baud, + e_stop_republish, + self.enable_cmd_vel_silence_switch, + self.linear_speed_presets, + self.angular_speed_presets, + ] = self.get_parameters( + [ + "port", + "baud", + "e_stop_republish", + "enable_cmd_vel_silence_switch", + "linear_speed_presets", + "angular_speed_presets", + ] + ) + + if len(self.linear_speed_presets.value) != 3 or len(self.angular_speed_presets.value) != 3: + raise ValueError("Speed presets must be a list of 3 values") + + self.serial = serial.Serial(port.value, baud.value, timeout=2) + + self.parser = CRSFParser() + self.parser.on_message = lambda msg: self.handle_message(msg) + + self.serial_parser_timer = self.create_timer(0.01, self.serial_parser_timer_cb) + + self.rc_estop_state = True + if e_stop_republish.value: + self.e_stop_republisher = self.create_timer(1, self.update_e_stop) + + def declare_node_parameters(self): + self.declare_parameter( + "port", "/dev/ttyUSB0", ParameterDescriptor(description="CRSF receiver serial port") + ) + self.declare_parameter( + "baud", 576000, ParameterDescriptor(description="CRSF receiver baud rate") + ) + self.declare_parameter( + "e_stop_republish", + False, + ParameterDescriptor(description="Rebroadcast asserted e-stop signal once per second"), + ) + self.declare_parameter( + "enable_cmd_vel_silence_switch", + False, + ParameterDescriptor( + description="Enable cmd_vel silence switch allowing other nodes to take control" + ), + ) + self.declare_parameter( + "linear_speed_presets", + [0.5, 1.0, 2.0], + ParameterDescriptor( + description="Selectable robot maximum linear speed for cmd_vel", + floating_point_range=[FloatingPointRange(from_value=0.0, to_value=10.0)], + ), + ) + self.declare_parameter( + "angular_speed_presets", + [0.5, 1.0, 2.0], + ParameterDescriptor( + description="Selectable robot maximum angular speed for cmd_vel", + floating_point_range=[FloatingPointRange(from_value=0.0, to_value=10.0)], + ), + ) + + def serial_parser_timer_cb(self): + if self.serial.in_waiting > 0: + self.parser.parse(self.serial.read(self.serial.in_waiting)) + + def handle_message(self, msg: CRSFMessage): + if msg.msg_type == PacketType.RC_CHANNELS_PACKED: + channels = unpack_channels(msg.payload) + channels = normalize_channel_values(channels) + + # Handle emergency stop from RC controller + # Asserted e-stop is retransmitted once per second by republish timer + # if enabled by the 'e_stop_republish' parameter + # Deasserted e-stop is transmitted only once + requested_e_stop = channels[Switch.SF] < REQUESTED_E_STOP_THRESHOLD + if requested_e_stop != self.rc_estop_state: + self.rc_estop_state = requested_e_stop + + if self.rc_estop_state: + self.e_stop_trigger.call_async(Trigger.Request()) + else: + self.e_stop_reset.call_async(Trigger.Request()) + + # Disable sending cmd_vel if override switch is asserted + if self.enable_cmd_vel_silence_switch.value: + send_cmd_vel = channels[Switch.SA] < SEND_CMD_VEL_THRESHOLD + else: + send_cmd_vel = True + + if send_cmd_vel: + linear_speed_modifier = self.linear_speed_presets.value[ + round(channels[Switch.SG]) + 1 + ] + angular_speed_modifier = self.angular_speed_presets.value[ + round(channels[Switch.SG]) + 1 + ] + + t = Twist() + t.linear.x = channels[Switch.RIGHT_HORIZONTAL] * linear_speed_modifier + t.angular.z = -channels[Switch.LEFT_VERTICAL] * angular_speed_modifier + + self.cmd_vel_publisher.publish(t) + + elif msg.msg_type == PacketType.LINK_STATISTICS: + last_lq = self.link_status.lq + + self.link_status.header.stamp = self.get_clock().now().to_msg() + + self.link_status.rssi_1 = -msg.payload[0] + self.link_status.rssi_2 = -msg.payload[1] + self.link_status.lq = msg.payload[2] + self.link_status.uplink_snr = int.from_bytes( + bytes(msg.payload[3]), byteorder="big", signed=True + ) + self.link_status.used_antenna = msg.payload[4] + self.link_status.mode = msg.payload[5] + self.link_status.tx_power = msg.payload[6] + self.link_status.downlink_rssi = -msg.payload[7] + self.link_status.downlink_lq = msg.payload[8] + self.link_status.downlink_snr = int.from_bytes( + bytes(msg.payload[9]), byteorder="big", signed=True + ) + + # Detect connection status + if last_lq != 0 and self.link_status.lq == 0: + self.get_logger().error("Connection lost") + + # Publish empty cmd_vel to stop the robot + self.cmd_vel_publisher.publish(Twist()) + + if last_lq == 0 and self.link_status.lq > 0: + self.get_logger().info("Connected") + + # Warn on low link quality + if last_lq != self.link_status.lq: + if self.link_status.lq < LINK_QUALITY_VERY_LOW_THRESHOLD: + self.get_logger().warn(f"Very low link quality: {self.link_status.lq}%") + elif self.link_status.lq < LINK_QUALITY_LOW_THRESHOLD: + self.get_logger().warn(f"Low link quality: {self.link_status.lq}%") + elif last_lq < 30 and self.link_status.lq >= LINK_QUALITY_LOW_THRESHOLD: + self.get_logger().info(f"Link quality restored: {self.link_status.lq}%") + + self.link_status_publisher.publish(self.link_status) + + else: + self.get_logger().warn( + f"Unknown CRSF message (Type: {msg.msg_type.name}, Length: {msg.length})" + ) + + def update_e_stop(self): + if self.rc_estop_state: + self.e_stop_trigger.call_async(Trigger.Request()) + + +def main(args=None): + rclpy.init(args=args) + + crsf_interface = CRSFInterface() + + rclpy.spin(crsf_interface) + + crsf_interface.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/husarion_ugv_crsf_teleop/launch/teleop.launch.py b/husarion_ugv_crsf_teleop/launch/teleop.launch.py new file mode 100644 index 0000000..4d4be19 --- /dev/null +++ b/husarion_ugv_crsf_teleop/launch/teleop.launch.py @@ -0,0 +1,56 @@ +#!/usr/bin/env python3 + +# 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 launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import ( + EnvironmentVariable, + LaunchConfiguration, + PathJoinSubstitution, +) +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + husarion_ugv_crsf_teleop_dir = FindPackageShare("husarion_ugv_crsf_teleop") + + namespace = LaunchConfiguration("namespace") + declare_namespace_arg = DeclareLaunchArgument( + "namespace", + default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""), + description="Add namespace to all launched nodes.", + ) + + husarion_ugv_crsf_teleop_node = Node( + package="husarion_ugv_crsf_teleop", + executable="crsf_teleop_node", + name="crsf_teleop", + parameters=[ + PathJoinSubstitution([husarion_ugv_crsf_teleop_dir, "config", "crsf_teleop.yaml"]), + ], + namespace=namespace, + emulate_tty=True, + respawn=True, + respawn_delay=2, + ) + + actions = [ + declare_namespace_arg, + husarion_ugv_crsf_teleop_node, + ] + + return LaunchDescription(actions) diff --git a/husarion_ugv_crsf_teleop/package.xml b/husarion_ugv_crsf_teleop/package.xml new file mode 100644 index 0000000..d8b8d6c --- /dev/null +++ b/husarion_ugv_crsf_teleop/package.xml @@ -0,0 +1,29 @@ + + + + husarion_ugv_crsf_teleop + 1.0.0 + Teleoperate robots using a CRSF compatible remote control + Husarion + Apache License 2.0 + + https://husarion.com/ + https://github.com/husarion/husarion_ugv_crsf_teleop + https://github.com/husarion/husarion_ugv_crsf_teleop/issues + + Milosz Lagan + + python3-serial + + geometry_msgs + husarion_ugv_crsf_interfaces + rclpy + sensor_msgs + std_srvs + + ament_copyright + + + ament_python + + diff --git a/husarion_ugv_crsf_teleop/resource/husarion_ugv_crsf_teleop b/husarion_ugv_crsf_teleop/resource/husarion_ugv_crsf_teleop new file mode 100644 index 0000000..e69de29 diff --git a/husarion_ugv_crsf_teleop/setup.cfg b/husarion_ugv_crsf_teleop/setup.cfg new file mode 100644 index 0000000..7a6c8b1 --- /dev/null +++ b/husarion_ugv_crsf_teleop/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/husarion_ugv_crsf_teleop +[install] +install_scripts=$base/lib/husarion_ugv_crsf_teleop diff --git a/husarion_ugv_crsf_teleop/setup.py b/husarion_ugv_crsf_teleop/setup.py new file mode 100644 index 0000000..f521755 --- /dev/null +++ b/husarion_ugv_crsf_teleop/setup.py @@ -0,0 +1,44 @@ +# 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. + +import os +from glob import glob + +from setuptools import find_packages, setup + +package_name = "husarion_ugv_crsf_teleop" + +setup( + name=package_name, + version="1.0.0", + packages=find_packages(exclude=["test"]), + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + (os.path.join("share", package_name, "config"), ["config/crsf_teleop.yaml"]), + (os.path.join("share", package_name), ["package.xml"]), + ( + os.path.join("share", package_name, "launch"), + glob(os.path.join("launch", "*launch.[pxy][yma]*")), + ), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="Milosz Lagan", + maintainer_email="milosz.lagan@husarion.com", + description="Teleoperate robots using a CRSF compatible remote control", + license="Apache License 2.0", + entry_points={ + "console_scripts": ["crsf_teleop_node = husarion_ugv_crsf_teleop.crsf_teleop_node:main"], + }, +)