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 UTILS_INTF_CREATOR(VehPoseSource, fake, gen, params, globals)
00031 {
00032 UTILS_INTF_REPORT(VehPoseSource, fake);
00033 FakeVehPoseSource* intf = new FakeVehPoseSource();
00034 if (!intf->init(*params)) {
00035 delete intf;
00036 return NULL;
00037 }
00038 return intf;
00039 }
00040
00041 bool FakeVehPoseSource::init(utils::ConfigFile& params)
00042 {
00043 _radius = params.getDouble("radius", 1000);
00044 _speed = params.getDouble("speed", 1);
00045 _start_time = TimeSource::now();
00046
00047 return true;
00048 }
00049
00050 bool FakeVehPoseSource::getPose(utils::Time time, VehPose& pose)
00051 {
00052
00053 utils::Time elapsed = time - _start_time;
00054 double theta = fmod(_speed/_radius*elapsed.getValue(), 2.0*M_PI);
00055 pose.pos.x = sin(theta)*_radius;
00056 pose.pos.y = _radius - cos(theta)*_radius;
00057 pose.pos.z = 0;
00058 pose.ori = utils::Rotation(utils::Vec3d(0, 0, 1), theta);
00059
00060 return true;
00061 }
00062
