Fawkes API  Fawkes Development Version
gazsim_timesource_source.cpp
1 /***************************************************************************
2  * gazsim_timesource_source.cpp - Plugin sets the fawkes time
3  * to the simulation time
4  *
5  * Created: Wed Sep 25 16:11:35 2013
6  * Copyright 2013 Frederik Zwilling
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "gazsim_timesource_source.h"
24 
25 using namespace fawkes;
26 
27 /**
28  * Constructor
29  * @param clock pointer to fawkes clock
30  */
32 {
33  clock_ = clock;
34 
35  last_sim_time_ = get_system_time();
36  last_real_time_factor_ = 1.0;
37  clock_->get_systime(&last_sys_recv_time_);
38  //registration will be done by plugin
39 }
40 
42 {
43 }
44 
45 /**
46  * The Simulation time is estimated by looking at the timeinterval to the last received msg and the last real time factor.
47  * With this method, I want to reduce the number of send messages from Gazebo
48  * @param tv timeinterval
49  */
50 void
51 GazsimTimesource::get_time(timeval *tv) const
52 {
53  //I do not use the Time - operator here because this would recursively call get_time
54  timeval now = get_system_time();
55  timeval interval = subtract(now, last_sys_recv_time_);
56 
57  //doing this: timeval estimated_sim_interval = interval * last_real_time_factor_;
58  timeval estimated_sim_interval;
59  estimated_sim_interval.tv_usec =
60  last_real_time_factor_ * (interval.tv_sec * 1000000 + interval.tv_usec);
61  estimated_sim_interval.tv_sec = estimated_sim_interval.tv_usec / 1000000;
62  estimated_sim_interval.tv_usec -= estimated_sim_interval.tv_sec * 1000000;
63 
64  timeval estimated_sim_now = add(last_sim_time_, estimated_sim_interval);
65 
66  //return
67  *tv = estimated_sim_now;
68 }
69 
70 timeval
71 GazsimTimesource::conv_to_realtime(const timeval *tv) const
72 {
73  timeval interval = subtract(*tv, last_sim_time_);
74 
75  //doing this: timeval est_real_interval = interval / last_real_time_factor_;
76  timeval est_real_interval;
77  est_real_interval.tv_usec =
78  (interval.tv_sec * 1000000 + interval.tv_usec) / last_real_time_factor_;
79  est_real_interval.tv_sec = est_real_interval.tv_usec / 1000000;
80  est_real_interval.tv_usec -= est_real_interval.tv_sec * 1000000;
81 
82  timeval result = add(last_sys_recv_time_, est_real_interval);
83  return result;
84 }
85 
86 timeval
88 {
89  timeval t_offset = subtract(*tv, last_native_sim_time_);
90  double offset = t_offset.tv_sec + t_offset.tv_usec / 1000000.f;
91  long offset_sec = ::ceil(offset);
92  long offset_usec = ::round(offset - offset_sec) * 1000000;
93 
94  timeval rv;
95  rv.tv_sec = last_sim_time_.tv_sec + offset_sec;
96  rv.tv_usec = last_sim_time_.tv_usec + offset_usec;
97 
98  return rv;
99 }
100 
101 /** store data from gazebo time message
102  * @param msg message
103  */
104 void
106 {
107  //we do not want to correct time back
108  get_time(&last_sim_time_);
109  last_real_time_factor_ = msg->real_time_factor();
110  clock_->get_systime(&last_sys_recv_time_);
111  last_native_sim_time_.tv_sec = msg->sim_time_sec();
112  last_native_sim_time_.tv_usec = msg->sim_time_nsec() / 1000;
113 }
114 
115 timeval
116 GazsimTimesource::get_system_time() const
117 {
118  timeval now_timeval;
119  gettimeofday(&now_timeval, NULL);
120  return now_timeval;
121 }
122 
123 timeval
124 GazsimTimesource::add(timeval a, timeval b) const
125 {
126  timeval res;
127  res.tv_sec = a.tv_sec + b.tv_sec;
128  res.tv_usec = a.tv_usec + b.tv_usec;
129  if (res.tv_usec > 1000000) {
130  res.tv_usec -= 1000000;
131  res.tv_sec++;
132  }
133  return res;
134 }
135 
136 timeval
137 GazsimTimesource::subtract(timeval a, timeval b) const
138 {
139  timeval res;
140  res.tv_sec = a.tv_sec - b.tv_sec;
141  if (a.tv_usec >= b.tv_usec) {
142  res.tv_usec = a.tv_usec - b.tv_usec;
143  } else {
144  res.tv_usec = 1000000 + a.tv_usec - b.tv_usec;
145  res.tv_sec--;
146  }
147  return res;
148 }
This is supposed to be the central clock in Fawkes.
Definition: clock.h:35
void get_systime(struct timeval *tv) const
Returns the system time.
Definition: clock.cpp:215
void on_time_sync_msg(ConstSimTimePtr &msg)
store data from gazebo time message
GazsimTimesource(Clock *clock)
Constructor.
virtual timeval conv_native_to_exttime(const timeval *tv) const
Convert a native time to the external time.
virtual void get_time(timeval *tv) const
The Simulation time is estimated by looking at the timeinterval to the last received msg and the last...
virtual timeval conv_to_realtime(const timeval *tv) const
Convert a time given w.r.t.
Fawkes library namespace.