Implementing Sensor Telemetry with ROS 2 rclcpp Publishers and Subscribers

The Problem: Reliable Sensor Data Flow

In robotics, sensor telemetry requires a consistent, asynchronous flow of data from a hardware driver to a processing node. A common failure point is the mismatch between how data is published and how it is consumed, leading to dropped packets or blocked execution threads. The goal is to implement a C++ node using rclcpp that handles periodic data transmission without stalling the rest of the system.

Prerequisites

• A Linux environment (Ubuntu 22.04 recommended) with ROS 2 Humble or Foxy installed.
• A sourced ROS 2 installation (e.g., source /opt/ros/humble/setup.bash).
• A compiled C++ workspace with colcon.
• The std_msgs package installed for basic data types.

Designing the Telemetry Node

To ensure maintainability, encapsulate the publisher and subscriber within a class that inherits from rclcpp::Node. This allows you to manage the node's lifecycle and internal state (like sensor readings) within a single object.

Implementation Procedure

Create a source file (e.g., telemetry_node.cpp) with the following structure. This example implements a node that publishes a simulated float value and subscribes to a feedback topic.

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float64.hpp"

class TelemetryNode : public rclcpp::Node {
public:
  TelemetryNode() : Node("telemetry_node") {
    // Publisher: Topic name, Queue size (10)
    publisher_ = this->create_publisher<std_msgs::msg::Float64>("sensor_data", 10);

    // Subscriber: Topic name, Queue size, Callback function
    subscriber_ = this->create_subscription<std_msgs::msg::Float64>(
      "sensor_feedback", 10, 
      std::bind(&TelemetryNode::feedback_callback, this, std::placeholders::_1));

    // Timer: Execute publish_data every 500ms
    timer_ = this->create_wall_timer(
      std::chrono::milliseconds(500), 
      std::bind(&TelemetryNode::publish_data, this));
  }

private:
  void publish_data() {
    auto message = std_msgs::msg::Float64();
    message.data = 42.0; // Replace with actual sensor read
    RCLCPP_INFO(this->get_logger(), "Publishing: %f", message.data);
    publisher_->publish(message);
  }

  void feedback_callback(const std_msgs::msg::Float64::SharedPtr msg) {
    RCLCPP_INFO(this->get_logger(), "Received feedback: %f", msg->data);
  }

  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr publisher_;
  rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr subscriber_;
  rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char ** argv) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<TelemetryNode>());
  rclcpp::shutdown();
  return 0;
}

Configuration Requirements

To compile this node, you must update your package.xml and CMakeLists.txt to include the necessary dependencies. Failure to do so will result in linker errors regarding std_msgs.

In package.xml:

• <depend>rclcpp</depend>
• <depend>std_msgs</depend>

In CMakeLists.txt:

find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)

add_executable(telemetry_node src/telemetry_node.cpp)
ament_target_dependencies(telemetry_node rclcpp std_msgs)

install(TARGETS telemetry_node
  DESTINATION lib/${PROJECT_NAME})

Critical Engineering Decisions

Quality of Service (QoS) Matching

By default, the example above uses a queue size of 10, which implies a "Reliable" reliability setting. If your publisher is set to "Best Effort" (common for high-frequency LiDAR or Camera data) but your subscriber is "Reliable", no data will be received. Ensure both ends match their QoS profiles.

Avoiding Executor Starvation

The rclcpp::spin() function uses a single-threaded executor by default. If feedback_callback performs a heavy computation (e.g., a complex matrix inversion), the publish_data timer will be delayed. For heavy workloads, implement a MultiThreadedExecutor to distribute callbacks across CPU cores.

Verification and Diagnostics

Run the node from your terminal. Use the following commands in separate terminals to verify the communication pipeline:

Goal	Command	Expected Result
Node Discovery	ros2 node list	/telemetry_node appears in list.
Data Flow	ros2 topic echo /sensor_data	Stream of float values appearing every 0.5s.
Frequency Check	ros2 topic hz /sensor_data	Average rate of ~2.0 Hz.
Topic Metadata	ros2 topic info /sensor_data	Type: std_msgs/msg/Float64, 1 Publisher.

Rollback and Cleanup

Since this operation primarily involves code changes and compilation, rollback consists of reverting the CMakeLists.txt and package.xml files to their previous state and deleting the build artifacts:

rm -rf build/ install/ log/