00001
00005
#include <stdio.h>
00006
#include <ipt/ipt.h>
00007
#include <ipt/sharedmem.h>
00008
00009
#include <utils/SymbolTable.h>
00010
#include <utils/ConfigFile.h>
00011
00012
#include "VehPoseDest.h"
00013
#include "VehPoseStructs.h"
00014
00016 class ShmemVehPoseDest :
public VehPoseDest {
00017
public:
00019
virtual bool outputPose(utils::Time time,
const VehPose& veh_pose);
00020
00022
bool init(utils::ConfigFile& params, utils::SymbolTable* globals);
00023
00024
private:
00025 IPSharedMemory* _shm;
00026
VehPoseShmemStruct _output_area;
00027 };
00028
00030 VehPoseDest*
create_VehPoseDest_shmem(
VehPoseDestGenerator*,
00031 utils::ConfigFile* params,
00032 utils::SymbolTable* globals)
00033 {
00034
ShmemVehPoseDest* intf =
new ShmemVehPoseDest();
00035
if (!intf->
init(*params, globals)) {
00036
delete intf;
00037
return NULL;
00038 }
00039
return intf;
00040 }
00041
00042 bool ShmemVehPoseDest::init(utils::ConfigFile& params,
00043 utils::SymbolTable* globals)
00044 {
00045
00046
00047 IPCommunicator* com =
00048 IPCommunicator::Communicator(globals,
00049 params.getString(
"ipt_spec",
00050
"unix: int port=0;"));
00051
if (!com)
00052
return false;
00053
00054
00055
const char* mem_name = params.getString(
"name",
VEH_POSE_SHMEM_NAME);
00056
char buffer[200];
00057
00058 sprintf(buffer,
"managed: name=%s; owner=true;", mem_name);
00059
00060
00061
const char* mem_spec = params.getString(
"mem", buffer);
00062
00063 _shm =
00064 com->OpenSharedMemory(mem_spec,
VEH_POSE_SHMEM_FMT,
00065
sizeof(
VehPoseShmemStruct));
00066
if (!_shm) {
00067 printf(
"Problem opening shared memory %s\n", mem_spec);
00068
return false;
00069 }
00070
00071
return true;
00072 }
00073
00074 bool ShmemVehPoseDest::outputPose(utils::Time time,
const VehPose& veh_pose)
00075 {
00076
00077 _output_area.
data.
x = veh_pose.
pos.x;
00078 _output_area.
data.
y = veh_pose.
pos.y;
00079 _output_area.
data.
z = veh_pose.
pos.z;
00080 _output_area.
data.
ori[0] = veh_pose.
ori[0];
00081 _output_area.
data.
ori[1] = veh_pose.
ori[1];
00082 _output_area.
data.
ori[2] = veh_pose.
ori[2];
00083 _output_area.
data.
ori[3] = veh_pose.
ori[3];
00084 time.getValue(_output_area.
secs, _output_area.
usecs);
00085
00086
00087 _shm->PutFormattedData((
void*) &_output_area);
00088
00089
return true;
00090 }
00091
00092
00093
00094