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