00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://mrpt.sourceforge.net/ | 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 */ 00049 class BASE_IMPEXP CRobot2DPoseEstimator 00050 { 00051 public: 00052 CRobot2DPoseEstimator( ); //!< Default constructor 00053 virtual ~CRobot2DPoseEstimator(); //!< Destructor 00054 void reset(); 00055 00056 /** Updates the filter so the pose is tracked to the current time */ 00057 void processUpdateNewPoseLocalization( 00058 const TPose2D &newPose, 00059 const CMatrixDouble33 &newPoseCov, 00060 TTimeStamp cur_tim); 00061 00062 /** Updates the filter so the pose is tracked to the current time */ 00063 void processUpdateNewOdometry( 00064 const TPose2D &newGlobalOdometry, 00065 TTimeStamp cur_tim, 00066 bool hasVelocities = false, 00067 float v = 0, 00068 float w = 0); 00069 00070 /** Get the current estimate, obtained as: 00071 * 00072 * last_loc (+) [ last_odo (-) odo_ref ] (+) extrapolation_from_vw 00073 * 00074 * \return true is the estimate can be trusted. False if the real observed data is too old or there is no valid data yet. 00075 * \sa getLatestRobotPose 00076 */ 00077 bool getCurrentEstimate( TPose2D &pose, float &v, float &w, TTimeStamp tim_query = mrpt::system::now() ) const; 00078 00079 /** Get the current estimate, obtained as: 00080 * 00081 * last_loc (+) [ last_odo (-) odo_ref ] (+) extrapolation_from_vw 00082 * 00083 * \return true is the estimate can be trusted. False if the real observed data is too old or there is no valid data yet. 00084 * \sa getLatestRobotPose 00085 */ 00086 bool getCurrentEstimate( CPose2D &pose, float &v, float &w, TTimeStamp tim_query = mrpt::system::now() ) const 00087 { 00088 TPose2D p; 00089 bool ret = getCurrentEstimate(p,v,w,tim_query); 00090 if (ret) 00091 pose = CPose2D(p); 00092 return ret; 00093 } 00094 00095 /** Get the latest known robot pose, either from odometry or localization. 00096 * This differs from getCurrentEstimate() in that this method does NOT extrapolate as getCurrentEstimate() does. 00097 * \return false if there is not estimation yet. 00098 * \sa getCurrentEstimate 00099 */ 00100 bool getLatestRobotPose(TPose2D &pose) const; 00101 00102 /** Get the latest known robot pose, either from odometry or localization. 00103 * \return false if there is not estimation yet. 00104 */ 00105 inline bool getLatestRobotPose(CPose2D &pose) const 00106 { 00107 TPose2D p; 00108 bool v = getLatestRobotPose(p); 00109 if (v) 00110 { 00111 pose.x(p.x); 00112 pose.y(p.y); 00113 pose.phi(p.phi); 00114 } 00115 return v; 00116 } 00117 00118 00119 struct TOptions 00120 { 00121 TOptions() : 00122 max_odometry_age ( 1.0 ), 00123 max_localiz_age ( 4.0 ) 00124 {} 00125 00126 double max_odometry_age; //!< To consider data old, in seconds 00127 double max_localiz_age; //!< To consider data old, in seconds 00128 }; 00129 00130 TOptions params; //!< parameters of the filter. 00131 00132 private: 00133 mrpt::synch::CCriticalSection m_cs; 00134 00135 TTimeStamp m_last_loc_time; 00136 TPose2D m_last_loc; //!< Last pose as estimated by the localization/SLAM subsystem. 00137 CMatrixDouble33 m_last_loc_cov; 00138 00139 TPose2D m_loc_odo_ref; //!< The interpolated odometry position for the last "m_robot_pose" (used as "coordinates base" for subsequent odo readings) 00140 00141 TTimeStamp m_last_odo_time; 00142 TPose2D m_last_odo; 00143 float m_robot_v; 00144 float m_robot_w; 00145 00146 /** An auxiliary method to extrapolate the pose of a robot located at "p" with velocities (v,w) after a time delay "delta_time". 00147 */ 00148 static void extrapolateRobotPose( 00149 const TPose2D &p, 00150 const float v, 00151 const float w, 00152 const double delta_time, 00153 TPose2D &new_p); 00154 00155 }; // end of class 00156 00157 } // End of namespace 00158 } // End of namespace 00159 00160 #endif
Page generated by Doxygen 1.7.2 for MRPT 0.9.4 SVN: at Mon Jan 10 22:30:30 UTC 2011 |