time operators (#351)
* time operators * explicitely cast to uint64_t and prevent overflow * check for negative seconds .. again * split into hpp/cpp * export symbols * change test macro * fix unsigned comparison * address comments * test for specific exception * Fix typo
This commit is contained in:
parent
d090ddc358
commit
def973e3dd
4 changed files with 371 additions and 61 deletions
|
@ -58,6 +58,7 @@ set(${PROJECT_NAME}_SRCS
|
|||
src/rclcpp/publisher.cpp
|
||||
src/rclcpp/service.cpp
|
||||
src/rclcpp/subscription.cpp
|
||||
src/rclcpp/time.cpp
|
||||
src/rclcpp/timer.cpp
|
||||
src/rclcpp/type_support.cpp
|
||||
src/rclcpp/utilities.cpp
|
||||
|
|
|
@ -15,84 +15,77 @@
|
|||
#ifndef RCLCPP__TIME_HPP_
|
||||
#define RCLCPP__TIME_HPP_
|
||||
|
||||
#include <utility>
|
||||
|
||||
#include "builtin_interfaces/msg/time.hpp"
|
||||
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
|
||||
#include "rcl/time.h"
|
||||
|
||||
#include "rclcpp/exceptions.hpp"
|
||||
|
||||
#include "rcutils/logging_macros.h"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
|
||||
class Time
|
||||
{
|
||||
public:
|
||||
template<rcl_time_source_type_t ClockT = RCL_SYSTEM_TIME>
|
||||
static Time
|
||||
now()
|
||||
{
|
||||
// TODO(karsten1987): This impl throws explicitely on RCL_ROS_TIME
|
||||
// we have to do this, because rcl_time_source_init returns RCL_RET_OK
|
||||
// for RCL_ROS_TIME, however defaults to system time under the hood.
|
||||
// ref: https://github.com/ros2/rcl/blob/master/rcl/src/rcl/time.c#L66-L77
|
||||
// This section can be removed when rcl supports ROS_TIME correctly.
|
||||
if (ClockT == RCL_ROS_TIME) {
|
||||
throw std::runtime_error("RCL_ROS_TIME is currently not implemented.");
|
||||
}
|
||||
RCLCPP_PUBLIC
|
||||
static
|
||||
Time
|
||||
now(rcl_time_source_type_t clock = RCL_SYSTEM_TIME);
|
||||
|
||||
rcl_ret_t ret = RCL_RET_ERROR;
|
||||
RCLCPP_PUBLIC
|
||||
Time(int32_t seconds, uint32_t nanoseconds, rcl_time_source_type_t clock = RCL_SYSTEM_TIME);
|
||||
|
||||
rcl_time_source_t time_source;
|
||||
ret = rcl_time_source_init(ClockT, &time_source);
|
||||
RCLCPP_PUBLIC
|
||||
explicit Time(uint64_t nanoseconds, rcl_time_source_type_t clock = RCL_SYSTEM_TIME);
|
||||
|
||||
if (ret != RCL_RET_OK) {
|
||||
rclcpp::exceptions::throw_from_rcl_error(
|
||||
ret, "could not initialize time source: ");
|
||||
}
|
||||
RCLCPP_PUBLIC
|
||||
Time(const builtin_interfaces::msg::Time & time_msg); // NOLINT
|
||||
|
||||
rcl_time_point_t time_point;
|
||||
ret = rcl_time_point_init(&time_point, &time_source);
|
||||
RCLCPP_PUBLIC
|
||||
virtual ~Time();
|
||||
|
||||
if (ret != RCL_RET_OK) {
|
||||
rclcpp::exceptions::throw_from_rcl_error(
|
||||
ret, "could not initialize time point: ");
|
||||
}
|
||||
RCLCPP_PUBLIC
|
||||
operator builtin_interfaces::msg::Time() const;
|
||||
|
||||
ret = rcl_time_point_get_now(&time_point);
|
||||
if (ret != RCL_RET_OK) {
|
||||
rclcpp::exceptions::throw_from_rcl_error(
|
||||
ret, "could not get current time stamp: ");
|
||||
}
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
operator=(const builtin_interfaces::msg::Time & time_msg);
|
||||
|
||||
return Time(std::move(time_point));
|
||||
}
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
operator==(const rclcpp::Time & rhs) const;
|
||||
|
||||
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));
|
||||
return msg_time;
|
||||
}
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
operator<(const rclcpp::Time & rhs) const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
operator<=(const rclcpp::Time & rhs) const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
operator>=(const rclcpp::Time & rhs) const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
operator>(const rclcpp::Time & rhs) const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
Time
|
||||
operator+(const rclcpp::Time & rhs) const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
Time
|
||||
operator-(const rclcpp::Time & rhs) const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
uint64_t
|
||||
nanoseconds() const;
|
||||
|
||||
private:
|
||||
rcl_time_source_t rcl_time_source_;
|
||||
rcl_time_point_t rcl_time_;
|
||||
|
||||
explicit Time(rcl_time_point_t && rcl_time)
|
||||
: rcl_time_(std::forward<decltype(rcl_time)>(rcl_time))
|
||||
{}
|
||||
|
||||
public:
|
||||
virtual ~Time()
|
||||
{
|
||||
if (rcl_time_point_fini(&rcl_time_) != RCL_RET_OK) {
|
||||
RCUTILS_LOG_FATAL("failed to reclaim rcl_time_point_t in destructor of rclcpp::Time")
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace rclcpp
|
||||
|
|
232
rclcpp/src/rclcpp/time.cpp
Normal file
232
rclcpp/src/rclcpp/time.cpp
Normal file
|
@ -0,0 +1,232 @@
|
|||
// Copyright 2017 Open Source Robotics Foundation, Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include "rclcpp/time.hpp"
|
||||
|
||||
#include <utility>
|
||||
|
||||
#include "builtin_interfaces/msg/time.hpp"
|
||||
|
||||
#include "rcl/time.h"
|
||||
|
||||
#include "rclcpp/exceptions.hpp"
|
||||
|
||||
#include "rcutils/logging_macros.h"
|
||||
|
||||
namespace
|
||||
{
|
||||
|
||||
rcl_time_source_t
|
||||
init_time_source(rcl_time_source_type_t clock = RCL_SYSTEM_TIME)
|
||||
{
|
||||
rcl_time_source_t time_source;
|
||||
auto ret = rcl_time_source_init(clock, &time_source);
|
||||
|
||||
if (ret != RCL_RET_OK) {
|
||||
rclcpp::exceptions::throw_from_rcl_error(
|
||||
ret, "could not initialize time source");
|
||||
}
|
||||
|
||||
return time_source;
|
||||
}
|
||||
|
||||
rcl_time_point_t
|
||||
init_time_point(rcl_time_source_t & time_source)
|
||||
{
|
||||
rcl_time_point_t time_point;
|
||||
auto ret = rcl_time_point_init(&time_point, &time_source);
|
||||
|
||||
if (ret != RCL_RET_OK) {
|
||||
rclcpp::exceptions::throw_from_rcl_error(
|
||||
ret, "could not initialize time point");
|
||||
}
|
||||
|
||||
return time_point;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
|
||||
Time
|
||||
Time::now(rcl_time_source_type_t clock)
|
||||
{
|
||||
// TODO(karsten1987): This impl throws explicitely on RCL_ROS_TIME
|
||||
// we have to do this, because rcl_time_source_init returns RCL_RET_OK
|
||||
// for RCL_ROS_TIME, however defaults to system time under the hood.
|
||||
// ref: https://github.com/ros2/rcl/blob/master/rcl/src/rcl/time.c#L66-L77
|
||||
// This section can be removed when rcl supports ROS_TIME correctly.
|
||||
if (clock == RCL_ROS_TIME) {
|
||||
throw std::runtime_error("RCL_ROS_TIME is currently not implemented.");
|
||||
}
|
||||
|
||||
Time now(0, 0, clock);
|
||||
|
||||
auto ret = rcl_time_point_get_now(&now.rcl_time_);
|
||||
if (ret != RCL_RET_OK) {
|
||||
rclcpp::exceptions::throw_from_rcl_error(
|
||||
ret, "could not get current time stamp");
|
||||
}
|
||||
|
||||
return now;
|
||||
}
|
||||
|
||||
Time::Time(int32_t seconds, uint32_t nanoseconds, rcl_time_source_type_t clock)
|
||||
: rcl_time_source_(init_time_source(clock)),
|
||||
rcl_time_(init_time_point(rcl_time_source_))
|
||||
{
|
||||
if (seconds < 0) {
|
||||
throw std::runtime_error("cannot store a negative time point in rclcpp::Time");
|
||||
}
|
||||
|
||||
rcl_time_.nanoseconds = RCL_S_TO_NS(static_cast<uint64_t>(seconds));
|
||||
rcl_time_.nanoseconds += nanoseconds;
|
||||
}
|
||||
|
||||
Time::Time(uint64_t nanoseconds, rcl_time_source_type_t clock)
|
||||
: rcl_time_source_(init_time_source(clock)),
|
||||
rcl_time_(init_time_point(rcl_time_source_))
|
||||
{
|
||||
rcl_time_.nanoseconds = nanoseconds;
|
||||
}
|
||||
|
||||
Time::Time(const builtin_interfaces::msg::Time & time_msg) // NOLINT
|
||||
: rcl_time_source_(init_time_source(RCL_SYSTEM_TIME)),
|
||||
rcl_time_(init_time_point(rcl_time_source_))
|
||||
{
|
||||
if (time_msg.sec < 0) {
|
||||
throw std::runtime_error("cannot store a negative time point in rclcpp::Time");
|
||||
}
|
||||
|
||||
rcl_time_.nanoseconds = RCL_S_TO_NS(static_cast<uint64_t>(time_msg.sec));
|
||||
rcl_time_.nanoseconds += time_msg.nanosec;
|
||||
}
|
||||
|
||||
Time::~Time()
|
||||
{
|
||||
if (rcl_time_point_fini(&rcl_time_) != RCL_RET_OK) {
|
||||
RCUTILS_LOG_FATAL("failed to reclaim rcl_time_point_t in destructor of rclcpp::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));
|
||||
return msg_time;
|
||||
}
|
||||
|
||||
void
|
||||
Time::operator=(const builtin_interfaces::msg::Time & time_msg)
|
||||
{
|
||||
if (time_msg.sec < 0) {
|
||||
throw std::runtime_error("cannot store a negative time point in rclcpp::Time");
|
||||
}
|
||||
|
||||
this->rcl_time_source_ = init_time_source();
|
||||
this->rcl_time_ = init_time_point(this->rcl_time_source_);
|
||||
|
||||
rcl_time_.nanoseconds = RCL_S_TO_NS(static_cast<uint64_t>(time_msg.sec));
|
||||
rcl_time_.nanoseconds += time_msg.nanosec;
|
||||
}
|
||||
|
||||
bool
|
||||
Time::operator==(const rclcpp::Time & rhs) const
|
||||
{
|
||||
if (rcl_time_.time_source->type != rhs.rcl_time_.time_source->type) {
|
||||
throw std::runtime_error("can't compare times with different time sources");
|
||||
}
|
||||
|
||||
return rcl_time_.nanoseconds == rhs.rcl_time_.nanoseconds;
|
||||
}
|
||||
|
||||
bool
|
||||
Time::operator<(const rclcpp::Time & rhs) const
|
||||
{
|
||||
if (rcl_time_.time_source->type != rhs.rcl_time_.time_source->type) {
|
||||
throw std::runtime_error("can't compare times with different time sources");
|
||||
}
|
||||
|
||||
return rcl_time_.nanoseconds < rhs.rcl_time_.nanoseconds;
|
||||
}
|
||||
|
||||
bool
|
||||
Time::operator<=(const rclcpp::Time & rhs) const
|
||||
{
|
||||
if (rcl_time_.time_source->type != rhs.rcl_time_.time_source->type) {
|
||||
throw std::runtime_error("can't compare times with different time sources");
|
||||
}
|
||||
|
||||
return rcl_time_.nanoseconds <= rhs.rcl_time_.nanoseconds;
|
||||
}
|
||||
|
||||
bool
|
||||
Time::operator>=(const rclcpp::Time & rhs) const
|
||||
{
|
||||
if (rcl_time_.time_source->type != rhs.rcl_time_.time_source->type) {
|
||||
throw std::runtime_error("can't compare times with different time sources");
|
||||
}
|
||||
|
||||
return rcl_time_.nanoseconds >= rhs.rcl_time_.nanoseconds;
|
||||
}
|
||||
|
||||
bool
|
||||
Time::operator>(const rclcpp::Time & rhs) const
|
||||
{
|
||||
if (rcl_time_.time_source->type != rhs.rcl_time_.time_source->type) {
|
||||
throw std::runtime_error("can't compare times with different time sources");
|
||||
}
|
||||
|
||||
return rcl_time_.nanoseconds > rhs.rcl_time_.nanoseconds;
|
||||
}
|
||||
|
||||
Time
|
||||
Time::operator+(const rclcpp::Time & rhs) const
|
||||
{
|
||||
if (rcl_time_.time_source->type != rhs.rcl_time_.time_source->type) {
|
||||
throw std::runtime_error("can't add times with different time sources");
|
||||
}
|
||||
|
||||
auto ns = rcl_time_.nanoseconds + rhs.rcl_time_.nanoseconds;
|
||||
if (ns < rcl_time_.nanoseconds) {
|
||||
throw std::overflow_error("addition leads to uint64_t overflow");
|
||||
}
|
||||
|
||||
return Time(rcl_time_.nanoseconds + rhs.rcl_time_.nanoseconds);
|
||||
}
|
||||
|
||||
Time
|
||||
Time::operator-(const rclcpp::Time & rhs) const
|
||||
{
|
||||
if (rcl_time_.time_source->type != rhs.rcl_time_.time_source->type) {
|
||||
throw std::runtime_error("can't add times with different time sources");
|
||||
}
|
||||
|
||||
auto ns = rcl_time_.nanoseconds - rhs.rcl_time_.nanoseconds;
|
||||
if (ns > rcl_time_.nanoseconds) {
|
||||
throw std::underflow_error("subtraction leads to uint64_t underflow");
|
||||
}
|
||||
|
||||
return Time(rcl_time_.nanoseconds - rhs.rcl_time_.nanoseconds);
|
||||
}
|
||||
|
||||
uint64_t
|
||||
Time::nanoseconds() const
|
||||
{
|
||||
return rcl_time_.nanoseconds;
|
||||
}
|
||||
|
||||
} // namespace rclcpp
|
|
@ -14,6 +14,8 @@
|
|||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <algorithm>
|
||||
#include <limits>
|
||||
#include <string>
|
||||
|
||||
#include "rcl/error_handling.h"
|
||||
|
@ -30,16 +32,16 @@ protected:
|
|||
}
|
||||
};
|
||||
|
||||
TEST(TestTime, rate_basics) {
|
||||
TEST(TestTime, time_sources) {
|
||||
using builtin_interfaces::msg::Time;
|
||||
// TODO(Karsten1987): Fix this test once ROS_TIME is implemented
|
||||
EXPECT_ANY_THROW(rclcpp::Time::now<RCL_ROS_TIME>());
|
||||
EXPECT_ANY_THROW(rclcpp::Time::now(RCL_ROS_TIME));
|
||||
|
||||
Time system_now = rclcpp::Time::now<RCL_SYSTEM_TIME>();
|
||||
Time system_now = rclcpp::Time::now(RCL_SYSTEM_TIME);
|
||||
EXPECT_NE(0, system_now.sec);
|
||||
EXPECT_NE(0u, system_now.nanosec);
|
||||
|
||||
Time steady_now = rclcpp::Time::now<RCL_STEADY_TIME>();
|
||||
Time steady_now = rclcpp::Time::now(RCL_STEADY_TIME);
|
||||
EXPECT_NE(0, steady_now.sec);
|
||||
EXPECT_NE(0u, steady_now.nanosec);
|
||||
|
||||
|
@ -48,3 +50,85 @@ TEST(TestTime, rate_basics) {
|
|||
EXPECT_NE(0, default_now.sec);
|
||||
EXPECT_NE(0u, default_now.nanosec);
|
||||
}
|
||||
|
||||
TEST(TestTime, convertions) {
|
||||
rclcpp::Time now = rclcpp::Time::now();
|
||||
builtin_interfaces::msg::Time now_msg = now;
|
||||
|
||||
rclcpp::Time now_again = now_msg;
|
||||
EXPECT_EQ(now.nanoseconds(), now_again.nanoseconds());
|
||||
|
||||
builtin_interfaces::msg::Time msg;
|
||||
msg.sec = 12345;
|
||||
msg.nanosec = 67890;
|
||||
|
||||
rclcpp::Time time = msg;
|
||||
EXPECT_EQ(
|
||||
RCL_S_TO_NS(static_cast<uint64_t>(msg.sec)) + static_cast<uint64_t>(msg.nanosec),
|
||||
time.nanoseconds());
|
||||
EXPECT_EQ(static_cast<uint64_t>(msg.sec), RCL_NS_TO_S(time.nanoseconds()));
|
||||
|
||||
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;
|
||||
});
|
||||
}
|
||||
|
||||
TEST(TestTime, operators) {
|
||||
rclcpp::Time old(1, 0);
|
||||
rclcpp::Time young(2, 0);
|
||||
|
||||
EXPECT_TRUE(old < young);
|
||||
EXPECT_TRUE(young > old);
|
||||
EXPECT_TRUE(old <= young);
|
||||
EXPECT_TRUE(young >= old);
|
||||
EXPECT_FALSE(young == old);
|
||||
|
||||
rclcpp::Time add = old + young;
|
||||
EXPECT_EQ(add.nanoseconds(), old.nanoseconds() + young.nanoseconds());
|
||||
EXPECT_EQ(add, old + young);
|
||||
|
||||
rclcpp::Time sub = young - old;
|
||||
EXPECT_EQ(sub.nanoseconds(), young.nanoseconds() - old.nanoseconds());
|
||||
EXPECT_EQ(sub, young - old);
|
||||
|
||||
rclcpp::Time system_time(1, 0, RCL_SYSTEM_TIME);
|
||||
rclcpp::Time steady_time(2, 0, RCL_STEADY_TIME);
|
||||
|
||||
EXPECT_ANY_THROW((void)(system_time == steady_time));
|
||||
EXPECT_ANY_THROW((void)(system_time <= steady_time));
|
||||
EXPECT_ANY_THROW((void)(system_time >= steady_time));
|
||||
EXPECT_ANY_THROW((void)(system_time < steady_time));
|
||||
EXPECT_ANY_THROW((void)(system_time > steady_time));
|
||||
EXPECT_ANY_THROW((void)(system_time + steady_time));
|
||||
EXPECT_ANY_THROW((void)(system_time - steady_time));
|
||||
|
||||
rclcpp::Time now = rclcpp::Time::now(RCL_SYSTEM_TIME);
|
||||
rclcpp::Time later = rclcpp::Time::now(RCL_STEADY_TIME);
|
||||
|
||||
EXPECT_ANY_THROW((void)(now == later));
|
||||
EXPECT_ANY_THROW((void)(now <= later));
|
||||
EXPECT_ANY_THROW((void)(now >= later));
|
||||
EXPECT_ANY_THROW((void)(now < later));
|
||||
EXPECT_ANY_THROW((void)(now > later));
|
||||
EXPECT_ANY_THROW((void)(now + later));
|
||||
EXPECT_ANY_THROW((void)(now - later));
|
||||
}
|
||||
|
||||
TEST(TestTime, overflows) {
|
||||
rclcpp::Time max(std::numeric_limits<uint64_t>::max());
|
||||
rclcpp::Time one(1);
|
||||
|
||||
EXPECT_THROW(max + one, std::overflow_error);
|
||||
EXPECT_THROW(one - max, std::underflow_error);
|
||||
}
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue