diff --git a/rclcpp/include/rclcpp/duration.hpp b/rclcpp/include/rclcpp/duration.hpp index 537e2a9bf6..6b48f441e3 100644 --- a/rclcpp/include/rclcpp/duration.hpp +++ b/rclcpp/include/rclcpp/duration.hpp @@ -18,6 +18,7 @@ #include #include "builtin_interfaces/msg/duration.hpp" +#include "builtin_interfaces/msg/time.hpp" #include "rcl/time.h" #include "rclcpp/visibility_control.hpp" @@ -158,6 +159,14 @@ class RCLCPP_PUBLIC Duration Duration() = default; }; +RCLCPP_PUBLIC +builtin_interfaces::msg::Time +operator+(const builtin_interfaces::msg::Time & lhs, const rclcpp::Duration & rhs); + +RCLCPP_PUBLIC +builtin_interfaces::msg::Time +operator-(const builtin_interfaces::msg::Time & lhs, const rclcpp::Duration & rhs); + } // namespace rclcpp #endif // RCLCPP__DURATION_HPP_ diff --git a/rclcpp/include/rclcpp/time.hpp b/rclcpp/include/rclcpp/time.hpp index b6aee4c972..15e063d1d2 100644 --- a/rclcpp/include/rclcpp/time.hpp +++ b/rclcpp/include/rclcpp/time.hpp @@ -222,6 +222,15 @@ RCLCPP_PUBLIC Time operator+(const rclcpp::Duration & lhs, const rclcpp::Time & rhs); +/// Convert rcl_time_point_value_t to builtin_interfaces::msg::Time +/** + * \param[in] time_point is a rcl_time_point_value_t + * \return the builtin_interfaces::msg::Time from the time_point + */ +RCLCPP_PUBLIC +builtin_interfaces::msg::Time +convert_rcl_time_to_sec_nanos(const rcl_time_point_value_t & time_point); + } // namespace rclcpp #endif // RCLCPP__TIME_HPP_ diff --git a/rclcpp/src/rclcpp/duration.cpp b/rclcpp/src/rclcpp/duration.cpp index 2eac7f6b58..7cb678456e 100644 --- a/rclcpp/src/rclcpp/duration.cpp +++ b/rclcpp/src/rclcpp/duration.cpp @@ -28,6 +28,8 @@ #include "rcutils/logging_macros.h" +#include "rclcpp/utilities.hpp" + namespace rclcpp { @@ -316,4 +318,50 @@ Duration::from_nanoseconds(rcl_duration_value_t nanoseconds) return ret; } +builtin_interfaces::msg::Time +operator+(const builtin_interfaces::msg::Time & lhs, const rclcpp::Duration & rhs) +{ + if (lhs.sec < 0) { + throw std::runtime_error("message time is negative"); + } + + rcl_time_point_value_t rcl_time; + rcl_time = RCL_S_TO_NS(static_cast(lhs.sec)); + rcl_time += lhs.nanosec; + + if (rclcpp::add_will_overflow(rcl_time, rhs.nanoseconds())) { + throw std::overflow_error("addition leads to int64_t overflow"); + } + if (rclcpp::add_will_underflow(rcl_time, rhs.nanoseconds())) { + throw std::underflow_error("addition leads to int64_t underflow"); + } + + rcl_time += rhs.nanoseconds(); + + return convert_rcl_time_to_sec_nanos(rcl_time); +} + +builtin_interfaces::msg::Time +operator-(const builtin_interfaces::msg::Time & lhs, const rclcpp::Duration & rhs) +{ + if (lhs.sec < 0) { + throw std::runtime_error("message time is negative"); + } + + rcl_time_point_value_t rcl_time; + rcl_time = RCL_S_TO_NS(static_cast(lhs.sec)); + rcl_time += lhs.nanosec; + + if (rclcpp::sub_will_overflow(rcl_time, rhs.nanoseconds())) { + throw std::overflow_error("addition leads to int64_t overflow"); + } + if (rclcpp::sub_will_underflow(rcl_time, rhs.nanoseconds())) { + throw std::underflow_error("addition leads to int64_t underflow"); + } + + rcl_time -= rhs.nanoseconds(); + + return convert_rcl_time_to_sec_nanos(rcl_time); +} + } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/time.cpp b/rclcpp/src/rclcpp/time.cpp index e5ff2af8d0..a0be141312 100644 --- a/rclcpp/src/rclcpp/time.cpp +++ b/rclcpp/src/rclcpp/time.cpp @@ -90,17 +90,7 @@ Time::~Time() Time::operator builtin_interfaces::msg::Time() const { - builtin_interfaces::msg::Time msg_time; - constexpr rcl_time_point_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; + return convert_rcl_time_to_sec_nanos(rcl_time_.nanoseconds); } Time & @@ -281,5 +271,20 @@ Time::max(rcl_clock_type_t clock_type) return Time(std::numeric_limits::max(), 999999999, clock_type); } +builtin_interfaces::msg::Time +convert_rcl_time_to_sec_nanos(const rcl_time_point_value_t & time_point) +{ + builtin_interfaces::msg::Time ret; + constexpr rcl_time_point_value_t kRemainder = RCL_S_TO_NS(1); + const auto result = std::div(time_point, kRemainder); + if (result.rem >= 0) { + ret.sec = static_cast(result.quot); + ret.nanosec = static_cast(result.rem); + } else { + ret.sec = static_cast(result.quot - 1); + ret.nanosec = static_cast(kRemainder + result.rem); + } + return ret; +} } // namespace rclcpp diff --git a/rclcpp/test/rclcpp/test_duration.cpp b/rclcpp/test/rclcpp/test_duration.cpp index d5908c7d4b..2347514d7a 100644 --- a/rclcpp/test/rclcpp/test_duration.cpp +++ b/rclcpp/test/rclcpp/test_duration.cpp @@ -79,6 +79,35 @@ TEST_F(TestDuration, operators) { EXPECT_TRUE(time == assignment_op_duration); } +TEST_F(TestDuration, operators_with_message_stamp) { + builtin_interfaces::msg::Time time_msg = rclcpp::Time(0, 100000000u); // 0.1s + rclcpp::Duration pos_duration(1, 100000000u); // 1.1s + rclcpp::Duration neg_duration(-2, 900000000u); // -1.1s + + builtin_interfaces::msg::Time res_addpos = time_msg + pos_duration; + EXPECT_EQ(res_addpos.sec, 1); + EXPECT_EQ(res_addpos.nanosec, 200000000u); + + builtin_interfaces::msg::Time res_addneg = time_msg + neg_duration; + EXPECT_EQ(res_addneg.sec, -1); + EXPECT_EQ(res_addneg.nanosec, 0); + + builtin_interfaces::msg::Time res_subpos = time_msg - pos_duration; + EXPECT_EQ(res_subpos.sec, -1); + EXPECT_EQ(res_subpos.nanosec, 0); + + builtin_interfaces::msg::Time res_subneg = time_msg - neg_duration; + EXPECT_EQ(res_subneg.sec, 1); + EXPECT_EQ(res_subneg.nanosec, 200000000u); + + builtin_interfaces::msg::Time neg_time_msg; + neg_time_msg.sec = -1; + auto max = rclcpp::Duration::from_nanoseconds(std::numeric_limits::max()); + + EXPECT_THROW(neg_time_msg + max, std::runtime_error); + EXPECT_THROW(time_msg + max, std::overflow_error); +} + TEST_F(TestDuration, chrono_overloads) { int64_t ns = 123456789l; auto chrono_ns = std::chrono::nanoseconds(ns);