00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://mrpt.sourceforge.net/ | 00005 | | 00006 | Copyright (C) 2005-2009 University of Malaga | 00007 | | 00008 | This software was written by the Perception and Robotics | 00009 | research group, 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, RNAVDLLIMPEXP) 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 RNAVDLLIMPEXP 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_float &obstacles, 00067 float maxRobotSpeed, 00068 float &desiredDirection, 00069 float &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 float entranceDistance; 00079 float 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(float a, int N); 00105 00106 /** Configuration: 00107 */ 00108 float TOO_CLOSE_OBSTACLE,WIDE_GAP_SIZE_PERCENT,RISK_EVALUATION_SECTORS_PERCENT; 00109 float RISK_EVALUATION_DISTANCE,MAX_SECTOR_DIST_FOR_D2_PERCENT; 00110 float TARGET_SLOW_APPROACHING_DISTANCE; 00111 00112 vector_float factorWeights; 00113 00114 /** Find gaps in the obtacles. 00115 */ 00116 void gapsEstimator( 00117 vector_float &obstacles, 00118 poses::CPoint2D &in_target, 00119 TGapArray &gaps ); 00120 00121 /** Search the best gap. 00122 */ 00123 void searchBestGap( 00124 vector_float &in_obstacles, 00125 float in_maxObsRange, 00126 TGapArray &in_gaps, 00127 poses::CPoint2D &in_target, 00128 int &out_selDirection, 00129 float &out_selEvaluation, 00130 TSituations &out_situation, 00131 float &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 poses::CPoint2D &target, 00139 vector_float &obstacles); 00140 00141 /** Evaluate each gap: 00142 */ 00143 void evaluateGaps( 00144 vector_float &in_obstacles, 00145 float in_maxObsRange, 00146 TGapArray &in_gaps, 00147 int TargetSector, 00148 float TargetDist, 00149 vector_float &out_gaps_evaluation ); 00150 00151 00152 00153 }; 00154 00155 /** A class for storing extra information about the execution of 00156 * CHolonomicND navigation. 00157 * \sa CHolonomicND, CHolonomicLogFileRecord 00158 */ 00159 class CLogFileRecord_ND : public CHolonomicLogFileRecord 00160 { 00161 DEFINE_SERIALIZABLE( CLogFileRecord_ND ) 00162 00163 public: 00164 /** Member data. 00165 */ 00166 vector_int gaps_ini,gaps_end; 00167 vector_float gaps_eval; 00168 int32_t selectedSector; 00169 float evaluation; 00170 float riskEvaluation; 00171 CHolonomicND::TSituations situation; 00172 }; 00173 00174 } 00175 } 00176 00177 00178 #endif 00179 00180 00181
Page generated by Doxygen 1.5.9 for MRPT 0.7.1 SVN: at Mon Aug 17 22:21:34 EDT 2009 |