Main MRPT website > C++ reference
MRPT logo

CPRRTNavigator.h

Go to the documentation of this file.
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 CPRRTNavigator_H
00029 #define CPRRTNavigator_H
00030 
00031 #include <mrpt/maps.h>
00032 #include <mrpt/poses.h>
00033 #include <mrpt/synch.h>
00034 #include <mrpt/system/threads.h>
00035 #include <mrpt/reactivenav/CParameterizedTrajectoryGenerator.h>
00036 
00037 #include <mrpt/reactivenav/link_pragmas.h>
00038 
00039 namespace mrpt
00040 {
00041   namespace reactivenav
00042   {
00043         using namespace mrpt;
00044         using namespace mrpt::slam;
00045         using namespace mrpt::math;
00046         using namespace mrpt::synch;
00047         using namespace mrpt::poses;
00048 
00049         /** This class is a multi-threaded mobile robot navigator, implementing an (anytime) PTG-based Rapidly-exploring Random Tree (PRRT) search algorithm.
00050          *
00051          *  <b>Usage:</b><br>
00052          *              - Create an instance of the CPRRTNavigator class (an object on the heap, i.e. no 'new', is preferred, but just for the convenience of the user).
00053          *              - Set all the parameters in CPRRTNavigator::params
00054          *              - Call CPRRTNavigator::initialize. If all the params are OK, true is returned and you can go on.
00055          *              - Start a navigation by calling CPRRTNavigator::navigate.
00056          *              - From your application, you must update all the sensory data (from the real robot or a simulator) on a timely-fashion. The navigator will stop the robot if the last sensory data is too old.
00057          *
00058          *   Note that all the public methods are thread-safe.
00059          *
00060          *  <b>About the algorithm:</b><br>
00061          *
00062          *
00063          *
00064          *
00065          *
00066          * <b>Changes history</b>
00067          *              - 05/JUL/2009: Creation (JLBC). This is an evolution from leassons learnt from the pure reactive navigator based on PTGs.
00068          */
00069         class REACTIVENAV_IMPEXP CPRRTNavigator
00070         {
00071         public:
00072                 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00073 
00074         public:
00075                 CPRRTNavigator();       //!< Constructor
00076         virtual ~CPRRTNavigator();      //!< Destructor
00077 
00078                 /** @name Navigation control methods
00079                 @{*/
00080                         /** Initialize the navigator using the parameters in "params"; read the usage instructions in CPRRTNavigator.
00081                           * This method should be called only once at the beginning of the robot operation.
00082                           * \return true on sucess, false on any error preparing the navigator (and no navigation command will be processed until that's fixed).
00083                           */
00084                         bool initialize();
00085 
00086                         /** Starts a navigation to a 2D pose (including a desired heading at the target).
00087                           * \note CPRRTNavigator::initialize must be called first.
00088                           */
00089                         void  navigate( const mrpt::math::TPose2D  &target_pose);
00090 
00091                         /** Starts a navigation to a given 2D point (any heading at the target).
00092                           * \note CPRRTNavigator::initialize must be called first.
00093                           */
00094                         void  navigate( const mrpt::math::TPoint2D  &target_point);
00095 
00096                         /** Cancel current navegacion.
00097                           * \note CPRRTNavigator::initialize must be called first.
00098                           */
00099                         void cancel();
00100 
00101                         /** Continues with suspended navigation.
00102                          * \sa suspend
00103                          * \note CPRRTNavigator::initialize must be called first.
00104                          */
00105                         void resume();
00106 
00107                         /** Suspend current navegation
00108                          * \sa resume
00109                          * \note CPRRTNavigator::initialize must be called first.
00110                          */
00111                         void  suspend();
00112 
00113                 /** @} */
00114 
00115                 /** @name Navigator data structures
00116                 @{*/
00117                         /** Each data point in m_planned_path */
00118                         struct REACTIVENAV_IMPEXP TPathData
00119                         {
00120                                 TPathData() :
00121                                         p(0,0,0),
00122                                         max_v(0.1),max_w(0.2),
00123                                         trg_v(0.1)
00124                                 {}
00125 
00126                                 TPose2D p;                      //!< Coordinates are "global"
00127                                 double max_v, max_w;    //!< Maximum velocities along this path segment.
00128                                 double trg_v;                   //!< Desired linear velocity at the target point, ie: the robot should program its velocities such as after this arc the speeds are the given ones.
00129                         };
00130 
00131                         typedef std::list<TPathData> TPlannedPath; //!< An ordered list of path poses.
00132 
00133                         class REACTIVENAV_IMPEXP TOptions : public mrpt::utils::CLoadableOptions
00134                         {
00135                         public:
00136                                 TOptions(); //!< Initial values
00137 
00138                                 /** This method load the options from a ".ini"-like file or memory-stored string list.
00139                                  */
00140                                 void  loadFromConfigFile(
00141                                         const mrpt::utils::CConfigFileBase      &source,
00142                                         const std::string               &section);
00143 
00144                                 /** This method displays clearly all the contents of the structure in textual form, sending it to a CStream. */
00145                                 void  dumpToTextStream( CStream &out) const;
00146 
00147                                 // Data:
00148                                 double  absolute_max_v; //!< Max linear speed (m/s)
00149                                 double  absolute_max_w; //!< Max angular speed (rad/s)
00150                                 double  max_accel_v;    //!< Max desired linear speed acceleration (m/s^2)
00151                                 double  max_accel_w;    //!< Max desired angular speed acceleration (rad/s^2)
00152 
00153                                 double  max_age_observations; //!< Max age (in seconds) for an observation to be considered invalid for navigation purposes.
00154 
00155                                 /** The robot shape used when computing collisions; it's loaded from the
00156                                   *  config file/text as a single 2xN matrix in MATLAB format, first row are Xs, second are Ys, e.g:
00157                                   *  \code
00158                                   *   robot_shape = [-0.2 0.2 0.2 -0.2; -0.1 -0.1 0.1 0.1]
00159                                   *  \endcode
00160                                   */
00161                                 TPolygon2D robot_shape;
00162 
00163                                 struct REACTIVENAV_IMPEXP TPathTrackingOpts
00164                                 {
00165                                         double radius_checkpoints;  //!< Radius of each checkpoint in the path, ie: when the robot get closer than this distance, the point is considered as visited and the next one is processed.
00166                                 };
00167                                 TPathTrackingOpts pathtrack;
00168 
00169                                 struct REACTIVENAV_IMPEXP TPlannerOpts
00170                                 {
00171                                         double max_time_expend_planning; //!< Maximum time to spend when planning, in seconds.
00172                                 };
00173                                 TPlannerOpts planner;
00174 
00175                         };
00176 
00177                         TOptions  params; //!< Change here all the parameters of the navigator.
00178 
00179                 /** @} */
00180 
00181                 /** @name Debug and logging
00182                 @{*/
00183                         /** Manually sets the short-time path to be followed by the robot (use 'navigate' instead, this method is for debugging mainly)
00184                           */
00185                         void setPathToFollow(const TPlannedPath &path );
00186 
00187                         /** Returns the current planned path the robot is following */
00188                         void getCurrentPlannedPath(TPlannedPath &path ) const;
00189 
00190 
00191                 /** @} */
00192 
00193 
00194                 /** @name Sensory data methods: call them to update the navigator knowledge on the outside world.
00195                 @{*/
00196                         /** Updates the navigator with a low or high-rate estimate from a localization (or SLAM) algorithm running somewhere else.
00197                           * \param new_robot_pose The (global) coordinates of the mean robot pose as estimated by some external module.
00198                           * \param new_robot_cov The 3x3 covariance matrix of that estimate.
00199                           * \param timestamp The associated timestamp of the data.
00200                           */
00201                         void processNewLocalization(const TPose2D &new_robot_pose, const CMatrixDouble33 &new_robot_cov, TTimeStamp timestamp );
00202 
00203                         /** Updates the navigator with high-rate odometry from the mobile base.
00204                           *  The odometry poses are dealed internally as increments only, so there is no problem is
00205                           *    arbitrary mismatches between localization (from a particle filter or SLAM) and the odometry.
00206                           * \param newOdoPose The global, cummulative odometry as computed by the robot.
00207                           * \param timestamp The associated timestamp of the measurement.
00208                           * \param hasVelocities If false, the next arguments are ignored.
00209                           * \param v Measured linear speed, in m/s.
00210                           * \param w Measured angular speed, in rad/s.
00211                           */
00212                         void processNewOdometryInfo( const TPose2D &newOdoPose, TTimeStamp timestamp, bool hasVelocities =false, float v =0, float w=0 );
00213 
00214                         /** Must be called in a timely fashion to let the navigator know about the obstacles in the environment.
00215                           * \param obstacles
00216                           * \param timestamp The associated timestamp of the sensed points.
00217                           */
00218                         void processNewObstaclesData(const mrpt::slam::CPointsMap* obstacles, TTimeStamp timestamp );
00219 
00220                 /** @} */
00221 
00222                 /** @name Virtual methods to be implemented by the user.
00223                 @{*/
00224                         /** This is the most important method the user must provide: to send an instantaneous velocity command to the robot.
00225                           * \param v Desired linear speed, in meters/second.
00226                           * \param w Desired angular speed, in rads/second.
00227                           * \return false on any error. In that case, the navigator will immediately stop the navigation and announce the error.
00228                           */
00229                         virtual bool onMotionCommand(float v, float w ) = 0;
00230 
00231                         /** Re-implement this method if you want to know when a new navigation has started.
00232                           */
00233                         virtual void onNavigationStart( ) { }
00234 
00235                         /** Re-implement this method if you want to know when and why a navigation has ended.
00236                           * \param targetReachedOK Will be false if the navigation failed.
00237                           */
00238                         virtual void onNavigationEnd( bool targetReachedOK ) { }
00239 
00240                         /** Re-implement this method if you want to know when the robot is approaching the target:
00241                           *  this event is raised before onNavigationEnd, when the robot is closer than a certain distance to the target.
00242                           */
00243                         virtual void onApproachingTarget( ) { }
00244 
00245                 /** @} */
00246 
00247                 static const double INVALID_HEADING; //!< Used to refer to undefined or "never mind" headings values.
00248 
00249         private:
00250                 // ----------- Internal methods & threads -----------
00251 
00252                 TThreadHandle  m_thr_planner; //!< Thread handle
00253                 TThreadHandle  m_thr_testcol; //!< Thread handle
00254                 TThreadHandle  m_thr_pathtrack; //!< Thread handle
00255 
00256                 void thread_planner();  //!< Thread function
00257                 void thread_test_collision();  //!< Thread function
00258                 void thread_path_tracking();  //!< Thread function
00259 
00260 
00261                 // ----------- Internal data   -----------
00262 
00263                 bool  m_initialized;   //!< The object is ready to navigate. Users must call "initialize" first.
00264                 bool  m_closingThreads; //!< set to true at destructor.
00265 
00266                 TPose2D                         m_target_pose; //!< Heading may be INVALID_HEADING to indicate "don't mind"
00267                 TTimeStamp                      m_target_pose_time;
00268                 CCriticalSection        m_target_pose_cs;
00269 
00270                 // Instead of a CSimplePointsMap, just store the X & Y vectors, since
00271                 //  this is a temporary variable. We'll let the planning thread to build
00272                 //  a CSimplePointsMap object with these points.
00273                 //mrpt::slam::CSimplePointsMap  m_last_obstacles;
00274                 std::vector<float>      m_last_obstacles_x,m_last_obstacles_y;
00275                 TTimeStamp                      m_last_obstacles_time;
00276                 CCriticalSection        m_last_obstacles_cs;
00277 
00278                 // The planned path, to be followed:
00279                 CCriticalSection        m_planned_path_cs;
00280                 TTimeStamp                      m_planned_path_time;  //!< The last modification time. INVALID_TIMESTAMP means there is no path.
00281                 TPlannedPath            m_planned_path;
00282 
00283                 /** The list of PTGs used by the anytime planner to explore the free-space.  */
00284                 TListPTGs                       m_PTGs;
00285                 CCriticalSection        m_PTGs_cs;
00286 
00287         public:
00288 
00289                 mrpt::poses::CRobot2DPoseEstimator m_robotStateFilter; //!< Object maintained by the robot-tracking thread (All methods are thread-safe).
00290 
00291 
00292 
00293         };
00294   }
00295 }
00296 
00297 
00298 #endif
00299 



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:40:17 UTC 2011