rclcpp  master
C++ ROS Client Library API
time.hpp
Go to the documentation of this file.
1 // Copyright 2017 Open Source Robotics Foundation, Inc.
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 // http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 
15 #ifndef RCLCPP__TIME_HPP_
16 #define RCLCPP__TIME_HPP_
17 
18 #include <utility>
19 
20 #include "builtin_interfaces/msg/time.hpp"
21 #include "rcl/time.h"
22 #include "rcutils/time.h"
23 
24 #include "rclcpp/exceptions.hpp"
25 
26 namespace rclcpp
27 {
28 
29 class Time
30 {
31 public:
32  template<rcl_time_source_type_t ClockT = RCL_SYSTEM_TIME>
33  static Time
34  now()
35  {
36  rcutils_time_point_value_t rcutils_now = 0;
38  if (ClockT == RCL_ROS_TIME) {
39  throw std::runtime_error("RCL_ROS_TIME is currently not implemented.");
40  ret = false;
41  } else if (ClockT == RCL_SYSTEM_TIME) {
42  ret = rcutils_system_time_now(&rcutils_now);
43  } else if (ClockT == RCL_STEADY_TIME) {
44  ret = rcutils_steady_time_now(&rcutils_now);
45  }
46  if (ret != RCUTILS_RET_OK) {
48  ret, "Could not get current time: ");
49  }
50 
51  return Time(std::move(rcutils_now));
52  }
53 
54  operator builtin_interfaces::msg::Time() const
55  {
56  builtin_interfaces::msg::Time msg_time;
57  msg_time.sec = static_cast<std::int32_t>(RCL_NS_TO_S(rcl_time_));
58  msg_time.nanosec = static_cast<std::uint32_t>(rcl_time_ % (1000 * 1000 * 1000));
59  return msg_time;
60  }
61 
62 private:
63  rcl_time_point_value_t rcl_time_;
64 
65  explicit Time(rcl_time_point_value_t && rcl_time)
66  : rcl_time_(std::forward<decltype(rcl_time)>(rcl_time))
67  {}
68 };
69 
70 } // namespace rclcpp
71 
72 #endif // RCLCPP__TIME_HPP_
rcutils_ret_t rcutils_system_time_now(rcutils_time_point_value_t *now)
Definition: allocator_common.hpp:24
int rcutils_ret_t
RCL_STEADY_TIME
rcutils_time_point_value_t rcl_time_point_value_t
void throw_from_rcl_error(rcl_ret_t ret, const std::string &prefix="", const rcl_error_state_t *error_state=nullptr, void(*reset_error)()=rcl_reset_error)
Throw a C++ std::exception which was created based on an rcl error.
rcutils_ret_t rcutils_steady_time_now(rcutils_time_point_value_t *now)
RCL_ROS_TIME
#define RCUTILS_RET_OK
static Time now()
Definition: time.hpp:34
#define RCL_NS_TO_S
Definition: time.hpp:29
RCL_SYSTEM_TIME
#define RCUTILS_RET_ERROR
uint64_t rcutils_time_point_value_t