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
00055 class RNAVDLLIMPEXP CAbstractReactiveNavigationSystem
00056 {
00057 public:
00058 struct TNavigationParams;
00059 struct TRobotMotionControl;
00060 struct TSensors;
00061 struct TDebug;
00062 struct TEventsLaunching;
00063
00066 CAbstractReactiveNavigationSystem(
00067 TRobotMotionControl &rmc,
00068 TSensors &sensors,
00069 void (*emul_printf)(const char *s),
00070 TEventsLaunching &evnts);
00071
00074 virtual ~CAbstractReactiveNavigationSystem()
00075 {
00076 }
00077
00080 void cancel();
00081
00085 void resume();
00086
00090 virtual float evaluate( TNavigationParams *params )=0;
00091
00094 void navigationStep();
00095
00098 virtual void navigate( TNavigationParams *params )=0;
00099
00102 virtual void setParams( TNavigationParams *params)=0;
00103
00107 virtual void suspend();
00108
00111 struct TNavigationParams
00112 {
00115 CPoint2D target;
00116
00119 float targetAllowedDistance;
00120
00123 bool targetIsRelative;
00124 };
00125
00128 struct TRobotMotionControl
00129 {
00130 TRobotMotionControl()
00131 {
00132 changeSpeeds = NULL;
00133 stop = NULL;
00134 startWatchdog = NULL;
00135 stopWatchdog = NULL;
00136 getCurrentPoseAndSpeeds = NULL;
00137 }
00138
00144 bool (*getCurrentPoseAndSpeeds)( mrpt::poses::CPose2D &curPose, float &curV, float &curW);
00145
00150 bool (*changeSpeeds)( float v, float w );
00153 bool (*stop)();
00157 bool (*startWatchdog)(float T_ms);
00160 bool (*stopWatchdog)();
00161 };
00162
00165 struct TSensors
00166 {
00167 TSensors()
00168 {
00169 sense = NULL;
00170 }
00171
00175 bool (*sense)(
00176 mrpt::slam::CSimplePointsMap &obstacles,
00177 mrpt::slam::COccupancyGridMap2D *obstaclesGridMap );
00178 };
00179
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 os::fclose(debug_output);
00192 debug_output = NULL;
00193 }
00194
00195
00196 void printf(const char *s)
00197 {
00198 if (emul_printf)
00199 emul_printf(s);
00200 if( debug_output )
00201 os::fprintf(debug_output,"%s",s);
00202 }
00203
00206 void (*emul_printf)(const char *s);
00207
00208 private:
00209 FILE * debug_output;
00210 };
00211
00212
00215 struct TEventsLaunching
00216 {
00217 TEventsLaunching() : sendNavigationStartEvent(NULL),
00218 sendNavigationEndEvent(NULL),
00219 sendNavigationEndDueToErrorEvent(NULL),
00220 sendWaySeemsBlockedEvent(NULL)
00221 {
00222 }
00223
00224 void (*sendNavigationStartEvent) ();
00225 void (*sendNavigationEndEvent) ();
00226 void (*sendNavigationEndDueToErrorEvent) ();
00227 void (*sendWaySeemsBlockedEvent) ();
00228 };
00229
00230
00233 enum TState
00234 {
00235 IDLE=0,
00236 NAVIGATING,
00237 SUSPENDED,
00238 NAV_ERROR
00239 };
00240
00243 TState getCurrentState() { return navigationState; }
00244
00245 private:
00248 TState lastNavigationState;
00249
00250 protected:
00253 virtual void performNavigationStep( )=0;
00254
00257 TState navigationState;
00258
00261 TNavigationParams navigationParams;
00262
00265 TRobotMotionControl RobotMotionControl;
00266 TSensors Sensors;
00267 TDebug Debug;
00268 TEventsLaunching EventsLaunching;
00269
00270 void TextoDebug( const char *formatStr, ...)
00271 {
00272 char auxStr[2001];
00273 va_list ap;
00274 va_start(ap, formatStr);
00275 os::vsprintf(auxStr,2000,formatStr,ap);
00276 va_end (ap);
00277 Debug.printf( auxStr );
00278 }
00279
00280
00281 };
00282 }
00283 }
00284
00285
00286 #endif
00287