00001
00005
#include <iostream>
00006
00007
#include <utils/ConfigFile.h>
00008
#include <utils/Generator.h>
00009
#include <TimeSource/TimeSource.h>
00010
#include <VehPoseDest/VehPoseDest.h>
00011
00012
#include "VehPoseSource.h"
00013
00017 class OutputVehPoseSource :
public VehPoseSource {
00018
public:
00019
OutputVehPoseSource();
00020
virtual ~
OutputVehPoseSource();
00021
00023
virtual bool getCurPose(utils::Time& time,
VehPose& pose,
00024
bool blocking =
true);
00025
00027
virtual bool getPose(utils::Time time,
VehPose& pose);
00028
00030
bool init(utils::ConfigFile& params,
00031
VehPoseSourceGenerator* gen, utils::SymbolTable* globals);
00032
00033
private:
00034
VehPoseSource* _contained;
00035
VehPoseDest* _output;
00036 };
00037
00039 VehPoseSource*
create_VehPoseSource_out(
VehPoseSourceGenerator* gen,
00040 utils::ConfigFile* params,
00041 utils::SymbolTable* globals)
00042 {
00043
OutputVehPoseSource* intf =
new OutputVehPoseSource();
00044
if (!intf->
init(*params, gen, globals)) {
00045
delete intf;
00046
return NULL;
00047 }
00048
return intf;
00049 }
00050
00053 VehPoseSource*
create_VehPoseSource_logger(
VehPoseSourceGenerator* gen,
00054 utils::ConfigFile* params,
00055 utils::SymbolTable* globals)
00056 {
00057
00058 utils::ConfigFile output_params;
00059 utils::ConfigFile::copy(*params, output_params);
00060 output_params.setString(
"tag",
"logger");
00061 output_params.set(
"contained",
"{}");
00062
00063
00064 utils::ConfigFile contained_params;
00065 params->getStruct(
"contained", contained_params);
00066
00067
00068 utils::ConfigFile final_params;
00069 final_params.setString(
"tag",
"output");
00070 final_params.setStruct(
"contained", contained_params);
00071 final_params.setStruct(
"output", output_params);
00072
00073
OutputVehPoseSource* intf =
new OutputVehPoseSource();
00074
if (!intf->
init(final_params, gen, globals)) {
00075
delete intf;
00076
return NULL;
00077 }
00078
return intf;
00079 }
00080
00083 VehPoseSource*
create_VehPoseSource_shmemPublish(
VehPoseSourceGenerator* gen,
00084 utils::ConfigFile* params,
00085 utils::SymbolTable* globals)
00086 {
00087
00088 utils::ConfigFile output_params;
00089 utils::ConfigFile::copy(*params, output_params);
00090 output_params.setString(
"tag",
"shmem");
00091 output_params.set(
"contained",
"{}");
00092
00093
00094 utils::ConfigFile contained_params;
00095 params->getStruct(
"contained", contained_params);
00096
00097
00098 utils::ConfigFile final_params;
00099 final_params.setString(
"tag",
"output");
00100 final_params.setStruct(
"contained", contained_params);
00101 final_params.setStruct(
"output", output_params);
00102
00103
OutputVehPoseSource* intf =
new OutputVehPoseSource();
00104
if (!intf->
init(final_params, gen, globals)) {
00105
delete intf;
00106
return NULL;
00107 }
00108
return intf;
00109 }
00110
00111 OutputVehPoseSource::OutputVehPoseSource()
00112 {
00113 _contained = NULL;
00114 _output = NULL;
00115 }
00116
00117 OutputVehPoseSource::~OutputVehPoseSource()
00118 {
00119
if (_contained)
00120 _contained->unref();
00121
if (_output)
00122 _output->unref();
00123 }
00124
00125 bool OutputVehPoseSource::init(utils::ConfigFile& params,
00126
VehPoseSourceGenerator* gen,
00127 utils::SymbolTable* globals)
00128 {
00129
00130 _contained = gen->
interface(params.getString(
"contained"), globals);
00131
if (!_contained) {
00132 cerr <<
"OutputVehPoseSource::init: could not create contained\n";
00133
return false;
00134 }
00135 _contained->ref();
00136
00137 _output =
VehPoseDest::generate(params.getString(
"output",
"default"), globals);
00138
if (!_output) {
00139 cerr <<
"OutputVehPoseSource::init: could not create output\n";
00140
return false;
00141 }
00142 _output->ref();
00143
00144
return true;
00145 }
00146
00147 bool OutputVehPoseSource::getCurPose(utils::Time& time,
VehPose& pose,
00148
bool blocking)
00149 {
00150
if (_contained->
getCurPose(time, pose, blocking)) {
00151 _output->
outputPose(time, pose);
00152
return true;
00153 }
00154
return false;
00155 }
00156
00157 bool OutputVehPoseSource::getPose(utils::Time time,
VehPose& pose)
00158 {
00159
if (_contained->
getPose(time, pose)) {
00160 _output->
outputPose(time, pose);
00161
return true;
00162 }
00163
return false;
00164 }