# ROS 2 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
srcdirectory. See the link in the previous point for details.offboard_control_modeandtrajectory_setpointmessages are configured in theurtps_bridge_topics.yamlfile 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:
- msg: offboard_control_mode receive: true ... - msg: vehicle_command receive: true ... - msg: vehicle_local_position_setpoint receive: true - msg: trajectory_setpoint # multi-topic / alias of vehicle_local_position_setpoint base: vehicle_local_position_setpoint receive: trueIn path_to_colcon_ws/src/px4_ros_com/templates/urtps_bridge_topics.yaml:
- msg: OffboardControlMode receive: true ... - msg: VehicleCommmand receive: true ... - msg: VehicleLocalPositionSetpoint receive: true - msg: TrajectorySetpoint base: VehicleLocalPositionSetpoint receive: truevehicle_commandmessage is configured in theurtps_bridge_topics.yamlfile both in the PX4-Autopilot and px4_ros_com package to send to the Autopilot.In PX4-Autopilot/msg/tools/urtps_bridge_topics.yaml:
- msg: vehicle_command receive: trueIn path_to_colcon_ws/src/px4_ros_com/templates/uorb_rtps_message_ids.yaml:
- msg: VehicleCommmand receive: true
注解
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 (opens new window).
Here are some details about the implementation:
timesync_sub_ = this->create_subscription<px4_msgs::msg::Timesync>("/fmu/timesync/out",
10,
[this](const px4_msgs::msg::Timesync::UniquePtr msg) {
timestamp_.store(msg->timestamp);
});
The above is required in order to obtain a synchronized timestamp to be set and sent with the offboard_control_mode and trajectory_setpoint messages.
auto timer_callback = [this]() -> void {
if (offboard_setpoint_counter_ == 10) {
// Change to Offboard mode after 10 setpoints
this->publish_vehicle_command(VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6);
// Arm the vehicle
this->arm();
}
// offboard_control_mode needs to be paired with trajectory_setpoint
publish_offboard_control_mode();
publish_trajectory_setpoint();
// stop the counter after reaching 11
if (offboard_setpoint_counter_ < 11) {
offboard_setpoint_counter_++;
}
};
timer_ = this->create_wall_timer(100ms, timer_callback);
}
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.
/**
* @brief Publish the offboard control mode.
* For this example, only position and altitude controls are active.
*/
void OffboardControl::publish_offboard_control_mode() const {
OffboardControlMode msg{};
msg.timestamp = timestamp_.load();
msg.position = true;
msg.velocity = false;
msg.acceleration = false;
msg.attitude = false;
msg.body_rate = false;
offboard_control_mode_publisher_->publish(msg);
}
/**
* @brief Publish a trajectory setpoint
* For this example, it sends a trajectory setpoint to make the
* vehicle hover at 5 meters with a yaw angle of 180 degrees.
*/
void OffboardControl::publish_trajectory_setpoint() const {
TrajectorySetpoint msg{};
msg.timestamp = timestamp_.load();
msg.x = 0.0;
msg.y = 0.0;
msg.z = -5.0;
msg.yaw = -3.14; // [-PI:PI]
trajectory_setpoint_publisher_->publish(msg);
}
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 (opens new window) library.
/**
* @brief Publish vehicle commands
* @param command Command code (matches VehicleCommand and MAVLink MAV_CMD codes)
* @param param1 Command parameter 1
* @param param2 Command parameter 2
*/
void OffboardControl::publish_vehicle_command(uint16_t command, float param1,
float param2) const {
VehicleCommand msg{};
msg.timestamp = timestamp_.load();
msg.param1 = param1;
msg.param2 = param2;
msg.command = command;
msg.target_system = 1;
msg.target_component = 1;
msg.source_system = 1;
msg.source_component = 1;
msg.from_external = true;
vehicle_command_publisher_->publish(msg);
}
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):
$ source path_to_colcon_workspace/install/setup.bash
$ ros2 run px4_ros_com offboard_control