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/utilities.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
#include <functional>
#include <limits>
#include <string>
#include <utility>
#include <vector>

#include "rclcpp/context.hpp"
Expand Down Expand Up @@ -330,6 +331,14 @@ RCLCPP_PUBLIC
std::vector<const char *>
get_c_vector_string(const std::vector<std::string> & strings_in);

/// Return the std::pair of seconds and nanoseconds from the given rcl_time_point_value_t.
/**
* \param[in] time_point is a rcl_time_point_value_t
* \return the std::pair of seconds and nanoseconds from the rcl_time_point_value_t
*/
std::pair<int32_t, uint32_t>
convert_rcl_time_to_sec_nanos(const rcl_time_point_value_t & time_point);

} // namespace rclcpp

#endif // RCLCPP__UTILITIES_HPP_
56 changes: 56 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,58 @@ 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();

builtin_interfaces::msg::Time ret;
auto sec_nanos = convert_rcl_time_to_sec_nanos(rcl_time);
ret.sec = sec_nanos.first;
ret.nanosec = sec_nanos.second;
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::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();

builtin_interfaces::msg::Time ret;
auto sec_nanos = convert_rcl_time_to_sec_nanos(rcl_time);
ret.sec = sec_nanos.first;
ret.nanosec = sec_nanos.second;
return ret;
}

} // namespace rclcpp
12 changes: 3 additions & 9 deletions rclcpp/src/rclcpp/time.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -91,15 +91,9 @@ 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);
}
auto sec_nanos = convert_rcl_time_to_sec_nanos(rcl_time_.nanoseconds);
msg_time.sec = sec_nanos.first;
msg_time.nanosec = sec_nanos.second;
return msg_time;
}

Expand Down
18 changes: 18 additions & 0 deletions rclcpp/src/rclcpp/utilities.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,7 @@

#include "rcl/error_handling.h"
#include "rcl/rcl.h"
#include "rcl/time.h"

namespace rclcpp
{
Expand Down Expand Up @@ -227,4 +228,21 @@ get_c_vector_string(const std::vector<std::string> & strings_in)
return cstrings;
}

std::pair<int32_t, uint32_t>
convert_rcl_time_to_sec_nanos(const rcl_time_point_value_t & time_point)
HuaTsai marked this conversation as resolved.
Show resolved Hide resolved
{
int32_t seconds;
uint32_t nanoseconds;
constexpr rcl_time_point_value_t kRemainder = RCL_S_TO_NS(1);
const auto result = std::div(time_point, kRemainder);
if (result.rem >= 0) {
seconds = static_cast<std::int32_t>(result.quot);
nanoseconds = static_cast<std::uint32_t>(result.rem);
} else {
seconds = static_cast<std::int32_t>(result.quot - 1);
nanoseconds = static_cast<std::uint32_t>(kRemainder + result.rem);
}
return {seconds, nanoseconds};
}

} // namespace rclcpp
31 changes: 31 additions & 0 deletions rclcpp/test/rclcpp/test_duration.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -79,6 +79,37 @@ TEST_F(TestDuration, operators) {
EXPECT_TRUE(time == assignment_op_duration);
}

TEST_F(TestDuration, operators_with_message_stamp) {
builtin_interfaces::msg::Time time_msg; // 0.1s
time_msg.sec = 0;
time_msg.nanosec = 100000000;
HuaTsai marked this conversation as resolved.
Show resolved Hide resolved
rclcpp::Duration pos_duration(1, 100000000); // 1.1s
rclcpp::Duration neg_duration(-2, 900000000); // -1.1s

builtin_interfaces::msg::Time res_addpos = time_msg + pos_duration;
EXPECT_EQ(res_addpos.sec, 1);
EXPECT_EQ(res_addpos.nanosec, 200000000);

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, 200000000);

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