diff --git a/test/msg_cache_unittest.cpp b/test/msg_cache_unittest.cpp index f7ab5c5..351470d 100644 --- a/test/msg_cache_unittest.cpp +++ b/test/msg_cache_unittest.cpp @@ -34,15 +34,14 @@ #include -#include -#include #include +#include +#include + +#include #include "message_filters/cache.h" #include "message_filters/message_traits.h" -using namespace std ; -using namespace message_filters ; - struct Header { rclcpp::Time stamp ; @@ -70,27 +69,24 @@ struct TimeStamp } } - -void fillCacheEasy(Cache& cache, unsigned int start, unsigned int end) - +void fillCacheEasy(message_filters::Cache& cache, unsigned int start, unsigned int end) { - for (unsigned int i=start; i < end; i++) - { - Msg* msg = new Msg ; - msg->data = i ; - msg->header.stamp= rclcpp::Time(i*10, 0) ; + for (unsigned int i = start; i < end; i++) { + Msg * msg = new Msg; + msg->data = i; + msg->header.stamp = rclcpp::Time(i * 10, 0); - std::shared_ptr msg_ptr(msg) ; - cache.add(msg_ptr) ; + std::shared_ptr msg_ptr(msg); + cache.add(msg_ptr); } } TEST(Cache, easyInterval) { - Cache cache(10) ; - fillCacheEasy(cache, 0, 5) ; + message_filters::Cache cache(10); + fillCacheEasy(cache, 0, 5); - vector > interval_data = cache.getInterval(rclcpp::Time(5, 0), rclcpp::Time(35, 0)) ; + std::vector > interval_data = cache.getInterval(rclcpp::Time(5, 0), rclcpp::Time(35, 0)); ASSERT_EQ(interval_data.size(), (unsigned int) 3) ; EXPECT_EQ(interval_data[0]->data, 1) ; @@ -109,10 +105,10 @@ TEST(Cache, easyInterval) TEST(Cache, easySurroundingInterval) { - Cache cache(10); + message_filters::Cache cache(10); fillCacheEasy(cache, 1, 6); - vector > interval_data; + std::vector > interval_data; interval_data = cache.getSurroundingInterval(rclcpp::Time(15,0), rclcpp::Time(35,0)) ; ASSERT_EQ(interval_data.size(), (unsigned int) 4); EXPECT_EQ(interval_data[0]->data, 1); @@ -147,7 +143,7 @@ std::shared_ptr buildMsg(int32_t seconds, int data) TEST(Cache, easyUnsorted) { - Cache cache(10) ; + message_filters::Cache cache(10); cache.add(buildMsg(10, 1)) ; cache.add(buildMsg(30, 3)) ; @@ -155,7 +151,7 @@ TEST(Cache, easyUnsorted) cache.add(buildMsg( 5, 0)) ; cache.add(buildMsg(20, 2)) ; - vector > interval_data = cache.getInterval(rclcpp::Time(3, 0), rclcpp::Time(15, 0)) ; + std::vector > interval_data = cache.getInterval(rclcpp::Time(3, 0), rclcpp::Time(15, 0)); ASSERT_EQ(interval_data.size(), (unsigned int) 2) ; EXPECT_EQ(interval_data[0]->data, 0) ; @@ -174,8 +170,8 @@ TEST(Cache, easyUnsorted) TEST(Cache, easyElemBeforeAfter) { - Cache cache(10) ; - std::shared_ptr elem_ptr ; + message_filters::Cache cache(10); + std::shared_ptr elem_ptr; fillCacheEasy(cache, 5, 10) ; @@ -195,29 +191,30 @@ TEST(Cache, easyElemBeforeAfter) struct EventHelper { public: - void cb(const MessageEvent& evt) + void cb(const message_filters::MessageEvent & evt) { event_ = evt; } - MessageEvent event_; + message_filters::MessageEvent event_; }; TEST(Cache, eventInEventOut) { - Cache c0(10); - Cache c1(c0, 10); + message_filters::Cache c0(10); + message_filters::Cache c1(c0, 10); EventHelper h; c1.registerCallback(&EventHelper::cb, &h); - MessageEvent evt(std::make_shared(), rclcpp::Time(4, 0)); + message_filters::MessageEvent evt(std::make_shared(), rclcpp::Time(4, 0)); c0.add(evt); EXPECT_EQ(h.event_.getReceiptTime(), evt.getReceiptTime()); EXPECT_EQ(h.event_.getMessage(), evt.getMessage()); } -int main(int argc, char **argv){ +int main(int argc, char **argv) +{ testing::InitGoogleTest(&argc, argv); rclcpp::init(argc, argv); return RUN_ALL_TESTS(); diff --git a/test/test_approximate_epsilon_time_policy.cpp b/test/test_approximate_epsilon_time_policy.cpp index 03ad851..1a6f6ef 100644 --- a/test/test_approximate_epsilon_time_policy.cpp +++ b/test/test_approximate_epsilon_time_policy.cpp @@ -33,6 +33,9 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ +#include +#include +#include #include #include @@ -42,16 +45,11 @@ #include "message_filters/sync_policies/approximate_epsilon_time.h" #include "message_filters/message_traits.h" -using namespace std::placeholders; -using namespace message_filters; -using namespace message_filters::sync_policies; - struct Header { rclcpp::Time stamp; }; - struct Msg { Header header; @@ -98,9 +96,9 @@ class ApproximateEpsilonTimeSynchronizerTest const std::vector &output, uint32_t queue_size, rclcpp::Duration epsilon) : input_(input), output_(output), output_position_(0), sync_( - ApproximateEpsilonTime{queue_size, epsilon}) + message_filters::sync_policies::ApproximateEpsilonTime{queue_size, epsilon}) { - sync_.registerCallback(std::bind(&ApproximateEpsilonTimeSynchronizerTest::callback, this, _1, _2)); + sync_.registerCallback(std::bind(&ApproximateEpsilonTimeSynchronizerTest::callback, this, std::placeholders::_1, std::placeholders::_2)); } void callback(const MsgConstPtr& p, const MsgConstPtr& q) @@ -136,10 +134,10 @@ class ApproximateEpsilonTimeSynchronizerTest } private: - const std::vector &input_; - const std::vector &output_; + const std::vector & input_; + const std::vector & output_; unsigned int output_position_; - typedef Synchronizer > Sync2; + typedef message_filters::Synchronizer> Sync2; public: Sync2 sync_; }; @@ -157,10 +155,10 @@ TEST(ApproxTimeSync, ExactMatch) { input.push_back(TimeAndTopic(t,0)); // a input.push_back(TimeAndTopic(t,1)); // A - input.push_back(TimeAndTopic(t+s*3,0)); // b - input.push_back(TimeAndTopic(t+s*3,1)); // B - input.push_back(TimeAndTopic(t+s*6,0)); // c - input.push_back(TimeAndTopic(t+s*6,1)); // C + input.push_back(TimeAndTopic(t+s*3,0)); // b + input.push_back(TimeAndTopic(t+s*3,1)); // B + input.push_back(TimeAndTopic(t+s*6,0)); // c + input.push_back(TimeAndTopic(t+s*6,1)); // C output.push_back(TimePair(t, t)); output.push_back(TimePair(t+s*3, t+s*3)); output.push_back(TimePair(t+s*6, t+s*6)); @@ -225,7 +223,8 @@ TEST(ApproxTimeSync, ImperfectMatch) { sync_test.run(); } -int main(int argc, char **argv) { +int main(int argc, char **argv) +{ testing::InitGoogleTest(&argc, argv); rclcpp::init(argc, argv); diff --git a/test/test_approximate_time_policy.cpp b/test/test_approximate_time_policy.cpp index 6333c8b..284e459 100644 --- a/test/test_approximate_time_policy.cpp +++ b/test/test_approximate_time_policy.cpp @@ -33,23 +33,21 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ +#include +#include +#include + #include #include #include "message_filters/synchronizer.h" #include "message_filters/sync_policies/approximate_time.h" #include "message_filters/message_traits.h" -#include - -using namespace std::placeholders; -using namespace message_filters; -using namespace message_filters::sync_policies; struct Header { rclcpp::Time stamp; }; - struct Msg { Header header; @@ -57,6 +55,7 @@ struct Msg }; typedef std::shared_ptr MsgPtr; typedef std::shared_ptr MsgConstPtr; + namespace message_filters { namespace message_traits @@ -64,7 +63,7 @@ namespace message_traits template<> struct TimeStamp { - static rclcpp::Time value(const Msg& m) + static rclcpp::Time value(const Msg & m) { return m.header.stamp; } @@ -94,18 +93,16 @@ class ApproximateTimeSynchronizerTest { public: - ApproximateTimeSynchronizerTest(const std::vector &input, - const std::vector &output, + ApproximateTimeSynchronizerTest(const std::vector & input, + const std::vector & output, uint32_t queue_size) : input_(input), output_(output), output_position_(0), sync_(queue_size) { - sync_.registerCallback(std::bind(&ApproximateTimeSynchronizerTest::callback, this, _1, _2)); + sync_.registerCallback(std::bind(&ApproximateTimeSynchronizerTest::callback, this, std::placeholders::_1, std::placeholders::_2)); } - void callback(const MsgConstPtr& p, const MsgConstPtr& q) + void callback(const MsgConstPtr & p, const MsgConstPtr & q) { - //printf("Call_back called\n"); - //printf("Call back: <%f, %f>\n", p->header.stamp.toSec(), q->header.stamp.toSec()); ASSERT_TRUE(p); ASSERT_TRUE(q); ASSERT_LT(output_position_, output_.size()); @@ -116,30 +113,25 @@ class ApproximateTimeSynchronizerTest void run() { - for (unsigned int i = 0; i < input_.size(); i++) - { - if (input_[i].second == 0) - { + for (size_t i = 0; i < input_.size(); i++) { + if (input_[i].second == 0) { MsgPtr p(std::make_shared()); p->header.stamp = input_[i].first; sync_.add<0>(p); - } - else - { + } else { MsgPtr q(std::make_shared()); q->header.stamp = input_[i].first; sync_.add<1>(q); } } - //printf("Done running test\n"); EXPECT_EQ(output_.size(), output_position_); } private: - const std::vector &input_; - const std::vector &output_; + const std::vector & input_; + const std::vector & output_; unsigned int output_position_; - typedef Synchronizer > Sync2; + typedef message_filters::Synchronizer> Sync2; public: Sync2 sync_; }; @@ -152,18 +144,16 @@ class ApproximateTimeSynchronizerTestQuad { public: - ApproximateTimeSynchronizerTestQuad(const std::vector &input, - const std::vector &output, + ApproximateTimeSynchronizerTestQuad(const std::vector & input, + const std::vector & output, uint32_t queue_size) : input_(input), output_(output), output_position_(0), sync_(queue_size) { - sync_.registerCallback(std::bind(&ApproximateTimeSynchronizerTestQuad::callback, this, _1, _2, _3, _4)); + sync_.registerCallback(std::bind(&ApproximateTimeSynchronizerTestQuad::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4)); } - void callback(const MsgConstPtr& p, const MsgConstPtr& q, const MsgConstPtr& r, const MsgConstPtr& s) + void callback(const MsgConstPtr & p, const MsgConstPtr & q, const MsgConstPtr & r, const MsgConstPtr & s) { - //printf("Call_back called\n"); - //printf("Call back: <%f, %f>\n", p->header.stamp.toSec(), q->header.stamp.toSec()); ASSERT_TRUE(p); ASSERT_TRUE(q); ASSERT_TRUE(r); @@ -178,8 +168,7 @@ class ApproximateTimeSynchronizerTestQuad void run() { - for (unsigned int i = 0; i < input_.size(); i++) - { + for (size_t i = 0; i < input_.size(); i++) { MsgPtr p(std::make_shared()); p->header.stamp = input_[i].first; switch (input_[i].second) @@ -198,7 +187,6 @@ class ApproximateTimeSynchronizerTestQuad break; } } - //printf("Done running test\n"); EXPECT_EQ(output_.size(), output_position_); } @@ -206,7 +194,7 @@ class ApproximateTimeSynchronizerTestQuad const std::vector &input_; const std::vector &output_; unsigned int output_position_; - typedef Synchronizer > Sync4; + typedef message_filters::Synchronizer > Sync4; public: Sync4 sync_; }; @@ -534,7 +522,8 @@ TEST(ApproxTimeSync, RateBound) { } -int main(int argc, char **argv) { +int main(int argc, char **argv) +{ testing::InitGoogleTest(&argc, argv); rclcpp::init(argc, argv); diff --git a/test/test_chain.cpp b/test/test_chain.cpp index 68c65e2..6b7894b 100644 --- a/test/test_chain.cpp +++ b/test/test_chain.cpp @@ -32,13 +32,13 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ +#include + #include #include #include "message_filters/chain.h" -using namespace message_filters; - struct Msg { }; @@ -60,13 +60,13 @@ class Helper int32_t count_; }; -typedef std::shared_ptr > PassThroughPtr; +typedef std::shared_ptr > PassThroughPtr; TEST(Chain, simple) { Helper h; - Chain c; - c.addFilter(std::make_shared >()); + message_filters::Chain c; + c.addFilter(std::make_shared >()); c.registerCallback(std::bind(&Helper::cb, &h)); c.add(std::make_shared()); @@ -78,11 +78,11 @@ TEST(Chain, simple) TEST(Chain, multipleFilters) { Helper h; - Chain c; - c.addFilter(std::make_shared >()); - c.addFilter(std::make_shared >()); - c.addFilter(std::make_shared >()); - c.addFilter(std::make_shared >()); + message_filters::Chain c; + c.addFilter(std::make_shared >()); + c.addFilter(std::make_shared >()); + c.addFilter(std::make_shared >()); + c.addFilter(std::make_shared >()); c.registerCallback(std::bind(&Helper::cb, &h)); c.add(std::make_shared()); @@ -94,16 +94,16 @@ TEST(Chain, multipleFilters) TEST(Chain, addingFilters) { Helper h; - Chain c; - c.addFilter(std::make_shared >()); - c.addFilter(std::make_shared >()); + message_filters::Chain c; + c.addFilter(std::make_shared >()); + c.addFilter(std::make_shared >()); c.registerCallback(std::bind(&Helper::cb, &h)); c.add(std::make_shared()); EXPECT_EQ(h.count_, 1); - c.addFilter(std::make_shared >()); - c.addFilter(std::make_shared >()); + c.addFilter(std::make_shared >()); + c.addFilter(std::make_shared >()); c.add(std::make_shared()); EXPECT_EQ(h.count_, 2); @@ -112,11 +112,11 @@ TEST(Chain, addingFilters) TEST(Chain, inputFilter) { Helper h; - Chain c; - c.addFilter(std::make_shared >()); + message_filters::Chain c; + c.addFilter(std::make_shared >()); c.registerCallback(std::bind(&Helper::cb, &h)); - PassThrough p; + message_filters::PassThrough p; c.connectInput(p); p.add(std::make_shared()); EXPECT_EQ(h.count_, 1); @@ -128,8 +128,8 @@ TEST(Chain, inputFilter) TEST(Chain, nonSharedPtrFilter) { Helper h; - Chain c; - PassThrough p; + message_filters::Chain c; + message_filters::PassThrough p; c.addFilter(&p); c.registerCallback(std::bind(&Helper::cb, &h)); @@ -141,43 +141,44 @@ TEST(Chain, nonSharedPtrFilter) TEST(Chain, retrieveFilter) { - Chain c; + message_filters::Chain c; - ASSERT_FALSE(c.getFilter >(0)); + ASSERT_FALSE(c.getFilter >(0)); - c.addFilter(std::make_shared >()); + c.addFilter(std::make_shared >()); - ASSERT_TRUE(c.getFilter >(0)); - ASSERT_FALSE(c.getFilter >(1)); + ASSERT_TRUE(c.getFilter >(0)); + ASSERT_FALSE(c.getFilter >(1)); } TEST(Chain, retrieveFilterThroughBaseClass) { - Chain c; - ChainBase* cb = &c; + message_filters::Chain c; + message_filters::ChainBase * cb = &c; - ASSERT_FALSE(cb->getFilter >(0)); + ASSERT_FALSE(cb->getFilter >(0)); - c.addFilter(std::make_shared >()); + c.addFilter(std::make_shared >()); - ASSERT_TRUE(cb->getFilter >(0)); - ASSERT_FALSE(cb->getFilter >(1)); + ASSERT_TRUE(cb->getFilter >(0)); + ASSERT_FALSE(cb->getFilter >(1)); } -struct PTDerived : public PassThrough +struct PTDerived : public message_filters::PassThrough { }; TEST(Chain, retrieveBaseClass) { - Chain c; + message_filters::Chain c; c.addFilter(std::make_shared()); - ASSERT_TRUE(c.getFilter >(0)); + ASSERT_TRUE(c.getFilter >(0)); ASSERT_TRUE(c.getFilter(0)); } -int main(int argc, char **argv){ +int main(int argc, char **argv) +{ testing::InitGoogleTest(&argc, argv); rclcpp::init(argc, argv); return RUN_ALL_TESTS(); diff --git a/test/test_exact_time_policy.cpp b/test/test_exact_time_policy.cpp index 43a494e..0727111 100644 --- a/test/test_exact_time_policy.cpp +++ b/test/test_exact_time_policy.cpp @@ -32,21 +32,20 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ +#include +#include + #include #include #include "message_filters/synchronizer.h" #include "message_filters/sync_policies/exact_time.h" -using namespace message_filters; -using namespace message_filters::sync_policies; - struct Header { rclcpp::Time stamp; }; - struct Msg { Header header; @@ -92,10 +91,10 @@ class Helper int32_t drop_count_; }; -typedef ExactTime Policy2; -typedef ExactTime Policy3; -typedef Synchronizer Sync2; -typedef Synchronizer Sync3; +typedef message_filters::sync_policies::ExactTime Policy2; +typedef message_filters::sync_policies::ExactTime Policy3; +typedef message_filters::Synchronizer Sync2; +typedef message_filters::Synchronizer Sync3; ////////////////////////////////////////////////////////////////////////////////////////////////// // From here on we assume that testing the 3-message version is sufficient, so as not to duplicate @@ -167,14 +166,14 @@ TEST(ExactTime, dropCallback) struct EventHelper { - void callback(const MessageEvent& e1, const MessageEvent& e2) + void callback(const message_filters::MessageEvent & e1, const message_filters::MessageEvent & e2) { e1_ = e1; e2_ = e2; } - MessageEvent e1_; - MessageEvent e2_; + message_filters::MessageEvent e1_; + message_filters::MessageEvent e2_; }; TEST(ExactTime, eventInEventOut) @@ -182,7 +181,7 @@ TEST(ExactTime, eventInEventOut) Sync2 sync(2); EventHelper h; sync.registerCallback(&EventHelper::callback, &h); - MessageEvent evt(std::make_shared(), rclcpp::Time(4, 0)); + message_filters::MessageEvent evt(std::make_shared(), rclcpp::Time(4, 0)); sync.add<0>(evt); sync.add<1>(evt); @@ -193,7 +192,8 @@ TEST(ExactTime, eventInEventOut) ASSERT_EQ(h.e2_.getReceiptTime(), evt.getReceiptTime()); } -int main(int argc, char **argv){ +int main(int argc, char **argv) +{ testing::InitGoogleTest(&argc, argv); rclcpp::init(argc, argv); return RUN_ALL_TESTS(); diff --git a/test/test_fuzz.cpp b/test/test_fuzz.cpp index dedbb17..055ef06 100644 --- a/test/test_fuzz.cpp +++ b/test/test_fuzz.cpp @@ -36,6 +36,9 @@ // https://github.com/ros2/message_filters/blob/1866ddb86db5b5c7746556ea7544b6f34c00415b/test/time_sequencer_unittest.cpp #include + +#include +#include #include #include @@ -45,8 +48,6 @@ #include "message_filters/chain.h" #include "sensor_msgs/msg/imu.hpp" -using namespace std::placeholders; -using namespace message_filters; typedef sensor_msgs::msg::Imu Msg; typedef std::shared_ptr MsgConstPtr; typedef std::shared_ptr MsgPtr; @@ -70,21 +71,22 @@ class Helper int32_t count_; }; -static void fuzz_msg(MsgPtr msg) { - static std::random_device seeder; - std::mt19937 gen(seeder()); - std::uniform_real_distribution distr(1.0, 3.0); - msg->linear_acceleration.x = distr(gen); - msg->linear_acceleration.y = distr(gen); - msg->linear_acceleration.z = distr(gen); +static void fuzz_msg(MsgPtr msg) +{ + static std::random_device seeder; + std::mt19937 gen(seeder()); + std::uniform_real_distribution distr(1.0, 3.0); + msg->linear_acceleration.x = distr(gen); + msg->linear_acceleration.y = distr(gen); + msg->linear_acceleration.z = distr(gen); } TEST(TimeSequencer, fuzz_sequencer) { rclcpp::Node::SharedPtr node = std::make_shared("test_node"); - TimeSequencer seq(rclcpp::Duration(0, 10000000), rclcpp::Duration(0, 1000000), 10, node); + message_filters::TimeSequencer seq(rclcpp::Duration(0, 10000000), rclcpp::Duration(0, 1000000), 10, node); Helper h; - seq.registerCallback(std::bind(&Helper::cb, &h, _1)); + seq.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); rclcpp::Clock ros_clock; auto start = ros_clock.now(); auto msg = std::make_shared(); @@ -105,16 +107,15 @@ TEST(TimeSequencer, fuzz_sequencer) TEST(TimeSynchronizer, fuzz_synchronizer) { - TimeSynchronizer sync(1); + message_filters::TimeSynchronizer sync(1); Helper h; - sync.registerCallback(std::bind(&Helper::cb2, &h, _1, _2)); + sync.registerCallback(std::bind(&Helper::cb2, &h, std::placeholders::_1, std::placeholders::_2)); rclcpp::Clock ros_clock; auto start = ros_clock.now(); auto msg1 = std::make_shared(); auto msg2 = std::make_shared(); - while ((ros_clock.now() - start) < rclcpp::Duration(5, 0)) - { + while ((ros_clock.now() - start) < rclcpp::Duration(5, 0)) { h.count_ = 0; fuzz_msg(msg1); msg1->header.stamp = rclcpp::Clock().now(); @@ -132,14 +133,13 @@ TEST(Subscriber, fuzz_subscriber) { auto node = std::make_shared("test_node"); Helper h; - Subscriber sub(node, "test_topic"); - sub.registerCallback(std::bind(&Helper::cb, &h, _1)); + message_filters::Subscriber sub(node, "test_topic"); + sub.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); auto pub = node->create_publisher("test_topic", 10); rclcpp::Clock ros_clock; auto start = ros_clock.now(); auto msg = std::make_shared(); - while ((ros_clock.now() - start) < rclcpp::Duration(5, 0)) - { + while ((ros_clock.now() - start) < rclcpp::Duration(5, 0)) { h.count_ = 0; fuzz_msg(msg); msg->header.stamp = ros_clock.now(); @@ -151,7 +151,8 @@ TEST(Subscriber, fuzz_subscriber) rclcpp::spin_some(node); } -int main(int argc, char **argv){ +int main(int argc, char **argv) +{ testing::InitGoogleTest(&argc, argv); rclcpp::init(argc, argv); diff --git a/test/test_latest_time_policy.cpp b/test/test_latest_time_policy.cpp index 0390212..a7aa6ed 100644 --- a/test/test_latest_time_policy.cpp +++ b/test/test_latest_time_policy.cpp @@ -47,12 +47,8 @@ #include "rosgraph_msgs/msg/clock.hpp" - -using namespace message_filters; -using namespace message_filters::sync_policies; using namespace std::chrono; - struct Header { rclcpp::Time stamp; @@ -73,7 +69,7 @@ class Helper { } - void cb(const MsgConstPtr& p, const MsgConstPtr& q, const MsgConstPtr& r) + void cb(const MsgConstPtr & p, const MsgConstPtr & q, const MsgConstPtr & r) { EXPECT_TRUE(p); EXPECT_TRUE(q); @@ -87,8 +83,8 @@ class Helper uint16_t count_{0U}; }; -typedef LatestTime Policy3; -typedef Synchronizer Sync3; +typedef message_filters::sync_policies::LatestTime Policy3; +typedef message_filters::Synchronizer Sync3; class LatestTimePolicy : public ::testing::Test { @@ -116,15 +112,12 @@ class LatestTimePolicy : public ::testing::Test p.reserve(12U); q.reserve(6U); r.reserve(3U); - for(std::size_t idx = 0U; idx < 12U; ++idx) - { + for(std::size_t idx = 0U; idx < 12U; ++idx) { MsgPtr p_idx(std::make_shared()); p_idx->data = idx; p.push_back(p_idx); - if(idx % 2U == 0U) - { + if (idx % 2U == 0U) { MsgPtr q_idx(std::make_shared()); q_idx->data = idx; q.push_back(q_idx); } - if(idx % 4U == 0U) - { + if (idx % 4U == 0U) { MsgPtr r_idx(std::make_shared()); r_idx->data = idx; r.push_back(r_idx); } } @@ -141,27 +134,21 @@ class LatestTimePolicy : public ::testing::Test TEST_F(LatestTimePolicy, Leading) { rclcpp::Rate rate(50.0); - for(std::size_t idx = 0U; idx < 8U; ++idx) - { - if(idx % 2U == 0U) - { + for (std::size_t idx = 0U; idx < 8U; ++idx) { + if (idx % 2U == 0U) { sync.add<1>(q[idx / 2U]); } - if(idx % 4U == 0U) - { + if (idx % 4U == 0U) { sync.add<2>(r[idx / 4U]); } sync.add<0>(p[idx]); EXPECT_EQ(h.count_, idx); - if(idx > 0) - { + if (idx > 0) { EXPECT_EQ(h.p_->data, p[idx]->data); EXPECT_EQ(h.q_->data, q[idx / 2U]->data); EXPECT_EQ(h.r_->data, r[idx / 4U]->data); - } - else - { + } else { EXPECT_FALSE(h.p_); EXPECT_FALSE(h.q_); EXPECT_FALSE(h.r_); @@ -174,27 +161,21 @@ TEST_F(LatestTimePolicy, Leading) TEST_F(LatestTimePolicy, Trailing) { rclcpp::Rate rate(50.0); - for(std::size_t idx = 0U; idx < 8U; ++idx) - { - if(idx % 2U == 1U) - { + for (std::size_t idx = 0U; idx < 8U; ++idx) { + if (idx % 2U == 1U) { sync.add<1>(q[(idx - 1U) / 2U]); } - if(idx % 4U == 3U) - { + if (idx % 4U == 3U) { sync.add<2>(r[(idx - 3U) / 4U]); } sync.add<0>(p[idx]); - if (idx > 2U) - { + if (idx > 2U) { EXPECT_EQ(h.count_, idx - 2U); EXPECT_EQ(h.p_->data, p[idx]->data); EXPECT_EQ(h.q_->data, q[(idx - 1U) / 2U]->data); EXPECT_EQ(h.r_->data, r[(idx - 3U) / 4U]->data); - } - else - { + } else { EXPECT_FALSE(h.p_); EXPECT_FALSE(h.q_); EXPECT_FALSE(h.r_); @@ -217,34 +198,26 @@ TEST_F(LatestTimePolicy, ChangeRateLeading) rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(node); - for(std::size_t idx = 0U; idx < 12U; ++idx) - { - if(idx % 2U == 0U) - { + for (std::size_t idx = 0U; idx < 12U; ++idx) { + if (idx % 2U == 0U) { sync.add<1>(q[idx/2U]); } - if(idx % 4U == 0U) - { + if (idx % 4U == 0U) { sync.add<2>(r[idx/4U]); } - if (idx < 4U) - { + if (idx < 4U) { sync.add<0>(p[idx]); - } - else // Change rate of p - { - if(idx % 3U == 0U) - { + } else { // Change rate of p + if (idx % 3U == 0U) { static std::size_t p_idx = 3U; sync.add<0>(p[++p_idx]); } } // operates like "Leading" test for idx <= 3 - if (idx >= 1U && idx < 4U) - { + if (idx >= 1U && idx < 4U) { EXPECT_EQ(h.count_, idx); EXPECT_EQ(h.p_->data, p[idx]->data); EXPECT_EQ(h.q_->data, q[idx / 2U]->data); @@ -255,8 +228,7 @@ TEST_F(LatestTimePolicy, ChangeRateLeading) // Will not publish again until idx==6 when q is found as new pivot. // Same behavior as initialization dropping faster messages until rates of all are known // or found to be late. - else if (idx >= 4U && idx < 6U) - { + else if (idx >= 4U && idx < 6U) { EXPECT_EQ(h.count_, (idx + 2U) / 2U); EXPECT_EQ(h.p_->data, p[3]->data); EXPECT_EQ(h.q_->data, q[1]->data); @@ -265,15 +237,12 @@ TEST_F(LatestTimePolicy, ChangeRateLeading) // New actual rate of p computed when is received after q when idx==6 // for idx >= 6, follows normal "Leading" pattern again // with pivot on q - else if (idx >= 6U) - { + else if (idx >= 6U) { EXPECT_EQ(h.count_, (idx + 2U) / 2U); EXPECT_EQ(h.p_->data, p[idx / 2U]->data); EXPECT_EQ(h.q_->data, q[idx / 2U]->data); EXPECT_EQ(h.r_->data, r[(idx - 2U) / 4U]->data); - } - else - { + } else { EXPECT_FALSE(h.p_); EXPECT_FALSE(h.q_); EXPECT_FALSE(h.r_); @@ -303,34 +272,26 @@ TEST_F(LatestTimePolicy, ChangeRateTrailing) rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(node); - for(std::size_t idx = 0U; idx < 12U; ++idx) - { - if(idx % 2U == 1U) - { + for (std::size_t idx = 0U; idx < 12U; ++idx) { + if(idx % 2U == 1U) { sync.add<1>(q[(idx - 1U) / 2U]); } - if(idx % 4U == 3U) - { + if (idx % 4U == 3U) { sync.add<2>(r[(idx - 3U) / 4U]); } - if (idx < 4U) - { + if (idx < 4U) { sync.add<0>(p[idx]); - } - else // Change rate of p (still 1kHz @ idx == 4) - { - if(idx % 3U == 1U) - { + } else { // Change rate of p (still 1kHz @ idx == 4) + if (idx % 3U == 1U) { static std::size_t p_idx = 3U; sync.add<0>(p[++p_idx]); } } // operates like "Trailing" test for idx <= 3 - if (idx > 2U && idx <= 4U) - { + if (idx > 2U && idx <= 4U) { EXPECT_EQ(h.count_, idx - 2U); EXPECT_EQ(h.p_->data, p[idx]->data); EXPECT_EQ(h.q_->data, q[(idx - 1U) / 2U]->data); @@ -341,8 +302,7 @@ TEST_F(LatestTimePolicy, ChangeRateTrailing) // At idx==5, policy still doesn't know that p is late when q is received. // Same behavior as initialization dropping faster messages until rates of all are known // or found to be late. - else if (idx > 4U && idx < 7U) - { + else if (idx > 4U && idx < 7U) { EXPECT_EQ(h.count_, 2U); EXPECT_EQ(h.p_->data, p[4U]->data); EXPECT_EQ(h.q_->data, q[1U]->data); @@ -355,15 +315,12 @@ TEST_F(LatestTimePolicy, ChangeRateTrailing) // New actual rate of p computed when is received after q when idx==7 // for idx >= 7, follows normal "Trailing" pattern again // with pivot on q - else if (idx >= 7U) - { + else if (idx >= 7U) { EXPECT_EQ(h.count_, (idx - 1U) / 2U); EXPECT_EQ(h.p_->data, p[(idx + 1U) / 2U]->data); EXPECT_EQ(h.q_->data, q[(idx - 1U) / 2U]->data); EXPECT_EQ(h.r_->data, r[(idx - 5U) / 4U]->data); - } - else - { + } else { EXPECT_FALSE(h.p_); EXPECT_FALSE(h.q_); EXPECT_FALSE(h.r_); @@ -380,8 +337,8 @@ TEST_F(LatestTimePolicy, ChangeRateTrailing) } } - -int main(int argc, char **argv){ +int main(int argc, char **argv) +{ testing::InitGoogleTest(&argc, argv); rclcpp::init(argc, argv); return RUN_ALL_TESTS(); diff --git a/test/test_simple.cpp b/test/test_simple.cpp index 7f3a82a..4e54dae 100644 --- a/test/test_simple.cpp +++ b/test/test_simple.cpp @@ -34,14 +34,13 @@ #include -#include +#include #include #include -#include -#include "message_filters/simple_filter.h" -using namespace message_filters; -using namespace std::placeholders; +#include + +#include "message_filters/simple_filter.h" struct Msg { @@ -49,11 +48,11 @@ struct Msg typedef std::shared_ptr MsgPtr; typedef std::shared_ptr MsgConstPtr; -struct Filter : public SimpleFilter +struct Filter : public message_filters::SimpleFilter { - typedef MessageEvent EventType; + typedef message_filters::MessageEvent EventType; - void add(const EventType& evt) + void add(const EventType & evt) { signalMessage(evt); } @@ -67,7 +66,7 @@ class Helper counts_.fill(0); } - void cb0(const MsgConstPtr&) + void cb0(const MsgConstPtr &) { ++counts_[0]; } @@ -82,7 +81,7 @@ class Helper ++counts_[2]; } - void cb3(const MessageEvent&) + void cb3(const message_filters::MessageEvent &) { ++counts_[3]; } @@ -92,7 +91,7 @@ class Helper ++counts_[4]; } - void cb5(const MsgPtr&) + void cb5(const MsgPtr &) { ++counts_[5]; } @@ -102,7 +101,7 @@ class Helper ++counts_[6]; } - void cb7(const MessageEvent&) + void cb7(const message_filters::MessageEvent &) { ++counts_[7]; } @@ -114,14 +113,14 @@ TEST(SimpleFilter, callbackTypes) { Helper h; Filter f; - f.registerCallback(std::bind(&Helper::cb0, &h, _1)); - f.registerCallback(std::bind(&Helper::cb1, &h, _1)); - f.registerCallback(std::bind(&Helper::cb2, &h, _1)); - f.registerCallback&>(std::bind(&Helper::cb3, &h, _1)); - f.registerCallback(std::bind(&Helper::cb4, &h, _1)); - f.registerCallback(std::bind(&Helper::cb5, &h, _1)); - f.registerCallback(std::bind(&Helper::cb6, &h, _1)); - f.registerCallback&>(std::bind(&Helper::cb7, &h, _1)); + f.registerCallback(std::bind(&Helper::cb0, &h, std::placeholders::_1)); + f.registerCallback(std::bind(&Helper::cb1, &h, std::placeholders::_1)); + f.registerCallback(std::bind(&Helper::cb2, &h, std::placeholders::_1)); + f.registerCallback&>(std::bind(&Helper::cb3, &h, std::placeholders::_1)); + f.registerCallback(std::bind(&Helper::cb4, &h, std::placeholders::_1)); + f.registerCallback(std::bind(&Helper::cb5, &h, std::placeholders::_1)); + f.registerCallback(std::bind(&Helper::cb6, &h, std::placeholders::_1)); + f.registerCallback&>(std::bind(&Helper::cb7, &h, std::placeholders::_1)); f.add(Filter::EventType(std::make_shared())); EXPECT_EQ(h.counts_[0], 1); @@ -136,9 +135,9 @@ TEST(SimpleFilter, callbackTypes) struct OldFilter { - Connection registerCallback(const std::function&) + message_filters::Connection registerCallback(const std::function&) { - return Connection(); + return message_filters::Connection(); } }; @@ -146,7 +145,7 @@ TEST(SimpleFilter, oldRegisterWithNewFilter) { OldFilter f; Helper h; - f.registerCallback(std::bind(&Helper::cb3, &h, _1)); + f.registerCallback(std::bind(&Helper::cb3, &h, std::placeholders::_1)); } int main(int argc, char **argv){ diff --git a/test/test_subscriber.cpp b/test/test_subscriber.cpp index 552c985..88d5ab2 100644 --- a/test/test_subscriber.cpp +++ b/test/test_subscriber.cpp @@ -34,17 +34,16 @@ #include -// see ros2/rclcpp#1619,1713 -// TODO: remove this comment, and the `NonConstHelper` tests -// once the deprecated signatures have been discontinued. -#define RCLCPP_AVOID_DEPRECATIONS_FOR_UNIT_TESTS 1 +#include +#include +#include + #include #include #include "message_filters/subscriber.h" #include "message_filters/chain.h" #include "sensor_msgs/msg/imu.hpp" -using namespace message_filters; typedef sensor_msgs::msg::Imu Msg; typedef std::shared_ptr MsgConstPtr; typedef std::shared_ptr MsgPtr; @@ -68,13 +67,12 @@ TEST(Subscriber, simple) { auto node = std::make_shared("test_node"); Helper h; - Subscriber sub(node, "test_topic"); + message_filters::Subscriber sub(node, "test_topic"); sub.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); auto pub = node->create_publisher("test_topic", 10); rclcpp::Clock ros_clock; auto start = ros_clock.now(); - while (h.count_ == 0 && (ros_clock.now() - start) < rclcpp::Duration(1, 0)) - { + while (h.count_ == 0 && (ros_clock.now() - start) < rclcpp::Duration(1, 0)) { pub->publish(Msg()); rclcpp::Rate(50).sleep(); rclcpp::spin_some(node); @@ -87,13 +85,12 @@ TEST(Subscriber, simple_raw) { auto node = std::make_shared("test_node"); Helper h; - Subscriber sub(node.get(), "test_topic"); + message_filters::Subscriber sub(node.get(), "test_topic"); sub.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); auto pub = node->create_publisher("test_topic", 10); rclcpp::Clock ros_clock; auto start = ros_clock.now(); - while (h.count_ == 0 && (ros_clock.now() - start) < rclcpp::Duration(1, 0)) - { + while (h.count_ == 0 && (ros_clock.now() - start) < rclcpp::Duration(1, 0)) { pub->publish(Msg()); rclcpp::Rate(50).sleep(); rclcpp::spin_some(node); @@ -106,7 +103,7 @@ TEST(Subscriber, subUnsubSub) { auto node = std::make_shared("test_node"); Helper h; - Subscriber sub(node, "test_topic"); + message_filters::Subscriber sub(node, "test_topic"); sub.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); auto pub = node->create_publisher("test_topic", 10); @@ -115,8 +112,7 @@ TEST(Subscriber, subUnsubSub) rclcpp::Clock ros_clock; auto start = ros_clock.now(); - while (h.count_ == 0 && (ros_clock.now() - start) < rclcpp::Duration(1, 0)) - { + while (h.count_ == 0 && (ros_clock.now() - start) < rclcpp::Duration(1, 0)) { pub->publish(Msg()); rclcpp::Rate(50).sleep(); rclcpp::spin_some(node); @@ -129,7 +125,7 @@ TEST(Subscriber, subUnsubSub_raw) { auto node = std::make_shared("test_node"); Helper h; - Subscriber sub(node.get(), "test_topic"); + message_filters::Subscriber sub(node.get(), "test_topic"); sub.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); auto pub = node->create_publisher("test_topic", 10); @@ -138,8 +134,7 @@ TEST(Subscriber, subUnsubSub_raw) rclcpp::Clock ros_clock; auto start = ros_clock.now(); - while (h.count_ == 0 && (ros_clock.now() - start) < rclcpp::Duration(1, 0)) - { + while (h.count_ == 0 && (ros_clock.now() - start) < rclcpp::Duration(1, 0)) { pub->publish(Msg()); rclcpp::Rate(50).sleep(); rclcpp::spin_some(node); @@ -152,7 +147,7 @@ TEST(Subscriber, switchRawAndShared) { auto node = std::make_shared("test_node"); Helper h; - Subscriber sub(node, "test_topic"); + message_filters::Subscriber sub(node, "test_topic"); sub.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); auto pub = node->create_publisher("test_topic2", 10); @@ -161,8 +156,7 @@ TEST(Subscriber, switchRawAndShared) rclcpp::Clock ros_clock; auto start = ros_clock.now(); - while (h.count_ == 0 && (ros_clock.now() - start) < rclcpp::Duration(1, 0)) - { + while (h.count_ == 0 && (ros_clock.now() - start) < rclcpp::Duration(1, 0)) { pub->publish(Msg()); rclcpp::Rate(50).sleep(); rclcpp::spin_some(node); @@ -175,15 +169,14 @@ TEST(Subscriber, subInChain) { auto node = std::make_shared("test_node"); Helper h; - Chain c; - c.addFilter(std::make_shared >(node, "test_topic")); + message_filters::Chain c; + c.addFilter(std::make_shared >(node, "test_topic")); c.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); auto pub = node->create_publisher("test_topic", 10); rclcpp::Clock ros_clock; auto start = ros_clock.now(); - while (h.count_ == 0 && (ros_clock.now() - start) < rclcpp::Duration(1, 0)) - { + while (h.count_ == 0 && (ros_clock.now() - start) < rclcpp::Duration(1, 0)) { pub->publish(Msg()); rclcpp::Rate(50).sleep(); rclcpp::spin_some(node); @@ -216,7 +209,7 @@ TEST(Subscriber, singleNonConstCallback) { auto node = std::make_shared("test_node"); NonConstHelper h; - Subscriber sub(node, "test_topic"); + message_filters::Subscriber sub(node, "test_topic"); sub.registerCallback(&NonConstHelper::cb, &h); auto pub = node->create_publisher("test_topic", 10); Msg msg; @@ -233,7 +226,7 @@ TEST(Subscriber, multipleNonConstCallbacksFilterSubscriber) { auto node = std::make_shared("test_node"); NonConstHelper h, h2; - Subscriber sub(node, "test_topic"); + message_filters::Subscriber sub(node, "test_topic"); sub.registerCallback(&NonConstHelper::cb, &h); sub.registerCallback(&NonConstHelper::cb, &h2); auto pub = node->create_publisher("test_topic", 10); @@ -254,7 +247,7 @@ TEST(Subscriber, multipleCallbacksSomeFilterSomeDirect) { auto node = std::make_shared("test_node"); NonConstHelper h, h2; - Subscriber sub(node, "test_topic"); + message_filters::Subscriber sub(node, "test_topic"); sub.registerCallback(&NonConstHelper::cb, &h); auto sub2 = node->create_subscription( "test_topic", 10, std::bind(&NonConstHelper::cb, &h2, std::placeholders::_1)); @@ -279,14 +272,13 @@ TEST(Subscriber, lifecycle) { auto node = std::make_shared("test_node"); Helper h; - Subscriber sub(node, "test_topic"); + message_filters::Subscriber sub(node, "test_topic"); sub.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); auto pub = node->create_publisher("test_topic", 10); pub->on_activate(); rclcpp::Clock ros_clock; auto start = ros_clock.now(); - while (h.count_ == 0 && (ros_clock.now() - start) < rclcpp::Duration(1, 0)) - { + while (h.count_ == 0 && (ros_clock.now() - start) < rclcpp::Duration(1, 0)) { pub->publish(Msg()); rclcpp::Rate(50).sleep(); rclcpp::spin_some(node->get_node_base_interface()); @@ -296,7 +288,8 @@ TEST(Subscriber, lifecycle) } -int main(int argc, char **argv){ +int main(int argc, char **argv) +{ testing::InitGoogleTest(&argc, argv); rclcpp::init(argc, argv); diff --git a/test/test_synchronizer.cpp b/test/test_synchronizer.cpp index 89de93b..2a51d8b 100644 --- a/test/test_synchronizer.cpp +++ b/test/test_synchronizer.cpp @@ -34,12 +34,11 @@ #include -#include -#include "message_filters/synchronizer.h" #include +#include -using namespace message_filters; -using namespace std::placeholders; +#include +#include "message_filters/synchronizer.h" struct Header { @@ -56,12 +55,12 @@ typedef std::shared_ptr MsgPtr; typedef std::shared_ptr MsgConstPtr; -template -struct NullPolicy : public PolicyBase +template +struct NullPolicy : public message_filters::PolicyBase { - typedef Synchronizer Sync; - typedef PolicyBase Super; + typedef message_filters::Synchronizer Sync; + typedef message_filters::PolicyBase Super; typedef typename Super::Messages Messages; typedef typename Super::Signal Signal; typedef typename Super::Events Events; @@ -69,8 +68,7 @@ struct NullPolicy : public PolicyBase NullPolicy() { - for (int i = 0; i < RealTypeCount::value; ++i) - { + for (int i = 0; i < RealTypeCount::value; ++i) { added_[i] = 0; } } @@ -80,7 +78,7 @@ struct NullPolicy : public PolicyBase } template - void add(const typename std::tuple_element::type&) + void add(const typename std::tuple_element::type &) { ++added_.at(i); } @@ -98,50 +96,50 @@ typedef NullPolicy Policy9; TEST(Synchronizer, compile2) { - NullFilter f0, f1; - Synchronizer sync(f0, f1); + message_filters::NullFilter f0, f1; + message_filters::Synchronizer sync(f0, f1); } TEST(Synchronizer, compile3) { - NullFilter f0, f1, f2; - Synchronizer sync(f0, f1, f2); + message_filters::NullFilter f0, f1, f2; + message_filters::Synchronizer sync(f0, f1, f2); } TEST(Synchronizer, compile4) { - NullFilter f0, f1, f2, f3; - Synchronizer sync(f0, f1, f2, f3); + message_filters::NullFilter f0, f1, f2, f3; + message_filters::Synchronizer sync(f0, f1, f2, f3); } TEST(Synchronizer, compile5) { - NullFilter f0, f1, f2, f3, f4; - Synchronizer sync(f0, f1, f2, f3, f4); + message_filters::NullFilter f0, f1, f2, f3, f4; + message_filters::Synchronizer sync(f0, f1, f2, f3, f4); } TEST(Synchronizer, compile6) { - NullFilter f0, f1, f2, f3, f4, f5; - Synchronizer sync(f0, f1, f2, f3, f4, f5); + message_filters::NullFilter f0, f1, f2, f3, f4, f5; + message_filters::Synchronizer sync(f0, f1, f2, f3, f4, f5); } TEST(Synchronizer, compile7) { - NullFilter f0, f1, f2, f3, f4, f5, f6; - Synchronizer sync(f0, f1, f2, f3, f4, f5, f6); + message_filters::NullFilter f0, f1, f2, f3, f4, f5, f6; + message_filters::Synchronizer sync(f0, f1, f2, f3, f4, f5, f6); } TEST(Synchronizer, compile8) { - NullFilter f0, f1, f2, f3, f4, f5, f6, f7; - Synchronizer sync(f0, f1, f2, f3, f4, f5, f6, f7); + message_filters::NullFilter f0, f1, f2, f3, f4, f5, f6, f7; + message_filters::Synchronizer sync(f0, f1, f2, f3, f4, f5, f6, f7); } TEST(Synchronizer, compile9) { - NullFilter f0, f1, f2, f3, f4, f5, f6, f7, f8; - Synchronizer sync(f0, f1, f2, f3, f4, f5, f6, f7, f8); + message_filters::NullFilter f0, f1, f2, f3, f4, f5, f6, f7, f8; + message_filters::Synchronizer sync(f0, f1, f2, f3, f4, f5, f6, f7, f8); } void function2(const MsgConstPtr&, const MsgConstPtr&) {} @@ -151,53 +149,53 @@ void function5(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const void function6(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} void function7(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} void function8(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} -void function9(const MsgConstPtr&, MsgConstPtr, const MsgPtr&, MsgPtr, const Msg&, Msg, const MessageEvent&, const MessageEvent&, const MsgConstPtr&) {} +void function9(const MsgConstPtr&, MsgConstPtr, const MsgPtr&, MsgPtr, const Msg&, Msg, const message_filters::MessageEvent&, const message_filters::MessageEvent&, const MsgConstPtr&) {} TEST(Synchronizer, compileFunction2) { - Synchronizer sync; + message_filters::Synchronizer sync; sync.registerCallback(function2); } TEST(Synchronizer, compileFunction3) { - Synchronizer sync; + message_filters::Synchronizer sync; sync.registerCallback(function3); } TEST(Synchronizer, compileFunction4) { - Synchronizer sync; + message_filters::Synchronizer sync; sync.registerCallback(function4); } TEST(Synchronizer, compileFunction5) { - Synchronizer sync; + message_filters::Synchronizer sync; sync.registerCallback(function5); } TEST(Synchronizer, compileFunction6) { - Synchronizer sync; + message_filters::Synchronizer sync; sync.registerCallback(function6); } TEST(Synchronizer, compileFunction7) { - Synchronizer sync; + message_filters::Synchronizer sync; sync.registerCallback(function7); } TEST(Synchronizer, compileFunction8) { - Synchronizer sync; + message_filters::Synchronizer sync; sync.registerCallback(function8); } TEST(Synchronizer, compileFunction9) { - Synchronizer sync; + message_filters::Synchronizer sync; sync.registerCallback(function9); } @@ -209,62 +207,62 @@ struct MethodHelper void method5(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} void method6(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} void method7(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} - void method8(const MsgConstPtr&, MsgConstPtr, const MsgPtr&, MsgPtr, const Msg&, Msg, const MessageEvent&, const MessageEvent&) {} + void method8(const MsgConstPtr&, MsgConstPtr, const MsgPtr&, MsgPtr, const Msg&, Msg, const message_filters::MessageEvent&, const message_filters::MessageEvent&) {} // Can only do 8 here because the object instance counts as a parameter and bind only supports 9 }; TEST(Synchronizer, compileMethod2) { MethodHelper h; - Synchronizer sync; + message_filters::Synchronizer sync; sync.registerCallback(&MethodHelper::method2, &h); } TEST(Synchronizer, compileMethod3) { MethodHelper h; - Synchronizer sync; + message_filters::Synchronizer sync; sync.registerCallback(&MethodHelper::method3, &h); } TEST(Synchronizer, compileMethod4) { MethodHelper h; - Synchronizer sync; + message_filters::Synchronizer sync; sync.registerCallback(&MethodHelper::method4, &h); } TEST(Synchronizer, compileMethod5) { MethodHelper h; - Synchronizer sync; + message_filters::Synchronizer sync; sync.registerCallback(&MethodHelper::method5, &h); } TEST(Synchronizer, compileMethod6) { MethodHelper h; - Synchronizer sync; + message_filters::Synchronizer sync; sync.registerCallback(&MethodHelper::method6, &h); } TEST(Synchronizer, compileMethod7) { MethodHelper h; - Synchronizer sync; + message_filters::Synchronizer sync; sync.registerCallback(&MethodHelper::method7, &h); } TEST(Synchronizer, compileMethod8) { MethodHelper h; - Synchronizer sync; + message_filters::Synchronizer sync; sync.registerCallback(&MethodHelper::method8, &h); } TEST(Synchronizer, add2) { - Synchronizer sync; + message_filters::Synchronizer sync; MsgPtr m(std::make_shared()); ASSERT_EQ(sync.added_[0], 0); @@ -277,7 +275,7 @@ TEST(Synchronizer, add2) TEST(Synchronizer, add3) { - Synchronizer sync; + message_filters::Synchronizer sync; MsgPtr m(std::make_shared()); ASSERT_EQ(sync.added_[0], 0); @@ -293,7 +291,7 @@ TEST(Synchronizer, add3) TEST(Synchronizer, add4) { - Synchronizer sync; + message_filters::Synchronizer sync; MsgPtr m(std::make_shared()); ASSERT_EQ(sync.added_[0], 0); @@ -312,7 +310,7 @@ TEST(Synchronizer, add4) TEST(Synchronizer, add5) { - Synchronizer sync; + message_filters::Synchronizer sync; MsgPtr m(std::make_shared()); ASSERT_EQ(sync.added_[0], 0); @@ -334,7 +332,7 @@ TEST(Synchronizer, add5) TEST(Synchronizer, add6) { - Synchronizer sync; + message_filters::Synchronizer sync; MsgPtr m(std::make_shared()); ASSERT_EQ(sync.added_[0], 0); @@ -359,7 +357,7 @@ TEST(Synchronizer, add6) TEST(Synchronizer, add7) { - Synchronizer sync; + message_filters::Synchronizer sync; MsgPtr m(std::make_shared()); ASSERT_EQ(sync.added_[0], 0); @@ -387,7 +385,7 @@ TEST(Synchronizer, add7) TEST(Synchronizer, add8) { - Synchronizer sync; + message_filters::Synchronizer sync; MsgPtr m(std::make_shared()); ASSERT_EQ(sync.added_[0], 0); @@ -418,7 +416,7 @@ TEST(Synchronizer, add8) TEST(Synchronizer, add9) { - Synchronizer sync; + message_filters::Synchronizer sync; MsgPtr m(std::make_shared()); ASSERT_EQ(sync.added_[0], 0); @@ -450,7 +448,8 @@ TEST(Synchronizer, add9) ASSERT_EQ(sync.added_[8], 1); } -int main(int argc, char **argv){ +int main(int argc, char **argv) +{ testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/test/time_sequencer_unittest.cpp b/test/time_sequencer_unittest.cpp index 4b9264d..744a3cf 100644 --- a/test/time_sequencer_unittest.cpp +++ b/test/time_sequencer_unittest.cpp @@ -34,17 +34,16 @@ #include +#include + #include #include "message_filters/time_sequencer.h" -using namespace message_filters; - struct Header { rclcpp::Time stamp; }; - struct Msg { Header header; @@ -86,7 +85,7 @@ class Helper TEST(TimeSequencer, simple) { rclcpp::Node::SharedPtr node = std::make_shared("test_node"); - TimeSequencer seq(rclcpp::Duration(0, 250000000), rclcpp::Duration(0, 10000000), 10, node); + message_filters::TimeSequencer seq(rclcpp::Duration(0, 250000000), rclcpp::Duration(0, 10000000), 10, node); Helper h; seq.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); MsgPtr msg(std::make_shared()); @@ -107,35 +106,34 @@ TEST(TimeSequencer, simple) TEST(TimeSequencer, compilation) { rclcpp::Node::SharedPtr node = std::make_shared("test_node"); - TimeSequencer seq(rclcpp::Duration(1, 0), rclcpp::Duration(0, 10000000), 10, node); - TimeSequencer seq2(rclcpp::Duration(1, 0), rclcpp::Duration(0, 10000000), 10, node); + message_filters::TimeSequencer seq(rclcpp::Duration(1, 0), rclcpp::Duration(0, 10000000), 10, node); + message_filters::TimeSequencer seq2(rclcpp::Duration(1, 0), rclcpp::Duration(0, 10000000), 10, node); seq2.connectInput(seq); } struct EventHelper { public: - void cb(const MessageEvent& evt) + void cb(const message_filters::MessageEvent & evt) { event_ = evt; } - MessageEvent event_; + message_filters::MessageEvent event_; }; TEST(TimeSequencer, eventInEventOut) { rclcpp::Node::SharedPtr nh = std::make_shared("test_node"); - TimeSequencer seq(rclcpp::Duration(1, 0), rclcpp::Duration(0, 10000000), 10, nh); - TimeSequencer seq2(seq, rclcpp::Duration(1, 0), rclcpp::Duration(0, 10000000), 10, nh); + message_filters::TimeSequencer seq(rclcpp::Duration(1, 0), rclcpp::Duration(0, 10000000), 10, nh); + message_filters::TimeSequencer seq2(seq, rclcpp::Duration(1, 0), rclcpp::Duration(0, 10000000), 10, nh); EventHelper h; seq2.registerCallback(&EventHelper::cb, &h); - MessageEvent evt(std::make_shared(), rclcpp::Clock().now()); + message_filters::MessageEvent evt(std::make_shared(), rclcpp::Clock().now()); seq.add(evt); - while (!h.event_.getMessage()) - { + while (!h.event_.getMessage()) { rclcpp::Rate(100).sleep(); rclcpp::spin_some(nh); } @@ -144,7 +142,8 @@ TEST(TimeSequencer, eventInEventOut) EXPECT_EQ(h.event_.getMessage(), evt.getMessage()); } -int main(int argc, char **argv){ +int main(int argc, char **argv) +{ testing::InitGoogleTest(&argc, argv); rclcpp::init(argc, argv); diff --git a/test/time_synchronizer_unittest.cpp b/test/time_synchronizer_unittest.cpp index 920f829..f3c5607 100644 --- a/test/time_synchronizer_unittest.cpp +++ b/test/time_synchronizer_unittest.cpp @@ -34,19 +34,18 @@ #include +#include + #include "message_filters/time_synchronizer.h" #include "message_filters/pass_through.h" #include "message_filters/message_traits.h" #include -using namespace message_filters; - struct Header { rclcpp::Time stamp; }; - struct Msg { Header header; @@ -94,50 +93,50 @@ class Helper TEST(TimeSynchronizer, compile2) { - NullFilter f0, f1; - TimeSynchronizer sync(f0, f1, 1); + message_filters::NullFilter f0, f1; + message_filters::TimeSynchronizer sync(f0, f1, 1); } TEST(TimeSynchronizer, compile3) { - NullFilter f0, f1, f2; - TimeSynchronizer sync(f0, f1, f2, 1); + message_filters::NullFilter f0, f1, f2; + message_filters::TimeSynchronizer sync(f0, f1, f2, 1); } TEST(TimeSynchronizer, compile4) { - NullFilter f0, f1, f2, f3; - TimeSynchronizer sync(f0, f1, f2, f3, 1); + message_filters::NullFilter f0, f1, f2, f3; + message_filters::TimeSynchronizer sync(f0, f1, f2, f3, 1); } TEST(TimeSynchronizer, compile5) { - NullFilter f0, f1, f2, f3, f4; - TimeSynchronizer sync(f0, f1, f2, f3, f4, 1); + message_filters::NullFilter f0, f1, f2, f3, f4; + message_filters::TimeSynchronizer sync(f0, f1, f2, f3, f4, 1); } TEST(TimeSynchronizer, compile6) { - NullFilter f0, f1, f2, f3, f4, f5; - TimeSynchronizer sync(f0, f1, f2, f3, f4, f5, 1); + message_filters::NullFilter f0, f1, f2, f3, f4, f5; + message_filters::TimeSynchronizer sync(f0, f1, f2, f3, f4, f5, 1); } TEST(TimeSynchronizer, compile7) { - NullFilter f0, f1, f2, f3, f4, f5, f6; - TimeSynchronizer sync(f0, f1, f2, f3, f4, f5, f6, 1); + message_filters::NullFilter f0, f1, f2, f3, f4, f5, f6; + message_filters::TimeSynchronizer sync(f0, f1, f2, f3, f4, f5, f6, 1); } TEST(TimeSynchronizer, compile8) { - NullFilter f0, f1, f2, f3, f4, f5, f6, f7; - TimeSynchronizer sync(f0, f1, f2, f3, f4, f5, f6, f7, 1); + message_filters::NullFilter f0, f1, f2, f3, f4, f5, f6, f7; + message_filters::TimeSynchronizer sync(f0, f1, f2, f3, f4, f5, f6, f7, 1); } TEST(TimeSynchronizer, compile9) { - NullFilter f0, f1, f2, f3, f4, f5, f6, f7, f8; - TimeSynchronizer sync(f0, f1, f2, f3, f4, f5, f6, f7, f8, 1); + message_filters::NullFilter f0, f1, f2, f3, f4, f5, f6, f7, f8; + message_filters::TimeSynchronizer sync(f0, f1, f2, f3, f4, f5, f6, f7, f8, 1); } void function2(const MsgConstPtr&, const MsgConstPtr&) {} @@ -147,53 +146,53 @@ void function5(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const void function6(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} void function7(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} void function8(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} -void function9(const MsgConstPtr&, MsgConstPtr, const MsgPtr&, MsgPtr, const Msg&, Msg, const MessageEvent&, const MessageEvent&, const MsgConstPtr&) {} +void function9(const MsgConstPtr&, MsgConstPtr, const MsgPtr&, MsgPtr, const Msg&, Msg, const message_filters::MessageEvent&, const message_filters::MessageEvent&, const MsgConstPtr&) {} TEST(TimeSynchronizer, compileFunction2) { - TimeSynchronizer sync(1); + message_filters::TimeSynchronizer sync(1); sync.registerCallback(function2); } TEST(TimeSynchronizer, compileFunction3) { - TimeSynchronizer sync(1); + message_filters::TimeSynchronizer sync(1); sync.registerCallback(function3); } TEST(TimeSynchronizer, compileFunction4) { - TimeSynchronizer sync(1); + message_filters::TimeSynchronizer sync(1); sync.registerCallback(function4); } TEST(TimeSynchronizer, compileFunction5) { - TimeSynchronizer sync(1); + message_filters::TimeSynchronizer sync(1); sync.registerCallback(function5); } TEST(TimeSynchronizer, compileFunction6) { - TimeSynchronizer sync(1); + message_filters::TimeSynchronizer sync(1); sync.registerCallback(function6); } TEST(TimeSynchronizer, compileFunction7) { - TimeSynchronizer sync(1); + message_filters::TimeSynchronizer sync(1); sync.registerCallback(function7); } TEST(TimeSynchronizer, compileFunction8) { - TimeSynchronizer sync(1); + message_filters::TimeSynchronizer sync(1); sync.registerCallback(function8); } TEST(TimeSynchronizer, compileFunction9) { - TimeSynchronizer sync(1); + message_filters::TimeSynchronizer sync(1); sync.registerCallback(function9); } @@ -205,62 +204,62 @@ struct MethodHelper void method5(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} void method6(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} void method7(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} - void method8(const MsgConstPtr&, MsgConstPtr, const MsgPtr&, MsgPtr, const Msg&, Msg, const MessageEvent&, const MessageEvent&) {} + void method8(const MsgConstPtr&, MsgConstPtr, const MsgPtr&, MsgPtr, const Msg&, Msg, const message_filters::MessageEvent&, const message_filters::MessageEvent&) {} // Can only do 8 here because the object instance counts as a parameter and bind only supports 9 }; TEST(TimeSynchronizer, compileMethod2) { MethodHelper h; - TimeSynchronizer sync(1); + message_filters::TimeSynchronizer sync(1); sync.registerCallback(&MethodHelper::method2, &h); } TEST(TimeSynchronizer, compileMethod3) { MethodHelper h; - TimeSynchronizer sync(1); + message_filters::TimeSynchronizer sync(1); sync.registerCallback(&MethodHelper::method3, &h); } TEST(TimeSynchronizer, compileMethod4) { MethodHelper h; - TimeSynchronizer sync(1); + message_filters::TimeSynchronizer sync(1); sync.registerCallback(&MethodHelper::method4, &h); } TEST(TimeSynchronizer, compileMethod5) { MethodHelper h; - TimeSynchronizer sync(1); + message_filters::TimeSynchronizer sync(1); sync.registerCallback(&MethodHelper::method5, &h); } TEST(TimeSynchronizer, compileMethod6) { MethodHelper h; - TimeSynchronizer sync(1); + message_filters::TimeSynchronizer sync(1); sync.registerCallback(&MethodHelper::method6, &h); } TEST(TimeSynchronizer, compileMethod7) { MethodHelper h; - TimeSynchronizer sync(1); + message_filters::TimeSynchronizer sync(1); sync.registerCallback(&MethodHelper::method7, &h); } TEST(TimeSynchronizer, compileMethod8) { MethodHelper h; - TimeSynchronizer sync(1); + message_filters::TimeSynchronizer sync(1); sync.registerCallback(&MethodHelper::method8, &h); } TEST(TimeSynchronizer, immediate2) { - TimeSynchronizer sync(1); + message_filters::TimeSynchronizer sync(1); Helper h; sync.registerCallback(std::bind(&Helper::cb, &h)); MsgPtr m(std::make_shared()); @@ -274,7 +273,7 @@ TEST(TimeSynchronizer, immediate2) TEST(TimeSynchronizer, immediate3) { - TimeSynchronizer sync(1); + message_filters::TimeSynchronizer sync(1); Helper h; sync.registerCallback(std::bind(&Helper::cb, &h)); MsgPtr m(std::make_shared()); @@ -290,7 +289,7 @@ TEST(TimeSynchronizer, immediate3) TEST(TimeSynchronizer, immediate4) { - TimeSynchronizer sync(1); + message_filters::TimeSynchronizer sync(1); Helper h; sync.registerCallback(std::bind(&Helper::cb, &h)); MsgPtr m(std::make_shared()); @@ -308,7 +307,7 @@ TEST(TimeSynchronizer, immediate4) TEST(TimeSynchronizer, immediate5) { - TimeSynchronizer sync(1); + message_filters::TimeSynchronizer sync(1); Helper h; sync.registerCallback(std::bind(&Helper::cb, &h)); MsgPtr m(std::make_shared()); @@ -328,7 +327,7 @@ TEST(TimeSynchronizer, immediate5) TEST(TimeSynchronizer, immediate6) { - TimeSynchronizer sync(1); + message_filters::TimeSynchronizer sync(1); Helper h; sync.registerCallback(std::bind(&Helper::cb, &h)); MsgPtr m(std::make_shared()); @@ -350,7 +349,7 @@ TEST(TimeSynchronizer, immediate6) TEST(TimeSynchronizer, immediate7) { - TimeSynchronizer sync(1); + message_filters::TimeSynchronizer sync(1); Helper h; sync.registerCallback(std::bind(&Helper::cb, &h)); MsgPtr m(std::make_shared()); @@ -374,7 +373,7 @@ TEST(TimeSynchronizer, immediate7) TEST(TimeSynchronizer, immediate8) { - TimeSynchronizer sync(1); + message_filters::TimeSynchronizer sync(1); Helper h; sync.registerCallback(std::bind(&Helper::cb, &h)); MsgPtr m(std::make_shared()); @@ -400,7 +399,7 @@ TEST(TimeSynchronizer, immediate8) TEST(TimeSynchronizer, immediate9) { - TimeSynchronizer sync(1); + message_filters::TimeSynchronizer sync(1); Helper h; sync.registerCallback(std::bind(&Helper::cb, &h)); MsgPtr m(std::make_shared()); @@ -432,7 +431,7 @@ TEST(TimeSynchronizer, immediate9) ////////////////////////////////////////////////////////////////////////////////////////////////// TEST(TimeSynchronizer, multipleTimes) { - TimeSynchronizer sync(2); + message_filters::TimeSynchronizer sync(2); Helper h; sync.registerCallback(std::bind(&Helper::cb, &h)); MsgPtr m(std::make_shared()); @@ -453,7 +452,7 @@ TEST(TimeSynchronizer, multipleTimes) TEST(TimeSynchronizer, queueSize) { - TimeSynchronizer sync(1); + message_filters::TimeSynchronizer sync(1); Helper h; sync.registerCallback(std::bind(&Helper::cb, &h)); MsgPtr m(std::make_shared()); @@ -479,7 +478,7 @@ TEST(TimeSynchronizer, queueSize) TEST(TimeSynchronizer, dropCallback) { - TimeSynchronizer sync(1); + message_filters::TimeSynchronizer sync(1); Helper h; sync.registerCallback(std::bind(&Helper::cb, &h)); sync.registerDropCallback(std::bind(&Helper::dropcb, &h)); @@ -496,22 +495,22 @@ TEST(TimeSynchronizer, dropCallback) struct EventHelper { - void callback(const MessageEvent& e1, const MessageEvent& e2) + void callback(const message_filters::MessageEvent & e1, const message_filters::MessageEvent & e2) { e1_ = e1; e2_ = e2; } - MessageEvent e1_; - MessageEvent e2_; + message_filters::MessageEvent e1_; + message_filters::MessageEvent e2_; }; TEST(TimeSynchronizer, eventInEventOut) { - TimeSynchronizer sync(2); + message_filters::TimeSynchronizer sync(2); EventHelper h; sync.registerCallback(&EventHelper::callback, &h); - MessageEvent evt(std::make_shared(), rclcpp::Time(4, 0)); + message_filters::MessageEvent evt(std::make_shared(), rclcpp::Time(4, 0)); sync.add<0>(evt); sync.add<1>(evt); @@ -524,8 +523,8 @@ TEST(TimeSynchronizer, eventInEventOut) TEST(TimeSynchronizer, connectConstructor) { - PassThrough pt1, pt2; - TimeSynchronizer sync(pt1, pt2, 1); + message_filters::PassThrough pt1, pt2; + message_filters::TimeSynchronizer sync(pt1, pt2, 1); Helper h; sync.registerCallback(std::bind(&Helper::cb, &h)); MsgPtr m(std::make_shared()); @@ -537,9 +536,8 @@ TEST(TimeSynchronizer, connectConstructor) ASSERT_EQ(h.count_, 1); } -//TEST(TimeSynchronizer, connectToSimple) - -int main(int argc, char **argv){ +int main(int argc, char **argv) +{ testing::InitGoogleTest(&argc, argv); rclcpp::init(argc, argv); return RUN_ALL_TESTS();