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 <RoadDest/RoadStructs.h>
00013
#include "RoadSource.h"
00014
00017 class ShmemRoadSource :
public RoadSource {
00018
public:
00019
ShmemRoadSource();
00020
00022
virtual bool getPoints(utils::Time& time,
00023 std::vector<utils::Vec3d>& points,
00024
bool blocking =
true);
00025
00027
bool init(utils::ConfigFile& params, utils::SymbolTable* globals);
00028
00029
private:
00030 IPSharedMemory* _shm;
00031
int _last_tag;
00032 };
00033
00035 RoadSource*
create_RoadSource_shmem(
RoadSourceGenerator*,
00036 utils::ConfigFile* params,
00037 utils::SymbolTable* globals)
00038 {
00039
ShmemRoadSource* intf =
new ShmemRoadSource();
00040
if (!intf->
init(*params, globals)) {
00041
delete intf;
00042
return NULL;
00043 }
00044
return intf;
00045 }
00046
00047 ShmemRoadSource::ShmemRoadSource()
00048 {
00049 _last_tag = -1;
00050 }
00051
00052 bool ShmemRoadSource::init(utils::ConfigFile& params,
00053 utils::SymbolTable* globals)
00054 {
00055
00056
00057 IPCommunicator* com =
00058 IPCommunicator::Communicator(globals,
00059 params.getString(
"ipt_spec",
00060
"unix: int port=0;"));
00061
if (!com)
00062
return false;
00063
00064
00065
00066
00067
const char* mem_name = params.getString(
"name",
ROAD_SHMEM_NAME);
00068
const char* machine = params.getString(
"machine");
00069
int port = params.getInt(
"port", 1389);
00070
char buffer[200];
00071
if (!*machine) {
00072 sprintf(buffer,
"managed: name=%s;", mem_name);
00073 }
else {
00074 sprintf(buffer,
"managed: name='%s@%s|%d';", mem_name, machine, port);
00075 }
00076
const char* mem_spec = params.getString(
"mem", buffer);
00077
00078
int max_points = params.getInt(
"max_points", 20);
00079
00080 _shm =
00081 com->OpenSharedMemory(mem_spec,
ROAD_SHMEM_FMT,
00082
sizeof(
RoadShmemStruct) +
00083 max_points*
sizeof(
RoadDataPoint));
00084
if (!_shm) {
00085 printf(
"Problem opening shared memory %s\n", mem_spec);
00086
return false;
00087 }
00088
00089
return true;
00090 }
00091
00092 bool ShmemRoadSource::getPoints(utils::Time& time,
00093 std::vector<utils::Vec3d>& points,
00094
bool blocking)
00095 {
00096
if (blocking) {
00097
00098
if (!_shm->Wait()) {
00099
00100 time = utils::Time();
00101
return false;
00102 }
00103 }
00104
00105
RoadShmemStruct input_area;
00106
if (!_shm->FormattedData(&input_area)) {
00107
00108 time = utils::Time();
00109
return false;
00110 }
00111
00112
00113 time.setValue(input_area.
secs, input_area.
usecs);
00114 points.clear();
00115
for (
int i=0;i<input_area.
data.
num_points;i++) {
00116
RoadDataPoint& pt = input_area.
data.
points[i];
00117 points.push_back(utils::Vec3d(pt.
x, pt.
y, pt.
z));
00118 }
00119
00120
00121 _shm->DeleteContents(&input_area);
00122
00123
00124
if (_shm->Tag() != _last_tag) {
00125 _last_tag = _shm->Tag();
00126
return true;
00127 }
else
00128
return false;
00129 }
00130