Fast RTPS Bridge between HoverGames (running PX4) and ROS 2

Introduction

This is a complete tutorial presenting all the steps required to be done in order to send/receive data (uUORB topics) from a quadcopter (a HoverGames drone in my case), running PX4 autopilot, to an application running in the frame of ROS 2 (Robotic Operating System) and placed on a different system – on an offboard computer. The steps will be particularized on the components used in the HoverGames 3 competition – but they will be as general as possible to be used in other cases with other quadcopters having different FMUs and different offboard computers.

The data exchange is based on the PX4-Fast RTPS(DDS) bridge or microRTPS (Real Time Publish Subscribe ) Bridge - the block diagram of this bridge is shown in the above figure (this figure is taken from the PX4 website). Based on this bridge, someone can exchange uORB messages between internal components of PX4 Autopilot and an application running on an offboard computer (NavQ Plus system, in my case).

To do all these data exchanges, you will need three main components:

1. The microRTPS Client is a middleware daemon process that runs on the flight controller (FMUK66). Based on the microRTPS Client, the PX4 uORB topics are sent to Agent, and the Agent data is published as uORB messages to the PX4.

2. The microRTPS Agent is a daemon process running on the offboard computer (NavQ Plus in my case, but it can be a Raspberry Pisystem or a Jetson Nano development board), and it is the bridge between microRTPS Client and the ROS 2.

3. The px4_ros_comis a part of ROS 2 that implements the link between the microRTPS Agent and the ROS 2 core system.

Together with the Fast-RTPS-Gen, generate the microRTPS Agent component of the microRTPS Bridge.

For the px4_ros_com and ROS 2 nodes to work, the px4_msgs component is required. The px4_msgs contains the IDL files needed to generate the microRTPS Agent, the type of data support, and the interface code for ROS 2 nodes.

At this point, we will use version 1.13.2 of the PX4 autopilot (the latest stable version available now) based on the Fast RTPS(DDS) Bridge. Starting with version 1.14 of the PX4 autopilot,the XRCE-DDS middleware will replace the Fast RTPS(DDS) Bridgeused in PX4 version 1.13.

The second constrain was given by the microRTPS Bridge (especially the px4_ros_com and px4_msgs associated components of the PX4 branches release/1.13) that was not planned to be supported on Ubuntu 22.04 and ROS 2 Humble. So as a direct result, ROS 2 Foxy and Ubuntu 20.04 are the required choices. The ROS 2 Foxy also use as a default middleware the Fast-RTPS Bridge.

ROS 2 installation and configuration

As a direct result of the existing constraints, the ROS 2 Foxy must be installed.So, use the following link that presents the steps required to install ROS 2 Foxy: https://docs.ros.org/en/foxy/Installation/Ubuntu-Install-Debians.html

