Skip to content

Commit

Permalink
restore assert -> RCUTILS_ASSERT and abort -> RCUTILS_BREAK
Browse files Browse the repository at this point in the history 
Provide migration path #define and TODO for resolving once provided
upstream in ros2/rcutils#112
  • Loading branch information
@tfoote
tfoote committed Aug 15, 2018
1 parent 62d8611 commit b8519b6
Show file tree
Hide file tree
Showing 3 changed files with 53 additions and 28 deletions.
10 changes: 9 additions & 1 deletion include/message_filters/message_event.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,14 @@
#include <type_traits>
#include <memory>

#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 <rcutils/assert.h>

namespace message_filters
{
typedef std::map< std::string, std::string > M_string;
Expand Down Expand Up @@ -202,7 +210,7 @@ class MessageEvent
return message_copy_;
}

assert(create_);
RCUTILS_ASSERT(create_);
message_copy_ = create_();
*message_copy_ = *message_;

Expand Down
68 changes: 43 additions & 25 deletions include/message_filters/sync_policies/approximate_time.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,23 @@
#include <string>
#include <tuple>

#include <rcutils/logging_macros.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 <cassert>
// 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 <rcutils/assert.h>

namespace message_filters
{
namespace sync_policies
Expand Down Expand Up @@ -105,7 +122,7 @@ struct ApproximateTime : public PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8>
, 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)
Expand Down Expand Up @@ -149,7 +166,7 @@ struct ApproximateTime : public PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8>
}
std::deque<typename std::tuple_element<i, Events>::type>& deque = std::get<i>(deques_);
std::vector<typename std::tuple_element<i, Events>::type>& v = std::get<i>(past_);
assert(!deque.empty());
RCUTILS_ASSERT(!deque.empty());
const typename std::tuple_element<i, Messages>::type &msg = *(deque.back()).getMessage();
rclcpp::Time msg_time = mt::TimeStamp<typename std::tuple_element<i, Messages>::type>::value(msg);
rclcpp::Time previous_msg_time;
Expand All @@ -171,15 +188,17 @@ struct ApproximateTime : public PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8>
}
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;
}
}
Expand Down Expand Up @@ -222,7 +241,7 @@ struct ApproximateTime : public PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8>
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)
Expand All @@ -239,19 +258,19 @@ struct ApproximateTime : public PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8>
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;
}

Expand All @@ -261,7 +280,7 @@ struct ApproximateTime : public PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8>
void dequeDeleteFront()
{
std::deque<typename std::tuple_element<i, Events>::type>& deque = std::get<i>(deques_);
assert(!deque.empty());
RCUTILS_ASSERT(!deque.empty());
deque.pop_front();
if (deque.empty())
{
Expand Down Expand Up @@ -302,7 +321,7 @@ struct ApproximateTime : public PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8>
dequeDeleteFront<8>();
break;
default:
std::abort();
RCUTILS_BREAK();
}
}

Expand All @@ -312,7 +331,7 @@ struct ApproximateTime : public PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8>
{
std::deque<typename std::tuple_element<i, Events>::type>& deque = std::get<i>(deques_);
std::vector<typename std::tuple_element<i, Events>::type>& vector = std::get<i>(past_);
assert(!deque.empty());
RCUTILS_ASSERT(!deque.empty());
vector.push_back(deque.front());
deque.pop_front();
if (deque.empty())
Expand Down Expand Up @@ -353,7 +372,7 @@ struct ApproximateTime : public PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8>
dequeMoveFrontToPast<8>();
break;
default:
std::abort();
RCUTILS_BREAK();
}
}

Expand Down Expand Up @@ -417,7 +436,7 @@ struct ApproximateTime : public PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8>

std::vector<typename std::tuple_element<i, Events>::type>& v = std::get<i>(past_);
std::deque<typename std::tuple_element<i, Events>::type>& q = std::get<i>(deques_);
assert(num_messages <= v.size());
RCUTILS_ASSERT(num_messages <= v.size());
while (num_messages > 0)
{
q.push_front(v.back());
Expand Down Expand Up @@ -471,7 +490,7 @@ struct ApproximateTime : public PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8>
v.pop_back();
}

assert(!q.empty());
RCUTILS_ASSERT(!q.empty());

q.pop_front();
if (!q.empty())
Expand Down Expand Up @@ -615,13 +634,13 @@ struct ApproximateTime : public PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8>
{
return rclcpp::Time(0,0); // Dummy return value
}
assert(pivot_ != NO_PIVOT);
RCUTILS_ASSERT(pivot_ != NO_PIVOT);

std::vector<typename std::tuple_element<i, Events>::type>& v = std::get<i>(past_);
std::deque<typename std::tuple_element<i, Events>::type>& q = std::get<i>(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<typename std::tuple_element<i, Messages>::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
Expand Down Expand Up @@ -662,7 +681,7 @@ struct ApproximateTime : public PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8>
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++)
Expand Down Expand Up @@ -745,7 +764,7 @@ struct ApproximateTime : public PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8>
}
}
// 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_
{
Expand Down Expand Up @@ -798,14 +817,14 @@ struct ApproximateTime : public PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8>
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)
Expand Down Expand Up @@ -840,4 +859,3 @@ struct ApproximateTime : public PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8>
} // namespace message_filters

#endif // MESSAGE_FILTERS_SYNC_APPROXIMATE_TIME_H

3 changes: 1 addition & 2 deletions include/message_filters/sync_policies/exact_time.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,6 @@
#include "message_filters/message_traits.h"

#include <rclcpp/rclcpp.hpp>
#include <cassert>
#include <deque>
#include <string>
#include <tuple>
Expand Down Expand Up @@ -102,7 +101,7 @@ struct ExactTime : public PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8>
template<int i>
void add(const typename std::tuple_element<i, Events>::type& evt)
{
assert(parent_);
RCUTILS_ASSERT(parent_);

namespace mt = message_filters::message_traits;

Expand Down

0 comments on commit b8519b6

Please sign in to comment.