Orca Robotics

00001 /*
00002  * Orca-Robotics Project: Components for robotics 
00003  *               http://orca-robotics.sf.net/
00004  * Copyright (c) 2004-2006 Alex Brooks, Alexei Makarenko, Tobias Kaupp
00005  *
00006  * This copy of Orca is licensed to you under the terms described in
00007  * the LICENSE file included in this distribution.
00008  *
00009  */
00010 #ifndef VFHDRIVER_H
00011 #define VFHDRIVER_H
00012 
00013 #include <orcalocalnav/idriver.h>
00014 #include <orcaice/context.h>
00015 #include <gbxsickacfr/gbxiceutilacfr/timer.h>
00016 #include <orcalocalnav/goal.h>
00017 #include <orcavfh/vfh_algorithm.h>
00018 #include <orcaice/heartbeater.h>
00019 
00020 namespace vfh {
00021 
00022 //
00023 // Implements VFH.
00024 // All the number-crunching is done by the VFH_Algorithm class.
00025 // This thing just feeds in configuration and information, plus
00026 // tries heuristic escape approaches if the robot gets stuck.
00027 //
00028 // @author Alex Brooks
00029 //
00030 class VfhDriver : public orcalocalnav::IDriver
00031 {
00032 
00033 public: 
00034 
00036 
00037     // Constants 
00038     enum DriverState 
00039     {
00040         STATE_GOAL_REACHED,
00041         STATE_TURNING_AT_GOAL,
00042         STATE_ESCAPING,
00043         STATE_MOVING_TO_GOAL
00044     };
00045 
00047 
00048     VfhDriver( const orcaice::Context                &context,
00049                const orca::VehicleDescription        &descr,
00050                const orca::RangeScanner2dDescription &scannerDescr );
00051     virtual ~VfhDriver();
00052 
00053     // Goal location is in robot's coordinate frame
00054     virtual hydronavutil::Velocity getCommand( const IDriver::Inputs &inputs );
00055 
00056 private: 
00057 
00058     void setSpeedConstraints( float maxSpeed, float maxTurnrate );
00059 
00060     // Functions for setting commands
00061     hydronavutil::Velocity escapeCommand(       const std::vector<float> &obsRanges );
00062     hydronavutil::Velocity turnToGoalCommand(   const orcalocalnav::Goal &goal );
00063     hydronavutil::Velocity approachGoalCommand( const orcalocalnav::Goal     &goal, 
00064                                                 const hydronavutil::Velocity &currentVelocity,
00065                                                 const std::vector<float>     &obsRanges );
00066     
00067     // If we stall a lot, our sensors must have missed something.  
00068     // We need to try something else.
00069     bool shouldEscape( bool stalled );
00070 
00071     // Copy to Player units
00072     void copyLaserScan( const std::vector<float> &obsRanges, double playerLaserScan[361][2] );
00073 
00074     void maybeSendHeartbeat();
00075 
00076     // Class to handle the internal VFH algorithm
00077     // (like maintaining histograms etc).
00078     // This is the guy that does all the number-crunching.
00079     VFH_Algorithm *vfhAlgorithm_;
00080 
00081     // Range and bearing values in player units, to give to vfh engine
00082     // The '361' is what vfh_algorithm expects
00083     double playerLaserScan_[361][2];
00084 
00085     // speed constraints
00086     float maxSpeed_;
00087     float maxTurnrate_;
00088 
00089     // An indicator of how much we've been stalling lately
00090     float stallRatio_;
00091 
00092     // Timing for escapes
00093     gbxsickacfr::gbxiceutilacfr::Timer             escapeTimer_;
00094     static const double        escapeTimeMs_ = 1000.0;
00095 
00096     // Current state of the algorithm
00097     DriverState currentState_;
00098 
00099     // Previous command
00100     hydronavutil::Velocity prevCmd_;
00101 
00102     // Configuration from file
00103     VfhAlgorithmConfig vfhConfig_;
00104 
00105     orcaice::Heartbeater heartbeater_;
00106 
00107     const orca::RangeScanner2dDescription scannerDescr_;
00108 
00109     orcaice::Context context_;
00110 };
00111 std::ostream &operator<<( std::ostream &s, VfhDriver::DriverState state );
00112 
00113 // Used for dynamically loading driver
00114 class VfhDriverFactory : public orcalocalnav::DriverFactory
00115 {
00116 public:
00117     orcalocalnav::IDriver *createDriver( const orcaice::Context &context,
00118                                          const orca::VehicleDescription &vehicleDescr,
00119                                          const orca::RangeScanner2dDescription &scannerDescr ) const
00120         {
00121             return new VfhDriver( context, vehicleDescr, scannerDescr );
00122         }
00123 };
00124 
00125 } // namespace
00126 
00127 // Used for dynamically loading driver
00128 extern "C" {
00129     orcalocalnav::DriverFactory *createDriverFactory();
00130 }
00131 
00132 #endif
vfhdriver.h
