ROS 2 Offboard Control Example
Last updated
Last updated
The following C++ example shows how to do position control in from a ROS 2 node.
The example starts sending setpoints, enters offboard mode, arms, ascends to 5 metres, and waits. While simple, it shows the main principles of how to use offboard control and how to send vehicle commands.
It has been tested on Ubuntu 20.04 with ROS 2 Foxy and PX4 main
after PX4 v1.13.
:::warning Offboard control is dangerous. If you are operating on a real vehicle be sure to have a way of gaining back manual control in case something goes wrong. :::
:::note ROS and PX4 make a number of different assumptions, in particular with respect to . There is no implicit conversion between frame types when topics are published or subscribed!
This example publishes positions in the NED frame, as expected by PX4. To subscribe to data coming from nodes that publish in a different frame (for example the ENU, which is the standard frame of reference in ROS/ROS 2), use the helper functions in the library. :::
Follow the instructions in to install PX and run the simulator, install ROS 2, and start the XRCE-DDS Agent.
After that we can follow a similar set of steps to those in to run the example.
To build and run the example:
Open a new terminal.
Create and navigate into a new colcon workspace directory using:
Clone the repo to the /src
directory (this repo is needed in every ROS 2 PX4 workspace!):
Clone the example repository to the /src
directory:
Source the ROS 2 development environment into the current terminal and compile the workspace using colcon
:
:::: tabs
::: tab humble
:::
::: tab foxy
:::
::::
Source the local_setup.bash
:
Launch the example.
The vehicle should arm, ascend 5 metres, and then wait (perpetually).
PX4 requires that the vehicle is already receiving OffboardControlMode
messages before it will arm in offboard mode, or before it will switch to offboard mode when flying. In addition, PX4 will switch out of offboard mode if the stream rate of OffboardControlMode
messages drops below approximately 2Hz. The required behaviour is implemented by the main loop spinning in the ROS 2 node, as shown below:
After 10 cycles publish_vehicle_command()
is called to change to offboard mode, and arm()
is called to arm the vehicle. After the vehicle arms and changes mode it starts tracking the position setpoints. The setpoints are still sent in every cycle so that the vehicle does not fall out of offboard mode.
The OffboardControlMode
is required in order to inform PX4 of the type of offboard control behing used. Here we're only using position control, so the position
field is set to true
and all the other fields are set to false
.
TrajectorySetpoint
provides the position setpoint. In this case, the x
, y
, z
and yaw
fields are hardcoded to certain values, but they can be updated dynamically according to an algorithm or even by a subscription callback for messages coming from another node.
The source code of the offboard control example can be found in in the directory .
:::note PX4 publishes all the messages used in this example as ROS topics by default (see ). :::
The loop runs on a 100ms timer. For the first 10 cycles it calls publish_offboard_control_mode()
and publish_trajectory_setpoint()
to send and messages to PX4. The OffboardControlMode
messages are streamed so that PX4 will allow arming once it switches to offboard mode, while the TrajectorySetpoint
messages are ignored (until the vehicle is in offboard mode).
The implementations of the publish_offboard_control_mode()
and publish_trajectory_setpoint()
methods are shown below. These publish the and messages to PX4 (respectively).
The publish_vehicle_command()
sends messages with commands to the flight controller. We use it above to change the mode to offboard mode, and also in arm()
to arm the vehicle. While we don't call disarm()
in this example, it is also used in the implementation of that function.
:::note is one of the simplest and most powerful ways to command PX4, and by subscribing to you can also confirm that setting a particular command was successful. The param and command fields map to and their parameter values. :::