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
25using namespace fawkes;
26
27/**
28 * Constructor
29 * @param clock pointer to fawkes clock
30 */
31GazsimTimesource::GazsimTimesource(Clock *clock)
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 */
50void
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
70timeval
71GazsimTimesource::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
86timeval
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 */
104void
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
115timeval
116GazsimTimesource::get_system_time() const
117{
118 timeval now_timeval;
119 gettimeofday(&now_timeval, NULL);
120 return now_timeval;
121}
122
123timeval
124GazsimTimesource::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
136timeval
137GazsimTimesource::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
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.