PX4 User Guide
  • Introduction
  • Getting Started
    • Basic Concepts
    • Vehicles/Frames
    • Flight Controllers
    • Sensors
    • Radio Systems
    • Flight Modes
    • Vehicle Status Notifications
      • LED Meanings
      • Tune/Sound Meanings
      • Preflight Checks
    • Payloads & Cameras
    • Flight Reporting
  • Basic Assembly
    • Mounting the Flight Controller
    • Mounting the GPS/Compass
    • Vibration Isolation
    • Cable Wiring
    • CUAV Pixhawk V6X Wiring QuickStart
    • CUAV V5+ Wiring Quickstart
    • CUAV V5 nano Wiring Quickstart
    • Holybro Pixhawk 6C Wiring Quickstart
    • Holybro Pixhawk 6X Wiring Quickstart
    • Holybro Pixhawk 5X Wiring Quickstart
    • Holybro Pixhawk 4 Wiring Quickstart - Discontinued
    • Holybro Pixhawk 4 Mini Wiring Quickstart - Discontinued
    • Holybro Durandal Wiring Quickstart
    • Holybro Pix32 v5 Wiring Quickstart
    • Cube Wiring Quickstart
    • Pixracer Wiring Quickstart
    • mRo (3DR) Pixhawk Wiring Quickstart
  • Standard Configuration
    • Firmware
    • Airframe
    • Sensor Orientation
    • Compass
    • Gyroscope
    • Accelerometer
    • Airspeed
    • Level Horizon Calibration
    • Radio Setup
    • Joystick Setup
    • Flight Modes
    • Battery
    • Safety
      • Failsafe Simulation
    • ESC Calibration
    • Actuators
    • Autotune
  • Vehicle Types & Setup
    • Multicopters
      • Multicopter Config/Tuning
        • MC Filter/Control Latency Tuning
        • MC PID Tuning (Manual/Basic)
        • MC PID Tuning Guide (Manual/Advanced)
        • MC Setpoint Tuning (Trajectory Generator)
          • MC Jerk-limited Type Trajectory
        • Multicopter Racer Setup
      • X500 v2 (Pixhawk 6C)
      • X500 v2 (Pixhawk 5X)
      • X500 (Pixhawk 4)
      • S500 V2 (Pixhawk 4)
      • DJI F450 (CUAV v5+)
      • DJI F450 (CUAV v5 nano)
      • QAV250 (Pixhawk4 Mini) - Discontinued
      • DJI F450 + RTK (Pixhawk 3 Pro)
      • QAV250 (Pixhawk Mini)
      • QAV-R 5" Racer (Pixracer)
      • Omnicopter
    • Planes
      • Fixed Wing Config/Tuning
        • Fixedwing PID Tuning Guide
        • Fixedwing Advanced Tuning Guide
        • Fixedwing Trimming Guide
      • Reptile Dragon 2 (ARK6X)
      • Turbo Timber Evolution (Pixhawk 4 Mini)
      • Wing Wing Z84 (Pixracer)
    • VTOL
      • VTOL Config/Tuning
        • QuadPlane Configuration
        • Back-transition Tuning
        • VTOL w/o Airspeed Sensor
        • VTOL Weather Vane
      • Standard VTOL
        • FunCub QuadPlane (Pixhawk)
        • Ranger QuadPlane (Pixhawk)
        • Falcon Vertigo QuadPlane (Dropix)
      • Tailsitter VTOL
        • Build: TBS Caipiroshka Tailsitter Build (Pixracer)
      • Tiltrotor VTOL
        • Build: Convergence Tiltrotor (Pixfalcon)
    • Airships (experimental)
    • Autogyros (experimental)
      • ThunderFly Auto-G2 (Holybro pix32)
    • Balloons (experimental)
    • Helicopter (experimental)
      • Helicopter Config/Tuning
    • Rovers (experimental)
      • Traxxas Stampede
    • Submarines (experimental)
      • BlueROV2
    • Airframes Reference
  • Flying
    • First Flight Guidelines
    • Flying 101
    • Missions
      • Package Delivery Mission
    • GeoFence
    • Safety Point Planning
    • Flight Modes
      • Position Mode (MC)
      • Altitude Mode (MC)
      • Manual/Stabilized Mode (MC)
      • Acro Mode (MC)
      • Orbit Mode (MC)
      • Position Mode (FW)
      • Altitude Mode (FW)
      • Stabilized Mode (FW)
      • Acro Mode (FW)
      • Manual Mode (FW)
      • Takeoff Mode
      • Land Mode
      • Return Mode
      • Hold Mode
      • Mission Mode
      • Follow Me Mode
      • Offboard Mode
    • Terrain Following/Holding
  • Flight Log Analysis
    • Log Analysis using Flight Review
    • Log Analysis using PlotJuggler
  • Advanced Configuration
    • Finding/Updating Parameters
    • Full Parameter Reference
    • ECL/EKF Overview & Tuning
    • Flight Termination Configuration
    • Bootloader Flashing onto Betaflight Systems
    • Land Detector Configuration
    • Prearm/Arm/Disarm Configuration
    • IMU Factory Calibration
    • Sensor Thermal Compensation
    • Compass Power Compensation
    • Advanced Controller Orientation
    • Static Pressure Buildup
    • Serial Port Configuration
    • MAVLink Telemetry (OSD/GCS)
    • PX4 Ethernet Setup
    • Bootloader Update
  • Hardware (Drones&Parts)
    • Complete Vehicles
      • ModalAI Starling
      • PX4 Vision Kit
      • MindRacer BNF & RTF
        • MindRacer 210
        • NanoMind 110
      • Holybro Kopis 2
      • Bitcraze Crazyflie 2.1
    • Flight Controllers (Autopilots)
      • Pixhawk Series
        • Silicon Errata
      • Pixhawk Standard Autopilots
        • CUAV Pixhawk V6X (FMUv6X)
        • Holybro Pixhawk 6X (FMUv6X)
        • Holybro Pixhawk 6C (FMUv6C)
        • Holybro Pixhawk 6C Mini(FMUv6C)
        • Holybro Pix32 v6 (FMUv6C)
        • Holybro Pixhawk 5X (FMUv5X)
        • Holybro Pixhawk 4 (FMUv5) - Discontinued
        • Holybro Pixhawk 4 Mini (FMUv5) - Discontinued
        • Drotek Pixhawk 3 Pro (FMUv4pro)
        • mRo Pixracer (FMUv4)
        • Hex Cube Black (FMUv3)
        • mRo Pixhawk (FMUv3)
        • Holybro Pixhawk Mini (FMUv3) - Discontinued
      • Manufacturer-Supported Autopilots
        • AirMind MindPX
        • AirMind MindRacer
        • ARK Electronics ARKV6X
        • CUAV X7
        • CUAV Nora
        • CUAV V5+ (FMUv5)
        • CUAV V5 nano (FMUv5)
        • CUAV Pixhack v3 (FMUv3)
        • CubePilot Cube Orange+ (CubePilot)
        • CubePilot Cube Orange (CubePilot)
        • CubePilot Cube Yellow (CubePilot)
        • Holybro Kakute H7v2
        • Holybro Kakute H7mini
        • Holybro Kakute H7
        • Holybro Durandal
        • Holybro Pix32 v5
        • ModalAI Flight Core v1
        • ModalAI VOXL Flight
        • ModalAI VOXL 2
        • mRobotics-X2.1 (FMUv2)
        • mRo Control Zero F7)
        • NXP RDDRONE-FMUK66 FMU
        • Sky-Drones AIRLink
        • SPRacing SPRacingH7EXTREME
        • ThePeach FCC-K1
        • ThePeach FCC-R1
      • Experimental Autopilots
        • BeagleBone Blue
        • Raspberry Pi 2/3 Navio2
        • Raspberry Pi 2/3/4 PilotPi
          • PilotPi with Raspberry Pi OS
          • PilotPi with Ubuntu Server
      • Discontinued Autopilots/Vehicles
        • Drotek Dropix (FMUv2)
        • Omnibus F4 SD
        • BetaFPV Beta75X 2S Brushless Whoop
        • Bitcraze Crazyflie 2.0
        • Aerotenna OcPoC-Zynq Mini
        • CUAV v5
        • Holybro Kakute F7 (Discontinued)
        • Holybro Pixfalcon
        • Holybro pix32 (FMUv2)
        • mRo AUAV-X2
        • 3DR Pixhawk 1
        • Snapdragon Flight
        • Intel® Aero RTF Drone (Discontinued)
      • Pixhawk Autopilot Bus (PAB) & Carriers
        • ARK Electronics Pixhawk Autopilot Bus Carrier
    • Flight Controller Peripherals
      • ADSB/FLARM (Traffic Avoidance)
      • Air Traffic Avoidance: ADSB/FLARM
      • Air Traffic Avoidance: UTM
      • Airspeed Sensors
        • TFSlot Airspeed Sensor
      • Barometers
      • Camera
      • Distance Sensors (Rangefinders)
        • Lightware SFxx Lidar
        • Ainstein US-D1 Standard Radar Altimeter
        • LeddarOne Lidar
        • Benewake TFmini Lidar
        • Lidar-Lite
        • TeraRanger
        • Lanbao PSK-CM8JL65-CC5
        • Avionics Anonymous Laser Altimeter UAVCAN Interface
      • ESCs & Motors
        • PWM ESCs and Servos
        • DShot ESCs
        • OneShot ESCs and Servos
        • DroneCAN ESCs
          • Zubax Telega
          • PX4 Sapog ESC Firmware
            • Holybro Kotleta
            • Zubax Orel
        • VESC
      • TBS Crossfire (CRSF) Telemetry
      • FrSky Telemetry
      • Gimbal (Mount) Configuration
      • GPS/Compass
        • ARK GPS
        • Holybro DroneCAN M8N GPS
        • LOCOSYS Hawk A1 GNSS
        • Hex Here2
        • Holybro M8N & M9N GPS
        • Sky-Drones SmartAP GPS
      • Grippers
        • Servo Gripper
      • Optical Flow
        • ARK Flow
        • PMW3901
        • PX4FLOW (Deprecated)
      • Precision Landing
      • Parachute
      • Power Modules/PDB
        • CUAV HV pm
        • CUAV CAN PMU
        • Holybro PM02
        • Holybro PM07
        • Holybro PM06 V2
        • Holybro PM02D (digital)
        • Holybro PM03D (digital)
        • Pomegranate Systems Power Module
        • Sky-Drones SmartAP PDB
      • Satellite Coms (Iridium/RockBlock)
      • Telemetry Radios
        • SiK Radio
          • RFD900 (SiK) Telemetry Radio
          • HolyBro (SIK) Telemetry Radio
        • Telemetry Wifi
          • ESP8266 WiFi Module
          • ESP32 WiFi Module
          • 3DR Telemetry Wifi (Discontinued)
        • Microhard Serial Telemetry Radio
          • ARK Electron Microhard Serial Telemetry Radio
          • Holybro Microhard P900 Telemetry Radio
        • CUAV P8 Telemetry Radio
        • HolyBro XBP9X - Discontinued
      • RTK GPS
        • ARK RTK GPS
        • RTK GPS Heading with Dual u-blox F9P
        • CUAV C-RTK
        • CUAV C-RTK2 PPK/RTK GNSS
        • CUAV C-RTK 9Ps
        • Femtones MINI2 Receiver
        • Freefly RTK GPS
        • Holybro H-RTK-F9P
        • Holybro H-RTK-M8P
        • Holybro H-RTK Unicore UM982 GPS
        • Locosys Hawk R1
        • Locosys Hawk R2
        • Septentrio AsteRx-RIB
        • Septentrio mosaic-go
        • Trimble MB-Two
        • CubePilot Here+ (Discontined)
      • Remote ID
      • Smart Batteries
        • Rotoye Batmon Battery Smartification Kit
      • Tachometers (Revolution Counters)
        • ThunderFly TFRPM01 Tachometer Sensor
      • I2C Peripherals
        • I2C bus accelerators
        • TFI2CADT01 I2C address translator
      • CAN Peripherals
      • DroneCAN Peripherals
        • PX4 DroneCAN Firmware
        • ARK CANnode
    • Companion Computers
      • Pixhawk + Companion Setup
        • RasPi Pixhawk Companion
      • Companion Computer Peripherals
      • Holybro Pixhawk RPI CM4 Baseboard
      • Auterion Skynode
      • Computer Vision
        • Obstacle Avoidance
        • Safe Landing
        • Collision Prevention
        • Path Planning Interface
        • Motion Capture (MoCap)
        • Visual Inertial Odometry (VIO)
          • Realsense T265 Tracking Camera (VIO)
      • Video Streaming
  • Development
    • Getting Started
      • Recommended Hardware/Setup
      • Toolchain Installation
        • MacOS Setup
        • Ubuntu Setup
        • Windows Setup
        • Visual Studio Code IDE
        • Other/Generic Tools
      • Building the Code
      • Writing your First Application
      • Application/Module Template
    • Concepts
      • PX4 Architecture
      • PX4 Flight Stack Architecture
        • Controller Diagrams
      • Events Interface
      • Flight Modes
      • Flight Tasks
      • Control Allocation
      • PWM limit state machine
      • System Startup
      • SD Card Layout
    • Simulation
      • jMAVSim Simulation
        • Multi-Vehicle Sim with JMAVSim
      • Gazebo Simulation
        • Vehicles
        • Multi-Vehicle Sim
      • Gazebo Classic Simulation
        • Vehicles
        • Worlds
        • Multi-Vehicle Sim
      • FlightGear Simulation
        • FlightGear Vehicles
        • Multi-Vehicle Sim with FlightGear
      • JSBSim Simulation
      • AirSim Simulation
      • Multi-Vehicle Simulation
      • Simulate Failsafes
      • HITL Simulation
      • Simulation-In-Hardware
    • Hardware
      • Flight Controller Reference Design
      • Manufacturer’s Board Support Guide
      • Flight Controller Porting Guide
        • PX4 Board Configuration (kconfig)
        • NuttX Board Porting Guide
      • Serial Port Mapping
      • Airframes
        • Adding a New Airframe
      • Device Drivers
      • Telemetry Radio
        • SiK Radio
      • Sensor and Actuator I/O
        • DroneCAN
        • I2C Bus
        • UART/Serial Ports
          • Port-Configurable Serial Drivers
      • RTK GPS (Integration)
    • Middleware
      • uORB Messaging
      • uORB Graph
      • uORB Message Reference
        • ActionRequest
        • ActuatorArmed
        • ActuatorControlsStatus
        • ActuatorMotors
        • ActuatorOutputs
        • ActuatorServos
        • ActuatorServosTrim
        • ActuatorTest
        • AdcReport
        • Airspeed
        • AirspeedValidated
        • AirspeedWind
        • AutotuneAttitudeControlStatus
        • BatteryStatus
        • ButtonEvent
        • CameraCapture
        • CameraStatus
        • CameraTrigger
        • CellularStatus
        • CollisionConstraints
        • CollisionReport
        • ControlAllocatorStatus
        • Cpuload
        • DebugArray
        • DebugKeyValue
        • DebugValue
        • DebugVect
        • DifferentialPressure
        • DistanceSensor
        • Ekf2Timestamps
        • EscReport
        • EscStatus
        • EstimatorAidSource1d
        • EstimatorAidSource2d
        • EstimatorAidSource3d
        • EstimatorBias
        • EstimatorBias3d
        • EstimatorEventFlags
        • EstimatorGpsStatus
        • EstimatorInnovations
        • EstimatorSelectorStatus
        • EstimatorSensorBias
        • EstimatorStates
        • EstimatorStatus
        • EstimatorStatusFlags
        • Event
        • FailsafeFlags
        • FailureDetectorStatus
        • FollowTarget
        • FollowTargetEstimator
        • FollowTargetStatus
        • GeneratorStatus
        • GeofenceResult
        • GimbalControls
        • GimbalDeviceAttitudeStatus
        • GimbalDeviceInformation
        • GimbalDeviceSetAttitude
        • GimbalManagerInformation
        • GimbalManagerSetAttitude
        • GimbalManagerSetManualControl
        • GimbalManagerStatus
        • GpioConfig
        • GpioIn
        • GpioOut
        • GpioRequest
        • GpsDump
        • GpsInjectData
        • Gripper
        • HealthReport
        • HeaterStatus
        • HomePosition
        • HoverThrustEstimate
        • InputRc
        • InternalCombustionEngineStatus
        • IridiumsbdStatus
        • IrlockReport
        • LandingGear
        • LandingGearWheel
        • LandingTargetInnovations
        • LandingTargetPose
        • LaunchDetectionStatus
        • LedControl
        • LogMessage
        • LoggerStatus
        • MagWorkerData
        • MagnetometerBiasEstimate
        • ManualControlSetpoint
        • ManualControlSwitches
        • MavlinkLog
        • MavlinkTunnel
        • Mission
        • MissionResult
        • ModeCompleted
        • MountOrientation
        • NavigatorMissionItem
        • NormalizedUnsignedSetpoint
        • NpfgStatus
        • ObstacleDistance
        • OffboardControlMode
        • OnboardComputerStatus
        • OrbTest
        • OrbTestLarge
        • OrbTestMedium
        • OrbitStatus
        • ParameterUpdate
        • Ping
        • PositionControllerLandingStatus
        • PositionControllerStatus
        • PositionSetpoint
        • PositionSetpointTriplet
        • PowerButtonState
        • PowerMonitor
        • PpsCapture
        • PwmInput
        • Px4ioStatus
        • QshellReq
        • QshellRetval
        • RadioStatus
        • RateCtrlStatus
        • RcChannels
        • RcParameterMap
        • Rpm
        • RtlTimeEstimate
        • SatelliteInfo
        • SensorAccel
        • SensorAccelFifo
        • SensorBaro
        • SensorCombined
        • SensorCorrection
        • SensorGnssRelative
        • SensorGps
        • SensorGyro
        • SensorGyroFft
        • SensorGyroFifo
        • SensorHygrometer
        • SensorMag
        • SensorOpticalFlow
        • SensorPreflightMag
        • SensorUwb
        • SensorSelection
        • SensorsStatus
        • SensorsStatusImu
        • SystemPower
        • TakeoffStatus
        • TaskStackInfo
        • TecsStatus
        • TelemetryStatus
        • TiltrotorExtraControls
        • TimesyncStatus
        • TrajectoryBezier
        • TrajectorySetpoint
        • TrajectoryWaypoint
        • TransponderReport
        • TuneControl
        • UavcanParameterRequest
        • UavcanParameterValue
        • UlogStream
        • UlogStreamAck
        • UwbDistance
        • UwbGrid
        • VehicleAcceleration
        • VehicleAirData
        • VehicleAngularAccelerationSetpoint
        • VehicleAngularVelocity
        • VehicleAttitude
        • VehicleAttitudeSetpoint
        • VehicleCommand
        • VehicleCommandAck
        • VehicleConstraints
        • VehicleControlMode
        • VehicleGlobalPosition
        • VehicleImu
        • VehicleImuStatus
        • VehicleLandDetected
        • VehicleLocalPosition
        • VehicleLocalPositionSetpoint
        • VehicleMagnetometer
        • VehicleOdometry
        • VehicleOpticalFlow
        • VehicleOpticalFlowVel
        • VehicleRatesSetpoint
        • VehicleRoi
        • VehicleStatus
        • VehicleThrustSetpoint
        • VehicleTorqueSetpoint
        • VehicleTrajectoryBezier
        • VehicleTrajectoryWaypoint
        • VtolVehicleStatus
        • Wind
        • YawEstimatorStatus
      • MAVLink Messaging
      • uXRCE-DDS (PX4-ROS 2/DDS Bridge)
    • Modules & Commands
      • Autotune
      • Commands
      • Communication
      • Controllers
      • Drivers
        • Airspeed Sensor
        • Baro
        • Distance Sensor
        • IMU
        • INS
        • Magnetometer
        • Optical Flow
        • Rpm Sensor
        • Transponder
      • Estimators
      • Simulations
      • System
      • Template
    • Debugging/Logging
      • FAQ
      • Consoles/Shells
        • MAVLink Shell
        • System Console
      • Debugging with GDB
        • SWD Debug Port
        • JLink Probe
        • Black Magic/DroneCode Probe
        • STLink Probe
        • Hardfault Debugging
      • Debugging with Eclipse
      • Failure Injection
      • Sensor/Topic Debugging
      • Simulation Debugging
      • Sending Debug Values
      • System-wide Replay
      • Profiling
      • Binary Size Profiling
      • Logging
      • Flight Log Analysis
      • ULog File Format
    • Tutorials
      • Long-distance Video Streaming
      • Connecting an RC Receiver on Linux
    • Advanced Topics
      • Parameters & Configs
      • Package Delivery Architecture
      • Computer Vision
        • Motion Capture (VICON, Optitrack, NOKOV)
      • Installing driver for Intel RealSense R200
      • Switching State Estimators
      • Out-of-Tree Modules
      • STM32 Bootloader
      • System Tunes
      • Advanced Linux Installation Cases
      • Windows Cygwin Toolchain Maintenance
      • Unsupported Developer Setup
        • CentOS Linux
        • Arch Linux
        • Windows VM Toolchain
        • Windows Cygwin Toolchain
        • Qt Creator IDE
    • Platform Testing and CI
      • Test Flights
        • Test MC_01 - Manual Modes
        • Test MC_02 - Full Autonomous
        • Test MC_03 - Auto Manual Mix
        • Test MC_04 - Failsafe Testing
        • Test MC_05 - Indoor Flight (Manual Modes)
      • Unit Tests
      • Continuous Integration
      • MAVSDK Integration Testing
      • ROS Integration Testing
      • Docker Containers
      • Maintenance
  • Drone Apps & APIs
    • Offboard Control from Linux
    • ROS
      • ROS 2
        • ROS 2 User Guide
        • ROS 2 Offboard Control Example
        • ROS 2 Multi Vehicle Simulation
      • ROS 1 with MAVROS
        • ROS/MAVROS Installation Guide
        • ROS/MAVROS Offboard Example (C++)
        • ROS/MAVROS Offboard Example (Python)
        • ROS/MAVROS Sending Custom Messages
        • ROS/MAVROS with Gazebo Classic Simulation
        • Gazebo Classic OctoMap Models with ROS 1
        • ROS/MAVROS Installation on RPi
        • External Position Estimation (Vision/Motion based)
    • DroneKit
  • Contribution (&Dev Call)
    • Dev Call
    • Support
    • Source Code Management
      • GIT Examples
    • Documentation
    • Translation
    • Terminology/Notation
    • Licenses
  • Releases
    • 1.14
    • 1.13
    • 1.12
