#include <path_planning.h>
Public Member Functions | |
void | AbsOrdMinMax (double x, double y, int *absMin, int *absMax, int *ordMin, int *ordMax) |
void | advanced_xy_desired (double x, double y, double *k, double *l) |
void | CellToXY (int i, int j, double *xfromcell, double *yfromcell) |
double | distance (int i, int j, int k, int l) |
void | init () |
void | InitializeGrid () |
PathPlanning () | |
Constructor. | |
void | publish_poseref () |
void | reset () |
void | SetRef (double x_ref, double y_ref, double z_ref, double rotZ_ref) |
bool | ThereIsAWallCell (int i, int j) |
void | UpdateMap (double x, double y) |
bool | xy_desired () |
void | yaw_desired () |
~PathPlanning () | |
Destructor. | |
Public Attributes | |
double | alt |
double | bestDist |
int | bordersList [SIDE *100] |
int | CellDown |
int | CellLeft |
int | CellRight |
int | CellUp |
ucl_drone::cellUpdate | cellUpdateMsg |
int | closestI |
int | closestJ |
bool | gridInitialized |
bool | instruction_publishing |
bool | landing |
ucl_drone::Pose3D | lastPoseReceived |
ucl_drone::StrategyMsg | lastStrategyReceived |
int | myAbsMax |
int | myAbsMin |
int | myGrid [SIDE *10][SIDE *10] |
int | myOrdMax |
int | myOrdMin |
double | next_rotZ |
double | next_x |
double | next_y |
double | next_z |
double | poseRefX |
double | poseRefY |
bool | takeoff |
double | xfromcell |
double | xfromcell2 |
double | XMax |
double | yfromcell |
double | yfromcell2 |
double | YMax |
Private Member Functions | |
void | poseCb (const ucl_drone::Pose3D::ConstPtr posePtr) |
Callback when pose is received. | |
void | strategyCb (const ucl_drone::StrategyMsg::ConstPtr strategyPtr) |
Private Attributes | |
int | i |
int | j |
std::string | mapcell_channel |
ros::Publisher | mapcell_pub |
ros::NodeHandle | nh |
std::string | pose_channel |
ros::Subscriber | pose_sub |
std::string | poseref_channel |
ros::Publisher | poseref_pub |
std::string | strategy_channel |
ros::Subscriber | strategy_sub |
Definition at line 31 of file path_planning.h.
Constructor.
Definition at line 18 of file path_planning.cpp.
Destructor.
Definition at line 51 of file path_planning.cpp.
void PathPlanning::AbsOrdMinMax | ( | double | x, |
double | y, | ||
int * | absMin, | ||
int * | absMax, | ||
int * | ordMin, | ||
int * | ordMax | ||
) |
Definition at line 188 of file path_planning.cpp.
void PathPlanning::advanced_xy_desired | ( | double | x, |
double | y, | ||
double * | k, | ||
double * | l | ||
) |
Definition at line 273 of file path_planning.cpp.
void PathPlanning::CellToXY | ( | int | i, |
int | j, | ||
double * | xfromcell, | ||
double * | yfromcell | ||
) |
Definition at line 198 of file path_planning.cpp.
double PathPlanning::distance | ( | int | i, |
int | j, | ||
int | k, | ||
int | l | ||
) |
Definition at line 265 of file path_planning.cpp.
void PathPlanning::init | ( | ) |
void PathPlanning::InitializeGrid | ( | ) |
Definition at line 145 of file path_planning.cpp.
void PathPlanning::poseCb | ( | const ucl_drone::Pose3D::ConstPtr | posePtr | ) | [private] |
Callback when pose is received.
Definition at line 95 of file path_planning.cpp.
void PathPlanning::publish_poseref | ( | ) |
Definition at line 77 of file path_planning.cpp.
void PathPlanning::reset | ( | ) |
Definition at line 56 of file path_planning.cpp.
void PathPlanning::SetRef | ( | double | x_ref, |
double | y_ref, | ||
double | z_ref, | ||
double | rotZ_ref | ||
) |
Definition at line 301 of file path_planning.cpp.
void PathPlanning::strategyCb | ( | const ucl_drone::StrategyMsg::ConstPtr | strategyPtr | ) | [private] |
Definition at line 103 of file path_planning.cpp.
bool PathPlanning::ThereIsAWallCell | ( | int | i, |
int | j | ||
) |
Definition at line 163 of file path_planning.cpp.
void PathPlanning::UpdateMap | ( | double | x, |
double | y | ||
) |
Definition at line 206 of file path_planning.cpp.
bool PathPlanning::xy_desired | ( | ) |
Definition at line 110 of file path_planning.cpp.
void PathPlanning::yaw_desired | ( | ) |
double PathPlanning::alt |
Definition at line 108 of file path_planning.h.
double PathPlanning::bestDist |
Definition at line 107 of file path_planning.h.
int PathPlanning::bordersList[SIDE *100] |
Definition at line 81 of file path_planning.h.
Definition at line 92 of file path_planning.h.
Definition at line 94 of file path_planning.h.
Definition at line 93 of file path_planning.h.
Definition at line 91 of file path_planning.h.
ucl_drone::cellUpdate PathPlanning::cellUpdateMsg |
Definition at line 62 of file path_planning.h.
Definition at line 104 of file path_planning.h.
Definition at line 103 of file path_planning.h.
Definition at line 79 of file path_planning.h.
int PathPlanning::i [private] |
Definition at line 48 of file path_planning.h.
Definition at line 68 of file path_planning.h.
int PathPlanning::j [private] |
Definition at line 49 of file path_planning.h.
Definition at line 71 of file path_planning.h.
ucl_drone::Pose3D PathPlanning::lastPoseReceived |
Definition at line 70 of file path_planning.h.
ucl_drone::StrategyMsg PathPlanning::lastStrategyReceived |
Definition at line 69 of file path_planning.h.
std::string PathPlanning::mapcell_channel [private] |
Definition at line 46 of file path_planning.h.
ros::Publisher PathPlanning::mapcell_pub [private] |
Definition at line 45 of file path_planning.h.
Definition at line 96 of file path_planning.h.
Definition at line 95 of file path_planning.h.
int PathPlanning::myGrid[SIDE *10][SIDE *10] |
Definition at line 80 of file path_planning.h.
Definition at line 98 of file path_planning.h.
Definition at line 97 of file path_planning.h.
double PathPlanning::next_rotZ |
Definition at line 67 of file path_planning.h.
double PathPlanning::next_x |
Definition at line 64 of file path_planning.h.
double PathPlanning::next_y |
Definition at line 65 of file path_planning.h.
double PathPlanning::next_z |
Definition at line 66 of file path_planning.h.
ros::NodeHandle PathPlanning::nh [private] |
Definition at line 34 of file path_planning.h.
std::string PathPlanning::pose_channel [private] |
Definition at line 41 of file path_planning.h.
ros::Subscriber PathPlanning::pose_sub [private] |
Definition at line 37 of file path_planning.h.
std::string PathPlanning::poseref_channel [private] |
Definition at line 40 of file path_planning.h.
ros::Publisher PathPlanning::poseref_pub [private] |
Definition at line 36 of file path_planning.h.
double PathPlanning::poseRefX |
Definition at line 105 of file path_planning.h.
double PathPlanning::poseRefY |
Definition at line 106 of file path_planning.h.
std::string PathPlanning::strategy_channel [private] |
Definition at line 42 of file path_planning.h.
ros::Subscriber PathPlanning::strategy_sub [private] |
Definition at line 38 of file path_planning.h.
Definition at line 72 of file path_planning.h.
double PathPlanning::xfromcell |
Definition at line 99 of file path_planning.h.
double PathPlanning::xfromcell2 |
Definition at line 101 of file path_planning.h.
double PathPlanning::XMax |
Definition at line 89 of file path_planning.h.
double PathPlanning::yfromcell |
Definition at line 100 of file path_planning.h.
double PathPlanning::yfromcell2 |
Definition at line 102 of file path_planning.h.
double PathPlanning::YMax |
Definition at line 90 of file path_planning.h.