Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Message Queue not cleared properly? #108

Open
GeoGkovo opened this issue Jan 15, 2024 · 0 comments
Open

Message Queue not cleared properly? #108

GeoGkovo opened this issue Jan 15, 2024 · 0 comments
Assignees

Comments

@GeoGkovo
Copy link

GeoGkovo commented Jan 15, 2024

Screenshot from 2024-01-15 13-11-13
I have the following piece of code (essentially a minimal node that synchronizes two topics, logs when the callback is triggered and publishes the result on a topic). I have noticed that after a certain amount of messages published to the input topics the callback stops triggering for some reason, while the node remains alive. The amount of callbacks triggered is directly related with the queue size of the SyncPolicy as well as the rate at which the input messages are being published. Am I missing something here or is this a bug?
I am using ros2 humble on Ubuntu 22.04. On the screenshot above you can see the time difference between the last message and the manual stopping of the node.

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/bool.hpp>
#include <std_msgs/msg/string.hpp>
// message filter related headers
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>

#include <memory>

class SyncNode : public rclcpp::Node {
 public:
  using SyncPolicy =
      message_filters::sync_policies::ApproximateTime<std_msgs::msg::Bool,
                                                      std_msgs::msg::String>;
  using Synchronizer = message_filters::Synchronizer<SyncPolicy>;

  SyncNode() : Node("sync_publisher") {
    // Setup publisher

    publisher_ =
        this->create_publisher<std_msgs::msg::String>("resulting_string", 5);

    bool_sub_.subscribe(this, "/bool_topic");
    string_sub_.subscribe(this, "/string_topic");

    sync_ =
        std::make_shared<Synchronizer>(SyncPolicy(200), bool_sub_, string_sub_);

    sync_->registerCallback(std::bind(&SyncNode::sync_cb, this,
                                      std::placeholders::_1,
                                      std::placeholders::_2));
    sync_->setMaxIntervalDuration(rclcpp::Duration(0, 5e+8));
  }

  void sync_cb(const std_msgs::msg::Bool::ConstSharedPtr& bool_msg,
               const std_msgs::msg::String::ConstSharedPtr& string_msg) {
    RCLCPP_INFO(this->get_logger(),
                "Received synchronized string and bool data");

    // Create a message and populate its data
    auto message = std::make_unique<std_msgs::msg::String>();
    message->data = string_msg->data;
    if (bool_msg->data) {
      message->data += " True";
    } 
    else {
      message->data += " False";
    }
    // Publish the message
    publisher_->publish(std::move(message));
  }

 private:
  message_filters::Subscriber<std_msgs::msg::Bool> bool_sub_;
  message_filters::Subscriber<std_msgs::msg::String> string_sub_;

  std::shared_ptr<Synchronizer> sync_;

  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
};

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<SyncNode>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

2 participants