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

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

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"
 

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 <utility>
+
+#include "builtin_interfaces/msg/time.hpp"
+
+#include "rclcpp/exceptions.hpp"
+
+namespace rclcpp
+{
+
+class Time
+{
+public:
+  template<rcl_time_source_type_t ClockT = RCL_SYSTEM_TIME>
+  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<std::int32_t>(RCL_NS_TO_S(rcl_time_));
+    msg_time.nanosec = static_cast<std::uint32_t>(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<decltype(rcl_time)>(rcl_time))
+  {}
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__TIME_HPP_

rclcpp/package.xml

@@ -11,10 +11,12 @@
 
   <build_export_depend>rmw</build_export_depend>
 
+  <build_depend>builtin_interfaces</build_depend>
   <build_depend>rcl_interfaces</build_depend>
   <build_depend>rosidl_generator_cpp</build_depend>
   <build_depend>rosidl_typesupport_c</build_depend>
   <build_depend>rosidl_typesupport_cpp</build_depend>
+  <build_export_depend>builtin_interfaces</build_export_depend>
   <build_export_depend>rcl_interfaces</build_export_depend>
   <build_export_depend>rosidl_generator_cpp</build_export_depend>
   <build_export_depend>rosidl_typesupport_c</build_export_depend>

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 <gtest/gtest.h>
+
+#include <string>
+
+#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<RCL_ROS_TIME>());
+
+  Time system_now = rclcpp::Time::now<RCL_SYSTEM_TIME>();
+  EXPECT_NE(0, system_now.sec);
+  EXPECT_NE(0, system_now.nanosec);
+
+  Time steady_now = rclcpp::Time::now<RCL_STEADY_TIME>();
+  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);
+}