ROS2 rclcpp subscriber callback uses same shared_ptr for each new message

Question

I have an rclcpp node with some message data coming in (images, scans, etc). I would like to apply temporal filtering to this data, and therefore store a copy of each incoming message.

I assumed simply copying the incoming shared_ptr would be an appropriate way to avoid copying the dense data, but it seems the shared_ptr is being re-used and modified with every new message that comes in.

Is this expected behavior? I wrote a simple ping and pong nodes that demonstrates the issue I'm having.

Ping

#include <deque>
#include <rclcpp/rclcpp.hpp>
#include "std_msgs/msg/int64.hpp"
#include "rclcpp_components/register_node_macro.hpp"

using namespace std::chrono_literals;

struct DataEntry {
  std_msgs::msg::Int64::SharedPtr pointer;
  int64_t verification;
};

class TestPing : public rclcpp::Node
{

  public:
    explicit TestPing(const rclcpp::NodeOptions &options = rclcpp::NodeOptions()) : Node("test_ping", options)
    {
    using namespace std::placeholders;

    this->ping_pub_ = this->create_publisher<std_msgs::msg::Int64>("test_ping", 10);

    this->pong_sub_ = this->create_subscription<std_msgs::msg::Int64>("test_pong", 10,
    [this](std_msgs::msg::Int64::SharedPtr msg) {

      DataEntry entry;
      entry.verification = msg->data;
      std::atomic_store(&entry.pointer, msg);

      while(this->data_queue_.size() >= this->capacity_){
        this->data_queue_.pop_front();
      }

      RCLCPP_INFO(this->get_logger(), "test_pong: Receive response with address: %p and value %li",
        static_cast<void*>(entry.pointer.get()),
        entry.verification);

      this->data_queue_.push_back(entry);
      this->verify();

    });

    this->heart_beat_timer_ = this->create_wall_timer(this->ping_interval_,
    [this]() {
      std_msgs::msg::Int64 ping;
      ping.data = this->count_;
      this->count_++;
      RCLCPP_INFO(this->get_logger(), "Publish count %li", ping.data);
      this->ping_pub_->publish(ping);
    });
    }

  private:
    int16_t count_ = 0;
    int16_t capacity_ = 4;

    // Use Float64 for example only.
    // Reality could be dense data, like laserscans or images.
    std::deque<DataEntry> data_queue_;

    std::chrono::seconds ping_interval_ = 1s;  
    rclcpp::TimerBase::SharedPtr heart_beat_timer_;

    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Int64>> ping_pub_;
    std::shared_ptr<rclcpp::Subscription<std_msgs::msg::Int64>> pong_sub_;

    void verify() {
      for (size_t i = 0; i < data_queue_.size(); i++) {
        if(data_queue_[i].pointer->data != data_queue_[i].verification){
          RCLCPP_ERROR(this->get_logger(),
          "Unexpected change in stored data at %li. pointer: %li verification: %li", 
          i,
          data_queue_[i].pointer->data,
          data_queue_[i].verification);
        }
      }
      std::unordered_set<void*> used_memory_locations;
      for (size_t i = 0; i < data_queue_.size(); i++) {
        if(used_memory_locations.count(static_cast<void*>(data_queue_[i].pointer.get()))){
          RCLCPP_ERROR(this->get_logger(),
          "Memory address re-used: %li %p", 
          i,
          static_cast<void*>(data_queue_[i].pointer.get()));
        } else {
          used_memory_locations.insert(static_cast<void*>(data_queue_[i].pointer.get()));
        }
      }

    }

};

RCLCPP_COMPONENTS_REGISTER_NODE(TestPing)

Pong

#include <rclcpp/rclcpp.hpp>
#include "std_msgs/msg/int64.hpp"

#include "rclcpp_components/register_node_macro.hpp"

using namespace std::chrono_literals;

class TestPong : public rclcpp::Node
{

  public:
    explicit TestPong(const rclcpp::NodeOptions &options = rclcpp::NodeOptions()) : Node("test_pong", options)
    {
    using namespace std::placeholders;

    this->pong_pub_ = this->create_publisher<std_msgs::msg::Int64>("test_pong", 10);
    this->ping_sub_ = this->create_subscription<std_msgs::msg::Int64>("test_ping",10,
    [this](std_msgs::msg::Int64::ConstPtr msg){
      auto pong = std::make_unique<std_msgs::msg::Int64>();
      pong->data = msg->data;
      RCLCPP_WARN(this->get_logger(), "Send response with data %li", msg->data);
      this->pong_pub_->publish(std::move(pong));
    });
    }

  private:
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Int64>> pong_pub_;
    std::shared_ptr<rclcpp::Subscription<std_msgs::msg::Int64>> ping_sub_;

};

RCLCPP_COMPONENTS_REGISTER_NODE(TestPong)

Output of Ping

