[PX4 ROS 2 Programming] Part 3: Writing Basic ROS 2 Nodes (Listener & Advertiser)

Hello again, future developers and researchers fascinated by robotics and autonomous flight systems! Welcome back.

In Part 2, we deeply explored three core concepts you must understand before writing any code: QoS (Quality of Service), Coordinate Frames, and Time Synchronization. Now that you know how to avoid the “walls of despair” like communication latency or your drone flying in completely wrong directions, you are perfectly prepared to put your hands on the keyboard and write some working C++ code.

In this Part 3, we will build the two most fundamental features of ROS 2 programming: a Listener (Subscriber) node that receives data, and an Advertiser (Publisher) node that sends data. Furthermore, we will also cover ROS 2 CLI (Command Line Interface) debugging tips so you can quickly and intuitively verify your communication status right from the terminal.

Let’s get started!

1. Receiving Data: Writing a Listener Node

The first step in flight control is accurately grasping the current state of the drone. To do this, we will create a Listener node that subscribes to the SensorCombined topic published from the PX4 flight controller via the uXRCE-DDS middleware. The SensorCombined message contains Accelerometer and Gyroscope data, which are absolutely essential for understanding the drone’s current attitude and movements.

The file we are going to write is named sensor_combined_listener.cpp. Let’s look at the complete code first, and then break down the important parts line by line.

#include <rclcpp/rclcpp.hpp>
#include <px4_msgs/msg/sensor_combined.hpp>
#include <iostream>

/**
 * @brief Callback class that receives the Sensor Combined uORB topic data
 */
class SensorCombinedListener : public rclcpp::Node
{
public:
    explicit SensorCombinedListener() : Node("sensor_combined_listener")
    {
        // 1. Set QoS Profile (Crucial Step!)
        rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data;
        auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 5), qos_profile);

        // 2. Create Subscriber and define Callback function
        subscription_ = this->create_subscription<px4_msgs::msg::SensorCombined>(
            "/fmu/out/sensor_combined", 
            qos,
            [this](const px4_msgs::msg::SensorCombined::UniquePtr msg) {
                // Print to terminal upon receiving data
                std::cout << "\nRECEIVED SENSOR COMBINED DATA" << std::endl;
                std::cout << "=============================" << std::endl;
                std::cout << "ts (Timestamp): " << msg->timestamp << std::endl;
                std::cout << "Gyro X (gyro_rad): " << msg->gyro_rad << std::endl;
                std::cout << "Accel Z (accelerometer_m_s2): " << msg->accelerometer_m_s2[3] << std::endl;
            });
    }

private:
    rclcpp::Subscription<px4_msgs::msg::SensorCombined>::SharedPtr subscription_;
};

int main(int argc, char *argv[])
{
    std::cout << "Starting sensor_combined listener node..." << std::endl;
    // Initialize ROS 2 node
    rclcpp::init(argc, argv);
    // Spin the node (wait for callbacks)
    rclcpp::spin(std::make_shared<SensorCombinedListener>());
    // Shutdown the node
    rclcpp::shutdown();
    return 0;
}

#include <rclcpp/rclcpp.hpp>
#include <px4_msgs/msg/sensor_combined.hpp>
#include <iostream>

/**
 * @brief Callback class that receives the Sensor Combined uORB topic data
 */
class SensorCombinedListener : public rclcpp::Node
{
public:
    explicit SensorCombinedListener() : Node("sensor_combined_listener")
    {
        // 1. Set QoS Profile (Crucial Step!)
        rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data;
        auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 5), qos_profile);

        // 2. Create Subscriber and define Callback function
        subscription_ = this->create_subscription<px4_msgs::msg::SensorCombined>(
            "/fmu/out/sensor_combined", 
            qos,
            [this](const px4_msgs::msg::SensorCombined::UniquePtr msg) {
                // Print to terminal upon receiving data
                std::cout << "\nRECEIVED SENSOR COMBINED DATA" << std::endl;
                std::cout << "=============================" << std::endl;
                std::cout << "ts (Timestamp): " << msg->timestamp << std::endl;
                std::cout << "Gyro X (gyro_rad): " << msg->gyro_rad << std::endl;
                std::cout << "Accel Z (accelerometer_m_s2): " << msg->accelerometer_m_s2[3] << std::endl;
            });
    }

private:
    rclcpp::Subscription<px4_msgs::msg::SensorCombined>::SharedPtr subscription_;
};

