From c07af50c5f88f6ac677a407080d9275a79fbd165 Mon Sep 17 00:00:00 2001 From: "Joao C. Monteiro" Date: Sat, 31 Aug 2019 14:26:23 -0300 Subject: [PATCH 1/2] add policy to store the latest messages --- CMakeLists.txt | 4 + .../message_filters/sync_policies/latest.h | 130 +++++++++++ .../sync_policies/latest_stamped.h | 142 ++++++++++++ test/test_latest_policy.cpp | 207 ++++++++++++++++++ 4 files changed, 483 insertions(+) create mode 100644 include/message_filters/sync_policies/latest.h create mode 100644 include/message_filters/sync_policies/latest_stamped.h create mode 100644 test/test_latest_policy.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 0d0d4316..5ad642a0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -119,6 +119,10 @@ if(BUILD_TESTING) if(TARGET ${PROJECT_NAME}-test_latest_time_policy) target_link_libraries(${PROJECT_NAME}-test_latest_time_policy ${PROJECT_NAME}) endif() + ament_add_gtest(${PROJECT_NAME}-test_latest_policy test/test_latest_policy.cpp) + if(TARGET ${PROJECT_NAME}-test_latest_policy) + target_link_libraries(${PROJECT_NAME}-test_latest_policy ${PROJECT_NAME}) + endif() ament_add_gtest(${PROJECT_NAME}-test_fuzz test/test_fuzz.cpp SKIP_TEST) if(TARGET ${PROJECT_NAME}-test_fuzz) diff --git a/include/message_filters/sync_policies/latest.h b/include/message_filters/sync_policies/latest.h new file mode 100644 index 00000000..f6c0ab7e --- /dev/null +++ b/include/message_filters/sync_policies/latest.h @@ -0,0 +1,130 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2009, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +#ifndef MESSAGE_FILTERS__SYNC_Latest_H_ +#define MESSAGE_FILTERS__SYNC_Latest_H_ + +#include + +#include + +#include "message_filters/connection.h" +#include "message_filters/message_traits.h" +#include "message_filters/null_types.h" +#include "message_filters/signal9.h" +#include "message_filters/synchronizer.h" + +namespace message_filters +{ +namespace sync_policies +{ + +/** + * The Latest policy stores the last received messages and DO NOT call any callback automatically. + * It is up to the programmer to call #call when it is time to process the information. This policy class + * is meant to be used in networks where information is processed periodically. + */ +template +struct Latest : public PolicyBase +{ + typedef Synchronizer Sync; + typedef PolicyBase Super; + typedef typename Super::Messages Messages; + typedef typename Super::Signal Signal; + typedef typename Super::Events Events; + typedef typename Super::RealTypeCount RealTypeCount; + typedef typename Super::M0Event M0Event; + typedef typename Super::M1Event M1Event; + typedef typename Super::M2Event M2Event; + typedef typename Super::M3Event M3Event; + typedef typename Super::M4Event M4Event; + typedef typename Super::M5Event M5Event; + typedef typename Super::M6Event M6Event; + typedef typename Super::M7Event M7Event; + typedef typename Super::M8Event M8Event; + + Latest() + : parent_(0) + { + } + + Latest(const Latest& e) + { + *this = e; + } + + Latest& operator=(const Latest& rhs) + { + parent_ = rhs.parent_; + events_ = rhs.events_; + + return *this; + } + + void initParent(Sync* parent) + { + parent_ = parent; + } + + template + void add(const typename std::tuple_element::type& evt) + { + RCUTILS_ASSERT(parent_); + + namespace mt = message_filters::message_traits; + + std::lock_guard lock(mutex_); + std::get(events_) = evt; + } + + void call() const + { + parent_->signal(std::get<0>(events_), std::get<1>(events_), std::get<2>(events_), + std::get<3>(events_), std::get<4>(events_), std::get<5>(events_), + std::get<6>(events_), std::get<7>(events_), std::get<8>(events_)); + } + +private: + Sync* parent_; + + Events events_; + + std::mutex mutex_; +}; + +} // namespace sync_policies +} // namespace message_filters + +#endif // MESSAGE_FILTERS__SYNC_Latest_H_ diff --git a/include/message_filters/sync_policies/latest_stamped.h b/include/message_filters/sync_policies/latest_stamped.h new file mode 100644 index 00000000..827a5f36 --- /dev/null +++ b/include/message_filters/sync_policies/latest_stamped.h @@ -0,0 +1,142 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2009, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +#ifndef MESSAGE_FILTERS__SYNC_LatestStamped_H_ +#define MESSAGE_FILTERS__SYNC_LatestStamped_H_ + +#include + +#include + +#include "message_filters/connection.h" +#include "message_filters/message_traits.h" +#include "message_filters/null_types.h" +#include "message_filters/signal9.h" +#include "message_filters/synchronizer.h" + +namespace message_filters +{ +namespace sync_policies +{ + +/** + * The LatestStamped policy stores the LatestStamped received messages (based on their timestamp) and DO NOT call any callback automatically. + * It is up to the programmer to call #call when it is time to process the information. This policy class + * is meant to be used in networks where information is processed periodically. + */ +template +struct LatestStamped : public PolicyBase +{ + typedef Synchronizer Sync; + typedef PolicyBase Super; + typedef typename Super::Messages Messages; + typedef typename Super::Signal Signal; + typedef typename Super::Events Events; + typedef typename Super::RealTypeCount RealTypeCount; + typedef typename Super::M0Event M0Event; + typedef typename Super::M1Event M1Event; + typedef typename Super::M2Event M2Event; + typedef typename Super::M3Event M3Event; + typedef typename Super::M4Event M4Event; + typedef typename Super::M5Event M5Event; + typedef typename Super::M6Event M6Event; + typedef typename Super::M7Event M7Event; + typedef typename Super::M8Event M8Event; + + LatestStamped() + : parent_(0) + { + } + + LatestStamped(const LatestStamped& e) + { + *this = e; + } + + LatestStamped& operator=(const LatestStamped& rhs) + { + parent_ = rhs.parent_; + events_ = rhs.events_; + + return *this; + } + + void initParent(Sync* parent) + { + parent_ = parent; + } + + template + void add(const typename std::tuple_element::type& evt) + { + RCUTILS_ASSERT(parent_); + + namespace mt = message_filters::message_traits; + + std::lock_guard lock(mutex_); + + typedef typename std::tuple_element::type MessageType; + auto& evt_old = std::get(events_); + bool has_old = (bool)evt_old.getMessage(); + if (has_old) + { + rclcpp::Time stamp_new = mt::TimeStamp::value(*evt.getMessage()); + rclcpp::Time stamp_old = mt::TimeStamp::value(*evt_old.getMessage()); + if (stamp_new > stamp_old) + evt_old = evt; + } + else + evt_old = evt; + } + + void call() const + { + parent_->signal(std::get<0>(events_), std::get<1>(events_), std::get<2>(events_), + std::get<3>(events_), std::get<4>(events_), std::get<5>(events_), + std::get<6>(events_), std::get<7>(events_), std::get<8>(events_)); + } + +private: + Sync* parent_; + + Events events_; + + std::mutex mutex_; +}; + +} // namespace sync_policies +} // namespace message_filters + +#endif // MESSAGE_FILTERS__SYNC_LatestStamped_H_ diff --git a/test/test_latest_policy.cpp b/test/test_latest_policy.cpp new file mode 100644 index 00000000..471471d7 --- /dev/null +++ b/test/test_latest_policy.cpp @@ -0,0 +1,207 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +#include + +#include +#include "message_filters/synchronizer.h" +#include "message_filters/sync_policies/latest.h" +#include "message_filters/sync_policies/latest_stamped.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; + Msg(rclcpp::Time stamp = rclcpp::Time(0,0)) + { + header.stamp = stamp; + } +}; +typedef std::shared_ptr MsgPtr; +typedef std::shared_ptr MsgConstPtr; + +struct MsgHeaderless +{ + int data; + MsgHeaderless(int data = -1) : data(data) + { + } +}; +typedef std::shared_ptr MsgHeaderlessPtr; +typedef std::shared_ptr MsgHeaderlessConstPtr; + +namespace message_filters +{ +namespace message_traits +{ +template<> +struct TimeStamp +{ + static rclcpp::Time value(const Msg& m) + { + return m.header.stamp; + } +}; +} +} + +struct EventHelper +{ + void callback(const MessageEvent& e1, const MessageEvent& e2, const MessageEvent& e3) + { + e1_ = e1; + e2_ = e2; + e3_ = e3; + } + + MessageEvent e1_; + MessageEvent e2_; + MessageEvent e3_; +}; + +struct EventHelperHeaderless +{ + void callback(const MessageEvent& e1, const MessageEvent& e2, const MessageEvent& e3) + { + e1_ = e1; + e2_ = e2; + e3_ = e3; + } + + MessageEvent e1_; + MessageEvent e2_; + MessageEvent e3_; +}; + +typedef LatestStamped Policy; +typedef Synchronizer Sync; + +TEST(LatestStamped, eventInEventOut) +{ + Sync sync; + EventHelper helper; + sync.registerCallback(&EventHelper::callback, &helper); + MessageEvent evt_1(std::make_shared( rclcpp::Time(1, 0) )); + MessageEvent evt_2(std::make_shared( rclcpp::Time(2, 1) )); + MessageEvent evt_3(std::make_shared( rclcpp::Time(2, 2) )); + + auto assert_events_1and2 = [&helper, &evt_1, &evt_2, &evt_3] (void) -> void + { + ASSERT_TRUE(helper.e1_.getMessage()); + ASSERT_TRUE(helper.e2_.getMessage()); + ASSERT_TRUE(helper.e1_ == evt_3); + ASSERT_TRUE(helper.e2_ == evt_2); + ASSERT_FALSE(helper.e1_ == evt_1); + ASSERT_FALSE(helper.e1_ == evt_2); + ASSERT_FALSE(helper.e2_ == evt_1); + ASSERT_FALSE(helper.e2_ == evt_3); + }; + + sync.add<0>(evt_1); + sync.add<0>(evt_2); + sync.add<0>(evt_3); + sync.add<1>(evt_2); + sync.add<1>(evt_1); + + sync.getPolicy()->call(); + assert_events_1and2(); + ASSERT_FALSE(helper.e3_.getMessage()); + + sync.add<2>(evt_1); + sync.getPolicy()->call(); + assert_events_1and2(); + ASSERT_TRUE(helper.e3_.getMessage()); + ASSERT_TRUE(helper.e3_ == evt_1); + ASSERT_FALSE(helper.e3_ == evt_2); + ASSERT_FALSE(helper.e3_ == evt_3); +} + + +typedef Latest PolicyHeaderless; +typedef Synchronizer SyncHeaderless; + +TEST(Latest, eventInEventOut) +{ + SyncHeaderless sync; + EventHelperHeaderless helper; + sync.registerCallback(&EventHelperHeaderless::callback, &helper); + + MessageEvent evt_1(std::make_shared( rclcpp::Time(1, 0) )); + MessageEvent evt_2(std::make_shared( rclcpp::Time(1, 0) )); + MessageEvent evt_3(std::make_shared( rclcpp::Time(1, 0) )); + + MessageEvent evt_1_hl(std::make_shared(1)); + MessageEvent evt_2_hl(std::make_shared(2)); + + sync.getPolicy()->call(); + ASSERT_FALSE(helper.e1_.getMessage()); + ASSERT_FALSE(helper.e2_.getMessage()); + ASSERT_FALSE(helper.e3_.getMessage()); + + sync.add<0>(evt_1); + sync.add<0>(evt_2); + sync.add<1>(evt_1_hl); + sync.add<1>(evt_2_hl); + sync.add<2>(evt_1); + sync.add<2>(evt_2); + sync.add<2>(evt_3); + + sync.getPolicy()->call(); + ASSERT_TRUE(helper.e1_ == evt_2); + ASSERT_TRUE(helper.e2_ == evt_2_hl); + ASSERT_TRUE(helper.e3_ == evt_3); + // + ASSERT_FALSE(helper.e1_ == evt_1); + ASSERT_FALSE(helper.e1_ == evt_3); + ASSERT_FALSE(helper.e2_ == evt_1_hl); + ASSERT_FALSE(helper.e3_ == evt_1); + ASSERT_FALSE(helper.e3_ == evt_2); +} + +int main(int argc, char **argv){ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + return RUN_ALL_TESTS(); +} + + From 473a263be84ed01edfbb8ae95cbbdfc912a321f7 Mon Sep 17 00:00:00 2001 From: lizhensheng Date: Fri, 12 Jan 2024 15:30:55 +0800 Subject: [PATCH 2/2] code refactor and clean. --- CMakeLists.txt | 7 +++-- .../sync_policies/{latest.h => manual_call.h} | 21 ++++++------- ...latest_stamped.h => manual_call_stamped.h} | 30 ++++++++++--------- ...policy.cpp => test_manual_call_policy.cpp} | 14 ++++----- 4 files changed, 37 insertions(+), 35 deletions(-) rename include/message_filters/sync_policies/{latest.h => manual_call.h} (88%) rename include/message_filters/sync_policies/{latest_stamped.h => manual_call_stamped.h} (85%) rename test/{test_latest_policy.cpp => test_manual_call_policy.cpp} (95%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5ad642a0..bb3ef55e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -119,9 +119,10 @@ if(BUILD_TESTING) if(TARGET ${PROJECT_NAME}-test_latest_time_policy) target_link_libraries(${PROJECT_NAME}-test_latest_time_policy ${PROJECT_NAME}) endif() - ament_add_gtest(${PROJECT_NAME}-test_latest_policy test/test_latest_policy.cpp) - if(TARGET ${PROJECT_NAME}-test_latest_policy) - target_link_libraries(${PROJECT_NAME}-test_latest_policy ${PROJECT_NAME}) + + ament_add_gtest(${PROJECT_NAME}-test_manual_call_policy test/test_manual_call_policy.cpp) + if(TARGET ${PROJECT_NAME}-test_manual_call_policy) + target_link_libraries(${PROJECT_NAME}-test_manual_call_policy ${PROJECT_NAME} rclcpp::rclcpp) endif() ament_add_gtest(${PROJECT_NAME}-test_fuzz test/test_fuzz.cpp SKIP_TEST) diff --git a/include/message_filters/sync_policies/latest.h b/include/message_filters/sync_policies/manual_call.h similarity index 88% rename from include/message_filters/sync_policies/latest.h rename to include/message_filters/sync_policies/manual_call.h index f6c0ab7e..6ec95a0c 100644 --- a/include/message_filters/sync_policies/latest.h +++ b/include/message_filters/sync_policies/manual_call.h @@ -32,10 +32,11 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#ifndef MESSAGE_FILTERS__SYNC_Latest_H_ -#define MESSAGE_FILTERS__SYNC_Latest_H_ +#ifndef MESSAGE_FILTERS__SYNC_POLICIES__MANUAL_CALL_H_ +#define MESSAGE_FILTERS__SYNC_POLICIES__MANUAL_CALL_H_ -#include +#include +#include #include @@ -51,15 +52,15 @@ namespace sync_policies { /** - * The Latest policy stores the last received messages and DO NOT call any callback automatically. + * The ManualCall policy stores the last received messages and DO NOT call any callback automatically. * It is up to the programmer to call #call when it is time to process the information. This policy class * is meant to be used in networks where information is processed periodically. */ template -struct Latest : public PolicyBase +struct ManualCall : public PolicyBase { - typedef Synchronizer Sync; + typedef Synchronizer Sync; typedef PolicyBase Super; typedef typename Super::Messages Messages; typedef typename Super::Signal Signal; @@ -75,17 +76,17 @@ struct Latest : public PolicyBase typedef typename Super::M7Event M7Event; typedef typename Super::M8Event M8Event; - Latest() + ManualCall() : parent_(0) { } - Latest(const Latest& e) + ManualCall(const ManualCall& e) { *this = e; } - Latest& operator=(const Latest& rhs) + ManualCall& operator=(const ManualCall& rhs) { parent_ = rhs.parent_; events_ = rhs.events_; @@ -127,4 +128,4 @@ struct Latest : public PolicyBase } // namespace sync_policies } // namespace message_filters -#endif // MESSAGE_FILTERS__SYNC_Latest_H_ +#endif // MESSAGE_FILTERS__SYNC_POLICIES__MANUAL_CALL_H_ diff --git a/include/message_filters/sync_policies/latest_stamped.h b/include/message_filters/sync_policies/manual_call_stamped.h similarity index 85% rename from include/message_filters/sync_policies/latest_stamped.h rename to include/message_filters/sync_policies/manual_call_stamped.h index 827a5f36..5330fc3a 100644 --- a/include/message_filters/sync_policies/latest_stamped.h +++ b/include/message_filters/sync_policies/manual_call_stamped.h @@ -32,10 +32,11 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#ifndef MESSAGE_FILTERS__SYNC_LatestStamped_H_ -#define MESSAGE_FILTERS__SYNC_LatestStamped_H_ +#ifndef MESSAGE_FILTERS__SYNC_POLICIES__MANUAL_CALL_STAMPED_H_ +#define MESSAGE_FILTERS__SYNC_POLICIES__MANUAL_CALL_STAMPED_H_ -#include +#include +#include #include @@ -51,15 +52,15 @@ namespace sync_policies { /** - * The LatestStamped policy stores the LatestStamped received messages (based on their timestamp) and DO NOT call any callback automatically. + * The ManualCallStamped policy stores the ManualCallStamped received messages (based on their timestamp) and DO NOT call any callback automatically. * It is up to the programmer to call #call when it is time to process the information. This policy class * is meant to be used in networks where information is processed periodically. */ template -struct LatestStamped : public PolicyBase +struct ManualCallStamped : public PolicyBase { - typedef Synchronizer Sync; + typedef Synchronizer Sync; typedef PolicyBase Super; typedef typename Super::Messages Messages; typedef typename Super::Signal Signal; @@ -75,17 +76,17 @@ struct LatestStamped : public PolicyBase typedef typename Super::M7Event M7Event; typedef typename Super::M8Event M8Event; - LatestStamped() + ManualCallStamped() : parent_(0) { } - LatestStamped(const LatestStamped& e) + ManualCallStamped(const ManualCallStamped& e) { *this = e; } - LatestStamped& operator=(const LatestStamped& rhs) + ManualCallStamped& operator=(const ManualCallStamped& rhs) { parent_ = rhs.parent_; events_ = rhs.events_; @@ -109,16 +110,17 @@ struct LatestStamped : public PolicyBase typedef typename std::tuple_element::type MessageType; auto& evt_old = std::get(events_); - bool has_old = (bool)evt_old.getMessage(); - if (has_old) + if (evt_old.getMessage() != nullptr) { rclcpp::Time stamp_new = mt::TimeStamp::value(*evt.getMessage()); rclcpp::Time stamp_old = mt::TimeStamp::value(*evt_old.getMessage()); - if (stamp_new > stamp_old) + if (stamp_new > stamp_old) { evt_old = evt; + } } - else + else { evt_old = evt; + } } void call() const @@ -139,4 +141,4 @@ struct LatestStamped : public PolicyBase } // namespace sync_policies } // namespace message_filters -#endif // MESSAGE_FILTERS__SYNC_LatestStamped_H_ +#endif // MESSAGE_FILTERS__SYNC_POLICIES__MANUAL_CALL_STAMPED_H_ diff --git a/test/test_latest_policy.cpp b/test/test_manual_call_policy.cpp similarity index 95% rename from test/test_latest_policy.cpp rename to test/test_manual_call_policy.cpp index 471471d7..352a4e31 100644 --- a/test/test_latest_policy.cpp +++ b/test/test_manual_call_policy.cpp @@ -36,8 +36,8 @@ #include #include "message_filters/synchronizer.h" -#include "message_filters/sync_policies/latest.h" -#include "message_filters/sync_policies/latest_stamped.h" +#include "message_filters/sync_policies/manual_call.h" +#include "message_filters/sync_policies/manual_call_stamped.h" #include "message_filters/sync_policies/exact_time.h" using namespace message_filters; @@ -113,10 +113,10 @@ struct EventHelperHeaderless MessageEvent e3_; }; -typedef LatestStamped Policy; +typedef ManualCallStamped Policy; typedef Synchronizer Sync; -TEST(LatestStamped, eventInEventOut) +TEST(ManualCallStamped, eventInEventOut) { Sync sync; EventHelper helper; @@ -157,10 +157,10 @@ TEST(LatestStamped, eventInEventOut) } -typedef Latest PolicyHeaderless; +typedef ManualCall PolicyHeaderless; typedef Synchronizer SyncHeaderless; -TEST(Latest, eventInEventOut) +TEST(ManualCall, eventInEventOut) { SyncHeaderless sync; EventHelperHeaderless helper; @@ -203,5 +203,3 @@ int main(int argc, char **argv){ rclcpp::init(argc, argv); return RUN_ALL_TESTS(); } - -