00001
00005
#include <iostream>
00006
00007
#include <utils/ConfigFile.h>
00008
#include <TimeSource/TimeSource.h>
00009
00010
#include "VehPoseSource.h"
00011
00015 class FakeVehPoseSource :
public VehPoseSource {
00016
public:
00018
virtual bool getPose(utils::Time time,
VehPose& pose);
00019
00021
bool init(utils::ConfigFile& params);
00022
00023
private:
00024
double _radius;
00025
double _speed;
00026 utils::Time _start_time;
00027 };
00028
00030 VehPoseSource*
create_VehPoseSource_fake(
VehPoseSourceGenerator*,
00031 utils::ConfigFile* params,
00032 utils::SymbolTable* globals)
00033 {
00034
FakeVehPoseSource* intf =
new FakeVehPoseSource();
00035
if (!intf->
init(*params)) {
00036
delete intf;
00037
return NULL;
00038 }
00039
return intf;
00040 }
00041
00042 bool FakeVehPoseSource::init(utils::ConfigFile& params)
00043 {
00044 _radius = params.getDouble(
"radius", 1000);
00045 _speed = params.getDouble(
"speed", 1);
00046 _start_time = TimeSource::now();
00047
00048
return true;
00049 }
00050
00051 bool FakeVehPoseSource::getPose(utils::Time time,
VehPose& pose)
00052 {
00053
00054 utils::Time elapsed = time - _start_time;
00055
double theta = fmod(_speed/_radius*elapsed.getValue(), 2.0*M_PI);
00056 pose.
pos.x = sin(theta)*_radius;
00057 pose.
pos.y = _radius - cos(theta)*_radius;
00058 pose.
pos.z = 0;
00059 pose.
ori = utils::Rotation(utils::Vec3d(0, 0, 1), theta);
00060
00061
return true;
00062 }
00063