00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://www.mrpt.org/ | 00005 | | 00006 | Copyright (C) 2005-2011 University of Malaga | 00007 | | 00008 | This software was written by the Machine Perception and Intelligent | 00009 | Robotics Lab, University of Malaga (Spain). | 00010 | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> | 00011 | | 00012 | This file is part of the MRPT project. | 00013 | | 00014 | MRPT is free software: you can redistribute it and/or modify | 00015 | it under the terms of the GNU General Public License as published by | 00016 | the Free Software Foundation, either version 3 of the License, or | 00017 | (at your option) any later version. | 00018 | | 00019 | MRPT is distributed in the hope that it will be useful, | 00020 | but WITHOUT ANY WARRANTY; without even the implied warranty of | 00021 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | 00022 | GNU General Public License for more details. | 00023 | | 00024 | You should have received a copy of the GNU General Public License | 00025 | along with MRPT. If not, see <http://www.gnu.org/licenses/>. | 00026 | | 00027 +---------------------------------------------------------------------------+ */ 00028 #ifndef CRobot2DPoseEstimator_H 00029 #define CRobot2DPoseEstimator_H 00030 00031 #include <mrpt/synch/CCriticalSection.h> 00032 #include <mrpt/math/lightweight_geom_data.h> 00033 00034 namespace mrpt 00035 { 00036 namespace poses 00037 { 00038 using namespace mrpt::math; 00039 using namespace mrpt::system; 00040 00041 /** A simple filter to estimate and extrapolate the robot 2D (x,y,phi) pose from asynchronous odometry and localization data. 00042 * The implemented model is a state vector: 00043 * - (x,y,phi,v,w) 00044 * for the robot pose (x,y,phi) and velocities (v,w). 00045 * 00046 * The filter can be asked for an extrapolation for some arbitrary time "t'", and it'll do a simple linear prediction. 00047 * All methods are thread-safe. 00048 * \ingroup poses_grp poses_pdf_grp 00049 */ 00050 class BASE_IMPEXP CRobot2DPoseEstimator 00051 { 00052 public: 00053 CRobot2DPoseEstimator( ); //!< Default constructor 00054 virtual ~CRobot2DPoseEstimator(); //!< Destructor 00055 void reset(); 00056 00057 /** Updates the filter so the pose is tracked to the current time */ 00058 void processUpdateNewPoseLocalization( 00059 const TPose2D &newPose, 00060 const CMatrixDouble33 &newPoseCov, 00061 TTimeStamp cur_tim); 00062 00063 /** Updates the filter so the pose is tracked to the current time */ 00064 void processUpdateNewOdometry( 00065 const TPose2D &newGlobalOdometry, 00066 TTimeStamp cur_tim, 00067 bool hasVelocities = false, 00068 float v = 0, 00069 float w = 0); 00070 00071 /** Get the current estimate, obtained as: 00072 * 00073 * last_loc (+) [ last_odo (-) odo_ref ] (+) extrapolation_from_vw 00074 * 00075 * \return true is the estimate can be trusted. False if the real observed data is too old or there is no valid data yet. 00076 * \sa getLatestRobotPose 00077 */ 00078 bool getCurrentEstimate( TPose2D &pose, float &v, float &w, TTimeStamp tim_query = mrpt::system::now() ) const; 00079 00080 /** Get the current estimate, obtained as: 00081 * 00082 * last_loc (+) [ last_odo (-) odo_ref ] (+) extrapolation_from_vw 00083 * 00084 * \return true is the estimate can be trusted. False if the real observed data is too old or there is no valid data yet. 00085 * \sa getLatestRobotPose 00086 */ 00087 bool getCurrentEstimate( CPose2D &pose, float &v, float &w, TTimeStamp tim_query = mrpt::system::now() ) const 00088 { 00089 TPose2D p; 00090 bool ret = getCurrentEstimate(p,v,w,tim_query); 00091 if (ret) 00092 pose = CPose2D(p); 00093 return ret; 00094 } 00095 00096 /** Get the latest known robot pose, either from odometry or localization. 00097 * This differs from getCurrentEstimate() in that this method does NOT extrapolate as getCurrentEstimate() does. 00098 * \return false if there is not estimation yet. 00099 * \sa getCurrentEstimate 00100 */ 00101 bool getLatestRobotPose(TPose2D &pose) const; 00102 00103 /** Get the latest known robot pose, either from odometry or localization. 00104 * \return false if there is not estimation yet. 00105 */ 00106 inline bool getLatestRobotPose(CPose2D &pose) const 00107 { 00108 TPose2D p; 00109 bool v = getLatestRobotPose(p); 00110 if (v) 00111 { 00112 pose.x(p.x); 00113 pose.y(p.y); 00114 pose.phi(p.phi); 00115 } 00116 return v; 00117 } 00118 00119 00120 struct TOptions 00121 { 00122 TOptions() : 00123 max_odometry_age ( 1.0 ), 00124 max_localiz_age ( 4.0 ) 00125 {} 00126 00127 double max_odometry_age; //!< To consider data old, in seconds 00128 double max_localiz_age; //!< To consider data old, in seconds 00129 }; 00130 00131 TOptions params; //!< parameters of the filter. 00132 00133 private: 00134 mrpt::synch::CCriticalSection m_cs; 00135 00136 TTimeStamp m_last_loc_time; 00137 TPose2D m_last_loc; //!< Last pose as estimated by the localization/SLAM subsystem. 00138 CMatrixDouble33 m_last_loc_cov; 00139 00140 TPose2D m_loc_odo_ref; //!< The interpolated odometry position for the last "m_robot_pose" (used as "coordinates base" for subsequent odo readings) 00141 00142 TTimeStamp m_last_odo_time; 00143 TPose2D m_last_odo; 00144 float m_robot_v; 00145 float m_robot_w; 00146 00147 /** An auxiliary method to extrapolate the pose of a robot located at "p" with velocities (v,w) after a time delay "delta_time". 00148 */ 00149 static void extrapolateRobotPose( 00150 const TPose2D &p, 00151 const float v, 00152 const float w, 00153 const double delta_time, 00154 TPose2D &new_p); 00155 00156 }; // end of class 00157 00158 } // End of namespace 00159 } // End of namespace 00160 00161 #endif
| Page generated by Doxygen 1.7.5 for MRPT 0.9.5 SVN: at Thu Oct 13 21:25:36 UTC 2011 |