diff --git a/CMakeLists.txt b/CMakeLists.txt index 61de8477..53589f0e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -108,6 +108,11 @@ if(BUILD_TESTING) target_link_libraries(${PROJECT_NAME}-test_approximate_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) target_link_libraries(${PROJECT_NAME}-test_fuzz ${PROJECT_NAME}) 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(); +} + +