Offboard mode for ROS2

I am trying to run offboard mode from ROS2 with SITL and gazebo, but the current documentation is outdated compared to PX4 stable branch (tag v1.13.1) and px4_ros_com (there are no tags).
I was able to make it somehow run with microdds_client enabled in PX4 and mircoROS agent.
I can list topics related to fmu

/fmu/in/DebugArray
/fmu/in/DebugKeyValue
/fmu/in/DebugValue
/fmu/in/DebugVect
/fmu/in/OffboardControlMode
/fmu/in/OnboardComputerStatus
/fmu/in/OpticalFlow
/fmu/in/PositionSetpoint
/fmu/in/PositionSetpointTriplet
/fmu/in/TelemetryStatus
/fmu/in/Timesync
/fmu/in/TrajectoryBezier
/fmu/in/TrajectorySetpoint
/fmu/in/VehicleCommand
/fmu/in/VehicleLocalPositionSetpoint
/fmu/in/VehicleMocapOdometry
/fmu/in/VehicleTrajectoryBezier
/fmu/in/VehicleTrajectoryWaypoint
/fmu/in/VehicleVisualOdometry
/fmu/out/CollisionConstraints
/fmu/out/SensorCombined
/fmu/out/Timesync
/fmu/out/TimesyncStatus
/fmu/out/TrajectoryWaypoint
/fmu/out/VehicleControlMode
/fmu/out/VehicleOdometry
/fmu/out/VehicleStatus
/fmu/out/VehicleTrajectoryWaypointDesired
/parameter_events
/rosout

My issue is that when I start the modified example of offboard node in ROS2, the drone is just armed, and nothing more happens.

INFO  [px4] Startup script returned successfully
pxh> INFO  [mavlink] partner IP: 127.0.0.1
INFO  [tone_alarm] home set
INFO  [tone_alarm] notify negative
INFO  [commander] Armed by external command
INFO  [tone_alarm] arming warning
INFO  [commander] Disarmed by auto preflight disarming
INFO  [tone_alarm] notify neutral
INFO  [logger] closed logfile, bytes written: 6893549

I don’t know what is wrong. My main issue is that I have no experience with PX4, and I don’t know how to debug/analyze what is happening there. Maybe, If I could see some verbose logs or better understand what is needed for offboard mode, I could find a problem.
I would be very glad for some advice or explanation of PX4 offboard mode. Or some working example with the newest PX4 firmware, which uses ROS2 and microdds_client.

Here is my offboard node code. Code is taken from px4_ros_com offboard_control.cpp. I just changed topics names because, in my case, topics start with a capital letter which differs from the current implementation.

#include <px4_msgs/msg/offboard_control_mode.hpp>
#include <px4_msgs/msg/trajectory_setpoint.hpp>
#include <px4_msgs/msg/vehicle_command.hpp>
#include <px4_msgs/msg/vehicle_control_mode.hpp>
#include <rclcpp/rclcpp.hpp>
#include <stdint.h>

#include <chrono>
#include <iostream>

using namespace std::chrono;
using namespace std::chrono_literals;
using namespace px4_msgs::msg;

class OffboardControl : public rclcpp::Node
{
public:
	OffboardControl() : Node("offboard_control")
	{

		offboard_control_mode_publisher_ = this->create_publisher<OffboardControlMode>("/fmu/in/OffboardControlMode", 10);
		trajectory_setpoint_publisher_ = this->create_publisher<TrajectorySetpoint>("/fmu/in/TrajectorySetpoint", 10);
		vehicle_command_publisher_ = this->create_publisher<VehicleCommand>("/fmu/in/VehicleCommand", 10);

		offboard_setpoint_counter_ = 0;

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

	void arm();
	void disarm();

private:
	rclcpp::TimerBase::SharedPtr timer_;

	rclcpp::Publisher<OffboardControlMode>::SharedPtr offboard_control_mode_publisher_;
	rclcpp::Publisher<TrajectorySetpoint>::SharedPtr trajectory_setpoint_publisher_;
	rclcpp::Publisher<VehicleCommand>::SharedPtr vehicle_command_publisher_;

	std::atomic<uint64_t> timestamp_;   //!< common synced timestamped

	uint64_t offboard_setpoint_counter_;   //!< counter for the number of setpoints sent

	void publish_offboard_control_mode();
	void publish_trajectory_setpoint();
	void publish_vehicle_command(uint16_t command, float param1 = 0.0, float param2 = 0.0);
};

/**
 * @brief Send a command to Arm the vehicle
 */
void OffboardControl::arm()
{
	publish_vehicle_command(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0);

	RCLCPP_INFO(this->get_logger(), "Arm command send");
}

/**
 * @brief Send a command to Disarm the vehicle
 */
void OffboardControl::disarm()
{
	publish_vehicle_command(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0);

	RCLCPP_INFO(this->get_logger(), "Disarm command send");
}

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

/**
 * @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);
}

int main(int argc, char *argv[])
{
	std::cout << "Starting offboard control node..." << std::endl;
	setvbuf(stdout, NULL, _IONBF, BUFSIZ);
	rclcpp::init(argc, argv);
	rclcpp::spin(std::make_shared<OffboardControl>());

	rclcpp::shutdown();
	return 0;
}

Do you have QGC opened and running?
By default, if there is no RC (or joystick in SITL) and datalink connection you will enter failsafe.
So, try opening QGC with virtual joystick first.
If nothing change or if you have already done that, then try this python example GitHub - Jaeyoung-Lim/px4-offboard

I tried also a virtual joystick, and it didn’t help. I didn’t try this python example, but it is almost the same as mine. It didn’t contain steps related to PX4.
And I don’t think that problem is on the ROS2 side because, in qgc, I can see that mode is changed from hold to offboard. So it means that something is sent to PX4.
Even if I check the status for microdds_client, I get

pxh> microdds_client status
INFO  [microdds_client] Running, connected
INFO  [microdds_client] Payload tx: 41961 B/s
INFO  [microdds_client] Payload rx: 1832 B/s

So, something is sent there. But my feeling is that TrajectorySetpoint is somehow ignored.

There are some initial docs coming along in https://github.com/PX4/PX4-user_guide/pull/2094 - they are a bit stalled while I wait on the dev team to answer some very basic questions, but there might be enough for you to get started.

Finally, it is working. I didn’t realise I needed a main branch for PX4-Autopilot. I was using tag v1.13.1, which probably doesn’t contain a working microDDS client or related stuff.

1 Like