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 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
00209 void (*emul_printf)(const char *s);
00210
00211 private:
00212 FILE * debug_output;
00213 };
00214
00215
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
00236 enum TState
00237 {
00238 IDLE=0,
00239 NAVIGATING,
00240 SUSPENDED,
00241 NAV_ERROR
00242 };
00243
00246 TState getCurrentState() { return navigationState; }
00247
00248 private:
00251 TState lastNavigationState;
00252
00253 protected:
00256 virtual void performNavigationStep( )=0;
00257
00260 TState navigationState;
00261
00264 TNavigationParams navigationParams;
00265
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