time_conversion.hpp
Go to the documentation of this file.
1 // Copyright (C) 2019 Matthew Pitropov, Joshua Whitley
2 // All rights reserved.
3 //
4 // Software License Agreement (BSD License 2.0)
5 //
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions
8 // are met:
9 //
10 // * Redistributions of source code must retain the above copyright
11 // notice, this list of conditions and the following disclaimer.
12 // * Redistributions in binary form must reproduce the above
13 // copyright notice, this list of conditions and the following
14 // disclaimer in the documentation and/or other materials provided
15 // with the distribution.
16 // * Neither the name of {copyright_holder} nor the names of its
17 // contributors may be used to endorse or promote products derived
18 // from this software without specific prior written permission.
19 //
20 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23 // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24 // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25 // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26 // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28 // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29 // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30 // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31 // POSSIBILITY OF SUCH DAMAGE.
32 
33 #ifndef VELODYNE_DRIVER_TIME_CONVERSION_HPP
34 #define VELODYNE_DRIVER_TIME_CONVERSION_HPP
35 
36 #include <ros/ros.h>
37 #include <ros/time.h>
38 
49 ros::Time resolveHourAmbiguity(const ros::Time &stamp, const ros::Time &nominal_stamp) {
50  const int HALFHOUR_TO_SEC = 1800;
51  ros::Time retval = stamp;
52  if (nominal_stamp.sec > stamp.sec) {
53  if (nominal_stamp.sec - stamp.sec > HALFHOUR_TO_SEC) {
54  retval.sec = retval.sec + 2*HALFHOUR_TO_SEC;
55  }
56  } else if (stamp.sec - nominal_stamp.sec > HALFHOUR_TO_SEC) {
57  retval.sec = retval.sec - 2*HALFHOUR_TO_SEC;
58  }
59  return retval;
60 }
61 
62 ros::Time rosTimeFromGpsTimestamp(const uint8_t * const data) {
63  const int HOUR_TO_SEC = 3600;
64  // time for each packet is a 4 byte uint
65  // It is the number of microseconds from the top of the hour
66  uint32_t usecs = (uint32_t) ( ((uint32_t) data[3]) << 24 |
67  ((uint32_t) data[2] ) << 16 |
68  ((uint32_t) data[1] ) << 8 |
69  ((uint32_t) data[0] ));
70  ros::Time time_nom = ros::Time::now(); // use this to recover the hour
71  uint32_t cur_hour = time_nom.sec / HOUR_TO_SEC;
72  ros::Time stamp = ros::Time((cur_hour * HOUR_TO_SEC) + (usecs / 1000000),
73  (usecs % 1000000) * 1000);
74  stamp = resolveHourAmbiguity(stamp, time_nom);
75  return stamp;
76 }
77 
78 #endif //VELODYNE_DRIVER_TIME_CONVERSION_HPP
ros::Time resolveHourAmbiguity(const ros::Time &stamp, const ros::Time &nominal_stamp)
Function used to check that hour assigned to timestamp in conversion is correct. Velodyne only return...
static Time now()
ros::Time rosTimeFromGpsTimestamp(const uint8_t *const data)


velodyne_driver
Author(s): Jack O'Quin, Patrick Beeson, Michael Quinlan, Yaxin Liu
autogenerated on Thu Jul 4 2019 19:09:28