Powered by GitBook
On this page
  • Code
  • Code explanation
  1. Drone Apps & APIs
  2. ROS
  3. ROS 1 with MAVROS

ROS/MAVROS Offboard Example (C++)

PreviousROS/MAVROS Installation GuideNextROS/MAVROS Offboard Example (Python)

Last updated 1 year ago

This tutorial shows the basics of Offboard control with MAVROS, using an Iris quadcopter simulated in Gazebo Classic/SITL. At the end of the tutorial, you should see the same behaviour as in the video below, i.e. a slow takeoff to an altitude of 2 meters.

:::warning 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. :::

:::tip This example uses C++. A very similar example for Python can be found in (also see the examples in . :::

Code

Create the offb_node.cpp file in your ROS package (by also adding it to your CMakeList.txt so it is compiled), and paste the following inside it:

/**
 * @file offb_node.cpp
 * @brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro Flight
 * Stack and tested in Gazebo Classic SITL
 */

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>

mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "offb_node");
    ros::NodeHandle nh;

    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            ("mavros/state", 10, state_cb);
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
            ("mavros/setpoint_position/local", 10);
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
            ("mavros/cmd/arming");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
            ("mavros/set_mode");

    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(20.0);

    // wait for FCU connection
    while(ros::ok() && !current_state.connected){
        ros::spinOnce();
        rate.sleep();
    }

    geometry_msgs::PoseStamped pose;
    pose.pose.position.x = 0;
    pose.pose.position.y = 0;
    pose.pose.position.z = 2;

    //send a few setpoints before starting
    for(int i = 100; ros::ok() && i > 0; --i){
        local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }

    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";

    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

    ros::Time last_request = ros::Time::now();

    while(ros::ok()){
        if( current_state.mode != "OFFBOARD" &&
            (ros::Time::now() - last_request > ros::Duration(5.0))){
            if( set_mode_client.call(offb_set_mode) &&
                offb_set_mode.response.mode_sent){
                ROS_INFO("Offboard enabled");
            }
            last_request = ros::Time::now();
        } else {
            if( !current_state.armed &&
                (ros::Time::now() - last_request > ros::Duration(5.0))){
                if( arming_client.call(arm_cmd) &&
                    arm_cmd.response.success){
                    ROS_INFO("Vehicle armed");
                }
                last_request = ros::Time::now();
            }
        }

        local_pos_pub.publish(pose);

        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}

