23 #include "gazsim_timesource_source.h" 35 last_sim_time_ = get_system_time();
36 last_real_time_factor_ = 1.0;
55 timeval now = get_system_time();
56 timeval interval = subtract(now, last_sys_recv_time_);
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;
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 = (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;
81 timeval result = add(last_sys_recv_time_, est_real_interval);
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;
94 rv.tv_sec = last_sim_time_.tv_sec + offset_sec;
95 rv.tv_usec = last_sim_time_.tv_usec + offset_usec;
108 last_real_time_factor_ = msg->real_time_factor();
110 last_native_sim_time_.tv_sec = msg->sim_time_sec();
111 last_native_sim_time_.tv_usec = msg->sim_time_nsec() / 1000;
115 GazsimTimesource::get_system_time()
const 118 gettimeofday(&now_timeval,NULL);
123 GazsimTimesource::add(timeval a, timeval b)
const 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)
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)
143 res.tv_usec = a.tv_usec - b.tv_usec;
147 res.tv_usec = 1000000 + a.tv_usec - b.tv_usec;
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.
This is supposed to be the central clock in Fawkes.
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
~GazsimTimesource()
Destructor.
virtual timeval conv_native_to_exttime(const timeval *tv) const
Convert a native time to the external time.