diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 7fb7fc9..fcce5dd 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -31,8 +31,10 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/any_executable.cpp src/rclcpp/callback_group.cpp src/rclcpp/client.cpp + src/rclcpp/clock.cpp src/rclcpp/context.cpp src/rclcpp/contexts/default_context.cpp + src/rclcpp/duration.cpp src/rclcpp/event.cpp src/rclcpp/exceptions.cpp src/rclcpp/executor.cpp @@ -59,6 +61,7 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/service.cpp src/rclcpp/subscription.cpp src/rclcpp/time.cpp + src/rclcpp/time_source.cpp src/rclcpp/timer.cpp src/rclcpp/type_support.cpp src/rclcpp/utilities.cpp @@ -266,6 +269,17 @@ if(BUILD_TESTING) endforeach() endif() + ament_add_gtest(test_duration test/test_duration.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") + if(TARGET test_duration) + ament_target_dependencies(test_duration + "rcl") + target_link_libraries(test_duration ${PROJECT_NAME}) + endif() + + ament_add_gmock(test_logging test/test_logging.cpp) + target_link_libraries(test_logging ${PROJECT_NAME}) + ament_add_gtest(test_time test/test_time.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") if(TARGET test_time) @@ -274,8 +288,13 @@ if(BUILD_TESTING) target_link_libraries(test_time ${PROJECT_NAME}) endif() - ament_add_gmock(test_logging test/test_logging.cpp) - target_link_libraries(test_logging ${PROJECT_NAME}) + ament_add_gtest(test_time_source test/test_time_source.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") + if(TARGET test_time_source) + ament_target_dependencies(test_time_source + "rcl") + target_link_libraries(test_time_source ${PROJECT_NAME}) + endif() endif() ament_package( diff --git a/rclcpp/include/rclcpp/clock.hpp b/rclcpp/include/rclcpp/clock.hpp new file mode 100644 index 0000000..a3b440e --- /dev/null +++ b/rclcpp/include/rclcpp/clock.hpp @@ -0,0 +1,143 @@ +// 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. + +#ifndef RCLCPP__CLOCK_HPP_ +#define RCLCPP__CLOCK_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/macros.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/visibility_control.hpp" + +#include "rcl/time.h" + +namespace rclcpp +{ + +class TimeSource; + +/* \brief A struct to represent a timejump. + * It epresents the time jump duration and whether it changed clock type. + */ +struct TimeJump +{ + typedef enum ClockChange_t + { + ROS_TIME_NO_CHANGE, ///< The time type before and after the jump is ROS_TIME + ROS_TIME_ACTIVATED, ///< The time type switched to ROS_TIME from SYSTEM_TIME + ROS_TIME_DEACTIVATED, ///< The time type switched to SYSTEM_TIME from ROS_TIME + SYSTEM_TIME_NO_CHANGE ///< The time type before and after the jump is SYSTEM_TIME + } ClockChange_t; + + ClockChange_t jump_type_; ///< The change in clock_type if there is one. + rcl_duration_t delta_; ///< The change in time value. +}; + +/* \brief A class to store a threshold for a TimeJump + * This class can be used to evaluate a time jump's magnitude. +*/ +class JumpThreshold +{ +public: + uint64_t min_forward_; ///< The minimum jump forward to be considered exceeded.. + uint64_t min_backward_; ///< The minimum backwards jump to be considered exceeded. + bool on_clock_change_; ///< Whether to trigger on any clock type change. + + // Test if the threshold is exceeded by a TimeJump + RCLCPP_PUBLIC + bool + is_exceeded(const TimeJump & jump); +}; + +class JumpHandler +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(JumpHandler) + JumpHandler( + std::function pre_callback, + std::function post_callback, + JumpThreshold & threshold); + + std::function pre_callback; + std::function post_callback; + JumpThreshold notice_threshold; +}; + +class Clock +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(Clock) + + RCLCPP_PUBLIC + explicit Clock(rcl_clock_type_t clock_type = RCL_SYSTEM_TIME); + + RCLCPP_PUBLIC + ~Clock(); + + RCLCPP_PUBLIC + Time + now(); + + RCLCPP_PUBLIC + bool + ros_time_is_active(); + + RCLCPP_PUBLIC + rcl_clock_type_t + get_clock_type(); + + // Add a callback to invoke if the jump threshold is exceeded. + /** + * These callback functions must remain valid as long as the + * returned shared pointer is valid. + */ + RCLCPP_PUBLIC + JumpHandler::SharedPtr + create_jump_callback( + std::function pre_callback, + std::function post_callback, + JumpThreshold & threshold); + +private: + // A method for TimeSource to get a list of callbacks to invoke while updating + RCLCPP_PUBLIC + std::vector + get_triggered_callback_handlers(const TimeJump & jump); + + // Invoke callbacks that are valid and outside threshold. + RCLCPP_PUBLIC + static void invoke_prejump_callbacks( + const std::vector & callbacks); + + RCLCPP_PUBLIC + static void invoke_postjump_callbacks( + const std::vector & callbacks, + const TimeJump & jump); + + /// Internal storage backed by rcl + rcl_clock_t rcl_clock_; + friend TimeSource; /// Allow TimeSource to access the rcl_clock_ datatype. + rcl_allocator_t allocator_; + + std::mutex callback_list_mutex_; + std::vector> active_jump_handlers_; +}; + +} // namespace rclcpp + +#endif // RCLCPP__CLOCK_HPP_ diff --git a/rclcpp/include/rclcpp/duration.hpp b/rclcpp/include/rclcpp/duration.hpp new file mode 100644 index 0000000..102ab74 --- /dev/null +++ b/rclcpp/include/rclcpp/duration.hpp @@ -0,0 +1,102 @@ +// 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. + +#ifndef RCLCPP__DURATION_HPP_ +#define RCLCPP__DURATION_HPP_ + +#include + +#include "builtin_interfaces/msg/duration.hpp" +#include "rcl/time.h" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +class Duration +{ +public: + RCLCPP_PUBLIC + Duration(int32_t seconds, uint32_t nanoseconds); + + RCLCPP_PUBLIC + explicit Duration( + rcl_duration_value_t nanoseconds); + + RCLCPP_PUBLIC + explicit Duration( + std::chrono::nanoseconds nanoseconds); + + RCLCPP_PUBLIC + Duration( + const builtin_interfaces::msg::Duration & duration_msg); + + RCLCPP_PUBLIC + explicit Duration(const rcl_duration_t & duration); + + RCLCPP_PUBLIC + Duration(const Duration & rhs); + + RCLCPP_PUBLIC + virtual ~Duration(); + + RCLCPP_PUBLIC + operator builtin_interfaces::msg::Duration() const; + + RCLCPP_PUBLIC + Duration & + operator=(const Duration & rhs); + + RCLCPP_PUBLIC + Duration & + operator=(const builtin_interfaces::msg::Duration & Duration_msg); + + RCLCPP_PUBLIC + bool + operator==(const rclcpp::Duration & rhs) const; + + RCLCPP_PUBLIC + bool + operator<(const rclcpp::Duration & rhs) const; + + RCLCPP_PUBLIC + bool + operator<=(const rclcpp::Duration & rhs) const; + + RCLCPP_PUBLIC + bool + operator>=(const rclcpp::Duration & rhs) const; + + RCLCPP_PUBLIC + bool + operator>(const rclcpp::Duration & rhs) const; + + RCLCPP_PUBLIC + Duration + operator+(const rclcpp::Duration & rhs) const; + + RCLCPP_PUBLIC + Duration + operator-(const rclcpp::Duration & rhs) const; + + RCLCPP_PUBLIC + rcl_duration_value_t + nanoseconds() const; + +private: + rcl_duration_t rcl_duration_; +}; + +} // namespace rclcpp + +#endif // RCLCPP__DURATION_HPP_ diff --git a/rclcpp/include/rclcpp/time.hpp b/rclcpp/include/rclcpp/time.hpp index cb38caa..f6c8df8 100644 --- a/rclcpp/include/rclcpp/time.hpp +++ b/rclcpp/include/rclcpp/time.hpp @@ -21,28 +21,32 @@ #include "rcl/time.h" +#include "rclcpp/duration.hpp" + namespace rclcpp { +class Clock; + class Time { public: RCLCPP_PUBLIC - static - Time - now(rcl_time_source_type_t clock = RCL_SYSTEM_TIME); + Time(int32_t seconds, uint32_t nanoseconds, rcl_clock_type_t clock_type = RCL_SYSTEM_TIME); RCLCPP_PUBLIC - Time(int32_t seconds, uint32_t nanoseconds, rcl_time_source_type_t clock = RCL_SYSTEM_TIME); - - RCLCPP_PUBLIC - explicit Time(uint64_t nanoseconds = 0, rcl_time_source_type_t clock = RCL_SYSTEM_TIME); + explicit Time(uint64_t nanoseconds = 0, rcl_clock_type_t clock = RCL_SYSTEM_TIME); RCLCPP_PUBLIC Time(const Time & rhs); RCLCPP_PUBLIC - Time(const builtin_interfaces::msg::Time & time_msg); // NOLINT + Time( + const builtin_interfaces::msg::Time & time_msg, + rcl_clock_type_t ros_time = RCL_ROS_TIME); + + RCLCPP_PUBLIC + explicit Time(const rcl_time_point_t & time_point); RCLCPP_PUBLIC virtual ~Time(); @@ -84,21 +88,32 @@ public: RCLCPP_PUBLIC Time - operator+(const rclcpp::Time & rhs) const; + operator+(const rclcpp::Duration & rhs) const; RCLCPP_PUBLIC - Time + Duration operator-(const rclcpp::Time & rhs) const; RCLCPP_PUBLIC - uint64_t + Time + operator-(const rclcpp::Duration & rhs) const; + + RCLCPP_PUBLIC + rcl_time_point_value_t nanoseconds() const; + RCLCPP_PUBLIC + rcl_clock_type_t + get_clock_type() const; + private: - rcl_time_source_t rcl_time_source_; rcl_time_point_t rcl_time_; + friend Clock; // Allow clock to manipulate internal data }; +Time +operator+(const rclcpp::Duration & lhs, const rclcpp::Time & rhs); + } // namespace rclcpp #endif // RCLCPP__TIME_HPP_ diff --git a/rclcpp/include/rclcpp/time_source.hpp b/rclcpp/include/rclcpp/time_source.hpp new file mode 100644 index 0000000..60de90e --- /dev/null +++ b/rclcpp/include/rclcpp/time_source.hpp @@ -0,0 +1,116 @@ +// 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. + +#ifndef RCLCPP__TIME_SOURCE_HPP_ +#define RCLCPP__TIME_SOURCE_HPP_ + +#include +#include + +#include "rcl/time.h" + +#include "rcl_interfaces/msg/parameter_event.hpp" + +#include "rclcpp/node.hpp" +#include "rclcpp/parameter_client.hpp" + + +namespace rclcpp +{ +class Clock; + +class TimeSource +{ +public: + RCLCPP_PUBLIC + explicit TimeSource(rclcpp::node::Node::SharedPtr node); + + RCLCPP_PUBLIC + TimeSource(); + + RCLCPP_PUBLIC + void attachNode(rclcpp::node::Node::SharedPtr node); + + RCLCPP_PUBLIC + void detachNode(); + + /// Attach a clock to the time source to be updated + /** + * \throws std::invalid_argument if node is nullptr + */ + RCLCPP_PUBLIC + void attachClock(rclcpp::Clock::SharedPtr clock); + + RCLCPP_PUBLIC + void detachClock(rclcpp::Clock::SharedPtr clock); + + RCLCPP_PUBLIC + ~TimeSource(); + +private: + // Preserve the node reference + rclcpp::node::Node::SharedPtr node_; + + // The subscription for the clock callback + using MessageT = builtin_interfaces::msg::Time; + using Alloc = std::allocator; + using SubscriptionT = rclcpp::subscription::Subscription; + std::shared_ptr clock_subscription_; + + // The clock callback itself + void clock_cb(const builtin_interfaces::msg::Time::SharedPtr msg); + + // Parameter Client pointer + std::shared_ptr parameter_client_; + + // Parameter Event subscription + using ParamMessageT = rcl_interfaces::msg::ParameterEvent; + using ParamSubscriptionT = rclcpp::subscription::Subscription; + std::shared_ptr parameter_subscription_; + + // Callback for parameter updates + void on_parameter_event(const rcl_interfaces::msg::ParameterEvent::SharedPtr event); + + // An enum to hold the parameter state + enum UseSimTimeParameterState {UNSET, SET_TRUE, SET_FALSE}; + UseSimTimeParameterState parameter_state_; + + // An internal method to use in the clock callback that iterates and enables all clocks + void enable_ros_time(); + // An internal method to use in the clock callback that iterates and disables all clocks + void disable_ros_time(); + + // Internal helper functions used inside iterators + static void enable_ros_time(rclcpp::Clock::SharedPtr clock); + static void disable_ros_time(rclcpp::Clock::SharedPtr clock); + static void set_clock( + const builtin_interfaces::msg::Time::SharedPtr msg, + bool set_ros_time_enabled, + rclcpp::Clock::SharedPtr clock); + + // Local storage of validity of ROS time + // This is needed when new clocks are added. + bool ros_time_active_; + // Last set message to be passed to newly registered clocks + builtin_interfaces::msg::Time::SharedPtr last_msg_set_; + + // A lock to protect iterating the associated_clocks_ field. + std::mutex clock_list_lock_; + // A vector to store references to associated clocks. + std::vector associated_clocks_; +}; + +} // namespace rclcpp + +#endif // RCLCPP__TIME_SOURCE_HPP_ diff --git a/rclcpp/src/rclcpp/clock.cpp b/rclcpp/src/rclcpp/clock.cpp new file mode 100644 index 0000000..45a13ea --- /dev/null +++ b/rclcpp/src/rclcpp/clock.cpp @@ -0,0 +1,164 @@ +// 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/clock.hpp" + +#include +#include +#include + +#include "builtin_interfaces/msg/time.hpp" + +#include "rcl/time.h" + +#include "rclcpp/exceptions.hpp" + +#include "rcutils/logging_macros.h" + +namespace rclcpp +{ +bool +JumpThreshold::is_exceeded(const TimeJump & jump) +{ + if (on_clock_change_ && + (jump.jump_type_ == TimeJump::ClockChange_t::ROS_TIME_ACTIVATED || + jump.jump_type_ == TimeJump::ClockChange_t::ROS_TIME_DEACTIVATED)) + { + return true; + } + if ((uint64_t)jump.delta_.nanoseconds > min_forward_ || + (uint64_t)jump.delta_.nanoseconds < min_backward_) + { + return true; + } + return false; +} + +JumpHandler::JumpHandler( + std::function pre_callback, + std::function post_callback, + JumpThreshold & threshold) +: pre_callback(pre_callback), + post_callback(post_callback), + notice_threshold(threshold) +{} + +Clock::Clock(rcl_clock_type_t clock_type) +{ + allocator_ = rcl_get_default_allocator(); + auto ret = rcl_clock_init(clock_type, &rcl_clock_, &allocator_); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "could not get current time stamp"); + } +} + +Clock::~Clock() +{ + auto ret = rcl_clock_fini(&rcl_clock_); + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR("Failed to fini rcl clock."); + } +} + +Time +Clock::now() +{ + Time now(0, 0, rcl_clock_.type); + + auto ret = rcl_clock_get_now(&rcl_clock_, &now.rcl_time_); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "could not get current time stamp"); + } + + return now; +} + +bool +Clock::ros_time_is_active() +{ + if (!rcl_clock_valid(&rcl_clock_)) { + RCUTILS_LOG_ERROR("ROS time not valid!"); + return false; + } + + bool is_enabled; + auto ret = rcl_is_enabled_ros_time_override(&rcl_clock_, &is_enabled); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "Failed to check ros_time_override_status"); + } + return is_enabled; +} + +rcl_clock_type_t +Clock::get_clock_type() +{ + return rcl_clock_.type; +} + +rclcpp::JumpHandler::SharedPtr +Clock::create_jump_callback( + std::function pre_callback, + std::function post_callback, + JumpThreshold & threshold) +{ + // JumpHandler jump_callback; + auto jump_callback = + std::make_shared(pre_callback, post_callback, threshold); + { + std::lock_guard guard(callback_list_mutex_); + active_jump_handlers_.push_back(jump_callback); + } + return jump_callback; +} + +std::vector +Clock::get_triggered_callback_handlers(const TimeJump & jump) +{ + std::vector callbacks; + std::lock_guard guard(callback_list_mutex_); + for (auto wjcb = active_jump_handlers_.begin(); wjcb != active_jump_handlers_.end(); wjcb++) { + if (auto jcb = wjcb->lock()) { + if (jcb->notice_threshold.is_exceeded(jump)) { + callbacks.push_back(jcb); + } + } else { + active_jump_handlers_.erase(wjcb); + } + } + return callbacks; +} + +void +Clock::invoke_prejump_callbacks( + const std::vector & callbacks) +{ + for (const auto cb : callbacks) { + cb->pre_callback(); + } +} + +void +Clock::invoke_postjump_callbacks( + const std::vector & callbacks, + const TimeJump & jump) +{ + for (auto cb : callbacks) { + cb->post_callback(jump); + } +} + +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/duration.cpp b/rclcpp/src/rclcpp/duration.cpp new file mode 100644 index 0000000..b76dc6b --- /dev/null +++ b/rclcpp/src/rclcpp/duration.cpp @@ -0,0 +1,190 @@ +// 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 +#include + +#include "rclcpp/clock.hpp" +#include "rclcpp/time.hpp" + +#include "builtin_interfaces/msg/duration.hpp" + +#include "rcl/time.h" + +#include "rclcpp/exceptions.hpp" + +#include "rcutils/logging_macros.h" + +namespace rclcpp +{ + +Duration::Duration(int32_t seconds, uint32_t nanoseconds) +{ + rcl_duration_.nanoseconds = RCL_S_TO_NS(static_cast(seconds)); + rcl_duration_.nanoseconds += nanoseconds; +} + +Duration::Duration(int64_t nanoseconds) +{ + rcl_duration_.nanoseconds = nanoseconds; +} + +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 builtin_interfaces::msg::Duration & duration_msg) +{ + rcl_duration_.nanoseconds = RCL_S_TO_NS(static_cast(duration_msg.sec)); + rcl_duration_.nanoseconds += duration_msg.nanosec; +} + +Duration::Duration(const rcl_duration_t & duration) +: rcl_duration_(duration) +{ + // noop +} + +Duration::~Duration() +{ +} + +Duration::operator builtin_interfaces::msg::Duration() const +{ + builtin_interfaces::msg::Duration msg_duration; + msg_duration.sec = static_cast(RCL_NS_TO_S(rcl_duration_.nanoseconds)); + msg_duration.nanosec = + static_cast(rcl_duration_.nanoseconds % (1000 * 1000 * 1000)); + return msg_duration; +} + +Duration & +Duration::operator=(const Duration & rhs) +{ + rcl_duration_.nanoseconds = rhs.rcl_duration_.nanoseconds; + return *this; +} + +Duration & +Duration::operator=(const builtin_interfaces::msg::Duration & duration_msg) +{ + if (duration_msg.sec < 0) { + throw std::runtime_error("cannot store a negative duration point in rclcpp::Duration"); + } + rcl_duration_.nanoseconds = RCL_S_TO_NS(static_cast(duration_msg.sec)); + rcl_duration_.nanoseconds += duration_msg.nanosec; + return *this; +} + +bool +Duration::operator==(const rclcpp::Duration & rhs) const +{ + return rcl_duration_.nanoseconds == rhs.rcl_duration_.nanoseconds; +} + +bool +Duration::operator<(const rclcpp::Duration & rhs) const +{ + return rcl_duration_.nanoseconds < rhs.rcl_duration_.nanoseconds; +} + +bool +Duration::operator<=(const rclcpp::Duration & rhs) const +{ + return rcl_duration_.nanoseconds <= rhs.rcl_duration_.nanoseconds; +} + +bool +Duration::operator>=(const rclcpp::Duration & rhs) const +{ + return rcl_duration_.nanoseconds >= rhs.rcl_duration_.nanoseconds; +} + +bool +Duration::operator>(const rclcpp::Duration & rhs) const +{ + return rcl_duration_.nanoseconds > rhs.rcl_duration_.nanoseconds; +} + +void +bounds_check_duration_sum(int64_t lhsns, int64_t rhsns, uint64_t max) +{ + auto abs_lhs = (uint64_t)std::abs(lhsns); + auto abs_rhs = (uint64_t)std::abs(rhsns); + + if (lhsns > 0 && rhsns > 0) { + if (abs_lhs + abs_rhs > (uint64_t) max) { + throw std::overflow_error("addition leads to int64_t overflow"); + } + } else if (lhsns < 0 && rhsns < 0) { + if (abs_lhs + abs_rhs > (uint64_t) max) { + throw std::underflow_error("addition leads to int64_t underflow"); + } + } +} + +Duration +Duration::operator+(const rclcpp::Duration & rhs) const +{ + bounds_check_duration_sum( + this->rcl_duration_.nanoseconds, + rhs.rcl_duration_.nanoseconds, + std::numeric_limits::max()); + return Duration( + rcl_duration_.nanoseconds + rhs.rcl_duration_.nanoseconds); +} + +void +bounds_check_duration_difference(int64_t lhsns, int64_t rhsns, uint64_t max) +{ + auto abs_lhs = (uint64_t)std::abs(lhsns); + auto abs_rhs = (uint64_t)std::abs(rhsns); + + if (lhsns > 0 && rhsns < 0) { + if (abs_lhs + abs_rhs > (uint64_t) max) { + throw std::overflow_error("duration subtraction leads to int64_t overflow"); + } + } else if (lhsns < 0 && rhsns > 0) { + if (abs_lhs + abs_rhs > (uint64_t) max) { + throw std::underflow_error("duration subtraction leads to int64_t underflow"); + } + } +} + +Duration +Duration::operator-(const rclcpp::Duration & rhs) const +{ + bounds_check_duration_difference( + this->rcl_duration_.nanoseconds, + rhs.rcl_duration_.nanoseconds, + std::numeric_limits::max()); + + return Duration( + rcl_duration_.nanoseconds - rhs.rcl_duration_.nanoseconds); +} + +rcl_duration_value_t +Duration::nanoseconds() const +{ + return rcl_duration_.nanoseconds; +} + +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/time.cpp b/rclcpp/src/rclcpp/time.cpp index 363a888..ed3afa2 100644 --- a/rclcpp/src/rclcpp/time.cpp +++ b/rclcpp/src/rclcpp/time.cpp @@ -12,10 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "rclcpp/time.hpp" - +#include #include +#include "rclcpp/clock.hpp" +#include "rclcpp/duration.hpp" + #include "builtin_interfaces/msg/time.hpp" #include "rcl/time.h" @@ -27,30 +29,11 @@ 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) +init_time_point(rcl_clock_type_t & clock_type) { 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"); - } + time_point.clock_type = clock_type; return time_point; } @@ -60,32 +43,8 @@ init_time_point(rcl_time_source_t & time_source) 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_)) +Time::Time(int32_t seconds, uint32_t nanoseconds, rcl_clock_type_t clock_type) +: rcl_time_(init_time_point(clock_type)) { if (seconds < 0) { throw std::runtime_error("cannot store a negative time point in rclcpp::Time"); @@ -95,24 +54,23 @@ Time::Time(int32_t seconds, uint32_t nanoseconds, rcl_time_source_type_t clock) 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_)) +Time::Time(uint64_t nanoseconds, rcl_clock_type_t clock_type) +: rcl_time_(init_time_point(clock_type)) { rcl_time_.nanoseconds = nanoseconds; } Time::Time(const Time & rhs) -: rcl_time_source_(init_time_source(rhs.rcl_time_source_.type)), - rcl_time_(init_time_point(rcl_time_source_)) +: rcl_time_(rhs.rcl_time_) { rcl_time_.nanoseconds = rhs.rcl_time_.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_)) +Time::Time( + const builtin_interfaces::msg::Time & time_msg, + rcl_clock_type_t ros_time) { + rcl_time_ = init_time_point(ros_time); if (time_msg.sec < 0) { throw std::runtime_error("cannot store a negative time point in rclcpp::Time"); } @@ -121,14 +79,14 @@ Time::Time(const builtin_interfaces::msg::Time & time_msg) // NOLINT rcl_time_.nanoseconds += time_msg.nanosec; } +Time::Time(const rcl_time_point_t & time_point) +: rcl_time_(time_point) +{ + // noop +} + Time::~Time() { - if (rcl_time_source_fini(&rcl_time_source_) != RCL_RET_OK) { - RCUTILS_LOG_FATAL("failed to reclaim rcl_time_source_t in destructor of rclcpp::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 @@ -142,9 +100,7 @@ Time::operator builtin_interfaces::msg::Time() const Time & Time::operator=(const Time & rhs) { - rcl_time_source_ = init_time_source(rhs.rcl_time_source_.type); - rcl_time_ = init_time_point(rcl_time_source_); - rcl_time_.nanoseconds = rhs.rcl_time_.nanoseconds; + rcl_time_ = rhs.rcl_time_; return *this; } @@ -155,8 +111,9 @@ Time::operator=(const builtin_interfaces::msg::Time & time_msg) 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_clock_type_t ros_time = RCL_ROS_TIME; + rcl_time_ = init_time_point(ros_time); // TODO(tfoote) hard coded ROS here rcl_time_.nanoseconds = RCL_S_TO_NS(static_cast(time_msg.sec)); rcl_time_.nanoseconds += time_msg.nanosec; @@ -166,7 +123,7 @@ Time::operator=(const builtin_interfaces::msg::Time & time_msg) bool Time::operator==(const rclcpp::Time & rhs) const { - if (rcl_time_.time_source->type != rhs.rcl_time_.time_source->type) { + if (rcl_time_.clock_type != rhs.rcl_time_.clock_type) { throw std::runtime_error("can't compare times with different time sources"); } @@ -182,7 +139,7 @@ Time::operator!=(const rclcpp::Time & rhs) const bool Time::operator<(const rclcpp::Time & rhs) const { - if (rcl_time_.time_source->type != rhs.rcl_time_.time_source->type) { + if (rcl_time_.clock_type != rhs.rcl_time_.clock_type) { throw std::runtime_error("can't compare times with different time sources"); } @@ -192,7 +149,7 @@ Time::operator<(const rclcpp::Time & rhs) const bool Time::operator<=(const rclcpp::Time & rhs) const { - if (rcl_time_.time_source->type != rhs.rcl_time_.time_source->type) { + if (rcl_time_.clock_type != rhs.rcl_time_.clock_type) { throw std::runtime_error("can't compare times with different time sources"); } @@ -202,7 +159,7 @@ Time::operator<=(const rclcpp::Time & rhs) const bool Time::operator>=(const rclcpp::Time & rhs) const { - if (rcl_time_.time_source->type != rhs.rcl_time_.time_source->type) { + if (rcl_time_.clock_type != rhs.rcl_time_.clock_type) { throw std::runtime_error("can't compare times with different time sources"); } @@ -212,7 +169,7 @@ Time::operator>=(const rclcpp::Time & rhs) const bool Time::operator>(const rclcpp::Time & rhs) const { - if (rcl_time_.time_source->type != rhs.rcl_time_.time_source->type) { + if (rcl_time_.clock_type != rhs.rcl_time_.clock_type) { throw std::runtime_error("can't compare times with different time sources"); } @@ -220,33 +177,55 @@ Time::operator>(const rclcpp::Time & rhs) const } Time -Time::operator+(const rclcpp::Time & rhs) const +Time::operator+(const rclcpp::Duration & 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) { + if (rhs.nanoseconds() > 0 && (uint64_t)rhs.nanoseconds() > + std::numeric_limits::max() - + (rcl_time_point_value_t)this->nanoseconds()) + { throw std::overflow_error("addition leads to uint64_t overflow"); } + return Time(this->nanoseconds() + rhs.nanoseconds(), this->get_clock_type()); +} - return Time(rcl_time_.nanoseconds + rhs.rcl_time_.nanoseconds); +Duration +Time::operator-(const rclcpp::Time & rhs) const +{ + if (rcl_time_.clock_type != rhs.rcl_time_.clock_type) { + throw std::runtime_error("can't subtract times with different time sources"); + } + + if (rcl_time_.nanoseconds > + (uint64_t)std::numeric_limits::max() + rhs.rcl_time_.nanoseconds) + { + throw std::underflow_error("time subtraction leads to int64_t overflow"); + } + + if (rcl_time_.nanoseconds < rhs.rcl_time_.nanoseconds) { + rcl_time_point_value_t negative_delta = rhs.rcl_time_.nanoseconds - rcl_time_.nanoseconds; + if (negative_delta > (uint64_t) std::numeric_limits::min()) { + throw std::underflow_error("time subtraction leads to int64_t underflow"); + } + } + + return Duration(rcl_time_.nanoseconds - rhs.rcl_time_.nanoseconds); } Time -Time::operator-(const rclcpp::Time & rhs) const +Time::operator-(const rclcpp::Duration & 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"); + if (rhs.nanoseconds() > 0 && rcl_time_.nanoseconds > + std::numeric_limits::max() - (uint64_t)rhs.nanoseconds()) + { + throw std::underflow_error("time subtraction leads to uint64_t overflow"); + } + if (rcl_time_.nanoseconds < (uint64_t) std::numeric_limits::max() && + (int64_t)rcl_time_.nanoseconds < (int64_t)rhs.nanoseconds()) + { + throw std::underflow_error("time subtraction leads to uint64_t underflow"); } - 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); + return Time(rcl_time_.nanoseconds - rhs.nanoseconds(), rcl_time_.clock_type); } uint64_t @@ -255,4 +234,22 @@ Time::nanoseconds() const return rcl_time_.nanoseconds; } +rcl_clock_type_t +Time::get_clock_type() const +{ + return rcl_time_.clock_type; +} + +Time +operator+(const rclcpp::Duration & lhs, const rclcpp::Time & rhs) +{ + if (rhs.nanoseconds() > + std::numeric_limits::max() - (rcl_time_point_value_t)lhs.nanoseconds()) + { + throw std::overflow_error("addition leads to uint64_t overflow"); + } + return Time(lhs.nanoseconds() + rhs.nanoseconds(), rhs.get_clock_type()); +} + + } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp new file mode 100644 index 0000000..4c3ff81 --- /dev/null +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -0,0 +1,262 @@ +// 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 +#include + +#include "builtin_interfaces/msg/time.hpp" + +#include "rcl/time.h" + +#include "rcutils/logging_macros.h" + +#include "rclcpp/clock.hpp" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/time_source.hpp" + + +namespace rclcpp +{ + +TimeSource::TimeSource(std::shared_ptr node) +: ros_time_active_(false) +{ + this->attachNode(node); +} + +TimeSource::TimeSource() +: ros_time_active_(false) +{ +} + +void TimeSource::attachNode(std::shared_ptr node) +{ + node_ = node; + // TODO(tfoote): Update QOS + clock_subscription_ = node_->create_subscription( + "clock", std::bind(&TimeSource::clock_cb, this, std::placeholders::_1), + rmw_qos_profile_default); + + parameter_client_ = std::make_shared(node); + parameter_subscription_ = + parameter_client_->on_parameter_event(std::bind(&TimeSource::on_parameter_event, + this, std::placeholders::_1)); +} + +void TimeSource::detachNode() +{ + this->ros_time_active_ = false; + clock_subscription_.reset(); + parameter_client_.reset(); + node_.reset(); + disable_ros_time(); +} + +void TimeSource::attachClock(std::shared_ptr clock) +{ + if (clock->get_clock_type() != RCL_ROS_TIME) { + throw std::invalid_argument("Cannot attach clock to a time source that's not a ROS clock"); + } + + std::lock_guard guard(clock_list_lock_); + associated_clocks_.push_back(clock); + // Set the clock if there's already data for it + if (last_msg_set_) { + set_clock(last_msg_set_, ros_time_active_, clock); + } +} + +void TimeSource::detachClock(std::shared_ptr clock) +{ + std::lock_guard guard(clock_list_lock_); + auto result = std::find(associated_clocks_.begin(), associated_clocks_.end(), clock); + if (result != associated_clocks_.end()) { + associated_clocks_.erase(result); + } else { + RCUTILS_LOG_ERROR("Failed to remove clock"); + } +} + +TimeSource::~TimeSource() +{ + if (node_) { + this->detachNode(); + } +} + +void TimeSource::set_clock( + const builtin_interfaces::msg::Time::SharedPtr msg, bool set_ros_time_enabled, + std::shared_ptr clock) +{ + // Compute diff + rclcpp::Time msg_time = rclcpp::Time(*msg); + rclcpp::Time now = clock->now(); + auto diff = now - msg_time; + rclcpp::TimeJump jump; + jump.delta_.nanoseconds = diff.nanoseconds(); + + // Compute jump type + if (clock->ros_time_is_active()) { + if (set_ros_time_enabled) { + jump.jump_type_ = TimeJump::ClockChange_t::ROS_TIME_NO_CHANGE; + } else { + jump.jump_type_ = TimeJump::ClockChange_t::ROS_TIME_DEACTIVATED; + } + } else if (!clock->ros_time_is_active()) { + if (set_ros_time_enabled) { + jump.jump_type_ = TimeJump::ClockChange_t::ROS_TIME_ACTIVATED; + } else { + jump.jump_type_ = TimeJump::ClockChange_t::SYSTEM_TIME_NO_CHANGE; + } + } + + if (jump.jump_type_ == TimeJump::ClockChange_t::SYSTEM_TIME_NO_CHANGE) { + // No change/no updates don't act. + return; + } + + auto active_callbacks = clock->get_triggered_callback_handlers(jump); + clock->invoke_prejump_callbacks(active_callbacks); + + // Do change + if (jump.jump_type_ == TimeJump::ClockChange_t::ROS_TIME_DEACTIVATED) { + disable_ros_time(clock); + } else if (jump.jump_type_ == TimeJump::ClockChange_t::ROS_TIME_ACTIVATED) { + enable_ros_time(clock); + } + + if (jump.jump_type_ == TimeJump::ClockChange_t::ROS_TIME_ACTIVATED || + jump.jump_type_ == TimeJump::ClockChange_t::ROS_TIME_NO_CHANGE) + { + auto ret = rcl_set_ros_time_override(&(clock->rcl_clock_), msg_time.nanoseconds()); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "Failed to set ros_time_override_status"); + } + } + // Post change callbacks + clock->invoke_postjump_callbacks(active_callbacks, jump); +} + +void TimeSource::clock_cb(const builtin_interfaces::msg::Time::SharedPtr msg) +{ + if (!this->ros_time_active_) { + enable_ros_time(); + } + // Cache the last message in case a new clock is attached. + last_msg_set_ = msg; + + std::lock_guard guard(clock_list_lock_); + for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) { + set_clock(msg, true, *it); + } +} + +void TimeSource::on_parameter_event(const rcl_interfaces::msg::ParameterEvent::SharedPtr event) +{ + for (auto & new_parameter : event->new_parameters) { + if (new_parameter.value.type == rclcpp::parameter::ParameterType::PARAMETER_BOOL) { + if (new_parameter.name == "use_sim_time") { + bool value = new_parameter.value.bool_value; + if (value) { + parameter_state_ = SET_TRUE; + enable_ros_time(); + } else { + parameter_state_ = SET_FALSE; + disable_ros_time(); + } + } + } + } + for (auto & changed_parameter : event->changed_parameters) { + if (changed_parameter.value.type == rclcpp::parameter::ParameterType::PARAMETER_BOOL) { + if (changed_parameter.name == "use_sim_time") { + bool value = changed_parameter.value.bool_value; + if (value) { + parameter_state_ = SET_TRUE; + enable_ros_time(); + } else { + parameter_state_ = SET_FALSE; + disable_ros_time(); + } + } + } + } + for (auto & deleted_parameter : event->deleted_parameters) { + if (deleted_parameter.name == "use_sim_time") { + // If the parameter is deleted mark it as unset but dont' change state. + parameter_state_ = UNSET; + } + } +} + +void TimeSource::enable_ros_time(std::shared_ptr clock) +{ + auto ret = rcl_enable_ros_time_override(&clock->rcl_clock_); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "Failed to enable ros_time_override_status"); + } +} + +void TimeSource::disable_ros_time(std::shared_ptr clock) +{ + auto ret = rcl_disable_ros_time_override(&clock->rcl_clock_); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "Failed to enable ros_time_override_status"); + } +} + +void TimeSource::enable_ros_time() +{ + if (ros_time_active_) { + // already enabled no-op + return; + } + + // Local storage + ros_time_active_ = true; + + // Update all attached clocks + std::lock_guard guard(clock_list_lock_); + for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) { + auto msg = std::make_shared(); + msg->sec = 0; + msg->nanosec = 0; + set_clock(msg, true, *it); + } +} + +void TimeSource::disable_ros_time() +{ + if (!ros_time_active_) { + // already disabled no-op + return; + } + + // Local storage + ros_time_active_ = false; + + // Update all attached clocks + std::lock_guard guard(clock_list_lock_); + for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) { + auto msg = std::make_shared(); + set_clock(msg, false, *it); + } +} + +} // namespace rclcpp diff --git a/rclcpp/test/test_duration.cpp b/rclcpp/test/test_duration.cpp new file mode 100644 index 0000000..1c072da --- /dev/null +++ b/rclcpp/test/test_duration.cpp @@ -0,0 +1,88 @@ +// 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 + +#include +#include +#include +#include + +#include "rcl/error_handling.h" +#include "rcl/time.h" +#include "rclcpp/clock.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/duration.hpp" + + +using namespace std::chrono_literals; + +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); + + EXPECT_TRUE(old < young); + EXPECT_TRUE(young > old); + EXPECT_TRUE(old <= young); + EXPECT_TRUE(young >= old); + EXPECT_FALSE(young == old); + + rclcpp::Duration add = old + young; + EXPECT_EQ(add.nanoseconds(), (rcl_duration_value_t)(old.nanoseconds() + young.nanoseconds())); + EXPECT_EQ(add, old + young); + + rclcpp::Duration sub = young - old; + EXPECT_EQ(sub.nanoseconds(), (rcl_duration_value_t)(young.nanoseconds() - old.nanoseconds())); + EXPECT_EQ(sub, young - old); + + rclcpp::Duration time = rclcpp::Duration(0, 0); + rclcpp::Duration copy_constructor_duration(time); + rclcpp::Duration assignment_op_duration = rclcpp::Duration(1, 0); + assignment_op_duration = time; + + EXPECT_TRUE(time == copy_constructor_duration); + EXPECT_TRUE(time == assignment_op_duration); +} + +TEST(TestDuration, chrono_overloads) { + int64_t ns = 123456789l; + auto chrono_ns = std::chrono::nanoseconds(ns); + auto d1 = rclcpp::Duration(ns); + auto d2 = rclcpp::Duration(chrono_ns); + auto d3 = rclcpp::Duration(123456789ns); + EXPECT_EQ(d1, d2); + EXPECT_EQ(d1, d3); + EXPECT_EQ(d2, d3); +} + +TEST(TestDuration, overflows) { + rclcpp::Duration max(std::numeric_limits::max()); + rclcpp::Duration min(std::numeric_limits::min()); + + rclcpp::Duration one(1); + rclcpp::Duration negative_one(-1); + + EXPECT_THROW(max + one, std::overflow_error); + EXPECT_THROW(min - one, std::underflow_error); + EXPECT_THROW(negative_one + min, std::underflow_error); + EXPECT_THROW(negative_one - max, std::underflow_error); +} diff --git a/rclcpp/test/test_time.cpp b/rclcpp/test/test_time.cpp index e04c6d3..448d123 100644 --- a/rclcpp/test/test_time.cpp +++ b/rclcpp/test/test_time.cpp @@ -20,6 +20,7 @@ #include "rcl/error_handling.h" #include "rcl/time.h" +#include "rclcpp/clock.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp/time.hpp" @@ -32,27 +33,39 @@ protected: } }; +TEST(TestTime, clock_type_access) { + rclcpp::Clock ros_clock(RCL_ROS_TIME); + EXPECT_EQ(RCL_ROS_TIME, ros_clock.get_clock_type()); + + rclcpp::Clock system_clock(RCL_SYSTEM_TIME); + EXPECT_EQ(RCL_SYSTEM_TIME, system_clock.get_clock_type()); + + rclcpp::Clock steady_clock(RCL_STEADY_TIME); + EXPECT_EQ(RCL_STEADY_TIME, steady_clock.get_clock_type()); +} + 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)); + rclcpp::Clock ros_clock(RCL_ROS_TIME); + Time ros_now = ros_clock.now(); + EXPECT_NE(0, ros_now.sec); + EXPECT_NE(0u, ros_now.nanosec); - Time system_now = rclcpp::Time::now(RCL_SYSTEM_TIME); + rclcpp::Clock system_clock(RCL_ROS_TIME); + Time system_now = system_clock.now(); EXPECT_NE(0, system_now.sec); EXPECT_NE(0u, system_now.nanosec); - Time steady_now = rclcpp::Time::now(RCL_STEADY_TIME); + rclcpp::Clock steady_clock(RCL_STEADY_TIME); + Time steady_now = steady_clock.now(); EXPECT_NE(0, steady_now.sec); EXPECT_NE(0u, steady_now.nanosec); - - // default - Time default_now = rclcpp::Time::now(); - EXPECT_NE(0, default_now.sec); - EXPECT_NE(0u, default_now.nanosec); } TEST(TestTime, conversions) { - rclcpp::Time now = rclcpp::Time::now(); + rclcpp::Clock system_clock(RCL_ROS_TIME); + + rclcpp::Time now = system_clock.now(); builtin_interfaces::msg::Time now_msg = now; rclcpp::Time now_again = now_msg; @@ -95,12 +108,8 @@ TEST(TestTime, operators) { EXPECT_FALSE(young == old); EXPECT_TRUE(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()); + rclcpp::Duration sub = young - old; + EXPECT_EQ(sub.nanoseconds(), (rcl_duration_value_t)(young.nanoseconds() - old.nanoseconds())); EXPECT_EQ(sub, young - old); rclcpp::Time system_time(0, 0, RCL_SYSTEM_TIME); @@ -112,11 +121,13 @@ TEST(TestTime, operators) { 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); + rclcpp::Clock system_clock(RCL_ROS_TIME); + rclcpp::Clock steady_clock(RCL_STEADY_TIME); + + rclcpp::Time now = system_clock.now(); + rclcpp::Time later = steady_clock.now(); EXPECT_ANY_THROW((void)(now == later)); EXPECT_ANY_THROW((void)(now != later)); @@ -124,7 +135,6 @@ TEST(TestTime, operators) { 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)); for (auto time_source : {RCL_ROS_TIME, RCL_SYSTEM_TIME, RCL_STEADY_TIME}) { @@ -139,9 +149,10 @@ TEST(TestTime, operators) { } TEST(TestTime, overflows) { - rclcpp::Time max(std::numeric_limits::max()); - rclcpp::Time one(1); + rclcpp::Time max_time(std::numeric_limits::max()); + rclcpp::Time min_time(std::numeric_limits::min()); + rclcpp::Duration one(1); - EXPECT_THROW(max + one, std::overflow_error); - EXPECT_THROW(one - max, std::underflow_error); + EXPECT_THROW(max_time + one, std::overflow_error); + EXPECT_THROW(min_time - one, std::underflow_error); } diff --git a/rclcpp/test/test_time_source.cpp b/rclcpp/test/test_time_source.cpp new file mode 100644 index 0000000..8f2e723 --- /dev/null +++ b/rclcpp/test/test_time_source.cpp @@ -0,0 +1,274 @@ +// 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 + +#include +#include +#include +#include + +#include "rcl/error_handling.h" +#include "rcl/time.h" +#include "rclcpp/clock.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/time_source.hpp" + +class TestTimeSource : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + void SetUp() + { + node = std::make_shared("my_node"); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::node::Node::SharedPtr node; +}; + + +TEST_F(TestTimeSource, detachUnattached) { + rclcpp::TimeSource ts; + + ASSERT_NO_THROW(ts.detachNode()); + + // Try multiple detach to see if error + ASSERT_NO_THROW(ts.detachNode()); +} + +TEST_F(TestTimeSource, reattach) { + rclcpp::TimeSource ts; + // Try reattach + ASSERT_NO_THROW(ts.attachNode(node)); + ASSERT_NO_THROW(ts.attachNode(node)); +} + +TEST_F(TestTimeSource, ROS_time_valid) { + rclcpp::TimeSource ts; + auto ros_clock = std::make_shared(RCL_ROS_TIME); + + EXPECT_FALSE(ros_clock->ros_time_is_active()); + ts.attachClock(ros_clock); + auto now = ros_clock->now(); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + + ts.attachNode(node); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + + ts.detachNode(); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + + ts.attachNode(node); + EXPECT_FALSE(ros_clock->ros_time_is_active()); +} + +TEST_F(TestTimeSource, clock) { + rclcpp::TimeSource ts(node); + auto ros_clock = std::make_shared(RCL_ROS_TIME); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + ts.attachClock(ros_clock); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + + auto clock_pub = node->create_publisher("clock", + rmw_qos_profile_default); + rclcpp::WallRate loop_rate(50); + for (int i = 0; i < 5; ++i) { + if (!rclcpp::ok()) { + break; // Break for ctrl-c + } + auto msg = std::make_shared(); + msg->sec = i; + msg->nanosec = 1000; + clock_pub->publish(msg); + // std::cout << "Publishing: '" << msg->sec << ".000000" << msg->nanosec << "'" << std::endl; + rclcpp::spin_some(node); + loop_rate.sleep(); + } + auto t_low = rclcpp::Time(1, 0, RCL_ROS_TIME); + auto t_high = rclcpp::Time(10, 100000, RCL_ROS_TIME); + + // Now that we've recieved a message it should be active with parameter unset + EXPECT_TRUE(ros_clock->ros_time_is_active()); + + auto t_out = ros_clock->now(); + + EXPECT_NE(0UL, t_out.nanoseconds()); + EXPECT_LT(t_low.nanoseconds(), t_out.nanoseconds()); + EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds()); +} + +class CallbackObject +{ +public: + CallbackObject() + : last_precallback_id_(0), + last_postcallback_id_(0) + {} + int last_precallback_id_; + void pre_callback(int id) {last_precallback_id_ = id;} + + int last_postcallback_id_; + rclcpp::TimeJump last_timejump_; + void post_callback(const rclcpp::TimeJump & jump, int id) + { + last_postcallback_id_ = id; last_timejump_ = jump; + } +}; + +TEST_F(TestTimeSource, callbacks) { + CallbackObject cbo; + rclcpp::JumpThreshold jump_threshold; + jump_threshold.min_forward_ = 0; + jump_threshold.min_backward_ = 0; + jump_threshold.on_clock_change_ = true; + + rclcpp::TimeSource ts(node); + auto ros_clock = std::make_shared(RCL_ROS_TIME); + + // Register a callback for time jumps + rclcpp::JumpHandler::SharedPtr callback_holder = ros_clock->create_jump_callback( + std::bind(&CallbackObject::pre_callback, &cbo, 1), + std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 1), + jump_threshold); + + EXPECT_EQ(0, cbo.last_precallback_id_); + EXPECT_EQ(0, cbo.last_postcallback_id_); + + EXPECT_FALSE(ros_clock->ros_time_is_active()); + + ts.attachClock(ros_clock); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + + auto clock_pub = node->create_publisher("clock", + rmw_qos_profile_default); + + rclcpp::WallRate loop_rate(50); + for (int i = 0; i < 5; ++i) { + if (!rclcpp::ok()) { + break; // Break for ctrl-c + } + auto msg = std::make_shared(); + msg->sec = i; + msg->nanosec = 1000; + clock_pub->publish(msg); + // std::cout << "Publishing: '" << msg->sec << ".000000" << msg->nanosec << "'" << std::endl; + rclcpp::spin_some(node); + loop_rate.sleep(); + } + auto t_low = rclcpp::Time(1, 0, RCL_ROS_TIME); + auto t_high = rclcpp::Time(10, 100000, RCL_ROS_TIME); + + EXPECT_EQ(1, cbo.last_precallback_id_); + EXPECT_EQ(1, cbo.last_postcallback_id_); + + // Now that we've recieved a message it should be active with parameter unset + EXPECT_TRUE(ros_clock->ros_time_is_active()); + + auto t_out = ros_clock->now(); + + EXPECT_NE(0UL, t_out.nanoseconds()); + EXPECT_LT(t_low.nanoseconds(), t_out.nanoseconds()); + EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds()); + + + // Change callbacks + callback_holder = ros_clock->create_jump_callback( + std::bind(&CallbackObject::pre_callback, &cbo, 2), + std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 2), + jump_threshold); + + for (int i = 0; i < 5; ++i) { + if (!rclcpp::ok()) { + break; // Break for ctrl-c + } + auto msg = std::make_shared(); + msg->sec = i; + msg->nanosec = 2000; + clock_pub->publish(msg); + // std::cout << "Publishing: '" << msg->sec << ".000000" << msg->nanosec << "'" << std::endl; + rclcpp::spin_some(node); + loop_rate.sleep(); + } + + EXPECT_EQ(2, cbo.last_precallback_id_); + EXPECT_EQ(2, cbo.last_postcallback_id_); + + // Now that we've recieved a message it should be active with parameter unset + EXPECT_TRUE(ros_clock->ros_time_is_active()); + + t_out = ros_clock->now(); + + EXPECT_NE(0UL, t_out.nanoseconds()); + EXPECT_LT(t_low.nanoseconds(), t_out.nanoseconds()); + EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds()); +} + + +TEST_F(TestTimeSource, parameter_activation) { + rclcpp::TimeSource ts(node); + auto ros_clock = std::make_shared(RCL_ROS_TIME); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + + ts.attachClock(ros_clock); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + + auto parameter_service = std::make_shared(node); + auto parameters_client = std::make_shared(node); + + using namespace std::chrono_literals; + EXPECT_TRUE(parameters_client->wait_for_service(2s)); + auto set_parameters_results = parameters_client->set_parameters({ + rclcpp::parameter::ParameterVariant("use_sim_time", true) + }); + for (auto & result : set_parameters_results) { + EXPECT_TRUE(result.successful); + } + rclcpp::spin_some(node); + EXPECT_TRUE(ros_clock->ros_time_is_active()); + + + set_parameters_results = parameters_client->set_parameters({ + rclcpp::parameter::ParameterVariant("use_sim_time", rclcpp::parameter::PARAMETER_NOT_SET) + }); + for (auto & result : set_parameters_results) { + EXPECT_TRUE(result.successful); + } + EXPECT_TRUE(ros_clock->ros_time_is_active()); + + set_parameters_results = parameters_client->set_parameters({ + rclcpp::parameter::ParameterVariant("use_sim_time", false) + }); + for (auto & result : set_parameters_results) { + EXPECT_TRUE(result.successful); + } + EXPECT_FALSE(ros_clock->ros_time_is_active()); + + set_parameters_results = parameters_client->set_parameters({ + rclcpp::parameter::ParameterVariant("use_sim_time", rclcpp::parameter::PARAMETER_NOT_SET) + }); + for (auto & result : set_parameters_results) { + EXPECT_TRUE(result.successful); + } + EXPECT_FALSE(ros_clock->ros_time_is_active()); +}