Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat: add/minus for msg::Time and rclcpp::Duration #2419

Merged
merged 13 commits into from
Feb 13, 2024
9 changes: 9 additions & 0 deletions rclcpp/include/rclcpp/duration.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
#include <chrono>

#include "builtin_interfaces/msg/duration.hpp"
#include "builtin_interfaces/msg/time.hpp"
#include "rcl/time.h"
#include "rclcpp/visibility_control.hpp"

Expand Down Expand Up @@ -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_
9 changes: 9 additions & 0 deletions rclcpp/include/rclcpp/time.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
fujitatomoya marked this conversation as resolved.
Show resolved Hide resolved
builtin_interfaces::msg::Time
convert_rcl_time_to_sec_nanos(const rcl_time_point_value_t & time_point);

} // namespace rclcpp

#endif // RCLCPP__TIME_HPP_
48 changes: 48 additions & 0 deletions rclcpp/src/rclcpp/duration.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,8 @@

#include "rcutils/logging_macros.h"

#include "rclcpp/utilities.hpp"

namespace rclcpp
{

Expand Down Expand Up @@ -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<int64_t>(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<int64_t>(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
27 changes: 16 additions & 11 deletions rclcpp/src/rclcpp/time.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::int32_t>(result.quot);
msg_time.nanosec = static_cast<std::uint32_t>(result.rem);
} else {
msg_time.sec = static_cast<std::int32_t>(result.quot - 1);
msg_time.nanosec = static_cast<std::uint32_t>(kRemainder + result.rem);
}
return msg_time;
return convert_rcl_time_to_sec_nanos(rcl_time_.nanoseconds);
}

Time &
Expand Down Expand Up @@ -281,5 +271,20 @@ Time::max(rcl_clock_type_t clock_type)
return Time(std::numeric_limits<int32_t>::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<std::int32_t>(result.quot);
ret.nanosec = static_cast<std::uint32_t>(result.rem);
} else {
ret.sec = static_cast<std::int32_t>(result.quot - 1);
ret.nanosec = static_cast<std::uint32_t>(kRemainder + result.rem);
}
return ret;
}

} // namespace rclcpp
29 changes: 29 additions & 0 deletions rclcpp/test/rclcpp/test_duration.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<rcl_duration_value_t>::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);
Expand Down