Main Page | Modules | Namespace List | Class Hierarchy | Class List | Directories | 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 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   // do simple calculation of vehicle position
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 

Generated on Fri Jun 16 13:21:19 2006 for ModUtils by  doxygen 1.4.4