From c07aee5cf02ab063bc9b466f1a96880ad3520dd3 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 21 Mar 2017 17:41:49 -0700 Subject: [PATCH] Rclcpp time (#311) * initial commit for rclcpp::time::now() * switch between times * introduce time class * add rclcpp/time.hpp to rclcpp/rclcpp.hpp * throw exceptions on time error * fix test_time to catch exceptions * explicit one-parameter constructor * fix msvc compiler warnings * address review comments * cleanup includes * re-add todo for fixing test once ros-time is there --- rclcpp/CMakeLists.txt | 11 +++++ rclcpp/include/rclcpp/rclcpp.hpp | 2 + rclcpp/include/rclcpp/time.hpp | 74 ++++++++++++++++++++++++++++++++ rclcpp/package.xml | 2 + rclcpp/test/test_time.cpp | 50 +++++++++++++++++++++ 5 files changed, 139 insertions(+) create mode 100644 rclcpp/include/rclcpp/time.hpp create mode 100644 rclcpp/test/test_time.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 49c02dc..a2ba496 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -3,6 +3,7 @@ cmake_minimum_required(VERSION 3.5) project(rclcpp) find_package(ament_cmake REQUIRED) +find_package(builtin_interfaces REQUIRED) find_package(rcl REQUIRED) find_package(rcl_interfaces REQUIRED) find_package(rmw REQUIRED) @@ -57,6 +58,7 @@ set(${PROJECT_NAME}_SRCS add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_SRCS}) ament_target_dependencies(${PROJECT_NAME} + "builtin_interfaces" "rcl" "rosidl_generator_cpp" "rosidl_typesupport_cpp") @@ -74,6 +76,7 @@ install( ) ament_export_dependencies(ament_cmake) +ament_export_dependencies(builtin_interfaces) ament_export_dependencies(rcl) ament_export_dependencies(rosidl_generator_cpp) ament_export_dependencies(rosidl_typesupport_c) @@ -171,6 +174,14 @@ if(BUILD_TESTING) mock_msgs ${typesupport_impl_c}) endforeach() endif() + + ament_add_gtest(test_time test/test_time.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") + if(TARGET test_time) + ament_target_dependencies(test_time + "rcl") + target_link_libraries(test_time ${PROJECT_NAME}) + endif() endif() ament_package( diff --git a/rclcpp/include/rclcpp/rclcpp.hpp b/rclcpp/include/rclcpp/rclcpp.hpp index 00e9abb..5d2d146 100644 --- a/rclcpp/include/rclcpp/rclcpp.hpp +++ b/rclcpp/include/rclcpp/rclcpp.hpp @@ -114,6 +114,7 @@ * - rclcpp/function_traits.hpp * - rclcpp/macros.hpp * - rclcpp/scope_exit.hpp + * - rclcpp/time.hpp * - rclcpp/utilities.hpp * - rclcpp/visibility_control.hpp */ @@ -130,6 +131,7 @@ #include "rclcpp/parameter_client.hpp" #include "rclcpp/parameter_service.hpp" #include "rclcpp/rate.hpp" +#include "rclcpp/time.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp/visibility_control.hpp" diff --git a/rclcpp/include/rclcpp/time.hpp b/rclcpp/include/rclcpp/time.hpp new file mode 100644 index 0000000..6a1d552 --- /dev/null +++ b/rclcpp/include/rclcpp/time.hpp @@ -0,0 +1,74 @@ +// 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_HPP_ +#define RCLCPP__TIME_HPP_ + +#include + +#include "builtin_interfaces/msg/time.hpp" + +#include "rclcpp/exceptions.hpp" + +namespace rclcpp +{ + +class Time +{ +public: + template + static Time + now() + { + rcl_time_point_value_t rcl_now = 0; + rcl_ret_t ret = RCL_RET_ERROR; + if (ClockT == RCL_ROS_TIME) { + throw std::runtime_error("RCL_ROS_TIME is currently not implemented."); + ret = false; + } else if (ClockT == RCL_SYSTEM_TIME) { + ret = rcl_system_time_now(&rcl_now); + } else if (ClockT == RCL_STEADY_TIME) { + ret = rcl_steady_time_now(&rcl_now); + } + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "Could not get current time: "); + } + + return Time(std::move(rcl_now)); + } + + operator builtin_interfaces::msg::Time() const + { + builtin_interfaces::msg::Time msg_time; + msg_time.sec = static_cast(RCL_NS_TO_S(rcl_time_)); + msg_time.nanosec = static_cast(rcl_time_ % (1000 * 1000 * 1000)); + return msg_time; + } + +private: + rcl_time_point_value_t rcl_time_; + + Time(std::uint32_t sec, std::uint32_t nanosec) + : rcl_time_(RCL_S_TO_NS(sec) + nanosec) + {} + + explicit Time(rcl_time_point_value_t && rcl_time) + : rcl_time_(std::forward(rcl_time)) + {} +}; + +} // namespace rclcpp + +#endif // RCLCPP__TIME_HPP_ diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 5f03209..fe5bd2d 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -11,10 +11,12 @@ rmw + builtin_interfaces rcl_interfaces rosidl_generator_cpp rosidl_typesupport_c rosidl_typesupport_cpp + builtin_interfaces rcl_interfaces rosidl_generator_cpp rosidl_typesupport_c diff --git a/rclcpp/test/test_time.cpp b/rclcpp/test/test_time.cpp new file mode 100644 index 0000000..87056d1 --- /dev/null +++ b/rclcpp/test/test_time.cpp @@ -0,0 +1,50 @@ +// 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 "rcl/error_handling.h" +#include "rcl/time.h" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/time.hpp" + +class TestTime : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } +}; + +TEST(TestTime, rate_basics) { + using builtin_interfaces::msg::Time; + // TODO(Karsten1987): Fix this test once ROS_TIME is implemented + EXPECT_ANY_THROW(rclcpp::Time::now()); + + Time system_now = rclcpp::Time::now(); + EXPECT_NE(0, system_now.sec); + EXPECT_NE(0, system_now.nanosec); + + Time steady_now = rclcpp::Time::now(); + EXPECT_NE(0, steady_now.sec); + EXPECT_NE(0, steady_now.nanosec); + + // default + Time default_now = rclcpp::Time::now(); + EXPECT_NE(0, default_now.sec); + EXPECT_NE(0, default_now.nanosec); +}