nd.h
00001 00002 /*************************************************************************************/ 00003 /* */ 00004 /* File: nd.h */ 00005 /* Author: Javier Minguez */ 00006 /* Modified: 20/10/2005 */ 00007 /* */ 00008 /* This library implements a mixture between: */ 00009 /* */ 00010 /* J. Minguez, L. Montano. */ 00011 /* Nearness Diagram Navigation (ND): Collision Avoidance in Troublesome Scenarios. */ 00012 /* IEEE Transactions on Robotics and Automation, pp 154, 2004. */ 00013 /* */ 00014 /* */ 00015 /* J. Minguez, J. Osuna, L. Montano. */ 00016 /* A Divide and Conquer Strategy based on Situations */ 00017 /* to Achieve Reactive Collision Avoidance in Troublesome Scenarios. */ 00018 /* IEEE International Conference on Robotics and Automation (ICRA 2004), */ 00019 /* 2004. New Orleans, USA. */ 00020 /* */ 00021 /*************************************************************************************/ 00022 /* 00023 * This program is free software; you can redistribute it and/or modify 00024 * it under the terms of the GNU General Public License as published by 00025 * the Free Software Foundation; either version 2 of the License, or 00026 * (at your option) any later version. 00027 * 00028 * This program is distributed in the hope that it will be useful, 00029 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00030 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00031 * GNU General Public License for more details. 00032 * 00033 * You should have received a copy of the GNU General Public License 00034 * along with this program; if not, write to the Free Software 00035 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 00036 * 00037 */ 00038 00039 /*****************************************************************************/ 00040 // 00041 // EVERYTHING IN THE INTERNATIONAL SYSTEM (METERS AND RADIANS) 00042 // 00043 /*****************************************************************************/ 00044 00045 00046 #ifndef nd_h 00047 #define nd_h 00048 00049 // ---------------------------------------------------------------------------- 00050 // GENERIC TYPES 00051 // ---------------------------------------------------------------------------- 00052 00053 // Cartesian coordinates. 00054 00055 typedef struct { 00056 float x; 00057 float y; 00058 } TCoordenadas; 00059 00060 // System of reference 00061 00062 typedef struct { 00063 TCoordenadas posicion; 00064 float orientacion; 00065 } TSR; 00066 00067 00068 00069 // ---------------------------------------------------------------------------- 00070 // SPECIFIC TYPES. 00071 // ---------------------------------------------------------------------------- 00072 00073 00074 00075 // ************************ 00076 00077 // TParametrosND (information of the robot and laser for the ND) 00078 00079 typedef struct { 00080 00081 // GEOMETRY 00082 // The vehicle is considered to be symetric at both sides of the X axis. 00083 // The flag is 1 if the robot is resctangular, 0 if it is circular 00084 short int geometryRect; 00085 00086 // --- RECTANGULAR --- 00087 // distance (m) from the wheels to the: 00088 // front: frontal part 00089 // back: back part 00090 // left: left side. Notice that the vehicle is symetric 00091 float front,back,left; 00092 00093 // --- CIRCULAR --- 00094 // radius of the robot is is circular 00095 float R; 00096 00097 // MOTION 00098 // The falg is 1 if the robot is holonomous, or 0 is diff-drive or syncro 00099 short int holonomic; 00100 00101 // Maximum linear and angular velocities 00102 float vlmax,vamax; 00103 00104 // Maximum linear and angular acelerations 00105 float almax,aamax; 00106 00107 // OTHER STUFF 00108 00109 // -- SECURITY DISTANCE --- 00110 // Distance to consider an obstacle dangerous (i.e. to start the avoidance maneouvre) 00111 // dsmax: Distance from the frontal robot bounds. 00112 // dsmin: Distance from the back robot bounds. 00113 // engorde: Inner value. The suggestion is 20% of the dsmin (i.e. 0.2*dsmin) 00114 float dsmax,dsmin,enlarge; 00115 00116 // -- DISCONTINUITY -- 00117 // Minimum space where the robot fits. I suggest same value than "izquierda" value. 00118 float discontinuity; 00119 00120 // -- SAMPLING PERIOD -- 00121 float T; 00122 00123 // LASER 00124 // Distance from the wheels axis to the laser, X axis. 00125 //float laser; 00126 00127 } TParametersND; 00128 00129 // ************************************** 00130 00131 00132 00133 00134 00135 00136 // ************************ 00137 00138 // TVelocities (information of linear v, and angular velocities w) 00139 00140 typedef struct { 00141 float v; // linear velocity 00142 float w; // angular velocity 00143 float v_theta; // velocity angle (just if holonomous vehicle) 00144 } TVelocities; 00145 00146 // ************************************** 00147 00148 00149 00150 00151 00152 // ************************ 00153 00154 // TInfoMovimiento (information of the robot) 00155 00156 typedef struct { 00157 TSR SR1; // Current vehicle location in GLOBAL coordinates 00158 TVelocities velocidades; // Current vehicle velocities 00159 } TInfoMovimiento; 00160 00161 // ************************************** 00162 00163 00164 00165 00166 // ************************ 00167 00168 // TInfoEntorno (list of obstacle points) 00169 00170 // Maximum number of points of the environment 00171 // This number depends on the maximum number of obstacle points that 00172 // you want to give to the ND 00173 00174 //#define MAX_POINTS_SCENARIO 1440 00175 #define MAX_POINTS_SCENARIO 10000 00176 00177 typedef struct { 00178 int longitud; 00179 TCoordenadas punto[MAX_POINTS_SCENARIO]; 00180 } TInfoEntorno; 00181 00182 // ************************************** 00183 00184 00185 00186 00187 00188 // ---------------------------------------------------------------------------- 00189 // FUNCTIONS 00190 // ---------------------------------------------------------------------------- 00191 00192 00193 00194 00195 // ********************************** 00196 // This function initialites the ND 00197 // Input-- 00198 // parametros:: information of the robot and laser used by the ND 00199 // Ouput-- 00200 00201 void InicializarND(TParametersND *parametros); 00202 00203 // ********************************** 00204 00205 00206 00207 00208 00209 00210 // ********************************** 00211 // This runs the ND. The input is the current obstacle list and the goal location 00212 // and the output the motion command. 00213 // Input-- 00214 // objetivo:: current objective in GLOBAL coordinates. Notice that this 00215 // location can change each time you call ND. 00216 // movimiento:: this is the current velocity of the robot. 00217 // mapa:: this is a list of the obstacle points in global coordinates. 00218 // You can use the current sensor reading or implement a kind of memory 00219 // to remember last scans. Whatever, ND wants a list of points in GLOBAL coordinates. 00220 // information:: variable for debug. 00221 // 00222 // Ouput-- 00223 // movimiento:: this is the output of the ND. 00224 // * Linear and angular velocities (and direction if holonomic). 00225 // * NULL an emergency stop is required 00226 // * pointer to (0,0) goal reached. 00227 00228 extern TVelocities *IterarND(TCoordenadas objetivo, 00229 float goal_tol, 00230 TInfoMovimiento *movimiento, 00231 TInfoEntorno *mapa, 00232 void *informacion); 00233 // if you do not want to see the internal information in nh2.h informacion = NULL 00234 00235 // ********************************** 00236 00237 00238 #endif 00239