From 764c721a8ac35af521ed17e6ab21c58bb3880019 Mon Sep 17 00:00:00 2001 From: Johannes Meyer Date: Fri, 19 Jun 2020 17:19:24 +0200 Subject: [PATCH] Fix conversion from negative Duration or Time to the respective message type ... and throw in Duration::to_rmw_time() if the duration is negative. rmw_time_t cannot represent negative durations. Constructors and assignment operators can be just defaulted. Other changes are mainly cosmetical, to make conversions between signed and unsigned types and between 32-bit and 64-bit types more explicit. Signed-off-by: Johannes Meyer --- rclcpp/src/rclcpp/duration.cpp | 35 ++++---- rclcpp/src/rclcpp/time.cpp | 23 +++-- rclcpp/test/rclcpp/test_duration.cpp | 91 +++++++++++++++++++- rclcpp/test/rclcpp/test_time.cpp | 123 ++++++++++++++++++++++----- 4 files changed, 217 insertions(+), 55 deletions(-) diff --git a/rclcpp/src/rclcpp/duration.cpp b/rclcpp/src/rclcpp/duration.cpp index 2114ddd16f..d555421f3e 100644 --- a/rclcpp/src/rclcpp/duration.cpp +++ b/rclcpp/src/rclcpp/duration.cpp @@ -47,17 +47,14 @@ Duration::Duration(std::chrono::nanoseconds nanoseconds) rcl_duration_.nanoseconds = nanoseconds.count(); } -Duration::Duration(const Duration & rhs) -{ - rcl_duration_.nanoseconds = rhs.rcl_duration_.nanoseconds; -} +Duration::Duration(const Duration & rhs) = default; Duration::Duration( const builtin_interfaces::msg::Duration & duration_msg) { rcl_duration_.nanoseconds = - static_cast(RCL_S_TO_NS(duration_msg.sec)); - rcl_duration_.nanoseconds += duration_msg.nanosec; + RCL_S_TO_NS(static_cast(duration_msg.sec)); + rcl_duration_.nanoseconds += static_cast(duration_msg.nanosec); } Duration::Duration(const rcl_duration_t & duration) @@ -69,24 +66,26 @@ Duration::Duration(const rcl_duration_t & duration) Duration::operator builtin_interfaces::msg::Duration() const { builtin_interfaces::msg::Duration msg_duration; - msg_duration.sec = static_cast(RCL_NS_TO_S(rcl_duration_.nanoseconds)); - msg_duration.nanosec = - static_cast(rcl_duration_.nanoseconds % (1000 * 1000 * 1000)); + constexpr rcl_duration_value_t kRemainder = RCL_S_TO_NS(1); + const auto result = std::div(rcl_duration_.nanoseconds, kRemainder); + if (result.rem >= 0) { + msg_duration.sec = static_cast(result.quot); + msg_duration.nanosec = static_cast(result.rem); + } else { + msg_duration.sec = static_cast(result.quot - 1); + msg_duration.nanosec = static_cast(kRemainder + result.rem); + } return msg_duration; } Duration & -Duration::operator=(const Duration & rhs) -{ - rcl_duration_.nanoseconds = rhs.rcl_duration_.nanoseconds; - return *this; -} +Duration::operator=(const Duration & rhs) = default; Duration & Duration::operator=(const builtin_interfaces::msg::Duration & duration_msg) { - rcl_duration_.nanoseconds = RCL_S_TO_NS(static_cast(duration_msg.sec)); - rcl_duration_.nanoseconds += duration_msg.nanosec; + rcl_duration_.nanoseconds = RCL_S_TO_NS(static_cast(duration_msg.sec)); + rcl_duration_.nanoseconds += static_cast(duration_msg.nanosec); return *this; } @@ -230,6 +229,10 @@ Duration::seconds() const rmw_time_t Duration::to_rmw_time() const { + if (rcl_duration_.nanoseconds < 0) { + throw std::overflow_error("rmw_time_t cannot be negative"); + } + // reuse conversion logic from msg creation builtin_interfaces::msg::Duration msg = *this; rmw_time_t result; diff --git a/rclcpp/src/rclcpp/time.cpp b/rclcpp/src/rclcpp/time.cpp index 63289161ee..f634a6da58 100644 --- a/rclcpp/src/rclcpp/time.cpp +++ b/rclcpp/src/rclcpp/time.cpp @@ -63,11 +63,7 @@ Time::Time(int64_t nanoseconds, rcl_clock_type_t clock_type) rcl_time_.nanoseconds = nanoseconds; } -Time::Time(const Time & rhs) -: rcl_time_(rhs.rcl_time_) -{ - rcl_time_.nanoseconds = rhs.rcl_time_.nanoseconds; -} +Time::Time(const Time & rhs) = default; Time::Time( const builtin_interfaces::msg::Time & time_msg, @@ -95,17 +91,20 @@ Time::~Time() Time::operator builtin_interfaces::msg::Time() const { builtin_interfaces::msg::Time msg_time; - msg_time.sec = static_cast(RCL_NS_TO_S(rcl_time_.nanoseconds)); - msg_time.nanosec = static_cast(rcl_time_.nanoseconds % (1000 * 1000 * 1000)); + constexpr rcl_duration_value_t kRemainder = RCL_S_TO_NS(1); + const auto result = std::div(rcl_time_.nanoseconds, kRemainder); + if (result.rem >= 0) { + msg_time.sec = static_cast(result.quot); + msg_time.nanosec = static_cast(result.rem); + } else { + msg_time.sec = static_cast(result.quot - 1); + msg_time.nanosec = static_cast(kRemainder + result.rem); + } return msg_time; } Time & -Time::operator=(const Time & rhs) -{ - rcl_time_ = rhs.rcl_time_; - return *this; -} +Time::operator=(const Time & rhs) = default; Time & Time::operator=(const builtin_interfaces::msg::Time & time_msg) diff --git a/rclcpp/test/rclcpp/test_duration.cpp b/rclcpp/test/rclcpp/test_duration.cpp index b69a790d28..60fae3a795 100644 --- a/rclcpp/test/rclcpp/test_duration.cpp +++ b/rclcpp/test/rclcpp/test_duration.cpp @@ -32,10 +32,6 @@ class TestDuration : public ::testing::Test { }; -// TEST(TestDuration, conversions) { -// TODO(tfoote) Implement conversion methods -// } - TEST(TestDuration, operators) { rclcpp::Duration old(1, 0); rclcpp::Duration young(2, 0); @@ -138,6 +134,7 @@ TEST(TestDuration, maximum_duration) { } static const int64_t HALF_SEC_IN_NS = 500 * 1000 * 1000; +static const int64_t ONE_SEC_IN_NS = 1000 * 1000 * 1000; static const int64_t ONE_AND_HALF_SEC_IN_NS = 3 * HALF_SEC_IN_NS; TEST(TestDuration, from_seconds) { @@ -153,3 +150,89 @@ TEST(TestDuration, std_chrono_constructors) { EXPECT_EQ(rclcpp::Duration(1, HALF_SEC_IN_NS), rclcpp::Duration(1.5s)); EXPECT_EQ(rclcpp::Duration(-1, 0), rclcpp::Duration(-1s)); } + +TEST(TestDuration, conversions) { + { + const rclcpp::Duration duration(HALF_SEC_IN_NS); + const auto duration_msg = static_cast(duration); + EXPECT_EQ(duration_msg.sec, 0); + EXPECT_EQ(duration_msg.nanosec, HALF_SEC_IN_NS); + EXPECT_EQ(rclcpp::Duration(duration_msg).nanoseconds(), HALF_SEC_IN_NS); + + const auto rmw_time = duration.to_rmw_time(); + EXPECT_EQ(rmw_time.sec, 0u); + EXPECT_EQ(rmw_time.nsec, static_cast(HALF_SEC_IN_NS)); + + const auto chrono_duration = duration.to_chrono(); + EXPECT_EQ(chrono_duration.count(), HALF_SEC_IN_NS); + } + + { + const rclcpp::Duration duration(ONE_SEC_IN_NS); + const auto duration_msg = static_cast(duration); + EXPECT_EQ(duration_msg.sec, 1); + EXPECT_EQ(duration_msg.nanosec, 0u); + EXPECT_EQ(rclcpp::Duration(duration_msg).nanoseconds(), ONE_SEC_IN_NS); + + const auto rmw_time = duration.to_rmw_time(); + EXPECT_EQ(rmw_time.sec, 1u); + EXPECT_EQ(rmw_time.nsec, 0u); + + const auto chrono_duration = duration.to_chrono(); + EXPECT_EQ(chrono_duration.count(), ONE_SEC_IN_NS); + } + + { + const rclcpp::Duration duration(ONE_AND_HALF_SEC_IN_NS); + auto duration_msg = static_cast(duration); + EXPECT_EQ(duration_msg.sec, 1); + EXPECT_EQ(duration_msg.nanosec, HALF_SEC_IN_NS); + EXPECT_EQ(rclcpp::Duration(duration_msg).nanoseconds(), ONE_AND_HALF_SEC_IN_NS); + + auto rmw_time = duration.to_rmw_time(); + EXPECT_EQ(rmw_time.sec, 1u); + EXPECT_EQ(rmw_time.nsec, static_cast(HALF_SEC_IN_NS)); + + auto chrono_duration = duration.to_chrono(); + EXPECT_EQ(chrono_duration.count(), ONE_AND_HALF_SEC_IN_NS); + } + + { + rclcpp::Duration duration(-HALF_SEC_IN_NS); + auto duration_msg = static_cast(duration); + EXPECT_EQ(duration_msg.sec, -1); + EXPECT_EQ(duration_msg.nanosec, HALF_SEC_IN_NS); + EXPECT_EQ(rclcpp::Duration(duration_msg).nanoseconds(), -HALF_SEC_IN_NS); + + EXPECT_THROW(duration.to_rmw_time(), std::overflow_error); + + auto chrono_duration = duration.to_chrono(); + EXPECT_EQ(chrono_duration.count(), -HALF_SEC_IN_NS); + } + + { + rclcpp::Duration duration(-ONE_SEC_IN_NS); + auto duration_msg = static_cast(duration); + EXPECT_EQ(duration_msg.sec, -1); + EXPECT_EQ(duration_msg.nanosec, 0u); + EXPECT_EQ(rclcpp::Duration(duration_msg).nanoseconds(), -ONE_SEC_IN_NS); + + EXPECT_THROW(duration.to_rmw_time(), std::overflow_error); + + auto chrono_duration = duration.to_chrono(); + EXPECT_EQ(chrono_duration.count(), -ONE_SEC_IN_NS); + } + + { + rclcpp::Duration duration(-ONE_AND_HALF_SEC_IN_NS); + auto duration_msg = static_cast(duration); + EXPECT_EQ(duration_msg.sec, -2); + EXPECT_EQ(duration_msg.nanosec, HALF_SEC_IN_NS); + EXPECT_EQ(rclcpp::Duration(duration_msg).nanoseconds(), -ONE_AND_HALF_SEC_IN_NS); + + EXPECT_THROW(duration.to_rmw_time(), std::overflow_error); + + auto chrono_duration = duration.to_chrono(); + EXPECT_EQ(chrono_duration.count(), -ONE_AND_HALF_SEC_IN_NS); + } +} diff --git a/rclcpp/test/rclcpp/test_time.cpp b/rclcpp/test/rclcpp/test_time.cpp index 830c0319e7..141064e13d 100644 --- a/rclcpp/test/rclcpp/test_time.cpp +++ b/rclcpp/test/rclcpp/test_time.cpp @@ -86,41 +86,118 @@ TEST(TestTime, time_sources) { EXPECT_NE(0u, steady_now.nanosec); } +static const int64_t HALF_SEC_IN_NS = 500 * 1000 * 1000; +static const int64_t ONE_SEC_IN_NS = 1000 * 1000 * 1000; +static const int64_t ONE_AND_HALF_SEC_IN_NS = 3 * HALF_SEC_IN_NS; + TEST(TestTime, conversions) { rclcpp::Clock system_clock(RCL_SYSTEM_TIME); - rclcpp::Time now = system_clock.now(); - builtin_interfaces::msg::Time now_msg = now; + { + rclcpp::Time now = system_clock.now(); + builtin_interfaces::msg::Time now_msg = now; + + rclcpp::Time now_again = now_msg; + EXPECT_EQ(now.nanoseconds(), now_again.nanoseconds()); + } - rclcpp::Time now_again = now_msg; - EXPECT_EQ(now.nanoseconds(), now_again.nanoseconds()); + { + rclcpp::Time positive_time = rclcpp::Time(12345, 67890u); + + builtin_interfaces::msg::Time msg = positive_time; + EXPECT_EQ(msg.sec, 12345); + EXPECT_EQ(msg.nanosec, 67890u); + + rclcpp::Time time = msg; + EXPECT_EQ(time.nanoseconds(), positive_time.nanoseconds()); + EXPECT_EQ( + RCL_S_TO_NS(static_cast(msg.sec)) + static_cast(msg.nanosec), + time.nanoseconds()); + EXPECT_EQ(static_cast(msg.sec), RCL_NS_TO_S(time.nanoseconds())); + } + + // throw on construction/assignment of negative times + { + builtin_interfaces::msg::Time negative_time_msg; + negative_time_msg.sec = -1; + negative_time_msg.nanosec = 1; + + EXPECT_ANY_THROW( + { + rclcpp::Time negative_time = negative_time_msg; + }); + + EXPECT_ANY_THROW(rclcpp::Time(-1, 1)); + + EXPECT_ANY_THROW( + { + rclcpp::Time assignment(1, 2); + assignment = negative_time_msg; + }); + } - builtin_interfaces::msg::Time msg; - msg.sec = 12345; - msg.nanosec = 67890; + { + const rclcpp::Time time(HALF_SEC_IN_NS); + const auto time_msg = static_cast(time); + EXPECT_EQ(time_msg.sec, 0); + EXPECT_EQ(time_msg.nanosec, HALF_SEC_IN_NS); + EXPECT_EQ(rclcpp::Time(time_msg).nanoseconds(), HALF_SEC_IN_NS); + } - rclcpp::Time time = msg; - EXPECT_EQ( - RCL_S_TO_NS(static_cast(msg.sec)) + static_cast(msg.nanosec), - time.nanoseconds()); - EXPECT_EQ(static_cast(msg.sec), RCL_NS_TO_S(time.nanoseconds())); + { + const rclcpp::Time time(ONE_SEC_IN_NS); + const auto time_msg = static_cast(time); + EXPECT_EQ(time_msg.sec, 1); + EXPECT_EQ(time_msg.nanosec, 0u); + EXPECT_EQ(rclcpp::Time(time_msg).nanoseconds(), ONE_SEC_IN_NS); + } - builtin_interfaces::msg::Time negative_time_msg; - negative_time_msg.sec = -1; - negative_time_msg.nanosec = 1; + { + const rclcpp::Time time(ONE_AND_HALF_SEC_IN_NS); + auto time_msg = static_cast(time); + EXPECT_EQ(time_msg.sec, 1); + EXPECT_EQ(time_msg.nanosec, HALF_SEC_IN_NS); + EXPECT_EQ(rclcpp::Time(time_msg).nanoseconds(), ONE_AND_HALF_SEC_IN_NS); + } - EXPECT_ANY_THROW( { - rclcpp::Time negative_time = negative_time_msg; - }); + // Can rclcpp::Time be negative or not? The following constructor works: + rclcpp::Time time(-HALF_SEC_IN_NS); + auto time_msg = static_cast(time); + EXPECT_EQ(time_msg.sec, -1); + EXPECT_EQ(time_msg.nanosec, HALF_SEC_IN_NS); + + // The opposite conversion throws... + EXPECT_ANY_THROW({ + rclcpp::Time negative_time(time_msg); + }); + } - EXPECT_ANY_THROW(rclcpp::Time(-1, 1)); + { + // Can rclcpp::Time be negative or not? The following constructor works: + rclcpp::Time time(-ONE_SEC_IN_NS); + auto time_msg = static_cast(time); + EXPECT_EQ(time_msg.sec, -1); + EXPECT_EQ(time_msg.nanosec, 0u); + + // The opposite conversion throws... + EXPECT_ANY_THROW({ + rclcpp::Time negative_time(time_msg); + }); + } - EXPECT_ANY_THROW( { - rclcpp::Time assignment(1, 2); - assignment = negative_time_msg; - }); + // Can rclcpp::Time be negative or not? The following constructor works: + rclcpp::Time time(-ONE_AND_HALF_SEC_IN_NS); + auto time_msg = static_cast(time); + EXPECT_EQ(time_msg.sec, -2); + EXPECT_EQ(time_msg.nanosec, HALF_SEC_IN_NS); + + // The opposite conversion throws... + EXPECT_ANY_THROW({ + rclcpp::Time negative_time(time_msg); + }); + } } TEST(TestTime, operators) {