From def973e3dd31a5b8ff4b70e3472ed850b5dff025 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 8 Aug 2017 15:18:17 -0700 Subject: [PATCH] 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 --- rclcpp/CMakeLists.txt | 1 + rclcpp/include/rclcpp/time.hpp | 107 +++++++-------- rclcpp/src/rclcpp/time.cpp | 232 +++++++++++++++++++++++++++++++++ rclcpp/test/test_time.cpp | 92 ++++++++++++- 4 files changed, 371 insertions(+), 61 deletions(-) create mode 100644 rclcpp/src/rclcpp/time.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 33e291f..5800113 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -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 diff --git a/rclcpp/include/rclcpp/time.hpp b/rclcpp/include/rclcpp/time.hpp index c9b92d6..7e9b99a 100644 --- a/rclcpp/include/rclcpp/time.hpp +++ b/rclcpp/include/rclcpp/time.hpp @@ -15,84 +15,77 @@ #ifndef RCLCPP__TIME_HPP_ #define RCLCPP__TIME_HPP_ -#include - #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 - 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(RCL_NS_TO_S(rcl_time_.nanoseconds)); - msg_time.nanosec = static_cast(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(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 diff --git a/rclcpp/src/rclcpp/time.cpp b/rclcpp/src/rclcpp/time.cpp new file mode 100644 index 0000000..102052d --- /dev/null +++ b/rclcpp/src/rclcpp/time.cpp @@ -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 + +#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(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(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(RCL_NS_TO_S(rcl_time_.nanoseconds)); + msg_time.nanosec = static_cast(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(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 diff --git a/rclcpp/test/test_time.cpp b/rclcpp/test/test_time.cpp index b879b89..153569a 100644 --- a/rclcpp/test/test_time.cpp +++ b/rclcpp/test/test_time.cpp @@ -14,6 +14,8 @@ #include +#include +#include #include #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()); + EXPECT_ANY_THROW(rclcpp::Time::now(RCL_ROS_TIME)); - Time system_now = rclcpp::Time::now(); + 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(); + 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(msg.sec)) + static_cast(msg.nanosec), + time.nanoseconds()); + EXPECT_EQ(static_cast(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::max()); + rclcpp::Time one(1); + + EXPECT_THROW(max + one, std::overflow_error); + EXPECT_THROW(one - max, std::underflow_error); +}