diff --git a/include/message_filters/message_event.h b/include/message_filters/message_event.h index bf73299..b39f924 100644 --- a/include/message_filters/message_event.h +++ b/include/message_filters/message_event.h @@ -33,6 +33,14 @@ #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 +// Uncomment below intead +//#include + namespace message_filters { typedef std::map< std::string, std::string > M_string; @@ -202,7 +210,7 @@ class MessageEvent return message_copy_; } - assert(create_); + RCUTILS_ASSERT(create_); message_copy_ = create_(); *message_copy_ = *message_; diff --git a/include/message_filters/sync_policies/approximate_time.h b/include/message_filters/sync_policies/approximate_time.h index b118664..ac1ea4d 100644 --- a/include/message_filters/sync_policies/approximate_time.h +++ b/include/message_filters/sync_policies/approximate_time.h @@ -48,6 +48,23 @@ #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 +#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 +// Uncomment below intead +//#include + namespace message_filters { namespace sync_policies @@ -105,7 +122,7 @@ struct ApproximateTime : public PolicyBase , inter_message_lower_bounds_(9, rclcpp::Duration(0, 0)) , warned_about_incorrect_bound_(9, false) { - assert(queue_size_ > 0); // The synchronizer will tend to drop many messages with a queue size of 1. At least 2 is recommended. + RCUTILS_ASSERT(queue_size_ > 0); // The synchronizer will tend to drop many messages with a queue size of 1. At least 2 is recommended. } ApproximateTime(const ApproximateTime& e) @@ -149,7 +166,7 @@ struct ApproximateTime : public PolicyBase } std::deque::type>& deque = std::get(deques_); std::vector::type>& v = std::get(past_); - assert(!deque.empty()); + RCUTILS_ASSERT(!deque.empty()); const typename std::tuple_element::type &msg = *(deque.back()).getMessage(); rclcpp::Time msg_time = mt::TimeStamp::type>::value(msg); rclcpp::Time previous_msg_time; @@ -171,15 +188,17 @@ struct ApproximateTime : public PolicyBase } if (msg_time < previous_msg_time) { - std::cout << "Messages of type " << i << " arrived out of order (will print only once)"; + RCUTILS_LOG_WARN_ONCE("Messages of type %d arrived out of order (will print only once)", i); warned_about_incorrect_bound_[i] = true; } else if ((msg_time - previous_msg_time) < inter_message_lower_bounds_[i]) { - std::cout << "Messages of type " << i << " arrived closer (" << - (msg_time - previous_msg_time).nanoseconds() << - ") than the lower bound you provided (" << - inter_message_lower_bounds_[i].nanoseconds() << ") (will print only once)"; + RCUTILS_LOG_WARN_ONCE("Messages of type %d arrived closer (" + "%g ) than the lower bound you provided (" + "%g) (will print only once)", + i, + (msg_time - previous_msg_time).nanoseconds(), + inter_message_lower_bounds_[i].nanoseconds()); warned_about_incorrect_bound_[i] = true; } } @@ -222,7 +241,7 @@ struct ApproximateTime : public PolicyBase recover<7>(); recover<8>(); // Drop the oldest message in the offending topic - assert(!deque.empty()); + RCUTILS_ASSERT(!deque.empty()); deque.pop_front(); has_dropped_messages_[i] = true; if (pivot_ != NO_PIVOT) @@ -239,19 +258,19 @@ struct ApproximateTime : public PolicyBase void setAgePenalty(double age_penalty) { // For correctness we only need age_penalty > -1.0, but most likely a negative age_penalty is a mistake. - assert(age_penalty >= 0); + RCUTILS_ASSERT(age_penalty >= 0); age_penalty_ = age_penalty; } void setInterMessageLowerBound(int i, rclcpp::Duration lower_bound) { // For correctness we only need age_penalty > -1.0, but most likely a negative age_penalty is a mistake. - assert(lower_bound >= rclcpp::Duration(0,0)); + RCUTILS_ASSERT(lower_bound >= rclcpp::Duration(0,0)); inter_message_lower_bounds_[i] = lower_bound; } void setMaxIntervalDuration(rclcpp::Duration max_interval_duration) { // For correctness we only need age_penalty > -1.0, but most likely a negative age_penalty is a mistake. - assert(max_interval_duration >= rclcpp::Duration(0,0)); + RCUTILS_ASSERT(max_interval_duration >= rclcpp::Duration(0,0)); max_interval_duration_ = max_interval_duration; } @@ -261,7 +280,7 @@ struct ApproximateTime : public PolicyBase void dequeDeleteFront() { std::deque::type>& deque = std::get(deques_); - assert(!deque.empty()); + RCUTILS_ASSERT(!deque.empty()); deque.pop_front(); if (deque.empty()) { @@ -302,7 +321,7 @@ struct ApproximateTime : public PolicyBase dequeDeleteFront<8>(); break; default: - std::abort(); + RCUTILS_BREAK(); } } @@ -312,7 +331,7 @@ struct ApproximateTime : public PolicyBase { std::deque::type>& deque = std::get(deques_); std::vector::type>& vector = std::get(past_); - assert(!deque.empty()); + RCUTILS_ASSERT(!deque.empty()); vector.push_back(deque.front()); deque.pop_front(); if (deque.empty()) @@ -353,7 +372,7 @@ struct ApproximateTime : public PolicyBase dequeMoveFrontToPast<8>(); break; default: - std::abort(); + RCUTILS_BREAK(); } } @@ -417,7 +436,7 @@ struct ApproximateTime : public PolicyBase std::vector::type>& v = std::get(past_); std::deque::type>& q = std::get(deques_); - assert(num_messages <= v.size()); + RCUTILS_ASSERT(num_messages <= v.size()); while (num_messages > 0) { q.push_front(v.back()); @@ -471,7 +490,7 @@ struct ApproximateTime : public PolicyBase v.pop_back(); } - assert(!q.empty()); + RCUTILS_ASSERT(!q.empty()); q.pop_front(); if (!q.empty()) @@ -615,13 +634,13 @@ struct ApproximateTime : public PolicyBase { return rclcpp::Time(0,0); // Dummy return value } - assert(pivot_ != NO_PIVOT); + RCUTILS_ASSERT(pivot_ != NO_PIVOT); std::vector::type>& v = std::get(past_); std::deque::type>& q = std::get(deques_); if (q.empty()) { - assert(!v.empty()); // Because we have a candidate + RCUTILS_ASSERT(!v.empty()); // Because we have a candidate rclcpp::Time last_msg_time = mt::TimeStamp::type>::value(*(v.back()).getMessage()); rclcpp::Time msg_time_lower_bound = last_msg_time + inter_message_lower_bounds_[i]; if (msg_time_lower_bound > pivot_time_) // Take the max @@ -662,7 +681,7 @@ struct ApproximateTime : public PolicyBase virtual_times[6] = getVirtualTime<6>(); virtual_times[7] = getVirtualTime<7>(); virtual_times[8] = getVirtualTime<8>(); - + time = virtual_times[0]; index = 0; for (int i = 0; i < RealTypeCount::value; i++) @@ -745,7 +764,7 @@ struct ApproximateTime : public PolicyBase } } // INVARIANT: we have a candidate and pivot - assert(pivot_ != NO_PIVOT); + RCUTILS_ASSERT(pivot_ != NO_PIVOT); //printf("start_index == %d, pivot_ == %d\n", start_index, pivot_); if (start_index == pivot_) // TODO: replace with start_time == pivot_time_ { @@ -798,14 +817,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 - assert(num_non_empty_deques_before_virtual_search == num_non_empty_deques_); + RCUTILS_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. - assert(start_index != pivot_); - assert(start_time < pivot_time_); + RCUTILS_ASSERT(start_index != pivot_); + RCUTILS_ASSERT(start_time < pivot_time_); dequeMoveFrontToPast(start_index); num_virtual_moves[start_index]++; } // while(1) @@ -840,4 +859,3 @@ struct ApproximateTime : public PolicyBase } // namespace message_filters #endif // MESSAGE_FILTERS_SYNC_APPROXIMATE_TIME_H - diff --git a/include/message_filters/sync_policies/exact_time.h b/include/message_filters/sync_policies/exact_time.h index 21f226d..573fe67 100644 --- a/include/message_filters/sync_policies/exact_time.h +++ b/include/message_filters/sync_policies/exact_time.h @@ -42,7 +42,6 @@ #include "message_filters/message_traits.h" #include -#include #include #include #include @@ -102,7 +101,7 @@ struct ExactTime : public PolicyBase template void add(const typename std::tuple_element::type& evt) { - assert(parent_); + RCUTILS_ASSERT(parent_); namespace mt = message_filters::message_traits;