You will need to run the following command on every new shell you open in order to have access to the ROS 2 commands (replace ".bash" with your shell if you're not using bash - possible values are: setup.bash, setup.sh, setup.zsh):

source /opt/ros/foxy/setup.bash

If you don’t want to have to source the setup file every time you open a new shell, then you can add the command to your shell startup script:

echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc

Sourcing ROS 2 setup files will set several environmentvariables necessary for operating ROS 2. If you have problems finding or using your ROS 2 packages, make sure that your environment is properly setup using the following command to check:

printenv | grep -i ROS

Check those variables like ROS_DISTRO and ROS_VERSION to see if they are correctly set.

ROS_VERSION=2
ROS_PYTHON_VERSION=3
ROS_DISTRO=foxy

If the environment variables are not set correctly, return to the ROS 2 package installation section of the installation guide you followed.

If the ROS_DISTRO isn't set in your environment, you probably don't have a clean ROS 2 installation. As a result, you may have issues with the rest of this tutorial. I recommend reinstalling your ROS 2Foxyinstallation.

The following environment variable is not required to be set or exist in order to have a functional bridge:

export ROS_DOMAIN_ID=<your_domain_id>

Now, create a directory:

mkdir -p ~/ros2_ws/src
cd ~/ros2_ws

At this point, the workspace contains a single empty directory src:

.
└── src
1 directory, 0 files

Installing the fastrtpsgen

The fastrtpsgen component is required to build the client application (on the development system: your laptop or desktop system) and agentapplication (on the offboard system - NavQ Plus development board). So, follow the steps:

• By default, Ubuntu 20.04 includes the Java 11 JDK. If Java is not installed on your system, then use the following command to install the Java JDK:

$ sudo apt install default-jdk

Now, check whether Java is installed on your system or not using the following command:

$ java --version

• Install SDKMAN:

$ curl -s "https://get.sdkman.io" | bash

• Please open a new terminal, or run the following in the existing one:

$ source "/home/$USER/.sdkman/bin/sdkman-init.sh"

• Now, install gradle with a package manager:

$ sdk install gradle 8.0.2

• Verify your installation. Open a console and run gradle -v to run gradle and display the version, e.g.:

$ gradle -v

• Install Fast-RTPS-Gen - Fast-RTPS-Gen is the Fast RTPS (DDS)IDL code generator tool required in version 1.0.4, no later. To install Fast-RTPS-Gen, gradle is required:

$ git clone --recursive https://github.com/eProsima/Fast-DDS-Gen.git -b v1.0.4 ~/Fast-RTPS-Gen
$ cd ~/Fast-RTPS-Gen
$ ./gradlew assemble
$ sudo ./gradlew install

Execute gradle from a serial CLI (tty), not from an SSH connection (pts). Otherwise, you will get the following error:

FAILURE: Build failed with an exception.
* What went wrong:
Could not open terminal for stdout: could not get termcap entry

Make sure the fastrtpsgen application is in your PATH. You can check with the following:

$ which fastrtpsgen

The result should be:

/usr/local/bin/fastrtpsgen

The client application

You can skip this step if the client application (microRTPS Client), micrortps_client, is present in the PX4 autopilot firmware. Check this using the NuttShell interpreter based on QGroundControl connection – use the help CLI command. In my PX4 autopilot image, it was not present. As a direct result in this case, follow the steps:

• Clone the PX4 autopilot firmware:

$ git clone -b v1.13.2 https://github.com/PX4/Firmware.git px4-firmware_v1.13.2

• Build the firmware. There are different configurations (targets) that can be built. To find all of them, use the following:

$ make list_config_targets

You will get a lot of them, but I’m interested in the ones related to the NXP HoverGames drone:

…
nxp_fmuk66-e[_default]
nxp_fmuk66-e_rtps
nxp_fmuk66-e_socketcan
nxp_fmuk66-v3[_default]
nxp_fmuk66-v3_rtps
nxp_fmuk66-v3_socketcan
nxp_fmuk66-v3_test
nxp_fmurt1062-v1[_default]
nxp_ucans32k146_canbootloader
nxp_ucans32k146[_default]
nxp_ucans32k146_servo
...

• The targets finishing in rtps are the focus here. So, start the building process:

$ make nxp_fmuk66-v3_rtps

If there are no errors in the building process, you have a PX4 autopilot firmware image that contains the microRTPS Client) - micrortps_client. To do this step, you must have previously installed the fastrtpsgen application in your system

• In other cases (e.g., when you get an error), you must manually generate the client. In such a case, follow the steps presented here:

https://docs.px4.io/v1.13/en/middleware/micrortps_manual_code_generation.html

• In the final, upload the binary file (nxp_fmuk66-v3_rtps.bin in my case) into the flight controller (FMUK66) using the QGroundControlapplication.

Install PX4 ROS Com, PX4 msg, and agent application

On the offboard system, clone the ROS 2 bridge packages px4_ros_com and px4_msgs to the /src directory (the branch release/1.13):

cd ~/ros2_ws/src
$ git clone -b release/1.13 https://github.com/PX4/px4_msgs.git px4_msgs
$ git clone -b release/1.13 https://github.com/PX4/px4_ros_com.git px4_ros_com

Now you can compile the applications from the ROS 2 workspace. Move to the right folder (~/ros2_ws) and build just the messages (I suppose that the ROS 2 is correctly sourced: source /opt/ros/foxy/setup.bash):

