Main MRPT website > C++ reference
MRPT logo

CHolonomicND.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 CHolonomicND_H
00029 #define CHolonomicND_H
00030 
00031 #include "CAbstractHolonomicReactiveMethod.h"
00032 
00033 namespace mrpt
00034 {
00035   namespace reactivenav
00036   {
00037         DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE(CLogFileRecord_ND, CHolonomicLogFileRecord, REACTIVENAV_IMPEXP)
00038 
00039         /** An implementation of the holonomic reactive navigation method "Nearness-Diagram".
00040          *   The algorithm "Nearness-Diagram" was proposed in:
00041          *
00042          *  Nearness diagram (ND) navigation: collision avoidance in troublesome scenarios, IEEE Transactions on
00043          *   Robotics and Automation, Minguez, J. and Montano, L., vol. 20, no. 1, pp. 45-59, 2004.
00044          *
00045          *  \sa CAbstractHolonomicReactiveMethod,CReactiveNavigationSystem
00046          */
00047         class REACTIVENAV_IMPEXP CHolonomicND : public CAbstractHolonomicReactiveMethod
00048         {
00049          public:
00050                  /**  Initialize the parameters of the navigator, from some configuration file, or default values if set to NULL.
00051                    */
00052                  CHolonomicND( const mrpt::utils::CConfigFileBase *INI_FILE = NULL );
00053 
00054                  /** This method performs the holonomic navigation itself.
00055                    *  \param target [IN] The relative location (x,y) of target point.
00056                    *  \param obstacles [IN] Distance to obstacles from robot location (0,0). First index refers to -PI direction, and last one to +PI direction. Distances can be dealed as "meters", although they are "pseudometers", see note below.
00057                    *  \param maxRobotSpeed [IN] Maximum robot speed, in "pseudometers/sec". See note below.
00058                    *  \param desiredDirection [OUT] The desired motion direction, in the range [-PI,PI]
00059                    *  \param desiredSpeed [OUT] The desired motion speed in that direction, in "pseudometers"/sec. (See note below)
00060                    *  \param logRecord [IN/OUT] A placeholder for a pointer to a log record with extra info about the execution. Set to NULL if not required. User <b>must free memory</b> using "delete logRecord" after using it.
00061                    *
00062                    *  NOTE: With "pseudometers" we refer to the distance unit in TP-Space, thus:
00063                    *     <br><center><code>pseudometer<sup>2</sup>= meter<sup>2</sup> + (rad ยท r)<sup>2</sup></code><br></center>
00064                    */
00065                  void  navigate(        poses::CPoint2D &target,
00066                                                         vector_double   &obstacles,
00067                                                         double                  maxRobotSpeed,
00068                                                         double                  &desiredDirection,
00069                                                         double                  &desiredSpeed,
00070                                                         CHolonomicLogFileRecordPtr &logRecord );
00071 
00072                  /** The structure used to store a detected gap in obstacles.
00073                    */
00074         struct TGap
00075                 {
00076                 int             ini;
00077                 int             end;
00078                 double  entranceDistance;
00079                 double  maxDistance;
00080                 int             representative_sector;
00081         };
00082 
00083                 typedef std::vector<TGap> TGapArray;
00084 
00085                 /** The set of posible situations for each trajectory.
00086                   */
00087         enum TSituations
00088                 {
00089                 SITUATION_TARGET_DIRECTLY = 1,
00090                 SITUATION_SMALL_GAP,
00091                 SITUATION_WIDE_GAP,
00092                 SITUATION_NO_WAY_FOUND
00093                 };
00094 
00095                  /**  Initialize the parameters of the navigator.
00096                    */
00097                  void  initialize( const mrpt::utils::CConfigFileBase &INI_FILE );
00098 
00099 
00100 
00101          private:
00102                  int    last_selected_sector;
00103 
00104                  int  direction2sector(double a, int N);
00105 
00106                 /** Configuration:
00107                   */
00108                 double TOO_CLOSE_OBSTACLE,WIDE_GAP_SIZE_PERCENT,RISK_EVALUATION_SECTORS_PERCENT;
00109                 double RISK_EVALUATION_DISTANCE,MAX_SECTOR_DIST_FOR_D2_PERCENT;
00110                 double TARGET_SLOW_APPROACHING_DISTANCE;
00111 
00112                 vector_double factorWeights;
00113 
00114                 /**  Find gaps in the obtacles.
00115                   */
00116         void  gapsEstimator(
00117                                         vector_double           &obstacles,
00118                                         poses::CPoint2D         &in_target,
00119                                         TGapArray                       &gaps );
00120 
00121                 /** Search the best gap.
00122                   */
00123         void  searchBestGap(
00124                                         vector_double           &in_obstacles,
00125                                         double                          in_maxObsRange,
00126                                         TGapArray                       &in_gaps,
00127                                         poses::CPoint2D         &in_target,
00128                                         int                                     &out_selDirection,
00129                                         double                          &out_selEvaluation,
00130                                         TSituations                     &out_situation,
00131                                         double                          &out_riskEvaluation,
00132                                         CLogFileRecord_NDPtr    log);
00133 
00134                 /** Fills in the representative sector field in the gap structure:
00135                   */
00136         void  calcRepresentativeSectorForGap(
00137                                         TGap                                    &gap,
00138                                         const poses::CPoint2D   &target,
00139                                         const vector_double             &obstacles);
00140 
00141                 /** Evaluate each gap:
00142                   */
00143                 void  evaluateGaps(
00144                     const vector_double &in_obstacles,
00145                                         const double                    in_maxObsRange,
00146                                         const TGapArray         &in_gaps,
00147                     const int                   TargetSector,
00148                     const double                        TargetDist,
00149                     vector_double               &out_gaps_evaluation );
00150         };
00151 
00152         /** A class for storing extra information about the execution of
00153          *    CHolonomicND navigation.
00154          * \sa CHolonomicND, CHolonomicLogFileRecord
00155          */
00156         class CLogFileRecord_ND : public CHolonomicLogFileRecord
00157         {
00158                 DEFINE_SERIALIZABLE( CLogFileRecord_ND )
00159 
00160          public:
00161                  /** Member data.
00162                    */
00163                 vector_int                              gaps_ini,gaps_end;
00164                                 vector_double                   gaps_eval;
00165                 int32_t                 selectedSector;
00166                 double                   evaluation;
00167                                 double                                  riskEvaluation;
00168                 CHolonomicND::TSituations      situation;
00169         };
00170 
00171   }
00172 }
00173 
00174 
00175 #endif
00176 
00177 
00178 



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN:exported at Tue Jan 25 21:56:31 UTC 2011