Code explanation

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

We create a simple callback which will save the current state of the autopilot. This will allow us to check connection, arming and Offboard flags.

ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("mavros/state", 10, state_cb);
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", 10);
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");

We instantiate a publisher to publish the commanded local position and the appropriate clients to request arming and mode change. Note that for your own system, the "mavros" prefix might be different as it will depend on the name given to the node in it's launch file.

//the setpoint publishing rate MUST be faster than 2Hz
ros::Rate rate(20.0);

PX4 has a timeout of 500ms between two Offboard commands. If this timeout is exceeded, the commander will fall back to the last mode the vehicle was in before entering Offboard mode. This is why the publishing rate must be faster than 2 Hz to also account for possible latencies. This is also the same reason why it is recommended to enter Offboard mode from Position mode, this way if the vehicle drops out of Offboard mode it will stop in its tracks and hover.

// wait for FCU connection
while(ros::ok() && !current_state.connected){
    ros::spinOnce();
    rate.sleep();
}

Before publishing anything, we wait for the connection to be established between MAVROS and the autopilot. This loop should exit as soon as a heartbeat message is received.

geometry_msgs::PoseStamped pose;
pose.pose.position.x = 0;
pose.pose.position.y = 0;
pose.pose.position.z = 2;

Even though the PX4 Pro Flight Stack operates in the aerospace NED coordinate frame, MAVROS translates these coordinates to the standard ENU frame and vice-versa. This is why we set z to positive 2.

