ucl_drone: path_planning.h Source File

Go to the documentation of this file.
00001 
00012 #ifndef UCL_DRONE_PATH_PLANNING_H
00013 #define UCL_DRONE_PATH_PLANNING_H
00014 
00015 // Header files
00016 #include <ros/ros.h>
00017 #include <signal.h>
00018 
00019 // #define USE_PROFILING
00020 #include <ucl_drone/profiling.h>
00021 
00022 // messages
00023 // #include <ardrone_autonomy/Navdata.h>
00024 #include <geometry_msgs/Twist.h>
00025 #include <std_msgs/Empty.h>
00026 #include <ucl_drone/Pose3D.h>
00027 #include <ucl_drone/PoseRef.h>
00028 #include <ucl_drone/StrategyMsg.h>
00029 #include <ucl_drone/ucl_drone.h>
00030 #include <ucl_drone/cellUpdate.h>
00031 class PathPlanning
00032 {
00033 private:
00034   ros::NodeHandle nh;
00035 
00036   ros::Publisher poseref_pub;
00037   ros::Subscriber pose_sub;
00038   ros::Subscriber strategy_sub;
00039 
00040   std::string poseref_channel;
00041   std::string pose_channel;
00042   std::string strategy_channel;
00043 
00044   //Just for some tests
00045   ros::Publisher mapcell_pub;
00046   std::string mapcell_channel;
00047 
00048   int i;
00049   int j;
00051   void poseCb(const ucl_drone::Pose3D::ConstPtr posePtr);
00052   void strategyCb(const ucl_drone::StrategyMsg::ConstPtr strategyPtr);
00053   // double distance(int i, int j, int k, int l);
00054 
00055 public:
00057   PathPlanning();
00059   ~PathPlanning();
00060 
00061 
00062   ucl_drone::cellUpdate cellUpdateMsg;
00063   void init();
00064   double next_x;
00065   double next_y;
00066   double next_z;
00067   double next_rotZ;
00068   bool instruction_publishing;
00069   ucl_drone::StrategyMsg lastStrategyReceived;
00070   ucl_drone::Pose3D lastPoseReceived;
00071   bool landing;
00072   bool takeoff;
00073   void publish_poseref();
00074   void yaw_desired();
00075   bool xy_desired();
00076   void reset();
00077   void SetRef(double x_ref, double y_ref, double z_ref, double rotZ_ref);
00078   void InitializeGrid();
00079   bool gridInitialized;
00080   int myGrid[SIDE * 10][SIDE * 10];
00081   int bordersList[SIDE * 100];  // What is the maximum number of frontier cells?
00082   // bool advanced_xy_desired();
00083   bool ThereIsAWallCell(int i, int j);
00084   void UpdateMap(double x, double y);
00085   void AbsOrdMinMax(double x, double y, int* absMin, int* absMax, int* ordMin, int* ordMax);
00086   void advanced_xy_desired(double x, double y, double* k, double* l);
00087   void CellToXY(int i, int j, double* xfromcell, double* yfromcell);
00088   double distance(int i, int j, int k, int l);
00089   double XMax;
00090   double YMax;
00091   int CellUp;
00092   int CellDown;
00093   int CellRight;
00094   int CellLeft;
00095   int myAbsMin;
00096   int myAbsMax;
00097   int myOrdMin;
00098   int myOrdMax;
00099   double xfromcell;
00100   double yfromcell;
00101   double xfromcell2;
00102   double yfromcell2;
00103   int closestJ;
00104   int closestI;
00105   double poseRefX;
00106   double poseRefY;
00107   double bestDist;
00108   double alt;
00109 };
00110 
00111 #endif /*UCL_DRONE_PATH_PLANNING_H */