MRXT: The Multi-Robot eXploration Tool: include/architecture/reactive/reactive.h Source File

00001 #ifndef __REACTIVE__INTERFACE___
00002 #define __REACTIVE__INTERFACE___
00003 
00004 #include "ClThread.h"
00005 #include "robotTypes.h"
00006 #include "slamInterface.h"
00007 #include "localPotentialField.h"
00008 #include "robotBase.h"
00009 #include "ConfigFile.h"
00010 
00011 
00012 /**
00013 * @brief Reactive behaviours interface and common functions
00014 *
00015 */
00016 
00017 class reactive: public ClThread{
00018 
00019 protected:
00020 
00021         slamInterface* mySlam;
00022         robotBase* rbase;
00023         int number;
00024 
00025         const float vmax;
00026         const float wmax;
00027         const float k1;
00028         const float k2;
00029 
00030         float resolution;
00031 
00032         bool avoidObstaclesEnabled;
00033         bool goToFrontierEnabled;
00034         bool goToUnexploredZonesEnabled;
00035         bool avoidOtherRobotsEnabled;
00036         bool goToPrecisePosesEnabled;
00037         bool goToGoalEnabled;
00038 
00039         float avoidObstaclesWeight;
00040         float goToFrontierWeight;
00041         float goToUnexploredZonesWeight;
00042         float avoidOtherRobotsWeight;
00043         float goToPrecisePosesWeight;
00044         float goToImprecisePosesWeight;
00045         float goToGoalWeight;
00046 
00047         float avoidObstaclesWidth;
00048         float goToFrontierWidth;
00049         float goToUnexploredZonesWidth;
00050         float avoidOtherRobotsWidth;
00051         float goToPrecisePosesWidth;
00052         float goToGoalWidth;
00053 
00054         float avoidObstaclesWidth_cells;
00055         float goToFrontierWidth_cells;
00056         float goToUnexploredZonesWidth_cells;
00057         float avoidOtherRobotsWidth_cells;
00058         float goToPrecisePosesWidth_cells;
00059         float goToGoalWidth_cells;
00060 
00061         bool localMinimumDetected;
00062         pointf getResponse(localPotentialField& lpf);
00063 
00064 public:
00065 
00066         reactive(const ConfigFile& file);
00067         virtual ~reactive(){};
00068 
00069         speed regulator(const pose& pos, pointf& f);
00070 
00071         void enableAvoidObstacles();
00072         void enableGoToFrontier();
00073         void enableGoToUnexploredZones();
00074         void enableAvoidOtherRobots();
00075         void enableGoToPrecisePoses();
00076         void enableGoToGoal();
00077 
00078         void disableAvoidObstacles();
00079         void disableGoToFrontier();
00080         void disableGoToUnexploredZones();
00081         void disableAvoidOtherRobots();
00082         void disableGoToPrecisePoses();
00083         void disableGoToGoal();
00084 
00085         void setWeightAvoidObstacles(float weight);
00086         void setWeightGoToFrontier(float weight);
00087         void setWeightGoToUnexploredZones(float weight);
00088         void setWeightAvoidOtherRobots(float weight);
00089         void setWeightGoToPrecisePoses(float weight);
00090         void setWeightGoToGoal(float weight);
00091 
00092         void setWidthAvoidObstacles(float width);
00093         void setWidthGoToFrontier(float width);
00094         void setWidthGoToUnexploredZones(float width);
00095         void setWidthAvoidOtherRobots(float width);
00096         void setWidthGoToPrecisePoses(float width);
00097         void setWidthGoToGoal(float width);
00098 
00099         void disableAll();
00100 
00101         bool localMinimum();
00102 
00103         virtual void setGoal(point goal){};
00104 
00105         void setSlam(slamInterface& slamProc);
00106         void setRBase(robotBase& rb);
00107         void setRobot(int number);
00108 };
00109 
00110 
00111 inline void reactive::enableAvoidObstacles()                                            {avoidObstaclesEnabled=true;}
00112 inline void reactive::enableGoToFrontier()                                              {goToFrontierEnabled=true;}
00113 inline void reactive::enableGoToUnexploredZones()                                       {goToUnexploredZonesEnabled=true;}
00114 inline void reactive::enableAvoidOtherRobots()                                          {avoidOtherRobotsEnabled=true;}
00115 inline void reactive::enableGoToPrecisePoses()                                          {goToPrecisePosesEnabled=true;}
00116 inline void reactive::enableGoToGoal()                                                  {goToGoalEnabled=true;}
00117 
00118 inline void reactive::disableAvoidObstacles()                                           {avoidObstaclesEnabled=false;}
00119 inline void reactive::disableGoToFrontier()                                             {goToFrontierEnabled=false;}
00120 inline void reactive::disableGoToUnexploredZones()                                      {goToUnexploredZonesEnabled=false;}
00121 inline void reactive::disableAvoidOtherRobots()                                         {avoidOtherRobotsEnabled=false;}
00122 inline void reactive::disableGoToPrecisePoses()                                         {goToPrecisePosesEnabled=false;}
00123 inline void reactive::disableGoToGoal()                                                 {goToGoalEnabled=false;}
00124 
00125 inline void reactive::setWeightAvoidObstacles(float weight)                             {avoidObstaclesWeight = weight;};
00126 inline void reactive::setWeightGoToFrontier(float weight)                               {goToFrontierWeight = weight;};
00127 inline void reactive::setWeightGoToUnexploredZones(float weight)                        {goToUnexploredZonesWeight = weight;};
00128 inline void reactive::setWeightAvoidOtherRobots(float weight)                           {avoidOtherRobotsWeight = weight;};
00129 inline void reactive::setWeightGoToPrecisePoses(float weight)                           {goToPrecisePosesWeight = weight;};
00130 inline void reactive::setWeightGoToGoal(float weight)                                   {goToGoalWeight = weight;};
00131 
00132 inline void reactive::setWidthAvoidObstacles(float width)                               {avoidObstaclesWidth = width;};
00133 inline void reactive::setWidthGoToFrontier(float width)                                 {goToFrontierWidth = width;};
00134 inline void reactive::setWidthGoToUnexploredZones(float width)                          {goToUnexploredZonesWidth = width;};
00135 inline void reactive::setWidthAvoidOtherRobots(float width)                             {avoidOtherRobotsWidth = width;};
00136 inline void reactive::setWidthGoToPrecisePoses(float width)                             {goToPrecisePosesWidth = width;};
00137 inline void reactive::setWidthGoToGoal(float width)                                     {goToGoalWidth = width;};
00138 
00139 inline void reactive::setSlam(slamInterface& slamProc)                                  {
00140         mySlam = &slamProc;
00141         resolution = mySlam->getResolution();
00142         avoidObstaclesWidth_cells = avoidObstaclesWidth/resolution;
00143         goToFrontierWidth_cells = goToFrontierWidth/resolution;
00144         goToUnexploredZonesWidth_cells = goToUnexploredZonesWidth/resolution;
00145         avoidOtherRobotsWidth_cells = avoidOtherRobotsWidth/resolution;
00146         goToPrecisePosesWidth_cells = goToPrecisePosesWidth/resolution;
00147         goToGoalWidth_cells = goToGoalWidth/resolution;
00148 }
00149 inline void reactive::setRBase(robotBase& rb)                                           {rbase = &rb;}
00150 inline void reactive::setRobot(int n)                                                   {number = n;}
00151 inline bool reactive::localMinimum()                                                    {return localMinimumDetected;}
00152 
00153 
00154 typedef Loki::Functor<reactive*, LOKI_TYPELIST_1(const ConfigFile&)> reactiveCreator;
00155 typedef Loki::SingletonHolder< Loki::Factory< reactive, int, LOKI_TYPELIST_1(const ConfigFile&)> > reactiveFactory;
00156 
00157 #endif