$ colcon build --symlink-install --packages-select px4_msgs

Now, you must also source the previously installed messages:

$ . install/local_setup.bash

And now you are ready to build px4_ros_com and the agent (micrortps_agent):

$ colcon build --packages-select px4_ros_com --event-handlers console_direct+

To ensure that you will have required information to solve an error, use the option --event-handlers console_direct+to add verbosity.As a direct result, px4_ros_com and micrortps_agent were built.

Check micrortps_agent:

$ which micrortps_agent
/home/user/ros2_ws/install/px4_ros_com/bin/micrortps_agent

Creating and configuring the bridge (micrortps_client & micrortps_agent)

The Client application can be launched from NuttShell/System Console. The command syntax is shown below (you can specify a large number of arguments to configure the client accordingly to your specific situation):

micrortps_client start|stop|status [options]

-t <transport>  [UART|UDP] Default UART
-d <device> UART device. Default /dev/ttyACM0
-l <loops> How many iterations will this program have. -1 for infinite. Default -1.
-w <sleep_time_ms> Time in ms for which each iteration sleep. Default 1ms
-b <baudrate> UART device baudrate. Default 460800
-p <poll_ms> Time in ms to poll over UART. Default 1ms
-r <reception port> UDP port for receiving. Default 2019
-s <sending port> UDP port for sending. Default 2020
-i <ip_address> Select IP address (remote) values: <x.x.x.x>. Default:127.0.0.1

The Client is in your PX4 autopilot by default and must be executed as a daemon. But it does not automatically start and must be manually launched or configured to be lunch automatically by the PX4 autopilot.

For example, to run the Client daemon with SITL connecting to the Agent via UDP:

micrortps_client start -t UDP

The physical link between FMU (FMUK66) and the offboard computer (NavQ Plus) is, in my case, a serial one. So, on the HoverGamesdrones FMU, I used TELEMETRY 2 port, which is /dev/ttyS1(SERIAL 2/TELEMETRY 2/IRDA). Based on this information, start the client:

> micrortps_client start -t UART -d /dev/ttyS1 -b 921600 115200

The command syntax for the Agent is listed below:

$ ./micrortps_agent [options]

-t <transport> [UART|UDP] Default UART.
-d <device> UART device. Default /dev/ttyACM0.
-w <sleep_time_us> Time in us for which each iteration sleep. Default 1ms.
-b <baudrate> UART device baudrate. Default 460800.
-p <poll_ms> Time in ms to poll over UART. Default 1ms.
-r <reception port> UDP port for receiving. Default 2019.
-s <sending port> UDP port for sending. Default 2020.
-n <set namespace> Set a namespace for the micrortps_agent.

To launch the Agent, run micrortps_agent with appropriate options for specifying the connection to the Client. As an example, to start the micrortps_agentwith a link through UDP, issue:

./micrortps_agent -t UDP

On my offboardcomputer (NavQ Plus):

$ micrortps_agent -t UART -d /dev/ttymxc2 -b 921600

Testing the bridge connection

To check the bridge, see if you can receive from the drone different topics:

$ cd ~/ros2_ws
$ . install/setup.sh
$ ros2 topic list

If the bridge is working, you should see a list of topics that match the ones sent from the NXP HoverGames drone:

/fmu/collision_constraints/out
/fmu/debug_array/in
/fmu/debug_key_value/in
/fmu/debug_value/in
/fmu/debug_vect/in
/fmu/offboard_control_mode/in
/fmu/onboard_computer_status/in
/fmu/optical_flow/in
/fmu/position_setpoint/in
/fmu/position_setpoint_triplet/in
/fmu/sensor_combined/out
/fmu/telemetry_status/in
/fmu/timesync/in
/fmu/timesync/out
/fmu/trajectory_bezier/in
/fmu/trajectory_setpoint/in
/fmu/trajectory_waypoint/out
/fmu/vehicle_command/in
/fmu/vehicle_control_mode/out
/fmu/vehicle_local_position_setpoint/in
/fmu/vehicle_mocap_odometry/in
/fmu/vehicle_odometry/out
/fmu/vehicle_status/out
/fmu/vehicle_trajectory_bezier/in
/fmu/vehicle_trajectory_waypoint/in
/fmu/vehicle_trajectory_waypoint_desired/out
/fmu/vehicle_visual_odometry/in
/parameter_events
/rosout
/timesync_status

