diff --git a/include/message_filters/time_sequencer.hpp b/include/message_filters/time_sequencer.hpp index 2690d74..f5a61d9 100644 --- a/include/message_filters/time_sequencer.hpp +++ b/include/message_filters/time_sequencer.hpp @@ -96,6 +96,7 @@ class TimeSequencer : public SimpleFilter , update_rate_(update_rate) , queue_size_(queue_size) , node_(node) + , last_time_ (0, 0, RCL_ROS_TIME) { init(); connectInput(f); @@ -118,6 +119,7 @@ class TimeSequencer : public SimpleFilter , update_rate_(update_rate) , queue_size_(queue_size) , node_(node) + , last_time_ (0, 0, RCL_ROS_TIME) { init(); } @@ -199,7 +201,7 @@ class TimeSequencer : public SimpleFilter while (!messages_.empty()) { const EventType & e = *messages_.begin(); rclcpp::Time stamp = mt::TimeStamp::value(*e.getMessage()); - if ((stamp + delay_) <= rclcpp::Clock().now()) { + if ((stamp + delay_) <= node_->get_clock()->now()) { last_time_ = stamp; to_call.push_back(e); messages_.erase(messages_.begin()); @@ -220,7 +222,9 @@ class TimeSequencer : public SimpleFilter void init() { - update_timer_ = node_->create_wall_timer( + update_timer_ = rclcpp::create_timer( + node_, + node_->get_clock(), std::chrono::nanoseconds(update_rate_.nanoseconds()), [this]() { dispatch(); }); diff --git a/test/test_fuzz.cpp b/test/test_fuzz.cpp index 0090b35..99c2594 100644 --- a/test/test_fuzz.cpp +++ b/test/test_fuzz.cpp @@ -85,7 +85,7 @@ TEST(TimeSequencer, fuzz_sequencer) 10, node); Helper h; seq.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); - rclcpp::Clock ros_clock; + rclcpp::Clock ros_clock(RCL_ROS_TIME); auto start = ros_clock.now(); auto msg = std::make_shared(); while ((ros_clock.now() - start) < rclcpp::Duration(5, 0)) { diff --git a/test/time_sequencer_unittest.cpp b/test/time_sequencer_unittest.cpp index 5108627..cb17b12 100644 --- a/test/time_sequencer_unittest.cpp +++ b/test/time_sequencer_unittest.cpp @@ -55,7 +55,7 @@ struct TimeStamp { static rclcpp::Time value(const Msg & m) { - return m.header.stamp; + return rclcpp::Time(m.header.stamp, RCL_ROS_TIME); } }; } // namespace message_traits @@ -86,7 +86,7 @@ TEST(TimeSequencer, simple) Helper h; seq.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); MsgPtr msg(std::make_shared()); - msg->header.stamp = rclcpp::Clock().now(); + msg->header.stamp = rclcpp::Clock(RCL_ROS_TIME).now(); seq.add(msg); rclcpp::Rate(10).sleep(); @@ -137,7 +137,7 @@ TEST(TimeSequencer, eventInEventOut) seq2.registerCallback(&EventHelper::cb, &h); message_filters::MessageEvent evt(std::make_shared(), - rclcpp::Clock().now()); + rclcpp::Clock(RCL_ROS_TIME).now()); seq.add(evt); while (!h.event_.getMessage()) {