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 /**
47  * The Simulation time is estimated by looking at the timeinterval to the last received msg and the last real time factor.
48  * With this method, I want to reduce the number of send messages from Gazebo
49  * @param tv timeinterval
50  */
51 void
52 GazsimTimesource::get_time(timeval* tv) const
53 {
54  //I do not use the Time - operator here because this would recursively call get_time
55  timeval now = get_system_time();
56  timeval interval = subtract(now, last_sys_recv_time_);
57 
58  //doing this: timeval estimated_sim_interval = interval * last_real_time_factor_;
59  timeval estimated_sim_interval;
60  estimated_sim_interval.tv_usec = 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 = (interval.tv_sec * 1000000 + interval.tv_usec) / last_real_time_factor_;
78  est_real_interval.tv_sec = est_real_interval.tv_usec / 1000000;
79  est_real_interval.tv_usec -= est_real_interval.tv_sec * 1000000;
80 
81  timeval result = add(last_sys_recv_time_, est_real_interval);
82  return result;
83 }
84 
85 timeval
87 {
88  timeval t_offset = subtract(*tv, last_native_sim_time_);
89  double offset = t_offset.tv_sec + t_offset.tv_usec / 1000000.f;
90  long offset_sec = ::ceil(offset);
91  long offset_usec = ::round(offset - offset_sec) * 1000000;
92 
93  timeval rv;
94  rv.tv_sec = last_sim_time_.tv_sec + offset_sec;
95  rv.tv_usec = last_sim_time_.tv_usec + offset_usec;
96 
97  return rv;
98 }
99 
100 /** store data from gazebo time message
101  * @param msg message
102  */
103 void
105 {
106  //we do not want to correct time back
107  get_time(&last_sim_time_);
108  last_real_time_factor_ = msg->real_time_factor();
109  clock_->get_systime(&last_sys_recv_time_);
110  last_native_sim_time_.tv_sec = msg->sim_time_sec();
111  last_native_sim_time_.tv_usec = msg->sim_time_nsec() / 1000;
112 }
113 
114 timeval
115 GazsimTimesource::get_system_time() const
116 {
117  timeval now_timeval;
118  gettimeofday(&now_timeval,NULL);
119  return now_timeval;
120 }
121 
122 timeval
123 GazsimTimesource::add(timeval a, timeval b) const
124 {
125  timeval res;
126  res.tv_sec = a.tv_sec + b.tv_sec;
127  res.tv_usec = a.tv_usec + b.tv_usec;
128  if(res.tv_usec > 1000000)
129  {
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  {
143  res.tv_usec = a.tv_usec - b.tv_usec;
144  }
145  else
146  {
147  res.tv_usec = 1000000 + a.tv_usec - b.tv_usec;
148  res.tv_sec--;
149  }
150  return res;
151 }
virtual timeval conv_to_realtime(const timeval *tv) const
Convert a time given w.r.t.
GazsimTimesource(Clock *clock)
Constructor.
Fawkes library namespace.
void get_systime(struct timeval *tv) const
Returns the system time.
Definition: clock.cpp:239
This is supposed to be the central clock in Fawkes.
Definition: clock.h:34
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...
void on_time_sync_msg(ConstSimTimePtr &msg)
store data from gazebo time message
virtual timeval conv_native_to_exttime(const timeval *tv) const
Convert a native time to the external time.