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:
Karsten Knese 2017-08-08 15:18:17 -07:00 committed by GitHub
parent d090ddc358
commit def973e3dd
4 changed files with 371 additions and 61 deletions

View file

@ -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

View file

@ -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
View 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

View file

@ -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);
}