int main(int argc, char *argv[])
{
    std::cout << "Starting sensor_combined listener node..." << std::endl;
    // Initialize ROS 2 node
    rclcpp::init(argc, argv);
    // Spin the node (wait for callbacks)
    rclcpp::spin(std::make_shared<SensorCombinedListener>());
    // Shutdown the node
    rclcpp::shutdown();
    return 0;
}

Detailed Code Breakdown

• Including Headers (#include): rclcpp/rclcpp.hpp is the core header for the ROS 2 C++ client library. Also, to use PX4’s message structures, you must include px4_msgs/msg/sensor_combined.hpp.
• QoS Settings (rmw_qos_profile_sensor_data): This is the part we emphasized so much in Part 2! Sensor data flying in from PX4 is incompatible with ROS 2’s default QoS (Reliable). Therefore, we set it to qos_profile_sensor_data (Best Effort-based) to receive the latest data without delay. The history depth is set to 5, meaning we only keep the latest 5 messages in the queue.
• Creating the Subscriber (create_subscription): The topic name is /fmu/out/sensor_combined. The “out” implies data coming out of PX4 into ROS 2. For the third argument, we used a C++ Lambda function to instantly write the callback logic that executes whenever data arrives.
• The main Function: We initialize the system with rclcpp::init, and call rclcpp::spin so the program doesn’t terminate but continues to wait for data and execute the callback function.

2. Sending Data: Writing an Advertiser Node

Now, let’s reverse the roles and write an Advertiser (Publisher) node that sends data from ROS 2 into PX4 over the network. To safely test communication before sending actual control commands, we will use a topic called DebugVect (Debug Vector). This topic is perfect for debugging as it can carry a name (string) and three float values (X, Y, Z).

The file we will write is named debug_vect_advertiser.cpp.

#include <chrono>
#include <rclcpp/rclcpp.hpp>
#include <px4_msgs/msg/debug_vect.hpp>

using namespace std::chrono_literals;

class DebugVectAdvertiser : public rclcpp::Node
{
public:
    DebugVectAdvertiser() : Node("debug_vect_advertiser") 
    {
        // 1. Create Publisher (Queue size 10)
        publisher_ = this->create_publisher<px4_msgs::msg::DebugVect>("/fmu/in/debug_vect", 10);

        // 2. Setup Timer Callback (500ms period)
        auto timer_callback = [this]() -> void {
            auto debug_vect = px4_msgs::msg::DebugVect();
            
            // Convert current time to microseconds and save it in the timestamp
            debug_vect.timestamp = std::chrono::time_point_cast<std::chrono::microseconds>(
                std::chrono::steady_clock::now()).time_since_epoch().count();
            
            // Set debug variable name (max 10 characters)
            std::string name = "test_vec";
            std::copy(name.begin(), name.end(), debug_vect.name.begin());
            
            // Assign test X, Y, Z values
            debug_vect.x = 1.0;
            debug_vect.y = 2.0;
            debug_vect.z = 3.0;

            RCLCPP_INFO(this->get_logger(), "Publishing debug_vect: x: %f  y: %f  z: %f",
                        debug_vect.x, debug_vect.y, debug_vect.z);
            
            // 3. Publish the message
            this->publisher_->publish(debug_vect);
        };

        // Execute timer_callback every 500ms (0.5 seconds)
        timer_ = this->create_wall_timer(500ms, timer_callback);
    }

private:
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher<px4_msgs::msg::DebugVect>::SharedPtr publisher_;
};

int main(int argc, char *argv[])
{
    std::cout << "Starting debug_vect advertiser node..." << std::endl;
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<DebugVectAdvertiser>());
    rclcpp::shutdown();
    return 0;
}

#include <chrono>
#include <rclcpp/rclcpp.hpp>
#include <px4_msgs/msg/debug_vect.hpp>

using namespace std::chrono_literals;

class DebugVectAdvertiser : public rclcpp::Node
{
public:
    DebugVectAdvertiser() : Node("debug_vect_advertiser") 
    {
        // 1. Create Publisher (Queue size 10)
        publisher_ = this->create_publisher<px4_msgs::msg::DebugVect>("/fmu/in/debug_vect", 10);

        // 2. Setup Timer Callback (500ms period)
        auto timer_callback = [this]() -> void {
            auto debug_vect = px4_msgs::msg::DebugVect();
            
            // Convert current time to microseconds and save it in the timestamp
            debug_vect.timestamp = std::chrono::time_point_cast<std::chrono::microseconds>(
                std::chrono::steady_clock::now()).time_since_epoch().count();
            
            // Set debug variable name (max 10 characters)
            std::string name = "test_vec";
            std::copy(name.begin(), name.end(), debug_vect.name.begin());
            
            // Assign test X, Y, Z values
            debug_vect.x = 1.0;
            debug_vect.y = 2.0;
            debug_vect.z = 3.0;

            RCLCPP_INFO(this->get_logger(), "Publishing debug_vect: x: %f  y: %f  z: %f",
                        debug_vect.x, debug_vect.y, debug_vect.z);
            
            // 3. Publish the message
            this->publisher_->publish(debug_vect);
        };

        // Execute timer_callback every 500ms (0.5 seconds)
        timer_ = this->create_wall_timer(500ms, timer_callback);
    }

private:
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher<px4_msgs::msg::DebugVect>::SharedPtr publisher_;
};

int main(int argc, char *argv[])
{
    std::cout << "Starting debug_vect advertiser node..." << std::endl;
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<DebugVectAdvertiser>());
    rclcpp::shutdown();
    return 0;
}

Detailed Code Breakdown

• Creating the Publisher (create_publisher): The topic name is /fmu/in/debug_vect (or fmu/debug_vect/in depending on your specific namespace setup). It has “in” because it goes into PX4 from ROS 2. Unlike sensor data, publishers generally work fine with ROS 2’s default QoS when communicating with PX4, so we simply specified a queue size of 10 without special QoS settings.
• Creating a Timer (create_wall_timer): Instead of sending the data just once, we created a timer with a 500ms (0.5 seconds) period to transmit it continuously. This means the data is sent twice every second.
• Assembling and Publishing the Message: We use std::chrono to extract the current system time in microseconds and put it into the timestamp field. Then, we insert an arbitrary name (test_vec) and coordinate values (x=1.0, y=2.0, z=3.0), and call the publish() function to shoot it across the network.

3. Peering into the Network: ROS 2 CLI Tips

Once you’ve written the code and finished building (build command: colcon build), it’s time to run the nodes. But before you do, let me introduce you to a powerful weapon that allows you to check if the communication between PX4 and ROS 2 is working properly using only terminal commands, without needing C++ code: The ROS 2 CLI (Command Line Interface) tools.

These commands act as lifelines when you’re hunting down bugs or intuitively verifying data flows during development.

1) ros2 topic list (Checking the List of Topics)

This is the very first command you use to see what data is flowing across the network.

ros2 topic list

ros2 topic list

When you type this, a list of all communicating topic names, such as /fmu/in/obstacle_distance or /fmu/out/sensor_combined, will pour out. If the /fmu/in/debug_vect you just published isn’t on this list, you should check if your Micro XRCE-DDS Agent is properly running.

2) ros2 topic echo <topic_name> (Viewing Data in Real-Time)

Use this when you want to see with your own eyes what numbers are actually inside a specific topic.

ros2 topic echo /fmu/out/vehicle_status

ros2 topic echo /fmu/out/vehicle_status

By entering this command, you will see real-time data pouring into the terminal, showing the drone’s current arming state (arming_state), navigation mode (nav_state), and more. It is highly useful for initially verifying whether sensor values are spiking or if abnormal data is coming in.

3) ros2 topic hz <topic_name> (Checking Publish Frequency)

This command calculates the frequency (Hz) at which data is arriving per second. In drone control, “how regularly and quickly data arrives” is extremely critical.

ros2 topic hz /fmu/out/sensor_combined

ros2 topic hz /fmu/out/sensor_combined

Typing the command above will show you something like “average rate: 248.187”, confirming that you are receiving sensor data at an average of roughly 250 times per second. If the frequency of essential data drops significantly while performing Offboard Control, you should suspect network latency or CPU overload.

Wrapping Up

Great job! In this Part 3, we successfully implemented a Listener and an Advertiser node that interact with PX4 using C++, and we even learned how to feel the system’s pulse using CLI tools.

You have now paved a solid highway to freely exchange data between the ROS 2 system and your PX4 drone. In the upcoming Part 4: Offboard Control (Drone Position and Trajectory Control), we will ride this highway to send actual control commands—arming the motors and flying the drone to specific coordinates in the sky in an exciting real-world programming exercise.

If you have any questions, feel free to leave a comment, and I will see you in Part 4 with even more useful content. Thank you!

YouTube Class

Author: maponarooo, CEO of QUAD Drone Lab

Date: February 27, 2026