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;
00078 IPSharedMemory* _shm;
00079
00080
bool _collector_running;
00081 pthread_t _collector_t;
00082 pthread_mutex_t _collector_mutex;
00083 pthread_cond_t _collector_cond;
00084
00085
int _history_length;
00086
int _num_poses;
00087
int _cur_pose_index;
00088
VehPoseShmemStruct* _poses;
00089
00090
int _last_secs, _last_usecs;
00091
00092
float _max_extrapolation;
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
00106
if (_shm)
00107 _com->CloseSharedMemory(_shm);
00108
00109
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
00134
00135
00136 _com =
00137 IPCommunicator::Communicator(globals,
00138 params.getString(
"ipt_spec",
00139
"unix:port=0;"));
00140
if (!_com)
00141
return false;
00142
00143
00144
00145
const char* mem_name = params.getString(
"name",
VEH_POSE_SHMEM_NAME);
00146
00147
const char* machine = params.getString(
"machine");
00148
00149
int port = params.getInt(
"port", 1389);
00150
char buffer[200];
00151
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
00158
const char* mem_spec = params.getString(
"mem", buffer);
00159 _shm =
00160 _com->OpenSharedMemory(mem_spec,
VEH_POSE_SHMEM_FMT,
00161
sizeof(
VehPoseShmemStruct));
00162
00163
if (!_shm) {
00164
00165 fprintf(stderr,
00166
"ShmemVehPoseSource::init: Problem opening shared memory %s\n",
00167 mem_spec);
00168
return false;
00169 }
00170
00171
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
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
00189
void* ShmemVehPoseSource::thread_entry(
void* data)
00190 {
00191 ((
ShmemVehPoseSource*) data)->collector_thread();
00192
return NULL;
00193 }
00194
00195
00196
void ShmemVehPoseSource::collector_thread()
00197 {
00198
VehPoseShmemStruct incoming;
00199
while (_collector_running) {
00200
00201
if (!_shm->Wait())
00202
continue;
00203
00204
00205 _shm->FormattedData((
void*) &incoming);
00206
00207
00208
if (!incoming.
secs && !incoming.
usecs)
00209
continue;
00210
00211
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
00220 pthread_cond_signal(&_collector_cond);
00221 }
00222 }
00223
00224
00225
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
00234
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
00244 bool ShmemVehPoseSource::getPose(utils::Time now,
VehPose& veh_pose)
00245 {
00246
00247 pthread_mutex_lock(&_collector_mutex);
00248
00249
00250
VehPoseShmemStruct* cur_pose = &_poses[_cur_pose_index];
00251 utils::Time t(cur_pose->
secs, cur_pose->
usecs);
00252
00253
00254
if (now > t) {
00255
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
00265 set_pose(*cur_pose, veh_pose);
00266
00267
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
00294
interpolate(*prev_pose, *cur_pose, 1 +
double(now-t)/(t-pt), veh_pose);
00295
00296
00297 pthread_mutex_unlock(&_collector_mutex);
00298
return true;
00299 }
00300
00301
00302
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
00313
00314 utils::Time cur(cur_pose->
secs, cur_pose->
usecs);
00315
double dist = (now-t).getValue()/(cur-t).getValue();
00316
00317
00318
interpolate(*prev_pose, *cur_pose, dist, veh_pose);
00319
00320
00321 pthread_mutex_unlock(&_collector_mutex);
00322
return true;
00323 }
00324 cur_pose = prev_pose;
00325 }
00326
00327
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
00337 bool ShmemVehPoseSource::getCurPose(utils::Time& time,
VehPose& veh_pose,
00338
bool blocking)
00339 {
00340
00341 pthread_mutex_lock(&_collector_mutex);
00342
00343
if (blocking) {
00344
00345
if (_cur_pose_index < 0 ||
00346 _poses[_cur_pose_index].
secs != _last_secs ||
00347 _poses[_cur_pose_index].
usecs != _last_usecs) {
00348
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
00359
VehPoseShmemStruct& result = _poses[_cur_pose_index];
00360 set_pose(result, veh_pose);
00361 time.setValue(result.
secs, result.
usecs);
00362
00363
00364
bool res = (_last_secs == result.
secs && _last_usecs == result.
usecs);
00365 _last_secs = result.
secs;
00366 _last_usecs = result.
usecs;
00367
00368
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 }