Skip to content

Commit

Permalink
Fix conversion from negative Duration or Time to the respective messa…
Browse files Browse the repository at this point in the history 
…ge 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 <[email protected]>
  • Loading branch information
@meyerj
meyerj committed Jun 19, 2020
1 parent a667583 commit 764c721
Show file tree
Hide file tree
Showing 4 changed files with 217 additions and 55 deletions.
35 changes: 19 additions & 16 deletions rclcpp/src/rclcpp/duration.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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_duration_value_t>(RCL_S_TO_NS(duration_msg.sec));
rcl_duration_.nanoseconds += duration_msg.nanosec;
RCL_S_TO_NS(static_cast<rcl_duration_value_t>(duration_msg.sec));
rcl_duration_.nanoseconds += static_cast<rcl_duration_value_t>(duration_msg.nanosec);
}

Duration::Duration(const rcl_duration_t & duration)
Expand All @@ -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<std::int32_t>(RCL_NS_TO_S(rcl_duration_.nanoseconds));
msg_duration.nanosec =
static_cast<std::uint32_t>(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<std::int32_t>(result.quot);
msg_duration.nanosec = static_cast<std::uint32_t>(result.rem);
} else {
msg_duration.sec = static_cast<std::int32_t>(result.quot - 1);
msg_duration.nanosec = static_cast<std::uint32_t>(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<int64_t>(duration_msg.sec));
rcl_duration_.nanoseconds += duration_msg.nanosec;
rcl_duration_.nanoseconds = RCL_S_TO_NS(static_cast<rcl_duration_value_t>(duration_msg.sec));
rcl_duration_.nanoseconds += static_cast<rcl_duration_value_t>(duration_msg.nanosec);
return *this;
}

Expand Down Expand Up @@ -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;
Expand Down
23 changes: 11 additions & 12 deletions rclcpp/src/rclcpp/time.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -95,17 +91,20 @@ Time::~Time()
Time::operator builtin_interfaces::msg::Time() const
{
builtin_interfaces::msg::Time msg_time;
msg_time.sec = static_cast<std::int32_t>(RCL_NS_TO_S(rcl_time_.nanoseconds));
msg_time.nanosec = static_cast<std::uint32_t>(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<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;
}

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)
Expand Down
91 changes: 87 additions & 4 deletions rclcpp/test/rclcpp/test_duration.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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) {
Expand All @@ -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<builtin_interfaces::msg::Duration>(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<uint64_t>(HALF_SEC_IN_NS));

const auto chrono_duration = duration.to_chrono<std::chrono::nanoseconds>();
EXPECT_EQ(chrono_duration.count(), HALF_SEC_IN_NS);
}

{
const rclcpp::Duration duration(ONE_SEC_IN_NS);
const auto duration_msg = static_cast<builtin_interfaces::msg::Duration>(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<std::chrono::nanoseconds>();
EXPECT_EQ(chrono_duration.count(), ONE_SEC_IN_NS);
}

{
const rclcpp::Duration duration(ONE_AND_HALF_SEC_IN_NS);
auto duration_msg = static_cast<builtin_interfaces::msg::Duration>(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<uint64_t>(HALF_SEC_IN_NS));

auto chrono_duration = duration.to_chrono<std::chrono::nanoseconds>();
EXPECT_EQ(chrono_duration.count(), ONE_AND_HALF_SEC_IN_NS);
}

{
rclcpp::Duration duration(-HALF_SEC_IN_NS);
auto duration_msg = static_cast<builtin_interfaces::msg::Duration>(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<std::chrono::nanoseconds>();
EXPECT_EQ(chrono_duration.count(), -HALF_SEC_IN_NS);
}

{
rclcpp::Duration duration(-ONE_SEC_IN_NS);
auto duration_msg = static_cast<builtin_interfaces::msg::Duration>(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<std::chrono::nanoseconds>();
EXPECT_EQ(chrono_duration.count(), -ONE_SEC_IN_NS);
}

{
rclcpp::Duration duration(-ONE_AND_HALF_SEC_IN_NS);
auto duration_msg = static_cast<builtin_interfaces::msg::Duration>(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<std::chrono::nanoseconds>();
EXPECT_EQ(chrono_duration.count(), -ONE_AND_HALF_SEC_IN_NS);
}
}
123 changes: 100 additions & 23 deletions rclcpp/test/rclcpp/test_time.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<int64_t>(msg.sec)) + static_cast<int64_t>(msg.nanosec),
time.nanoseconds());
EXPECT_EQ(static_cast<int64_t>(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<builtin_interfaces::msg::Time>(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<int64_t>(msg.sec)) + static_cast<int64_t>(msg.nanosec),
time.nanoseconds());
EXPECT_EQ(static_cast<int64_t>(msg.sec), RCL_NS_TO_S(time.nanoseconds()));
{
const rclcpp::Time time(ONE_SEC_IN_NS);
const auto time_msg = static_cast<builtin_interfaces::msg::Time>(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<builtin_interfaces::msg::Time>(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<builtin_interfaces::msg::Time>(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<builtin_interfaces::msg::Time>(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<builtin_interfaces::msg::Time>(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) {
Expand Down

0 comments on commit 764c721

Please sign in to comment.