//send a few setpoints before starting
for(int i = 100; ros::ok() && i > 0; --i){
  local_pos_pub.publish(pose);
  ros::spinOnce();
  rate.sleep();
}

Before entering Offboard mode, you must have already started streaming setpoints. Otherwise the mode switch will be rejected. Here, 100 was chosen as an arbitrary amount.

mavros_msgs::SetMode offb_set_mode;
offb_set_mode.request.custom_mode = "OFFBOARD";
mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value = true;

ros::Time last_request = ros::Time::now();

while(ros::ok()){
  if( current_state.mode != "OFFBOARD" &&
    (ros::Time::now() - last_request > ros::Duration(5.0))){
    if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent) {
      ROS_INFO("Offboard enabled");
    }
    last_request = ros::Time::now();
  } else {
    if( !current_state.armed && (ros::Time::now() - last_request > ros::Duration(5.0))) {
      if( arming_client.call(arm_cmd) && arm_cmd.response.success) {
        ROS_INFO("Vehicle armed");
      }
      last_request = ros::Time::now();
    }
  }

  local_pos_pub.publish(pose);

  ros::spinOnce();
  rate.sleep();
}

The rest of the code is pretty self explanatory. We attempt to switch to Offboard mode, after which we arm the quad to allow it to fly. We space out the service calls by 5 seconds so to not flood the autopilot with the requests. In the same loop, we continue sending the requested pose at the appropriate rate.

:::tip This code has been simplified to the bare minimum for illustration purposes. In larger systems, it is often useful to create a new thread which will be in charge of periodically publishing the setpoints. :::

The mavros_msgs package contains all of the custom messages required to operate services and topics provided by the MAVROS package. All services and topics as well as their corresponding message types are documented in the .

We set the custom mode to OFFBOARD. A list of is available for reference.

ROS/MAVROS Offboard Example (Python)
integrationtests/python_src/px4_it/mavros
mavros wiki
supported modes