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: at Sat Mar 26 06:16:28 UTC 2011 |