ROS 2 Offboard Control Приклад

The following C++ example shows how to do position control in offboard mode from a ROS 2 node.

Приклад починає відправляти установочні точки, входить у режим offboard, озброюється, піднімається на 5 метрів і чекає. Незважаючи на простоту, він демонструє основні принципи використання offboard control і способи надсилання команд транспортному засобу.

It has been tested on Ubuntu 20.04 with ROS 2 Foxy and PX4 v1.14.

WARNING

Offboard control is dangerous. Якщо ви керуєте реальним транспортним засобом, то обов'язково майте можливість отримати назад ручне керування на випадок, якщо щось піде не так.

INFO

ROS and PX4 make a number of different assumptions, in particular with respect to frame conventions. Не існує неявного перетворення між типами кадрів при публікації або підписці на теми!

У цьому прикладі публікуються позиції у фреймі NED, як і очікує 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 frame_transforms library.

Випробування

Follow the instructions in ROS 2 User Guide 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 ROS 2 User Guide > Build ROS 2 Workspace to run the example.

TIP

Make sure that QGC is connected to PX4 before running the ROS 2 node. This is needed because, by default, you cannot arm a vehicle without a connection to ground station (QGC) or an established RC connection (this ensures there is always the option to regain manual control).

Створити та запустити приклад:

1. Відкрийте новий термінал.

2. Створіть новий каталог робочого простору colcon і перейдіть до нього за допомогою:

   mkdir -p ~/ws_offboard_control/src/
   cd ~/ws_offboard_control/src/

3. Clone the px4_msgs repo to the /src directory (this repo is needed in every ROS 2 PX4 workspace!):

   git clone https://github.com/PX4/px4_msgs.git
   # checkout the matching release branch if not using PX4 main.

4. Clone the example repository px4_ros_com to the /src directory:

   git clone https://github.com/PX4/px4_ros_com.git

5. Source the ROS 2 development environment into the current terminal and compile the workspace using colcon:

   cd ..
   source /opt/ros/humble/setup.bash
   colcon build

:::

::::

6. Source the local_setup.bash:

   source install/local_setup.bash

7. Запустіть приклад.

   ros2 run px4_ros_com offboard_control

Транспортний засіб повинен озброїтися, піднятися на 5 метрів і потім зачекати (вічно).

Імплементація

The source code of the offboard control example can be found in PX4/px4_ros_com in the directory /src/examples/offboard/offboard_control.cpp.

INFO

PX4 publishes all the messages used in this example as ROS topics by default (see dds_topics.yaml).

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. Необхідна поведінка реалізується за допомогою головного циклу, що виконується у вузлі ROS 2, як показано нижче:

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();
	}

	// OffboardControlMode needs to be paired with TrajectorySetpoint
	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);

Цикл працює на таймері тривалістю 100 мс. For the first 10 cycles it calls publish_offboard_control_mode() and publish_trajectory_setpoint() to send OffboardControlMode and TrajectorySetpoint 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).

After 10 cycles publish_vehicle_command() is called to change to offboard mode, and arm() is called to arm the vehicle. Після того, як транспортний засіб озброюється та змінює режим, він починає відстежувати встановлені точки. Задані точки все ще відправляються в кожному циклі, щоб транспортний засіб не виходив з режиму поза платформою.

The implementations of the publish_offboard_control_mode() and publish_trajectory_setpoint() methods are shown below. These publish the OffboardControlMode and TrajectorySetpoint messages to PX4 (respectively).

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.

/**
 * @brief Publish the offboard control mode.
 *        For this example, only position and altitude controls are active.
 */
void OffboardControl::publish_offboard_control_mode()
{
	OffboardControlMode msg{};
	msg.position = true;
	msg.velocity = false;
	msg.acceleration = false;
	msg.attitude = false;
	msg.body_rate = false;
	msg.thrust_and_torque = false;
	msg.direct_actuator = false;
	msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
	offboard_control_mode_publisher_->publish(msg);
}

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.

/**
 * @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()
{
	TrajectorySetpoint msg{};
	msg.position = {0.0, 0.0, -5.0};
	msg.yaw = -3.14; // [-PI:PI]
	msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
	trajectory_setpoint_publisher_->publish(msg);
}

The publish_vehicle_command() sends VehicleCommand 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.

/**
 * @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)
{
	VehicleCommand msg{};
	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;
	msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
	vehicle_command_publisher_->publish(msg);
}

INFO

VehicleCommand is one of the simplest and most powerful ways to command PX4, and by subscribing to VehicleCommandAck you can also confirm that setting a particular command was successful. The param and command fields map to MAVLink commands and their parameter values.