Main MRPT website > C++ reference for MRPT 1.5.6
CRobot2DPoseEstimator.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 #include "base-precomp.h" // Precompiled headers
11 
12 #include <mrpt/poses/CPose2D.h> // for CPose2D
13 #include <mrpt/poses/CRobot2DPoseEstimator.h> // for CRobot2DPoseEstimator
14 #include <cmath> // for cos, sin, abs
15 #include <exception> // for exception
16 #include <iostream> // for operator<<, basic_ostream
17 #include "mrpt/math/lightweight_geom_data.h" // for TPose2D, TTwist2D, TPo...
18 #include "mrpt/synch/CCriticalSection.h" // for CCriticalSection, CCri...
19 #include "mrpt/system/datetime.h" // for INVALID_TIMESTAMP, tim...
20 #include "mrpt/utils/mrpt_macros.h" // for MRPT_END, MRPT_START, e
21 
22 using namespace mrpt;
23 using namespace mrpt::poses;
24 using namespace mrpt::math;
25 using namespace mrpt::utils;
26 using namespace mrpt::synch;
27 using namespace mrpt::system;
28 using namespace std;
29 
30 
31 /* --------------------------------------------------------
32  Ctor
33  -------------------------------------------------------- */
35 {
36  reset();
37 }
38 
39 /* --------------------------------------------------------
40  Dtor
41  -------------------------------------------------------- */
43 {
44 }
45 
46 /* --------------------------------------------------------
47  reset
48  -------------------------------------------------------- */
50 {
51  CCriticalSectionLocker lock(&m_cs);
52 
53  m_last_loc_time = INVALID_TIMESTAMP;
54  m_last_odo_time = INVALID_TIMESTAMP;
55 
56  m_last_loc = TPose2D(0,0,0);
57  m_loc_odo_ref = TPose2D(0,0,0);
58  m_last_odo = TPose2D(0,0,0);
59 
60  m_robot_vel_local = TTwist2D(.0, .0, .0);
61 }
62 
63 /** Updates the filter so the pose is tracked to the current time */
65  const TPose2D &newPose,
66  TTimeStamp cur_tim)
67 {
68  CCriticalSectionLocker lock(&m_cs);
69 
70  // Overwrite old localization data:
71  m_last_loc_time = cur_tim;
72  m_last_loc = newPose;
73 
74  // And create interpolated odometry data to work as "reference pose":
75  if (m_last_odo_time!=INVALID_TIMESTAMP)
76  {
77  const double dT = timeDifference(m_last_odo_time,cur_tim);
78  extrapolateRobotPose(m_last_odo,m_robot_vel_local,dT,m_loc_odo_ref);
79  }
80 }
81 
82 /** Updates the filter so the pose is tracked to the current time */
84  const TPose2D &newGlobalOdometry,
85  TTimeStamp cur_tim,
86  bool hasVelocities,
87  const mrpt::math::TTwist2D &velLocal
88  )
89 {
91 
92  CCriticalSectionLocker lock(&m_cs);
93 
94  if (m_last_odo_time!=INVALID_TIMESTAMP)
95  {
96  const double dT = timeDifference(m_last_odo_time,cur_tim);
97  if (dT<=0) std::cerr << "[CRobot2DPoseEstimator::processUpdateNewOdometry] WARNING: Diff. in timestamps between odometry should be >0, and it's " << dT << "\n";
98  }
99 
100  // First, update velocities:
101  if (hasVelocities)
102  {
103  m_robot_vel_local = velLocal;
104  }
105  else
106  { // Note: JLBC 23/Nov/2016: I have removed an estimation of velocity from increments of odometry
107  // because it was really bad. Just don't make up things: if the user doesn't provide us velocities,
108  // we don't use velocities.
109  m_robot_vel_local = TTwist2D(.0, .0, .0);
110  }
111 
112  // And now times & odo:
113  m_last_odo_time = cur_tim;
114  m_last_odo = newGlobalOdometry;
115 
116  MRPT_END
117 }
118 
120 {
121  if (m_last_odo_time == INVALID_TIMESTAMP || m_last_loc_time == INVALID_TIMESTAMP)
122  return false;
123 
124  const double dTimeLoc = timeDifference(m_last_loc_time, tim_query);
125  if (dTimeLoc>params.max_localiz_age)
126  return false;
127 
128  // Overall estimate:
129  // last_loc (+) [ last_odo (-) odo_ref ] (+) extrapolation_from_vw
130  const TPose2D p = TPose2D(CPose2D(m_last_loc) + (CPose2D(m_last_odo) - CPose2D(m_loc_odo_ref)));
131 
132  // Add the extrapolation:
133  const double dTimeOdo = timeDifference(m_last_odo_time, tim_query);
134  if (dTimeOdo >params.max_odometry_age)
135  return false;
136 
137  extrapolateRobotPose(p, m_robot_vel_local,dTimeOdo,pose);
138 
139  // Constant speed model:
140  velLocal = m_robot_vel_local;
141  velGlobal = velLocal;
142  velGlobal.rotate(pose.phi);
143 
144  return true;
145 }
146 
148  TPose2D &pose, float &v, float &w,
149  TTimeStamp tim_query
150  ) const
151 {
152  mrpt::math::TTwist2D velLocal, velGlobal;
153  bool ret = getCurrentEstimate(pose, velLocal, velGlobal, tim_query);
154  v = velLocal.vx;
155  w = velLocal.omega;
156  return ret;
157 }
158 
159 /** get the current estimate
160 * \return true is the estimate can be trusted. False if the real observed data is too old.
161 */
163 {
164  if (m_last_odo_time == INVALID_TIMESTAMP &&
165  m_last_loc_time == INVALID_TIMESTAMP)
166  {
167  pose = TPose2D(0, 0, 0);
168  return false;
169  }
170 
171  bool ret_odo;
172  if (m_last_odo_time!=INVALID_TIMESTAMP && m_last_loc_time!=INVALID_TIMESTAMP)
173  ret_odo = (m_last_odo_time>m_last_loc_time);
174  else if (m_last_odo_time!=INVALID_TIMESTAMP)
175  ret_odo=true;
176  else ret_odo=false;
177 
178  if (ret_odo)
179  pose= CPose2D(m_last_loc) + ( CPose2D(m_last_odo) - CPose2D(m_loc_odo_ref) );
180  else
181  pose= m_last_loc;
182 
183  return true;
184 }
185 
186 
187 // An auxiliary method to extrapolate the pose of a robot located at "p"
188 // with velocities (v,w) after a time delay "delta_time".
190  const TPose2D &p,
191  const mrpt::math::TTwist2D &velLocal,
192  const double delta_time,
193  TPose2D &new_p)
194 {
195  if (velLocal.vx==0 && velLocal.vy ==0 && velLocal.omega == 0)
196  { // Still
197  new_p = p;
198  }
199  else
200  if (std::abs(velLocal.vy)>1e-2)
201  { // non-Ackermann-like vehicle: extrapolate as a straight line:
202  const TPoint2D dp=TPoint2D(delta_time*velLocal.vx, delta_time*velLocal.vy);
203  TPoint2D pg;
204  CPose2D(p).composePoint(dp, pg);
205  new_p.x = pg.x;
206  new_p.y = pg.y;
207  new_p.phi = p.phi + delta_time * velLocal.omega;
208  }
209  else
210  {
211  // vy==0, assume we are in a Ackermann-like steering vehicle: compute an arc:
212  const double R = velLocal.vx / velLocal.omega; // Radius
213  const double theta = velLocal.omega*delta_time; // traversed arc
214  const double cc = cos(p.phi);
215  const double ss = sin(p.phi);
216 
217  const double arc_x = R*sin(theta);
218  const double arc_y = R*(1-cos(theta));
219 
220  new_p.x = p.x + cc*arc_x - ss*arc_y;
221  new_p.y = p.y + ss*arc_x + cc*arc_y;
222  new_p.phi = p.phi + theta;
223  }
224 }
225 
227 {
229  mrpt::math::TTwist2D velLocal, velGlobal;
230  bool ret = getCurrentEstimate(p, velLocal, velGlobal, tim_query);
231  if (ret)
232  pose = CPose2D(p);
233  v = velLocal.vx;
234  w = velLocal.omega;
235  return ret;
236 }
237 
239 {
241  bool v = getLatestRobotPose(p);
242  if (v)
243  {
244  pose.x(p.x);
245  pose.y(p.y);
246  pose.phi(p.phi);
247  }
248  return v;
249 }
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:30
const GLdouble * v
Definition: glew.h:1296
A class acquiring a CCriticalSection at its constructor, and releasing it at destructor.
void rotate(const double ang)
Transform the (vx,vy) components for a counterclockwise rotation of ang radians.
double y
X,Y coordinates.
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
GLubyte GLubyte GLubyte GLubyte w
Definition: glew.h:1797
void reset()
Resets all internal state.
2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
bool getCurrentEstimate(mrpt::math::TPose2D &pose, mrpt::math::TTwist2D &velLocal, mrpt::math::TTwist2D &velGlobal, mrpt::system::TTimeStamp tim_query=mrpt::system::now()) const
Get the estimate for a given timestamp (defaults to now()), obtained as:
#define MRPT_END
void processUpdateNewPoseLocalization(const mrpt::math::TPose2D &newPose, mrpt::system::TTimeStamp tim)
Updates the filter with new global-coordinates localization data from a localization or SLAM source...
bool getLatestRobotPose(mrpt::math::TPose2D &pose) const
Get the latest known robot pose, either from odometry or localization.
double vy
Velocity components: X,Y (m/s)
GLfloat GLfloat p
Definition: glew.h:10113
GLboolean reset
Definition: glew.h:2897
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:17
void processUpdateNewOdometry(const mrpt::math::TPose2D &newGlobalOdometry, mrpt::system::TTimeStamp cur_tim, bool hasVelocities=false, const mrpt::math::TTwist2D &newRobotVelLocal=mrpt::math::TTwist2D())
Updates the filter with new odometry readings.
double y
X,Y coordinates.
#define MRPT_START
void composePoint(double lx, double ly, double &gx, double &gy) const
An alternative, slightly more efficient way of doing with G and L being 2D points and P this 2D pose...
Definition: CPose2D.cpp:178
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
const float R
Lightweight 2D pose.
GLfloat * params
Definition: glew.h:1436
double BASE_IMPEXP timeDifference(const mrpt::system::TTimeStamp t_first, const mrpt::system::TTimeStamp t_later)
Returns the time difference from t1 to t2 (positive if t2 is posterior to t1), in seconds...
Definition: datetime.cpp:205
static void extrapolateRobotPose(const mrpt::math::TPose2D &p, const mrpt::math::TTwist2D &robot_vel_local, const double delta_time, mrpt::math::TPose2D &new_p)
An auxiliary method to extrapolate the pose of a robot located at "p" with velocities (v...
Lightweight 2D point.
double phi
Orientation (rads)
double omega
Angular velocity (rad/s)



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018