realtime_clock.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2013 hiDOF, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 /*
31  * Publishing ROS messages is difficult, as the publish function is
32  * not realtime safe. This class provides the proper locking so that
33  * you can call publish in realtime and a separate (non-realtime)
34  * thread will ensure that the message gets published over ROS.
35  *
36  * Author: Wim Meeussen
37  */
38 
40 #include <chrono>
41 
42 namespace realtime_tools
43 {
44 
45 
47  : running_(true),
48  thread_(std::thread(&RealtimeClock::loop, this))
49  {
50  }
51 
52 
54  {
55  running_ = false;
56  thread_.join();
57  }
58 
59 
60 
62  {
63  std::unique_lock<std::mutex> guard(mutex_, std::try_to_lock);
64  if (guard.owns_lock())
65  {
66  // update time offset when we have a new system time measurement in the last cycle
67  if (lock_misses_ == 0 && system_time_ != ros::Time())
68  {
69  // get additional offset caused by period of realtime loop
70  ros::Duration period_offset;
72  period_offset = ros::Duration((realtime_time - last_realtime_time_).toSec()/2.0);
73 
74  if (!initialized_)
75  {
76  clock_offset_ = system_time_ + period_offset - realtime_time;
77  initialized_ = true;
78  }
79  else
80  clock_offset_ = clock_offset_*0.9999 + (system_time_ + period_offset - realtime_time)*0.0001;
81  }
83  lock_misses_ = 0;
84  }
85 
86  else
87  lock_misses_++;
88 
89  last_realtime_time_ = realtime_time;
90 
91  // return time
92  return realtime_time + clock_offset_;
93  }
94 
95 
96 
98  {
99  ros::Rate r(750);
100  while (running_)
101  {
102 #ifdef NON_POLLING
103  std::lock_guard<std::mutex> guard(mutex_);
104 #else
105  std::unique_lock<std::mutex> guard(mutex_, std::try_to_lock);
106  while (!guard.owns_lock()) {
107  std::this_thread::sleep_for(std::chrono::microseconds(500));
108  guard.try_lock();
109  }
110 #endif
111 
112  // store system time
114 
115  // warning, using non-locked 'lock_misses_', but it's just for debugging
116  if (lock_misses_ > 100)
117  ROS_WARN_THROTTLE(1.0, "Time estimator has trouble transferring data between non-RT and RT");
118 
119  // release lock
120  guard.unlock();
121  r.sleep();
122  }
123  }
124 }// namespace
125 
ros::Time getSystemTime(const ros::Time &realtime_time)
#define ROS_WARN_THROTTLE(period,...)
bool sleep()
static Time now()


realtime_tools
Author(s): Stuart Glaser
autogenerated on Mon Feb 28 2022 23:22:45