diff --git a/include/message_filters/message_event.h b/include/message_filters/message_event.h index c0c92b8..ecb5005 100644 --- a/include/message_filters/message_event.h +++ b/include/message_filters/message_event.h @@ -32,19 +32,14 @@ #ifndef MESSAGE_FILTERS__MESSAGE_EVENT_H_ #define MESSAGE_FILTERS__MESSAGE_EVENT_H_ -#include +#include #include #include #include +#include #include -#ifndef RCUTILS_ASSERT -// TODO(tfoote) remove this after it's implemented upstream -// https://github.com/ros2/rcutils/pull/112 -#define RCUTILS_ASSERT assert -#endif - namespace message_filters { typedef std::map M_string; @@ -59,13 +54,7 @@ struct DefaultMessageCreator } }; -/* -template -ROS_DEPRECATED inline std::shared_ptr defaultMessageCreateFunction() -{ - return DefaultMessageCreator()(); -} -*/ + /** * \brief Event type for subscriptions, const message_filters::MessageEvent & can be used in your callback instead of const std::shared_ptr& * @@ -225,7 +214,7 @@ class MessageEvent return message_copy_; } - RCUTILS_ASSERT(create_); + assert(create_); message_copy_ = create_(); *message_copy_ = *message_; diff --git a/include/message_filters/sync_policies/approximate_epsilon_time.h b/include/message_filters/sync_policies/approximate_epsilon_time.h index b377123..1ae6630 100644 --- a/include/message_filters/sync_policies/approximate_epsilon_time.h +++ b/include/message_filters/sync_policies/approximate_epsilon_time.h @@ -105,7 +105,7 @@ class ApproximateEpsilonTime : public PolicyBase void add(const typename std::tuple_element::type & evt) { - RCUTILS_ASSERT(parent_); + assert(parent_); std::lock_guard lock(mutex_); diff --git a/include/message_filters/sync_policies/approximate_time.h b/include/message_filters/sync_policies/approximate_time.h index 931cc23..f89d3d9 100644 --- a/include/message_filters/sync_policies/approximate_time.h +++ b/include/message_filters/sync_policies/approximate_time.h @@ -33,6 +33,7 @@ #include +#include #include #include #include @@ -47,19 +48,6 @@ #include "message_filters/signal9.h" #include "message_filters/synchronizer.h" - -#ifndef RCUTILS_ASSERT -// TODO(tfoote) remove this after it's implemented upstream -// https://github.com/ros2/rcutils/pull/112 -#define RCUTILS_ASSERT assert -#endif -#ifndef RCUTILS_BREAK -#include -// TODO(tfoote) remove this after it's implemented upstream -// https://github.com/ros2/rcutils/pull/112 -#define RCUTILS_BREAK std::abort -#endif - namespace message_filters { namespace sync_policies @@ -122,7 +110,7 @@ struct ApproximateTime : public PolicyBase { // The synchronizer will tend to drop many messages with a queue size of 1. // At least 2 is recommended. - RCUTILS_ASSERT(queue_size_ > 0); + assert(queue_size_ > 0); } ApproximateTime(const ApproximateTime & e) @@ -165,7 +153,7 @@ struct ApproximateTime : public PolicyBase } std::deque::type> & deque = std::get(deques_); std::vector::type> & v = std::get(past_); - RCUTILS_ASSERT(!deque.empty()); + assert(!deque.empty()); const typename std::tuple_element::type & msg = *(deque.back()).getMessage(); rclcpp::Time msg_time = mt::TimeStamp::type>::value(msg); @@ -237,7 +225,7 @@ struct ApproximateTime : public PolicyBase recover<7>(); recover<8>(); // Drop the oldest message in the offending topic - RCUTILS_ASSERT(!deque.empty()); + assert(!deque.empty()); deque.pop_front(); has_dropped_messages_[i] = true; if (pivot_ != NO_PIVOT) { @@ -254,7 +242,7 @@ struct ApproximateTime : public PolicyBase { // For correctness we only need age_penalty > -1.0, // but most likely a negative age_penalty is a mistake. - RCUTILS_ASSERT(age_penalty >= 0); + assert(age_penalty >= 0); age_penalty_ = age_penalty; } @@ -262,7 +250,7 @@ struct ApproximateTime : public PolicyBase { // For correctness we only need age_penalty > -1.0, // but most likely a negative age_penalty is a mistake. - RCUTILS_ASSERT(lower_bound >= rclcpp::Duration(0, 0)); + assert(lower_bound >= rclcpp::Duration(0, 0)); inter_message_lower_bounds_[i] = lower_bound; } @@ -270,7 +258,7 @@ struct ApproximateTime : public PolicyBase { // For correctness we only need age_penalty > -1.0, // but most likely a negative age_penalty is a mistake. - RCUTILS_ASSERT(max_interval_duration >= rclcpp::Duration(0, 0)); + assert(max_interval_duration >= rclcpp::Duration(0, 0)); max_interval_duration_ = max_interval_duration; } @@ -280,7 +268,7 @@ struct ApproximateTime : public PolicyBase void dequeDeleteFront() { std::deque::type> & deque = std::get(deques_); - RCUTILS_ASSERT(!deque.empty()); + assert(!deque.empty()); deque.pop_front(); if (deque.empty()) { --num_non_empty_deques_; @@ -319,7 +307,7 @@ struct ApproximateTime : public PolicyBase dequeDeleteFront<8>(); break; default: - RCUTILS_BREAK(); + std::abort(); } } @@ -329,7 +317,7 @@ struct ApproximateTime : public PolicyBase { std::deque::type> & deque = std::get(deques_); std::vector::type> & vector = std::get(past_); - RCUTILS_ASSERT(!deque.empty()); + assert(!deque.empty()); vector.push_back(deque.front()); deque.pop_front(); if (deque.empty()) { @@ -368,7 +356,7 @@ struct ApproximateTime : public PolicyBase dequeMoveFrontToPast<8>(); break; default: - RCUTILS_BREAK(); + std::abort(); } } @@ -422,7 +410,7 @@ struct ApproximateTime : public PolicyBase std::vector::type> & v = std::get(past_); std::deque::type> & q = std::get(deques_); - RCUTILS_ASSERT(num_messages <= v.size()); + assert(num_messages <= v.size()); while (num_messages > 0) { q.push_front(v.back()); v.pop_back(); @@ -469,7 +457,7 @@ struct ApproximateTime : public PolicyBase v.pop_back(); } - RCUTILS_ASSERT(!q.empty()); + assert(!q.empty()); q.pop_front(); if (!q.empty()) { @@ -597,12 +585,12 @@ struct ApproximateTime : public PolicyBase if (i >= RealTypeCount::value) { return rclcpp::Time(0, 0); // Dummy return value } - RCUTILS_ASSERT(pivot_ != NO_PIVOT); + assert(pivot_ != NO_PIVOT); std::vector::type> & v = std::get(past_); std::deque::type> & q = std::get(deques_); if (q.empty()) { - RCUTILS_ASSERT(!v.empty()); // Because we have a candidate + assert(!v.empty()); // Because we have a candidate rclcpp::Time last_msg_time = mt::TimeStamp::type>::value( *(v.back()).getMessage()); @@ -715,7 +703,7 @@ struct ApproximateTime : public PolicyBase } } // INVARIANT: we have a candidate and pivot - RCUTILS_ASSERT(pivot_ != NO_PIVOT); + assert(pivot_ != NO_PIVOT); rclcpp::Duration age_check = (end_time - candidate_end_) * (1 + age_penalty_); if (start_index == pivot_) { // TODO(anyone): replace with start_time == pivot_time_ // We have exhausted all possible candidates for this pivot, we now can output the best one @@ -764,14 +752,14 @@ struct ApproximateTime : public PolicyBase recover<7>(num_virtual_moves[7]); recover<8>(num_virtual_moves[8]); (void)num_non_empty_deques_before_virtual_search; // unused variable warning stopper - RCUTILS_ASSERT(num_non_empty_deques_before_virtual_search == num_non_empty_deques_); + assert(num_non_empty_deques_before_virtual_search == num_non_empty_deques_); break; } // Note: we cannot reach this point with start_index == pivot_ since in that case we would // have start_time == pivot_time, in which case the two tests above are the negation // of each other, so that one must be true. Therefore the while loop always terminates. - RCUTILS_ASSERT(start_index != pivot_); - RCUTILS_ASSERT(start_time < pivot_time_); + assert(start_index != pivot_); + assert(start_time < pivot_time_); dequeMoveFrontToPast(start_index); num_virtual_moves[start_index]++; } // while(1) diff --git a/include/message_filters/sync_policies/exact_time.h b/include/message_filters/sync_policies/exact_time.h index def90cc..4f4e4c5 100644 --- a/include/message_filters/sync_policies/exact_time.h +++ b/include/message_filters/sync_policies/exact_time.h @@ -99,7 +99,7 @@ struct ExactTime : public PolicyBase template void add(const typename std::tuple_element::type & evt) { - RCUTILS_ASSERT(parent_); + assert(parent_); namespace mt = message_filters::message_traits; diff --git a/include/message_filters/sync_policies/latest_time.h b/include/message_filters/sync_policies/latest_time.h index ffcd016..6aaf2e6 100644 --- a/include/message_filters/sync_policies/latest_time.h +++ b/include/message_filters/sync_policies/latest_time.h @@ -147,7 +147,7 @@ struct LatestTime : public PolicyBase template void add(const typename std::tuple_element::type & evt) { - RCUTILS_ASSERT(parent_); + assert(parent_); std::lock_guard lock(data_mutex_); diff --git a/test/directed.py b/test/directed.py index 97afbcd..b01ce9e 100644 --- a/test/directed.py +++ b/test/directed.py @@ -41,12 +41,11 @@ # # wide.registerCallback(boost::bind(&PersonDataRecorder::wideCB, this, _1, _2, _3, _4)); -import rclpy import random import unittest from builtin_interfaces.msg import Time as TimeMsg -from message_filters import SimpleFilter, Subscriber, Cache, TimeSynchronizer +from message_filters import SimpleFilter, TimeSynchronizer class MockHeader: diff --git a/test/test_approxsync.py b/test/test_approxsync.py index c156e0a..1c3f27d 100644 --- a/test/test_approxsync.py +++ b/test/test_approxsync.py @@ -30,7 +30,6 @@ from message_filters import ApproximateTimeSynchronizer import random -import rclpy from rclpy.clock import ROSClock from rclpy.duration import Duration from rclpy.time import Time