rate.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2009, Willow Garage, Inc.
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of the Willow Garage nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *
35  * Author: Eitan Marder-Eppstein
36  *********************************************************************/
37 #include <ros/rate.h>
38 
39 namespace rs2rosinternal
40 {
41 
42 Rate::Rate(double frequency)
43 : start_(Time::now())
44 , expected_cycle_time_(1.0 / frequency)
45 , actual_cycle_time_(0.0)
46 { }
47 
49  : start_(Time::now())
50  , expected_cycle_time_(d.sec, d.nsec)
51  , actual_cycle_time_(0.0)
52 { }
53 
54 
55 
57 {
58  Time expected_end = start_ + expected_cycle_time_;
59 
60  Time actual_end = Time::now();
61 
62  // detect backward jumps in time
63  if (actual_end < start_)
64  {
65  expected_end = actual_end + expected_cycle_time_;
66  }
67 
68  //calculate the time we'll sleep for
69  Duration sleep_time = expected_end - actual_end;
70 
71  //set the actual amount of time the loop took in case the user wants to know
72  actual_cycle_time_ = actual_end - start_;
73 
74  //make sure to reset our start time
75  start_ = expected_end;
76 
77  //if we've taken too much time we won't sleep
78  if(sleep_time <= Duration(0.0))
79  {
80  // if we've jumped forward in time, or the loop has taken more than a full extra
81  // cycle, reset our cycle
82  if (actual_end > expected_end + expected_cycle_time_)
83  {
84  start_ = actual_end;
85  }
86  // return false to show that the desired rate was not met
87  return false;
88  }
89 
90  return sleep_time.sleep();
91 }
92 
94 {
95  start_ = Time::now();
96 }
97 
99 {
100  return actual_cycle_time_;
101 }
102 
103 WallRate::WallRate(double frequency)
104 : start_(WallTime::now())
105 , expected_cycle_time_(1.0 / frequency)
106 , actual_cycle_time_(0.0)
107 {}
108 
110 : start_(WallTime::now())
111 , expected_cycle_time_(d.sec, d.nsec)
112 , actual_cycle_time_(0.0)
113 {}
114 
116 {
117  WallTime expected_end = start_ + expected_cycle_time_;
118 
119  WallTime actual_end = WallTime::now();
120 
121  // detect backward jumps in time
122  if (actual_end < start_)
123  {
124  expected_end = actual_end + expected_cycle_time_;
125  }
126 
127  //calculate the time we'll sleep for
128  WallDuration sleep_time = expected_end - actual_end;
129 
130  //set the actual amount of time the loop took in case the user wants to know
131  actual_cycle_time_ = actual_end - start_;
132 
133  //make sure to reset our start time
134  start_ = expected_end;
135 
136  //if we've taken too much time we won't sleep
137  if(sleep_time <= WallDuration(0.0))
138  {
139  // if we've jumped forward in time, or the loop has taken more than a full extra
140  // cycle, reset our cycle
141  if (actual_end > expected_end + expected_cycle_time_)
142  {
143  start_ = actual_end;
144  }
145  return false;
146  }
147 
148  return sleep_time.sleep();
149 }
150 
152 {
153  start_ = WallTime::now();
154 }
155 
157 {
158  return actual_cycle_time_;
159 }
160 
161 
162 }
bool sleep() const
sleep for the amount of time specified by this Duration. If a signal interrupts the sleep...
Definition: time.cpp:462
Duration actual_cycle_time_
Definition: rate.h:85
d
Definition: rmse.py:171
static WallTime now()
Returns the current wall clock time.
Definition: time.cpp:440
void reset()
Sets the start time for the rate to now.
Definition: rate.cpp:93
Duration cycleTime() const
Get the actual run time of a cycle from start to sleep.
Definition: rate.cpp:98
::std_msgs::Duration_< std::allocator< void > > Duration
Definition: Duration.h:47
bool sleep()
Sleeps for any leftover time in a cycle. Calculated from the last time sleep, reset, or the constructor was called.
Definition: rate.cpp:56
Duration representation for use with the Time class.
Definition: duration.h:108
WallDuration expected_cycle_time_
Definition: rate.h:126
Duration representation for use with the WallTime class.
Definition: duration.h:136
bool sleep() const
sleep for the amount of time specified by this Duration. If a signal interrupts the sleep...
Definition: time.cpp:393
Time representation. Always wall-clock time.
Definition: time.h:233
Time representation. May either represent wall clock time or ROS clock time.
Definition: time.h:177
WallRate(double frequency)
Constructor, creates a Rate.
Definition: rate.cpp:103
WallDuration actual_cycle_time_
Definition: rate.h:126
static Time now()
Retrieve the current time. If ROS clock time is in use, this returns the time according to the ROS cl...
Definition: time.cpp:244
Duration expected_cycle_time_
Definition: rate.h:85
bool sleep()
Sleeps for any leftover time in a cycle. Calculated from the last time sleep, reset, or the constructor was called.
Definition: rate.cpp:115
WallDuration cycleTime() const
Get the actual run time of a cycle from start to sleep.
Definition: rate.cpp:156
Rate(double frequency)
Constructor, creates a Rate.
Definition: rate.cpp:42
void reset()
Sets the start time for the rate to now.
Definition: rate.cpp:151


librealsense2
Author(s): Sergey Dorodnicov , Doron Hirshberg , Mark Horn , Reagan Lopez , Itay Carpis
autogenerated on Mon May 3 2021 02:47:39