00001
00018
00019 #include <unistd.h>
00020 #include <stdio.h>
00021 #include <sys/time.h>
00022 #include <sstream>
00023 #include <vector>
00024 #include <algorithm>
00025
00026
00027 #include "BirdTrack_impl.h"
00028 #include "tracker_impl.h"
00029 #include "transformCoords.h"
00030
00031 #define DOUBLE_VECTOR_LENGTH 6
00032
00033 #define NOTE_KEY_1 "X"
00034 #define NOTE_KEY_2 "Y"
00035 #define NOTE_KEY_3 "Z"
00036 #define NOTE_KEY_4 "RX"
00037 #define NOTE_KEY_5 "RY"
00038 #define NOTE_KEY_6 "RZ"
00039
00040 extern unsigned int debugLevel;
00041
00042 tracker_impl::tracker_impl(trackerID t, BirdTrack_impl* b, std::string trackerName)
00043 {
00044 unsigned int err;
00046 printf("connecting to rtdb...\n");
00047
00048 err = kogmo_rtdb_connect_initinfo (&dbinfo, "", trackerName.c_str(), 0.02); DIEonERR(err);
00049
00050 pid = kogmo_rtdb_connect (&dbc, &dbinfo); DIEonERR(pid);
00051
00052 err = kogmo_rtdb_obj_initinfo (dbc, &dataobj_info_data,trackerName.c_str(), KOGMO_RTDB_OBJTYPE_E1_FOBTRACKER,
00053 sizeof (kogmo_rtdb_obj_e1_fobtracker_t)); DIEonERR(err);
00054 std::string trackerName_raw = trackerName;
00055 trackerName_raw+="_raw";
00056 err = kogmo_rtdb_obj_initinfo (dbc, &dataobj_info_raw,trackerName_raw.c_str(), KOGMO_RTDB_OBJTYPE_E1_FOBTRACKER_RAW,
00057 sizeof (kogmo_rtdb_obj_e1_fobtracker_t)); DIEonERR(err);
00058
00059
00060 oid = kogmo_rtdb_obj_insert (dbc, &dataobj_info_data); DIEonERR(oid);
00061 oid = kogmo_rtdb_obj_insert (dbc, &dataobj_info_raw); DIEonERR(oid);
00062
00063 dataobj_data = (kogmo_rtdb_obj_e1_fobtracker_t *) malloc(dataobj_info_data.size_max);
00064 err = kogmo_rtdb_obj_initdata (dbc, &dataobj_info_data, dataobj_data); DIEonERR(err);
00065
00066
00067 printf("done!\n");
00068
00070 myID = t;
00071 myBird = b;
00072
00074 threadRunning = false;
00075 myTransformer =new transformCoords();
00076
00077
00078 }
00079
00080 tracker_impl::~tracker_impl(){
00081 threadRunning = false;
00082
00083 kogmo_rtdb_obj_delete (dbc, &dataobj_info_data);
00084 kogmo_rtdb_obj_delete (dbc, &dataobj_info_raw);
00085 kogmo_rtdb_disconnect (dbc, NULL);
00086
00087 free(dataobj_raw);
00088 free(dataobj_data);
00089 }
00090
00091
00092 void
00093 *tracker_impl::s_workerThread(void* arg)
00094 {
00095 tracker_impl* myobj = (tracker_impl*)arg;
00096 printf("static WorkerThread method called!\n");
00097
00098 myobj->workerThread();
00099 printf( "Worker thread exit.\n");
00100 return NULL;
00101 }
00102
00103
00104
00105 #define MAX_SETS 1
00106 #define PAUSE_SET 30000
00107 #define PAUSE_WRITE 30000
00108
00109 double getMedian(double (&vAcc)[MAX_SETS][6], unsigned int index){
00110 if (MAX_SETS == 1)
00111 return vAcc[0][index];
00112
00113 std::vector<double> data;
00114 for (unsigned int i=0; i<MAX_SETS; i++)
00115 data.push_back(vAcc[i][index]);
00116 std::sort(data.begin(), data.end());
00117
00118 if (MAX_SETS % 2 == 0)
00119 return ((data[MAX_SETS / 2 - 1] + data[MAX_SETS / 2]) / 2);
00120 else
00121 return data[(MAX_SETS - 1) / 2];
00122 }
00123
00124
00125 void
00126 tracker_impl::workerThread(){
00127
00128 double v[6];
00129 double vAcc[MAX_SETS][6];
00130
00131 while(threadRunning){
00132
00133
00135 for (unsigned int i=0; i<MAX_SETS; i++){
00137 myBird->get_posangles(myID, vAcc[i]);
00138
00139 if (i == 0)
00140 {
00141
00142 if (debugLevel > 2)
00143 printf("RAW ");
00144 writeNotification(dataobj_info_raw, vAcc[0]);
00145 }
00146
00147 if (i+1 < MAX_SETS)
00148 usleep(PAUSE_SET);
00149 }
00150
00151 for (unsigned int i=0; i<6; i++){
00152 v[i] = getMedian(vAcc, i);
00153 }
00154
00155
00156 myTransformer->transform(v);
00157
00158
00159 if (debugLevel > 0 )
00160 {
00161 printf( "%s %3.3lf %3.3lf %3.3lf %3.3lf %3.3lf %3.3lf ",
00162 myID == leftTracker ? "LEFT" : "RIGHT",
00163 v[0], v[1], v[2], v[3], v[4], v[5]);
00164
00165 if (myID == rightTracker)
00166 printf("\n");
00167 }
00168
00169 double temp = v[5];
00170 v[5] = v[3];
00171 v[3] = temp;
00172
00173
00174
00175
00176
00177 if (debugLevel > 2)
00178 printf("CAL ");
00179 if (!writeNotification(dataobj_info_data, v)){
00180 printf( "Couldn't write Notification to NotificationManager. Error!\n");
00181 exit(-1);
00182 }
00183
00184 usleep(PAUSE_WRITE);
00185 }
00186 threadRunning = false;
00187 }
00188
00189 void
00190 tracker_impl::start(){
00191 printf( "Starting myBird\n");
00192 myBird->start();
00193
00195 if(!threadRunning){
00196 threadRunning = true;
00197 pthread_create(&workerThreadHandle,NULL, &s_workerThread, (void*)this);
00198 }
00199 else
00200 printf( "Thread started, but was already running.\n");
00201 printf( "Thread running.\n");
00202 }
00203
00204 void
00205 tracker_impl::stop(){
00206 printf( "Stopping myBird\n");
00207 myBird->stop();
00208
00210 threadRunning = false;
00211 }
00212
00213
00214 bool
00215 tracker_impl::loadCalibFile(const char *srcFileName)
00216 {
00217
00218
00219
00220 bool res;
00221 std::string filename;
00222 std::stringstream s;
00223
00224 int fcountLeft, fcountRight;
00225 fulcrum fulcrumarrayLeft[255], fulcrumarrayRight[255];
00226 FILE *f;
00227 res = ((f=fopen(srcFileName, "r"))!=NULL);
00228 if (res)
00229 {
00230 fread(&fcountLeft,sizeof(fcountLeft),1,f);
00231 fread(&fcountRight,sizeof(fcountRight),1,f);
00232
00233 if (debugLevel > 0) {
00234 printf("loadCalibFile %s\n",srcFileName);
00235 printf("fcountLeft:%i, fcountRight:%i\n",fcountLeft,fcountRight);
00236 }
00237 fread(&fulcrumarrayLeft,sizeof(fulcrumarrayLeft[0]),fcountLeft,f);
00238 fread(&fulcrumarrayRight,sizeof(fulcrumarrayRight[0]),fcountRight,f);
00239
00240 if (debugLevel > 1) {
00241 printf("fulcrumyArrayLeft(%i):\n",fcountLeft);
00242 for(int i=0;i < fcountLeft;i++) {
00243 printf("\nentry %i:\n",i);
00244 printFulcrum(fulcrumarrayLeft[i]);
00245 }
00246 printf("fulcrumyArrayRight(%i):\n",fcountRight);
00247 for(int i=0;i < fcountRight;i++) {
00248 printf("\nentry %i:\n",i);
00249 printFulcrum(fulcrumarrayRight[i]);
00250 }
00251 }
00252
00253 s << "Loaded " << (fcountLeft+fcountRight) << " fulcrums\n";
00254 fclose(f);
00255 }
00256 else {
00257 printf("error loading calibration data: 2%s:%u\n",__FILE__,__LINE__);
00258 return false;
00259 }
00260
00261 if (myID == rightTracker) {
00262 if (debugLevel > 0)
00263 printf("loading Calibration into rightTracker\n");
00264 res = loadCalibration(fulcrumarrayRight, fcountRight);
00265 } else {
00266 if (debugLevel > 0)
00267 printf("loading Calibration into leftTracker\n");
00268 res = loadCalibration(fulcrumarrayLeft, fcountLeft);
00269 }
00270 if(res)
00271 printf("Success! Calibration data loaded\n");
00272 else
00273 printf("error loading calibration data: %s:%u\n",__FILE__,__LINE__);
00274
00275 return res;
00276 }
00277
00278
00279 void tracker_impl::printFulcrum(fulcrum fulc) {
00280 printf("sensorVal: %3.3f, %3.3f, %3.3f\n",fulc.sensorVal[0] ,fulc.sensorVal[1], fulc.sensorVal[2]);
00281 printf("sensorAngle: %3.3f, %3.3f, %3.3f\n",fulc.sensorAngle[0] ,fulc.sensorAngle[1], fulc.sensorAngle[2]);
00282 printf("worldVal: %3.3f, %3.3f, %3.3f\n",fulc.worldVal[0] ,fulc.worldVal[1], fulc.worldVal[2]);
00283 printf("worldAngle: %3.3f, %3.3f, %3.3f\n",fulc.worldAngle[0] ,fulc.worldAngle[1], fulc.worldAngle[2]);
00284 }
00285
00286 bool
00287 tracker_impl::loadCalibration(const fulcrum cal[255], unsigned int length)
00288 {
00289 double world[6],sensor[6];
00290 myTransformer->resetCalibration();
00291 for (unsigned int i=0; i < length; i++)
00292 {
00293 for (int j=0;j<3;j++)
00294 {
00295 sensor[j]=cal[i].sensorVal[j];
00296 sensor[j+3]=cal[i].sensorAngle[j];
00297 world[j]=cal[i].worldVal[j];
00298 world[j+3]=cal[i].worldAngle[j];
00299 }
00300 myTransformer->addCalibrationData(sensor,world);
00301 }
00302 return myTransformer->isCalibrated();
00303 }
00304
00305 bool
00306 tracker_impl::writeNotification(kogmo_rtdb_obj_info_t &dataobj_info, double* data) {
00307 unsigned int i;
00308 unsigned int err;
00309
00310
00311
00312 dataobj_data->data.trackerID = myID;
00313 for (i=0; i<6; i++) {
00314 dataobj_data->data.data[i] = data[i];
00315 }
00316
00317 if (myID == leftTracker)
00318 if (debugLevel > 2 )
00319 printf( "LEFT %3.3lf %3.3lf %3.3lf %3.3lf %3.3lf %3.3lf\n",
00320 data[0], data[1], data[2], data[3], data[4], data[5]);
00321 if (myID == rightTracker)
00322 if (debugLevel > 2 )
00323 printf( "RIGHT %3.3lf %3.3lf %3.3lf %3.3lf %3.3lf %3.3lf\n",
00324 data[0], data[1], data[2], data[3], data[4], data[5]);
00325
00326
00327 err = kogmo_rtdb_obj_writedata (dbc, dataobj_info.oid, dataobj_data); DIEonERR(err);
00328
00329
00330 kogmo_rtdb_cycle_done(dbc,0);
00331
00332 return true;
00333 }
00334
00335
00336
00337
00338
00339 #if tracker_impl_test
00340 int main(int argc, char **argv)
00341 {
00342 bgcorba::init(argc, argv);
00343
00344
00345 tracker_impl *impl = new tracker_impl;
00346
00347 int retval = bgcorba::main(impl);
00348
00349 delete impl;
00350
00351 return retval;
00352 }
00353 #endif
00354