libalmath  2.5.7.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
qirosmsg.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2016 Aldebaran Robotics. All rights reserved.
3  * Use of this source code is governed by a BSD-style license that can be
4  * found in the COPYING file.
5  */
6 
7 #pragma once
8 #ifndef _LIBALMATH_ALMATH_SCENEGRAPH_QIROSMSG_H_
9 #define _LIBALMATH_ALMATH_SCENEGRAPH_QIROSMSG_H_
10 
11 #include <qi/clock.hpp>
12 #include <ros/time.h>
13 
14 namespace AL {
15 namespace Math {
16 
17 // return a ros::Time, clipped to [ros::TIME_MIN, ros::TIME_MAX]
18 //
19 // Note:
20 //
21 // altough this can be changed with an #ifdef, ros::Time typically uses
22 // std::chrono::system_clock as its underlying time source.
23 // As a consequence, ros::Time typically matches libqi's
24 // qi::SystemClock::time_point.
25 //
26 // However, since the system clock is subject to clock jumps, we tend
27 // to favour qi::Clock for timestamping sensor data, and when mapping
28 // this data to ros messages for offline use, it is more convenient
29 // to set the ros::Time with the value from qi::Clock instead of
30 // qi::SystemClock.
31 //
32 // Given that both use case are valid, this conversion function does not
33 // operate on qi::Clock::time_point nor qi::SystemClock::time_point but
34 // rather on qi::Duration.
35 inline ros::Time toValidRosTime(qi::Duration time_since_epoch) {
36  // We need to care for several problems:
37  //
38  // * ros::Time::fromNSec expects an unsigned number of nano seconds,
39  // but libqi clocks use signed time_points
40  //
41  // * ros::Time range is smaller than libqi clocks range
42  // (many bits are wasted in ros::Time because it splits seconds and
43  // nanoseconds in two 32 bits members)
44  //
45  // * ros::Time(0, 0) stands for "Not a Time". Thus
46  // ros::TIME_MIN == ros::Time(0, 1)
47  //
48  // In the end we have:
49  //
50  // using qiTime = qi::SystemClock::time_point;
51  // qiTime::min() < 0 < ros::TIME_MIN < ros::TIME_MAX < qiTime::max()
52  using qirep = qi::Duration::rep;
53  constexpr qirep rosMin = 1;
54  constexpr qirep rosMax =
55  999999999 +
56  static_cast<qirep>(std::numeric_limits<uint32_t>::max()) * 1000000000;
57  assert(rosMin == static_cast<qirep>(ros::TIME_MIN.toNSec()));
58  assert(rosMax == static_cast<qirep>(ros::TIME_MAX.toNSec()));
59  ros::Time ret;
60  ret.fromNSec(std::min(rosMax, std::max(rosMin, time_since_epoch.count())));
61  return ret;
62 }
63 
64 inline qi::Duration toQiDuration(ros::Time time) {
65  using qirep = qi::Duration::rep;
66  // if time <= ros::TIME_MAX, we know it can be casted to a qirep,
67  // which is signed, without wrapping over.
68  assert(time <= ros::TIME_MAX);
69  return qi::Duration(static_cast<qirep>(time.toNSec()));
70 }
71 }
72 }
73 #endif
qi::Duration toQiDuration(ros::Time time)
Definition: qirosmsg.h:64
ros::Time toValidRosTime(qi::Duration time_since_epoch)
Definition: qirosmsg.h:35