ROS 2 microRTPS Offboard Control Example
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.
ROS 2 interaction with PX4, done through the microRTPS bridge, requires that the user understands how the PX4 internals work! The same understanding is required for PX4 offboard control via ROS 2, where the user publishes directly to the required uORB topics (without any level of abstraction between ROS and PX4 data formats/conventions).
If you are unsure of PX4 internals work, we recommend that you instead use a workflow that depends on the MAVLink microservices and abstraction layer to execute offboard control or any other kind of interaction through the microRTPS bridge.
The following C++ example shows how to use the microRTPS bridge and the px4_ros_com package to do offboard position control from a ROS 2 node.
Requirements
For this example, PX4 SITL is being used, so it is assumed, first of all, that the user has the simulation environment properly configured. Besides that:
The user already has their ROS 2 environment properly configured Check the PX4-ROS 2 bridge document for details on how to do it.
px4_msgs and px4_ros_com should be already on your colcon workspace
src
directory. See the link in the previous point for details.offboard_control_mode
andtrajectory_setpoint
messages are configured in theurtps_bridge_topics.yaml
file both in the PX4-Autopilot and px4_ros_com package to be received in the Autopilot.In PX4-Autopilot/msg/tools/urtps_bridge_topics.yaml:
In path_to_colcon_ws/src/px4_ros_com/templates/urtps_bridge_topics.yaml:
vehicle_command
message is configured in theurtps_bridge_topics.yaml
file both in the PX4-Autopilot and px4_ros_com package to send to the Autopilot.In PX4-Autopilot/msg/tools/urtps_bridge_topics.yaml:
In path_to_colcon_ws/src/px4_ros_com/templates/uorb_rtps_message_ids.yaml:
At time of writing, the above topic is already configured to be sent.
Implementation
The source code of the offboard control example can be found in offboard_control.cpp.
Here are some details about the implementation:
The above is required in order to obtain a syncronized timestamp to be set and sent with the offboard_control_mode
and trajectory_setpoint
messages.
The above is the main loop spinning on the ROS 2 node. It first sends 10 setpoint messages before sending the command to allow PX4 to change to offboard mode. At the same time, both offboard_control_mode
and trajectory_setpoint
messages are sent to the flight controller.
The above functions show how the fields on both offboard_control_mode
and trajectory_setpoint
messages can be set. Notice that the above example is applicable for offboard position control, where on the offboard_control_mode
message, the position
field is set to true
, while all the others get set to false
. Also, 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 position is published in the NED coordinate frame for simplicity. If a user wants 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), they can use the helper functions in the frame_transforms library.
As the description suggests, the above code serves the purpose of sending vehicle_command
messages with commands to the flight controller.
Usage
After building the colcon workspace, and after starting PX4 SITL (make px4_sitl_rtps gazebo
, which starts the microRTPS client automatically on UDP ports 2019 and 2020) and the microRTPS agent (micrortps_agent -t UDP
, starting the agent connected to UDP ports 2020 and 2019):
Demo with PX4 SITL and Gazebo
Last updated