Main Page
Namespaces
Classes
Files
File List
src
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
39
#include <
realtime_tools/realtime_clock.h
>
40
41
namespace
realtime_tools
42
{
43
44
45
RealtimeClock::RealtimeClock
()
46
:lock_misses_(0),
47
system_time_(
ros
::Time()),
48
last_realtime_time_(
ros
::Time()),
49
running_(true),
50
initialized_(false)
51
{
52
// thread for time loop
53
thread_
= boost::thread(boost::bind(&
RealtimeClock::loop
,
this
));
54
}
55
56
57
RealtimeClock::~RealtimeClock
()
58
{
59
running_
=
false
;
60
thread_
.join();
61
}
62
63
64
65
ros::Time
RealtimeClock::getSystemTime
(
const
ros::Time
& realtime_time)
66
{
67
if
(
mutex_
.try_lock())
68
{
69
// update time offset when we have a new system time measurement in the last cycle
70
if
(
lock_misses_
== 0 &&
system_time_
!=
ros::Time
())
71
{
72
// get additional offset caused by period of realtime loop
73
ros::Duration
period_offset;
74
if
(
last_realtime_time_
!=
ros::Time
())
75
period_offset =
ros::Duration
((realtime_time -
last_realtime_time_
).toSec()/2.0);
76
77
if
(!
initialized_
)
78
{
79
clock_offset_
=
system_time_
+ period_offset - realtime_time;
80
initialized_
=
true
;
81
}
82
else
83
clock_offset_
=
clock_offset_
*0.9999 + (
system_time_
+ period_offset - realtime_time)*0.0001;
84
}
85
system_time_
=
ros::Time
();
86
lock_misses_
= 0;
87
mutex_
.unlock();
88
}
89
90
else
91
lock_misses_
++;
92
93
last_realtime_time_
= realtime_time;
94
95
// return time
96
return
realtime_time +
clock_offset_
;
97
}
98
99
100
101
void
RealtimeClock::loop
()
102
{
103
ros::Rate
r(750);
104
while
(
running_
)
105
{
106
// get lock
107
lock
();
108
109
// store system time
110
system_time_
=
ros::Time::now
();
111
112
// warning, using non-locked 'lock_misses_', but it's just for debugging
113
if
(
lock_misses_
> 100)
114
ROS_WARN_THROTTLE
(1.0,
"Time estimator has trouble transferring data between non-RT and RT"
);
115
116
// release lock
117
mutex_
.unlock();
118
r.
sleep
();
119
}
120
}
121
122
123
void
RealtimeClock::lock
()
124
{
125
#ifdef NON_POLLING
126
mutex_
.lock();
127
#else
128
while
(!
mutex_
.try_lock())
129
usleep(500);
130
#endif
131
}
132
133
}
// namespace
134
ROS_WARN_THROTTLE
#define ROS_WARN_THROTTLE(rate,...)
realtime_tools::RealtimeClock::lock
void lock()
Definition:
realtime_clock.cpp:123
ros::Time
ros::Rate
realtime_tools::RealtimeClock::initialized_
bool initialized_
Definition:
realtime_clock.h:67
realtime_tools::RealtimeClock::last_realtime_time_
ros::Time last_realtime_time_
Definition:
realtime_clock.h:66
realtime_tools::RealtimeClock::getSystemTime
ros::Time getSystemTime(const ros::Time &realtime_time)
Definition:
realtime_clock.cpp:65
realtime_tools::RealtimeClock::~RealtimeClock
~RealtimeClock()
Definition:
realtime_clock.cpp:57
realtime_clock.h
realtime_tools::RealtimeClock::mutex_
boost::mutex mutex_
Definition:
realtime_clock.h:68
ros
ros::Rate::sleep
bool sleep()
realtime_tools::RealtimeClock::RealtimeClock
RealtimeClock()
Definition:
realtime_clock.cpp:45
ros::Duration
realtime_tools::RealtimeClock::running_
bool running_
Definition:
realtime_clock.h:67
realtime_tools
Definition:
realtime_box.h:40
realtime_tools::RealtimeClock::lock_misses_
unsigned int lock_misses_
Definition:
realtime_clock.h:62
ros::Time::now
static Time now()
realtime_tools::RealtimeClock::thread_
boost::thread thread_
Definition:
realtime_clock.h:69
realtime_tools::RealtimeClock::system_time_
ros::Time system_time_
Definition:
realtime_clock.h:63
realtime_tools::RealtimeClock::loop
void loop()
Definition:
realtime_clock.cpp:101
realtime_tools::RealtimeClock::clock_offset_
ros::Duration clock_offset_
Definition:
realtime_clock.h:64
realtime_tools
Author(s): Stuart Glaser
autogenerated on Mon Mar 22 2021 02:20:09