23 #include "gazsim_timesource_source.h"
35 last_sim_time_ = get_system_time();
36 last_real_time_factor_ = 1.0;
54 timeval now = get_system_time();
55 timeval interval = subtract(now, last_sys_recv_time_);
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;
64 timeval estimated_sim_now = add(last_sim_time_, estimated_sim_interval);
67 *tv = estimated_sim_now;
73 timeval interval = subtract(*tv, last_sim_time_);
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;
82 timeval result = add(last_sys_recv_time_, est_real_interval);
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;
95 rv.tv_sec = last_sim_time_.tv_sec + offset_sec;
96 rv.tv_usec = last_sim_time_.tv_usec + offset_usec;
109 last_real_time_factor_ = msg->real_time_factor();
111 last_native_sim_time_.tv_sec = msg->sim_time_sec();
112 last_native_sim_time_.tv_usec = msg->sim_time_nsec() / 1000;
116 GazsimTimesource::get_system_time()
const
119 gettimeofday(&now_timeval, NULL);
124 GazsimTimesource::add(timeval a, timeval b)
const
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;
137 GazsimTimesource::subtract(timeval a, timeval b)
const
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;
144 res.tv_usec = 1000000 + a.tv_usec - b.tv_usec;
This is supposed to be the central clock in Fawkes.
void get_systime(struct timeval *tv) const
Returns the system time.
void on_time_sync_msg(ConstSimTimePtr &msg)
store data from gazebo time message
GazsimTimesource(Clock *clock)
Constructor.
~GazsimTimesource()
Destructor.
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.