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 <core/exception.h>
00026 #include <cmath>
00027 #include <models/velocity/globfromrel.h>
00028 #include <utils/time/time.h>
00029
00030
00031
00032
00033 using namespace std;
00034 using namespace fawkes;
00035
00036 namespace firevision {
00037 #if 0
00038 }
00039 #endif
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049 VelocityGlobalFromRelative::VelocityGlobalFromRelative(VelocityModel *rel_velo_model,
00050 RelativePositionModel *rel_pos_model)
00051 {
00052 this->relative_velocity = rel_velo_model;
00053 this->relative_position = rel_pos_model;
00054
00055 if ( rel_velo_model->getCoordinateSystem() != COORDSYS_ROBOT_CART ) {
00056
00057
00058
00059
00060
00061 throw Exception("Given velocity model does not return robot-centric cartesian coordinates. WILL NOT WORK!");
00062 }
00063
00064 robot_ori = robot_poseage = 0.f;
00065
00066 velocity_x = velocity_y = 0.f;
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088 }
00089
00090
00091
00092 VelocityGlobalFromRelative::~VelocityGlobalFromRelative()
00093 {
00094 }
00095
00096
00097 void
00098 VelocityGlobalFromRelative::setPanTilt(float pan, float tilt)
00099 {
00100 }
00101
00102
00103 void
00104 VelocityGlobalFromRelative::setRobotPosition(float x, float y, float ori, timeval t)
00105 {
00106 timeval now;
00107 gettimeofday(&now, NULL);
00108 robot_ori = ori;
00109 robot_poseage = time_diff_sec(now, t);
00110 }
00111
00112
00113 void
00114 VelocityGlobalFromRelative::setRobotVelocity(float rel_vel_x, float rel_vel_y, timeval t)
00115 {
00116 }
00117
00118 void
00119 VelocityGlobalFromRelative::setTime(timeval t)
00120 {
00121 }
00122
00123
00124 void
00125 VelocityGlobalFromRelative::setTimeNow()
00126 {
00127 }
00128
00129
00130 void
00131 VelocityGlobalFromRelative::getTime(long int *sec, long int *usec)
00132 {
00133 *sec = 0;
00134 *usec = 0;
00135 }
00136
00137
00138 void
00139 VelocityGlobalFromRelative::getVelocity(float *vel_x, float *vel_y)
00140 {
00141 if (vel_x != NULL) {
00142 *vel_x = velocity_x;
00143 }
00144 if (vel_y != NULL) {
00145 *vel_y = velocity_y;
00146 }
00147 }
00148
00149
00150 float
00151 VelocityGlobalFromRelative::getVelocityX()
00152 {
00153 return velocity_x;
00154 }
00155
00156
00157 float
00158 VelocityGlobalFromRelative::getVelocityY()
00159 {
00160 return velocity_y;
00161 }
00162
00163
00164
00165 void
00166 VelocityGlobalFromRelative::calc()
00167 {
00168
00169 relative_velocity->getVelocity( &rel_vel_x, &rel_vel_y );
00170 sin_ori = sin( robot_ori );
00171 cos_ori = cos( robot_ori );
00172 rel_dist = relative_position->get_distance();
00173
00174 velocity_x = rel_vel_x * cos_ori - rel_vel_y * sin_ori;
00175 velocity_y = rel_vel_x * sin_ori + rel_vel_y * cos_ori;
00176
00177
00178
00179 }
00180
00181
00182 void
00183 VelocityGlobalFromRelative::reset()
00184 {
00185
00186 avg_vx_sum = 0.f;
00187 avg_vx_num = 0;
00188 avg_vy_sum = 0.f;
00189 avg_vy_num = 0;
00190 velocity_x = 0.f;
00191 velocity_y = 0.f;
00192 }
00193
00194
00195 const char *
00196 VelocityGlobalFromRelative::getName() const
00197 {
00198 return "VelocityModel::VelocityGlobalFromRelative";
00199 }
00200
00201
00202 coordsys_type_t
00203 VelocityGlobalFromRelative::getCoordinateSystem()
00204 {
00205 return COORDSYS_WORLD_CART;
00206 }
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241
00242
00243
00244
00245
00246
00247
00248
00249
00250 }