globvelo.cpp
00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #include <cmath>
00026 #include <models/velocity/globvelo.h>
00027
00028 #include <utils/time/time.h>
00029
00030 namespace firevision {
00031 #if 0
00032 }
00033 #endif
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044 VelocityFromGlobal::VelocityFromGlobal(GlobalPositionModel* model,
00045 unsigned int history_length,
00046 unsigned int calc_interval)
00047 {
00048 this->global_pos_model = model;
00049 this->history_length = history_length;
00050 this->calc_interval = calc_interval;
00051
00052 robot_pos_x = robot_pos_y = robot_pos_ori = 0.0f;
00053
00054 velocity_x = velocity_y = 0.f;
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076 }
00077
00078
00079
00080 VelocityFromGlobal::~VelocityFromGlobal()
00081 {
00082 }
00083
00084
00085 void
00086 VelocityFromGlobal::setPanTilt(float pan, float tilt)
00087 {
00088 }
00089
00090
00091 void
00092 VelocityFromGlobal::setRobotPosition(float x, float y, float ori, timeval t)
00093 {
00094 timeval _now;
00095 gettimeofday(&_now, 0);
00096 robot_pos_x = x;
00097 robot_pos_y = y;
00098 robot_pos_ori = ori;
00099 robot_pos_age = fawkes::time_diff_sec(_now, t);
00100 }
00101
00102
00103 void
00104 VelocityFromGlobal::setRobotVelocity(float vel_x, float vel_y, timeval t)
00105 {
00106 }
00107
00108
00109 void
00110 VelocityFromGlobal::setTime(timeval t)
00111 {
00112 now.tv_sec = t.tv_sec;
00113 now.tv_usec = t.tv_usec;
00114 }
00115
00116
00117 void
00118 VelocityFromGlobal::setTimeNow()
00119 {
00120 gettimeofday(&now, 0);
00121 }
00122
00123
00124 void
00125 VelocityFromGlobal::getTime(long int *sec, long int *usec)
00126 {
00127 *sec = now.tv_sec;
00128 *usec = now.tv_usec;
00129 }
00130
00131
00132 void
00133 VelocityFromGlobal::getVelocity(float *vel_x, float *vel_y)
00134 {
00135 if (vel_x != 0) {
00136 *vel_x = velocity_x;
00137 }
00138 if (vel_y != 0) {
00139 *vel_y = velocity_y;
00140 }
00141 }
00142
00143
00144 float
00145 VelocityFromGlobal::getVelocityX()
00146 {
00147 return velocity_x;
00148 }
00149
00150
00151 float
00152 VelocityFromGlobal::getVelocityY()
00153 {
00154 return velocity_y;
00155 }
00156
00157
00158
00159 void
00160 VelocityFromGlobal::calc()
00161 {
00162
00163
00164 current_x = global_pos_model->get_x();
00165 current_y = global_pos_model->get_y();
00166
00167 last_x.push_back( current_x );
00168 last_y.push_back( current_y );
00169
00170 last_time.push_back(now);
00171
00172 velocity_total_x = 0.f;
00173 velocity_total_y = 0.f;
00174 velocity_num = 0;
00175
00176 if (last_x.size() > calc_interval) {
00177
00178 unsigned int m = (last_x.size() < last_y.size()) ? last_x.size() : last_y.size() ;
00179 for (unsigned int i = calc_interval; i < m; i += calc_interval) {
00180 diff_x = last_x[i] - last_x[i - calc_interval];
00181 diff_y = last_y[i] - last_y[i - calc_interval];
00182
00183 diff_sec = last_time[i].tv_sec - last_time[i - calc_interval].tv_sec;
00184 diff_usec = last_time[i].tv_usec - last_time[i - calc_interval].tv_usec;
00185 if (diff_usec < 0) {
00186 diff_sec -= 1;
00187 diff_usec += 1000000;
00188 }
00189
00190 f_diff_sec = diff_sec + diff_usec / 1000000.f;
00191
00192 velocity_total_x += diff_x / f_diff_sec;
00193 velocity_total_y += diff_y / f_diff_sec;
00194 velocity_num++;
00195 }
00196 }
00197
00198
00199 velocity_x = velocity_total_x / velocity_num;
00200 velocity_y = velocity_total_y / velocity_num;
00201
00202
00203
00204 while (last_x.size() > history_length) {
00205 last_x.erase( last_x.begin() );
00206 last_y.erase( last_y.begin() );
00207 }
00208
00209 }
00210
00211
00212 void
00213 VelocityFromGlobal::reset()
00214 {
00215
00216 }
00217
00218
00219 const char *
00220 VelocityFromGlobal::getName() const
00221 {
00222 return "VelocityModel::VelocityFromGlobal";
00223 }
00224
00225
00226 coordsys_type_t
00227 VelocityFromGlobal::getCoordinateSystem()
00228 {
00229 return COORDSYS_WORLD_CART;
00230 }
00231
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241
00242
00243
00244
00245
00246
00247
00248
00249
00250
00251
00252
00253
00254
00255
00256
00257 }