$ ros2 run ping_pong test_ping_composable_node 
[INFO] [1715272119.381580161] [test_ping]: Publish count 0
[INFO] [1715272119.383094435] [test_ping]: test_pong: Receive response with address: 0x56215a761100 and value 0
[INFO] [1715272120.381543339] [test_ping]: Publish count 1
[INFO] [1715272120.382455616] [test_ping]: test_pong: Receive response with address: 0x56215a761100 and value 1
[ERROR] [1715272120.382527406] [test_ping]: Unexpected change in stored data at 0. pointer: 1 verification: 0
[ERROR] [1715272120.382660842] [test_ping]: Memory address re-used: 1 0x56215a761100
[INFO] [1715272121.381445332] [test_ping]: Publish count 2
[INFO] [1715272121.382255522] [test_ping]: test_pong: Receive response with address: 0x56215a761100 and value 2
[ERROR] [1715272121.382324987] [test_ping]: Unexpected change in stored data at 0. pointer: 2 verification: 0
[ERROR] [1715272121.382364272] [test_ping]: Unexpected change in stored data at 1. pointer: 2 verification: 1
[ERROR] [1715272121.382405496] [test_ping]: Memory address re-used: 1 0x56215a761100
[ERROR] [1715272121.382422092] [test_ping]: Memory address re-used: 2 0x56215a761100
[INFO] [1715272122.381529065] [test_ping]: Publish count 3
[INFO] [1715272122.382330536] [test_ping]: test_pong: Receive response with address: 0x56215a761100 and value 3
[ERROR] [1715272122.382373963] [test_ping]: Unexpected change in stored data at 0. pointer: 3 verification: 0
[ERROR] [1715272122.382412279] [test_ping]: Unexpected change in stored data at 1. pointer: 3 verification: 1
[ERROR] [1715272122.382443272] [test_ping]: Unexpected change in stored data at 2. pointer: 3 verification: 2
[ERROR] [1715272122.382481207] [test_ping]: Memory address re-used: 1 0x56215a761100
[ERROR] [1715272122.382498813] [test_ping]: Memory address re-used: 2 0x56215a761100
[ERROR] [1715272122.382513971] [test_ping]: Memory address re-used: 3 0x56215a761100
[INFO] [1715272123.381507773] [test_ping]: Publish count 4
[INFO] [1715272123.382427804] [test_ping]: test_pong: Receive response with address: 0x56215a761100 and value 4
[ERROR] [1715272123.382512603] [test_ping]: Unexpected change in stored data at 0. pointer: 4 verification: 1
[ERROR] [1715272123.382568645] [test_ping]: Unexpected change in stored data at 1. pointer: 4 verification: 2
[ERROR] [1715272123.382593076] [test_ping]: Unexpected change in stored data at 2. pointer: 4 verification: 3
[ERROR] [1715272123.382637780] [test_ping]: Memory address re-used: 1 0x56215a761100
[ERROR] [1715272123.382656192] [test_ping]: Memory address re-used: 2 0x56215a761100
[ERROR] [1715272123.382676299] [test_ping]: Memory address re-used: 3 0x56215a761100

One way around this that I have found is to use the unique_ptr variation of the subscriber callback. This version of the callback uses a different memory address for each new message coming in.

Example change to Ping node

    this->pong_sub_ = this->create_subscription<std_msgs::msg::Int64>("test_pong", 10,
    [this](std_msgs::msg::Int64::UniquePtr msg) {

      DataEntry entry;
      entry.verification = msg->data;
      entry.pointer = std::move(msg);

      while(this->data_queue_.size() >= this->capacity_){
        this->data_queue_.pop_front();
      }

      RCLCPP_INFO(this->get_logger(), "test_pong: Receive response with address: %p and value %li",
        static_cast<void*>(entry.pointer.get()),
        entry.verification);

      this->data_queue_.push_back(std::move(entry));
      this->verify();

    });

And the result:

$ ros2 run ping_pong test_ping_composable_node 
[INFO] [1715278005.750424671] [test_ping]: Publish count 0
[INFO] [1715278005.752367086] [test_ping]: test_pong: Receive response with address: 0x560093e73f00 and value 0
[INFO] [1715278006.750481230] [test_ping]: Publish count 1
[INFO] [1715278006.751553313] [test_ping]: test_pong: Receive response with address: 0x560093e73ee0 and value 1
[INFO] [1715278007.750443476] [test_ping]: Publish count 2
[INFO] [1715278007.751183124] [test_ping]: test_pong: Receive response with address: 0x560093c678d0 and value 2
[INFO] [1715278008.750360397] [test_ping]: Publish count 3
[INFO] [1715278008.751179122] [test_ping]: test_pong: Receive response with address: 0x560093bbe960 and value 3
[INFO] [1715278009.750368113] [test_ping]: Publish count 4

Answer 1

Following advice from Steve Macenski, updating to a more recent version of ROS fixes this problem.

In reviewing the changelog, the relevant ticket and fix is here:

https://github.com/ros2/rclcpp/pull/2336