MotionModuleState.h
Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00030
00031 #ifndef __MOTION_MODULE_STATE_H_
00032 #define __MOTION_MODULE_STATE_H_
00033
00034 #include <SE2.h>
00035 #include <LogTrace.h>
00036 #include <list>
00037
00038 namespace MipBaselib{
00039
00040 enum TimestampLocation{
00041 TSPOS_IS_INSIDE,
00042 TSPOS_IS_BEFORE,
00043 TSPOS_IS_AFTER,
00044 TSPOS_NUM
00045 };
00046
00050 class MotionModuleState : public MIPObject{
00051
00052 public:
00053 Pose pose;
00054 Decimal linVel;
00055 Decimal angVel;
00056
00061 MotionModuleState(Pose p=Pose(),Decimal lv=0.0,Decimal av=0.0);
00062
00064 MotionModuleState(const MotionModuleState &v);
00065
00067 MotionModuleState& operator=(const MotionModuleState& v);
00068
00070 MotionModuleState& operator+=(const MotionModuleState& a);
00071
00073 MotionModuleState& operator-=(const MotionModuleState& a);
00074
00076 const MotionModuleState operator+(const MotionModuleState &other) const {
00077 return MotionModuleState(*this) += other;
00078 }
00079
00081 const MotionModuleState operator-(const MotionModuleState &other) const {
00082 return MotionModuleState(*this) -= other;
00083 }
00084
00087 string getObjectName() const {
00088 return "MotionModuleState";
00089 }
00090
00092 Position velVector(void);
00093
00095 virtual string print();
00096 };
00097
00102 class MotionModuleTState : public MotionModuleState{
00103 public:
00104 Time timeStamp;
00105
00110 MotionModuleTState(Pose p=Pose(),Decimal lv=0.0,Decimal av=0.0,Time ts=Time());
00111
00113 MotionModuleTState(const MotionModuleTState &v);
00114
00116 MotionModuleTState& operator=(const MotionModuleTState& v){
00117 if (this != &v){
00118 pose = v.pose;
00119 linVel = v.linVel;
00120 angVel = v.angVel;
00121 timeStamp = v.timeStamp;
00122 }
00123 return *this;
00124 }
00126 string print();
00127 };
00128
00132 class MotionModuleTStateLog : public list <MotionModuleTState>{
00133 private:
00137 MotionModuleTState _interpolation(list<MotionModuleTState>::reverse_iterator rit,list<MotionModuleTState>::reverse_iterator ritBefore,Time ts);
00138 public:
00139
00140
00141
00142
00143
00144
00145
00146
00147
00153 TimestampLocation getTState(MotionModuleTState& outputTState, Time timeStamp);
00154
00162 TimestampLocation correctTState(MotionModuleTState& correctedTState);
00163
00164 string print(){
00165 int thisSize = this->size();
00166 stringstream ss;
00167
00168 ss << this->begin()->pose.print() << " " << this->begin()->timeStamp.dCast() << endl;
00169 ss << this->rbegin()->pose.print()<< " " << this->rbegin()->timeStamp.dCast() << endl;
00170
00171 return ss.str();
00172 }
00173
00174 };
00175
00176 }
00177 #endif