00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #ifndef CAbstractReactiveNavigationSystem_H
00029 #define CAbstractReactiveNavigationSystem_H
00030
00031 #include <mrpt/slam.h>
00032 #include <mrpt/poses.h>
00033
00034 #include <mrpt/reactivenav/link_pragmas.h>
00035
00036
00037 #include <cstdarg>
00038
00039 namespace mrpt
00040 {
00041 namespace reactivenav
00042 {
00043 using namespace mrpt;
00044 using namespace mrpt::slam;
00045 using namespace mrpt::poses;
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055 class RNAVDLLIMPEXP CAbstractReactiveNavigationSystem
00056 {
00057 public:
00058 struct TNavigationParams;
00059 struct TRobotMotionControl;
00060 struct TSensors;
00061 struct TDebug;
00062 struct TEventsLaunching;
00063
00064
00065
00066 CAbstractReactiveNavigationSystem(
00067 TRobotMotionControl &rmc,
00068 TSensors &sensors,
00069 void (*emul_printf)(const char *s),
00070 TEventsLaunching &evnts);
00071
00072
00073
00074 virtual ~CAbstractReactiveNavigationSystem()
00075 {
00076 }
00077
00078
00079
00080 void cancel();
00081
00082
00083
00084
00085 void resume();
00086
00087
00088
00089
00090 virtual float evaluate( TNavigationParams *params )=0;
00091
00092
00093
00094 void navigationStep();
00095
00096
00097
00098 virtual void navigate( TNavigationParams *params )=0;
00099
00100
00101
00102 virtual void setParams( TNavigationParams *params)=0;
00103
00104
00105
00106
00107 virtual void suspend();
00108
00109
00110
00111 struct TNavigationParams
00112 {
00113
00114
00115 mrpt::poses::TPoint2D target;
00116
00117
00118
00119 float targetAllowedDistance;
00120
00121
00122
00123 bool targetIsRelative;
00124 };
00125
00126
00127
00128 struct TRobotMotionControl
00129 {
00130 TRobotMotionControl()
00131 {
00132 changeSpeeds = NULL;
00133 stop = NULL;
00134 startWatchdog = NULL;
00135 stopWatchdog = NULL;
00136 getCurrentPoseAndSpeeds = NULL;
00137 }
00138
00139
00140
00141
00142
00143
00144 bool (*getCurrentPoseAndSpeeds)( mrpt::poses::CPose2D &curPose, float &curV, float &curW);
00145
00146
00147
00148
00149
00150 bool (*changeSpeeds)( float v, float w );
00151
00152
00153 bool (*stop)();
00154
00155
00156
00157 bool (*startWatchdog)(float T_ms);
00158
00159
00160 bool (*stopWatchdog)();
00161 };
00162
00163
00164
00165 struct TSensors
00166 {
00167 TSensors()
00168 {
00169 sense = NULL;
00170 }
00171
00172
00173
00174
00175 bool (*sense)(
00176 mrpt::slam::CSimplePointsMap &obstacles,
00177 mrpt::slam::COccupancyGridMap2D *obstaclesGridMap );
00178 };
00179
00180
00181
00182 struct TDebug
00183 {
00184 TDebug()
00185 {
00186 emul_printf=NULL;
00187 debug_output = os::fopen("reactive_nav_debug_output.txt","wt");
00188 };
00189 ~TDebug()
00190 {
00191 if( debug_output )
00192 {
00193 os::fclose(debug_output);
00194 debug_output = NULL;
00195 }
00196 }
00197
00198
00199 void printf(const char *s)
00200 {
00201 if (emul_printf)
00202 emul_printf(s);
00203 if( debug_output )
00204 os::fprintf(debug_output,"%s",s);
00205 }
00206
00207
00208
00209 void (*emul_printf)(const char *s);
00210
00211 private:
00212 FILE * debug_output;
00213 };
00214
00215
00216
00217
00218 struct TEventsLaunching
00219 {
00220 TEventsLaunching() : sendNavigationStartEvent(NULL),
00221 sendNavigationEndEvent(NULL),
00222 sendNavigationEndDueToErrorEvent(NULL),
00223 sendWaySeemsBlockedEvent(NULL)
00224 {
00225 }
00226
00227 void (*sendNavigationStartEvent) ();
00228 void (*sendNavigationEndEvent) ();
00229 void (*sendNavigationEndDueToErrorEvent) ();
00230 void (*sendWaySeemsBlockedEvent) ();
00231 };
00232
00233
00234
00235
00236 enum TState
00237 {
00238 IDLE=0,
00239 NAVIGATING,
00240 SUSPENDED,
00241 NAV_ERROR
00242 };
00243
00244
00245
00246 TState getCurrentState() { return navigationState; }
00247
00248 private:
00249
00250
00251 TState lastNavigationState;
00252
00253 protected:
00254
00255
00256 virtual void performNavigationStep( )=0;
00257
00258
00259
00260 TState navigationState;
00261
00262
00263
00264 TNavigationParams navigationParams;
00265
00266
00267
00268 TRobotMotionControl RobotMotionControl;
00269 TSensors Sensors;
00270 TDebug Debug;
00271 TEventsLaunching EventsLaunching;
00272
00273 void TextoDebug( const char *formatStr, ...)
00274 {
00275 char auxStr[2001];
00276 va_list ap;
00277 va_start(ap, formatStr);
00278 os::vsprintf(auxStr,2000,formatStr,ap);
00279 va_end (ap);
00280 Debug.printf( auxStr );
00281 }
00282
00283
00284 };
00285 }
00286 }
00287
00288
00289 #endif
00290