Main Page | Modules | Class Hierarchy | Class List | File List | Class Members | File Members | Related Pages

ShmemVehPoseSource.cc

Go to the documentation of this file.
00001 00005 #include <stdio.h> 00006 #include <pthread.h> 00007 00008 #include <utils/ConfigFile.h> 00009 #include <TimeSource/TimeSource.h> 00010 00011 #include <ipt/ipt.h> 00012 #include <ipt/sharedmem.h> 00013 00014 #include "VehPoseSource.h" 00015 00016 #include <VehPoseDest/VehPoseStructs.h> 00017 00049 class ShmemVehPoseSource : public VehPoseSource { 00050 public: 00051 ShmemVehPoseSource(); 00052 virtual ~ShmemVehPoseSource(); 00053 00055 virtual bool getPose(utils::Time time, VehPose& pose); 00056 00058 virtual bool getCurPose(utils::Time& time, 00059 VehPose& pose, bool blocking = false); 00060 00062 bool init(utils::ConfigFile& params, utils::SymbolTable* globals); 00063 00065 static void interpolate(const VehPoseShmemStruct& prev_pose, 00066 const VehPoseShmemStruct& next_pose, double t, 00067 VehPose& veh_pose); 00068 00069 00070 private: 00071 static void* thread_entry(void*); 00072 void collector_thread(); 00073 void error(VehPose&); 00074 static void set_pose(const VehPoseShmemStruct& input, VehPose& output); 00075 00076 private: 00077 IPCommunicator* _com; // the IPT communicator 00078 IPSharedMemory* _shm; // the pose source shared memory region 00079 00080 bool _collector_running; // true if the collector thread is running 00081 pthread_t _collector_t; // the collector thread ID 00082 pthread_mutex_t _collector_mutex; // mutex which guards ring buffer 00083 pthread_cond_t _collector_cond; // conditional which signals new pose 00084 00085 int _history_length; // the size of the pose ring buffer 00086 int _num_poses; // number of poses in the ring buffer 00087 int _cur_pose_index; // most recent pose index in the ring buffer 00088 VehPoseShmemStruct* _poses; // the ring buffer 00089 00090 int _last_secs, _last_usecs; // time stamp of the last pose received 00091 // by getCurPose 00092 float _max_extrapolation; // how far can we extrapolate pose information? 00093 }; 00094 00095 ShmemVehPoseSource::ShmemVehPoseSource() 00096 { 00097 _shm = NULL; 00098 _collector_running = false; 00099 _poses = NULL; 00100 _last_secs = _last_usecs = -1; 00101 } 00102 00103 ShmemVehPoseSource::~ShmemVehPoseSource() 00104 { 00105 // close shared memory, if necessary 00106 if (_shm) 00107 _com->CloseSharedMemory(_shm); 00108 00109 // shutdown collector thread and cleanup, if necessary 00110 if (_collector_running) { 00111 _collector_running = false; 00112 pthread_cancel(_collector_t); 00113 pthread_mutex_destroy(&_collector_mutex); 00114 pthread_cond_destroy(&_collector_cond); 00115 } 00116 00117 delete [] _poses; 00118 } 00119 00120 void ShmemVehPoseSource::interpolate(const VehPoseShmemStruct& prev_pose, 00121 const VehPoseShmemStruct& cur_pose, 00122 double t, VehPose& veh_pose) 00123 { 00124 VehPose prev_real_pose, cur_real_pose; 00125 set_pose(prev_pose, prev_real_pose); 00126 set_pose(cur_pose, cur_real_pose); 00127 VehPoseSource::interpolate(prev_real_pose, cur_real_pose, t, veh_pose); 00128 } 00129 00130 bool ShmemVehPoseSource::init(utils::ConfigFile& params, 00131 utils::SymbolTable* globals) 00132 { 00133 // IPCommunicator::Communicator creates an IPT communicator only if 00134 // one has not been created and cached in globals already. 00135 // If it does create one, it caches it in globals. 00136 _com = 00137 IPCommunicator::Communicator(globals, 00138 params.getString("ipt_spec", 00139 "unix:port=0;")); 00140 if (!_com) 00141 return false; 00142 00143 // create the shared memory region 00144 // for convenience, the user can just specify name 00145 const char* mem_name = params.getString("name", VEH_POSE_SHMEM_NAME); 00146 // optionally, machine (empty means on local host 00147 const char* machine = params.getString("machine"); 00148 // and the port that the memory manager is running on 00149 int port = params.getInt("port", 1389); 00150 char buffer[200]; 00151 // to create a default memory specification for managed shared memory 00152 if (!*machine) { 00153 sprintf(buffer, "managed: name=%s;", mem_name); 00154 } else { 00155 sprintf(buffer, "managed: name='%s@%s|%d';", mem_name, machine, port); 00156 } 00157 // of course, this can be overridden through explicit specification of "mem" 00158 const char* mem_spec = params.getString("mem", buffer); 00159 _shm = 00160 _com->OpenSharedMemory(mem_spec, VEH_POSE_SHMEM_FMT, 00161 sizeof(VehPoseShmemStruct)); 00162 // if we can't open shared memory 00163 if (!_shm) { 00164 // print an error and return bad 00165 fprintf(stderr, 00166 "ShmemVehPoseSource::init: Problem opening shared memory %s\n", 00167 mem_spec); 00168 return false; 00169 } 00170 00171 // now create and set up the ring buffer 00172 _history_length = params.getInt("history_length", 100); 00173 _num_poses = 0; 00174 _cur_pose_index = -1; 00175 _poses = new VehPoseShmemStruct[_history_length]; 00176 00177 _max_extrapolation = params.getFloat("max_extrapolation", 0.2); 00178 00179 // and finally, kick off the collector thread 00180 pthread_mutex_init(&_collector_mutex, NULL); 00181 pthread_cond_init(&_collector_cond, NULL); 00182 _collector_running = true; 00183 pthread_create(&_collector_t, NULL, thread_entry, this); 00184 00185 return true; 00186 } 00187 00188 // entry point for the collector thread 00189 void* ShmemVehPoseSource::thread_entry(void* data) 00190 { 00191 ((ShmemVehPoseSource*) data)->collector_thread(); 00192 return NULL; 00193 } 00194 00195 // the collector thread 00196 void ShmemVehPoseSource::collector_thread() 00197 { 00198 VehPoseShmemStruct incoming; 00199 while (_collector_running) { 00200 // wait for new data in the shared memory 00201 if (!_shm->Wait()) 00202 continue; 00203 00204 // unmarshal it and stick pose in incoming 00205 _shm->FormattedData((void*) &incoming); 00206 // the time tag of the pose can get set to 0 on startup and shutdown 00207 // of the module producing the poses. Just skip these. 00208 if (!incoming.secs && !incoming.usecs) 00209 continue; 00210 00211 // Put the new pose in the ring buffer 00212 pthread_mutex_lock(&_collector_mutex); 00213 if (_num_poses != _history_length) 00214 _num_poses++; 00215 _cur_pose_index = (_cur_pose_index + 1) % _history_length; 00216 _poses[_cur_pose_index] = incoming; 00217 pthread_mutex_unlock(&_collector_mutex); 00218 00219 // signal we have new data for anyone blocking in getCurPose 00220 pthread_cond_signal(&_collector_cond); 00221 } 00222 } 00223 00224 // convenience method for invalidating sensor pose and releasing the 00225 // collector mutex 00226 void ShmemVehPoseSource::error(VehPose& veh_pose) 00227 { 00228 veh_pose.pos = utils::Vec3d(); 00229 veh_pose.ori = utils::Rotation(); 00230 pthread_mutex_unlock(&_collector_mutex); 00231 } 00232 00233 // convenience method for setting a sensor pose output from the data in the 00234 // shared memory region input 00235 void ShmemVehPoseSource::set_pose(const VehPoseShmemStruct& input, 00236 VehPose& output) 00237 { 00238 output.pos = utils::Vec3d(input.data.x, input.data.y, input.data.z); 00239 output.ori = utils::Rotation(input.data.ori[0], input.data.ori[1], 00240 input.data.ori[2], input.data.ori[3]); 00241 } 00242 00243 // Lookup the sensor pose at time now and put it in sensor pose, if possible 00244 bool ShmemVehPoseSource::getPose(utils::Time now, VehPose& veh_pose) 00245 { 00246 // lock the ring buffer 00247 pthread_mutex_lock(&_collector_mutex); 00248 00249 // Get the latest sensor pose 00250 VehPoseShmemStruct* cur_pose = &_poses[_cur_pose_index]; 00251 utils::Time t(cur_pose->secs, cur_pose->usecs); 00252 00253 // if the requested time is after the latest sensor pose 00254 if (now > t) { 00255 // figure out if we can extrapolate 00256 double elapsed = (now - t).getValue(); 00257 if (elapsed > _max_extrapolation) { 00258 fprintf(stderr, "ShmemVehPoseSource::getPose: " 00259 "Pose lookup time too far in the future, delta = %f\n", 00260 (now - t).getValue()); 00261 error(veh_pose); 00262 return false; 00263 } 00264 // default all fields to cur_pose value. 00265 set_pose(*cur_pose, veh_pose); 00266 00267 // get previous pose 00268 VehPoseShmemStruct* prev_pose; 00269 utils::Time pt; 00270 int prev_index = _cur_pose_index; 00271 while (1) { 00272 prev_index--; 00273 if (prev_index < 0 && _num_poses < _history_length) { 00274 fprintf(stderr, "ShmemVehPoseSource::getPose: " 00275 "Cannot extrapolate yet\n", 00276 (now - t).getValue()); 00277 error(veh_pose); 00278 return false; 00279 } 00280 prev_pose = &_poses[prev_index]; 00281 pt.setValue(prev_pose->secs, prev_pose->usecs); 00282 if (pt > t) { 00283 fprintf(stderr, "ShmemVehPoseSource::getPose: " 00284 "Cannot extrapolate with current history\n", 00285 (now - t).getValue()); 00286 error(veh_pose); 00287 return false; 00288 } 00289 if (pt != t) 00290 break; 00291 } 00292 00293 // extrapolate pose (with t > 1) 00294 interpolate(*prev_pose, *cur_pose, 1 + double(now-t)/(t-pt), veh_pose); 00295 00296 // unlock the ring buffer and return 00297 pthread_mutex_unlock(&_collector_mutex); 00298 return true; 00299 } 00300 00301 // go through the ring buffer looking for the elements bracketing 00302 // the requested time 00303 int cur_index = _cur_pose_index; 00304 VehPoseShmemStruct* prev_pose = cur_pose; 00305 for (int i=0;i<_num_poses-1;i++) { 00306 cur_index--; 00307 if (cur_index < 0) 00308 cur_index = _history_length-1; 00309 prev_pose = &_poses[cur_index]; 00310 t.setValue(prev_pose->secs, prev_pose->usecs); 00311 if (now > t) { 00312 // we have found the bracketing elements 00313 00314 utils::Time cur(cur_pose->secs, cur_pose->usecs); 00315 double dist = (now-t).getValue()/(cur-t).getValue(); 00316 00317 // interpolate appropriately 00318 interpolate(*prev_pose, *cur_pose, dist, veh_pose); 00319 00320 // unlock the ring buffer and return 00321 pthread_mutex_unlock(&_collector_mutex); 00322 return true; 00323 } 00324 cur_pose = prev_pose; 00325 } 00326 00327 // requrest time is too far in the past 00328 fprintf(stderr, "ShmemVehPoseSource::getPose: " 00329 "State lookup time too old, delta = %f\n", 00330 (now - TimeSource::now()).getValue()); 00331 error(veh_pose); 00332 00333 return false; 00334 } 00335 00336 // get the latest sensor pose, blocking if necessary 00337 bool ShmemVehPoseSource::getCurPose(utils::Time& time, VehPose& veh_pose, 00338 bool blocking) 00339 { 00340 // lock the ring buffer 00341 pthread_mutex_lock(&_collector_mutex); 00342 00343 if (blocking) { 00344 // if we are not at the first one and do not have new data 00345 if (_cur_pose_index < 0 || 00346 _poses[_cur_pose_index].secs != _last_secs || 00347 _poses[_cur_pose_index].usecs != _last_usecs) { 00348 // wait for new data 00349 if (pthread_cond_wait(&_collector_cond, &_collector_mutex)) { 00350 pthread_mutex_unlock(&_collector_mutex); 00351 perror("ShmemVehPoseSource::getCurPose: waiting for condition"); 00352 error(veh_pose); 00353 return false; 00354 } 00355 } 00356 } 00357 00358 // get the latest sensor pose 00359 VehPoseShmemStruct& result = _poses[_cur_pose_index]; 00360 set_pose(result, veh_pose); 00361 time.setValue(result.secs, result.usecs); 00362 00363 // mark if this is new data or not 00364 bool res = (_last_secs == result.secs && _last_usecs == result.usecs); 00365 _last_secs = result.secs; 00366 _last_usecs = result.usecs; 00367 00368 // unlock the ring buffer 00369 pthread_mutex_unlock(&_collector_mutex); 00370 00371 return res; 00372 } 00373 00375 VehPoseSource* create_VehPoseSource_shmem(VehPoseSourceGenerator*, 00376 utils::ConfigFile* params, 00377 utils::SymbolTable* globals) 00378 { 00379 ShmemVehPoseSource* player = new ShmemVehPoseSource(); 00380 if (!player->init(*params, globals)) { 00381 delete player; 00382 return NULL; 00383 } 00384 return player; 00385 }

Generated on Tue Sep 7 20:37:49 2004 for ModUtils by doxygen 1.3.8