Main Page | Modules | Class Hierarchy | Class List | File List | Class Members | File Members | Related Pages

FakeVehPoseSource.cc

Go to the documentation of this file.
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 // do simple calculation of vehicle position 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

Generated on Tue Sep 7 20:37:49 2004 for ModUtils by doxygen 1.3.8