Let's read a topic:

$ ros2 topic echo /fmu/sensor_combined/out

And you will get something similar with this:

...
---
timestamp: 1679659797387031
gyro_rad:
- -0.015279840677976608
- 0.0010648384923115373
- 0.001764044864103198
gyro_integral_dt: 5030
accelerometer_timestamp_relative: 0
accelerometer_m_s2:
- -0.4627833068370819
- -0.21343198418617249
- -9.804315567016602
accelerometer_integral_dt: 5024
accelerometer_clipping: 0
accel_calibration_count: 1
gyro_calibration_count: 0
---
...

Or you can check another topic if you desire:

$ ros2 topic echo /fmu/vehicle_odometry/out

Another sanity check can be done through "listener" using the provided launch file:

$ ros2 launch px4_ros_com sensor_combined_listener.launch.py

If the bridge is working correctly, you will be able to see the data being printed on the terminal/console where you launched the ROS listener:

[INFO] [launch]: All log files can be found below /home/user/.ros/log/2023-03-24-12-11-31-808509-imx8mpnavq-1622
[INFO] [launch]: Default logging verbosity is set to INFO
/home/user/ros2_ws/install/px4_ros_com/share/px4_ros_com/launch/sensor_combined_listener.launch.py:50: UserWarning: The parameter 'node_executable' is deprecated, use 'executable' instead
     launch_ros.actions.Node(
[INFO] [sensor_combined_listener-1]: process started with pid [1624]
[sensor_combined_listener-1] Starting sensor_combined listener node...[sensor_combined_listener-1]
[sensor_combined_listener-1]
[sensor_combined_listener-1]
[sensor_combined_listener-1]
[sensor_combined_listener-1] RECEIVED SENSOR COMBINED DATA
[sensor_combined_listener-1] =============================
sensor_combined_listener-1] ts: 1679659894495059
[sensor_combined_listener-1] gyro_rad[0]: -0.00908325
[sensor_combined_listener-1] gyro_rad[1]: 0.00989111
[sensor_combined_listener-1] gyro_rad[2]: -0.00115527
[sensor_combined_listener-1] gyro_integral_dt: 5019
[sensor_combined_listener-1] accelerometer_timestamp_relative: 0
[sensor_combined_listener-1] accelerometer_m_s2[0]: -0.440195
[sensor_combined_listener-1] accelerometer_m_s2[1]: -0.256383
[sensor_combined_listener-1] accelerometer_m_s2[2]: -9.85126
[sensor_combined_listener-1] accelerometer_integral_dt: 5229
[sensor_combined_listener-1]
[sensor_combined_listener-1]

Starting micrortps_client at boot

You have two different methods to achieve this objective: start the client when the PX4 autopilot boot. Both methods are presented here and work flawlessly (I check them personally). But, I recommend the first one which is in the main conception of the PX4 autopilot.

Based on parameters set through QGroundControl

To have the micrortps_client starting at boot on the desired port with the desired baud rate, use the following parameters: RTPS_CONFIG and SER_TEL2_BAUD.

The parameter RTPS_CONFIG will start and configure which serial port micrortps_client will use – see https://docs.px4.io/v1.9.0/en/advanced_config/parameter_reference.html.

So, choose the serial port you want to use and reboot.

After the system boot, select the desired baud rate based on the associated SER_***_BAUD parameter – SER_TEL2_BAUD for the practical situation presented here.

Based on a startup file placed on the SD card

In the second method, you must have an SD card inserted to make the microRTPS client start at boot on the FMU. On this SD card, create a file at /etc/extras.txt and insert the following options:

set +e
# For UART communication over the IR/TELM2 port:
micrortps_client start -d /dev/ttyS1 -b 921600
set -e