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
00027
00028 #ifndef CRobotSimulator_H
00029 #define CRobotSimulator_H
00030
00031 #include <mrpt/utils/CSerializable.h>
00032 #include <mrpt/poses/CPose2D.h>
00033
00034 namespace mrpt
00035 {
00036 namespace slam
00037 {
00038 using namespace mrpt::poses;
00039 using namespace mrpt::utils;
00040
00056 class MRPTDLLIMPEXP CRobotSimulator
00057 {
00058 private:
00059
00060
00063 double X,Y,PHI;
00064
00067 double v;
00068
00071 double w;
00072
00075 double t;
00076
00079 double odo_x,odo_y,odo_phi;
00080
00083 bool usar_error_odometrico;
00084
00091 double Command_Time,
00092 Command_v, Command_w,
00093 Command_v0, Command_w0;
00094
00097 float cTAU;
00098
00101 float cDELAY;
00102
00103 double m_Ax_err_bias, m_Ax_err_std;
00104 double m_Ay_err_bias, m_Ay_err_std;
00105 double m_Aphi_err_bias, m_Aphi_err_std;
00106
00107 public:
00110 CRobotSimulator( float TAU = 1.9f, float DELAY = 0.3f);
00111
00114 virtual ~CRobotSimulator();
00115
00119 void SetOdometryErrors(
00120 bool enabled,
00121 double Ax_err_bias = 1e-6,
00122 double Ax_err_std = 10e-6,
00123 double Ay_err_bias = 1e-6,
00124 double Ay_err_std = 10e-6,
00125 double Aphi_err_bias = DEG2RAD(1e-6),
00126 double Aphi_err_std = DEG2RAD(10e-6)
00127 )
00128 {
00129 usar_error_odometrico=enabled;
00130 m_Ax_err_bias=Ax_err_bias;
00131 m_Ax_err_std=Ax_err_std;
00132 m_Ay_err_bias=Ay_err_bias;
00133 m_Ay_err_std=Ay_err_std;
00134 m_Aphi_err_bias=Aphi_err_bias;
00135 m_Aphi_err_std=Aphi_err_std;
00136 }
00137
00140 void SetX(double X) { this->X=odo_x=X; }
00143 void SetY(double Y) { this->Y=odo_y=Y; }
00146 void SetPHI(double PHI) { this->PHI=odo_phi=PHI; }
00147
00150 double GetX() { return X; }
00153 double GetY() { return Y; }
00156 double GetPHI() { return PHI; }
00159 double GetT() { return t; }
00160
00163 double GetV() { return v; }
00166 double GetW() { return w; }
00167
00171 void SetV(double v) { this->v=v; }
00172 void SetW(double w) { this->w=w; }
00173
00176 void MovementCommand ( double lin_vel, double ang_vel );
00177
00180 void ResetStatus();
00181
00184 void ResetTime() { t = 0.0; }
00185
00188 void SimulateInterval( double At);
00189
00192 void ResetOdometry( double x, double y, double phi )
00193 {
00194 odo_x = x;
00195 odo_y = y;
00196 odo_phi = phi;
00197 }
00201 void GetOdometry (float *x, float *y, float *phi)
00202 {
00203 *x= odo_x;
00204 *y= odo_y;
00205 *phi= odo_phi;
00206 }
00207
00211 void GetOdometry ( CPose2D &pose )
00212 {
00213 pose.x = odo_x;
00214 pose.y = odo_y;
00215 pose.phi = odo_phi;
00216 }
00217
00221 void GetRealPose ( CPose2D &pose )
00222 {
00223 pose.x = X;
00224 pose.y = Y;
00225 pose.phi = PHI;
00226 }
00227 };
00228
00229 }
00230 }
00231
00232 #endif