#include <robodim.h>
#include <math.h>
#include <stdlib.h>
+#include <stdio.h>
+#include <assert.h>
-//#include "robot.h"
-
-#ifdef AALGORITHM_DEBUG
+#ifdef AALGORITHM_DEBUG_PNG
#include "map_2_png.h"
#endif
*/
#define SQRT2 M_SQRT2 /**< @brief Root square of two.*/
#define WALL_COST 1000 /**< Cost of jumping a wall. */
-#define PI M_PI /**< @brief Pi constant.*/
-#define NBR_OF_POLYGON_LINES 4 // number of polygon lines, polygon is shape of robot, if robot is rectagle, then number is 4
+#define PI M_PI /**< @brief Pi constant.*/
+
+// number of lines in polygon, polygon is shape of robot, if robot is rectagle, then number is 4
+#define MAX_NBR_OF_POLYGON_LINES 4 /**< @brief Number of lines in polygon. Polygon is shape of robot. */
/**@}*/
/**
#endif
+// type to define cell in mask
+// typedef struct {
+// int x;
+// int y;
+// } cell_coordinates_t;
-typedef struct {
- int x;
- int y;
-} cell_coordinates_t;
-
+// type to define point in vector mask
typedef struct {
double x;
double y;
} vect_coordinates_t;
-typedef struct {
- cell_coordinates_t *p_first_cell;
- int nbr_mask_cells;
-} mask_t;
+// typedef struct {
+// cell_coordinates_t *p_first_cell;
+// int nbr_mask_cells;
+// } mask_t;
+// to define shape of the robot, in float coordinates
typedef struct {
- vect_coordinates_t points[NBR_OF_POLYGON_LINES];
+ int nbr_of_lines;
+ vect_coordinates_t points[MAX_NBR_OF_POLYGON_LINES];
+ bool is_closed_polygon;
} vect_mask_t;
-static cell_coordinates_t angle_idx_2_xy[] = {{ 1, 0}, { 1, 1}, { 0, 1}, {-1, 1}, {-1, 0},{-1,-1}, { 0,-1}, { 1,-1}};
+// look up table to transform angle index to coordinates of neighboring cell
+static cell_coordinates_t angle_idx_2_xy[] = {
+ { 1, 0},
+ { 1, -1},
+ { 0, -1},
+ {-1, -1},
+ {-1, 0},
+ {-1, 1},
+ { 0, 1},
+ { 1, 1}
+};
+
+mask_t mask_0pi = {
+ NULL,
+ 0,
+ DIRECT_MASK
+};
+
+mask_t mask_pi4 = {
+ NULL,
+ 0,
+ DIRECT_MASK
+};
+
+mask_t mask_rot = {
+ NULL,
+ 0,
+ ROTATE_MASK
+};
+
+// look up table to transform angle index to rotation matrix
+// 2. and 4. rows are exchanged, y axis in graph is negative, angle is greater to right
+const struct { int r1; int r2; int r3; int r4;} angle_idx_2_rot_mat[] = {
+ { 1, 0, 0, 1},
+ {0, 1, -1, 0},
+ { -1, 0, 0, -1},
+ { 0, -1, 1, 0}
+};
-static mask_t mask_0pi, mask_pi4, mask_rot;
-const struct { int r1; int r2; int r3; int r4;} angle_idx_2_rot_mat[] = { { 1, 0, 1, 0}, { 0, 1, -1, 0}, { -1, 0, 0, -1}, {0, -1, 1,0}};
-
-//int get_angle_between_cells(int x_0, int y_0, int x_1, int y_1);
int get_angle_index(double angle);
-
-void vect_m_2_grid_m(vect_mask_t *p_vect_mask, int vect_mask_dim, mask_t *p_grid_mask, int nbr_grid_mask_cells);
-int init_aalgorithm(void);
+void vect_m_2_grid_m(vect_mask_t *p_vect_mask, mask_t *p_grid_mask, int nbr_grid_mask_cells);
void point_2_cell(double x, double y ,cell_coordinates_t *p_new_coor);
-void arc_2_grid(double r, double x1, double y1, double x2, double y2, double x_r, double y_r, mask_t *p_grid_mask, int nbr_grid_mask_cells);
-static bool is_pos_reach(int *p_x_ref, int *p_y_ref, mask_t *p_mask, int angle_index);
+void arc_2_grid(double x1, double y1, double x2, double y2, mask_t *p_grid_mask, int nbr_grid_mask_cells);
+static bool is_pos_reach(int x_ref, int y_ref, mask_t *p_mask, int angle_index);
static bool is_angle_idx_even(int angle_index);
-static cell_coordinates_t transform( cell_coordinates_t coor, int angle_index);
-
+void print_quene(GraphMapCell *graph_queue);
+void clear_flag(struct map *map, map_cell_flag_t flag);
+//static cell_coordinates_t transform( cell_coordinates_t coor, int angle_index, TYPE_OF_MASK type_of_mask);
+void transform_vect_mask(vect_mask_t *to_transform, vect_mask_t *transformed, double phi);
static void pushNodeInOrder(GraphMapCell *cell);
float cFunction(int x1, int y1, int x2, int y2);
void initGraphStructure(void);
int getPathLength(int xstart, int ystart, int xgoal, int ygoal);
-int init_aalgorithm(void) {
-
+// transform vect mask to angle phi
+void transform_vect_mask(vect_mask_t *to_transform, vect_mask_t *transformed, double phi) {
+ int i;
+ vect_coordinates_t new_coor;
+ for(i = 0; (i < sizeof(to_transform->points) / sizeof(to_transform->points[0])); i++) {
+ new_coor.x = cos(phi) * to_transform->points[i].x - sin(phi) * to_transform->points[i].y;
+ new_coor.y = sin(phi) * to_transform->points[i].x + cos(phi) * to_transform->points[i].y;
+ transformed->points[i] = new_coor;
+
#ifdef AALGORITHM_DEBUG
- printf("PATHPLAN: init_aalgorithm : ----------------------------- start of function------------------------------ \n");
-#endif
-
- vect_mask_t vect_mask_0pi, vect_mask_pi4;
+ printf("PATHPLAN: init_aalgorithm : vect mask point %d, mask to transform, transformed mask are ([x, y]): [%2.2f, %2.2f], [%2.2f, %2.2f] \n", i, to_transform->points[i].x, to_transform->points[i].y, transformed->points[i].x, transformed->points[i].y);
+#endif
+
+ }
+}
+
+// print priority set in console
+void print_quene(GraphMapCell *graph_queue) {
- mask_rot.nbr_mask_cells = 0;
- mask_0pi.nbr_mask_cells = 0;
- mask_pi4.nbr_mask_cells = 0;
+ GraphMapCell *cell_pointer = graph_queue;
+ int x, y, x_back, y_back;
-#ifdef AALGORITHM_DEBUG
- printf("PATHPLAN: init_aalgorithm : number of mask cells, rot, 0pi, pi4: %d, %d, %d \n", mask_rot.nbr_mask_cells, mask_0pi.nbr_mask_cells, mask_pi4.nbr_mask_cells);
-#endif
+ while(cell_pointer!=NULL){
+ GraphCell2XY(cell_pointer, &x, &y);
+ if (cell_pointer->backpointer != NULL){
+ GraphCell2XY(cell_pointer->backpointer, &x_back, &y_back);
+ }
+ else {
+ x_back = -1;
+ y_back = -1;
+ }
+ printf("PATHPLAN: print_quene: cell %d %d, func. f %3f = %3f + %3f, back cell %d %d\n", x, y, cell_pointer->f, cell_pointer->g, cell_pointer->h, x_back, y_back);
+ cell_pointer = cell_pointer->next;
+ }
+ printf("No more cells in the queue!\n");
+}
+
+// clear flage in whole map
+void clear_flag(struct map *map, map_cell_flag_t flag) {
+ // Clear previous masks flags
+ int x,y;
+ for (y=0;y<MAP_HEIGHT;y++){
+ for(x=0;x<MAP_WIDTH;x++){
+ map->cells[y][x].flags &= ~(flag);
+ }
+ }
+}
+
+/**
+ * @name A* Init function
+ * @{
+ */
+
+/**
+ * @brief Init mask for aAlgorithm function
+ * @return 1 mask memory is not alocated, 0 if success.
+ *
+ * This is an implementation of extended A* Algorithm
+ * diploma thesis Collison-free Pathplannig for Mobile Robots, CTU
+ * Matous Pokorny 2012
+ */
+
+// init mask, its needed call before aAlgoritm (ifndef HOLOMIC)
+int init_aalgorithm(void) {
+ vect_mask_t vect_mask_0pi, vect_mask_pi4, vect_mask_0pi_front_line, vect_mask_pi4_front_line;
+
// init vect_mask_0pi
- vect_mask_0pi.points[0].x = - ROBOT_AXIS_TO_BACK_M;
- vect_mask_0pi.points[0].y = (double) ( - ROBOT_WIDTH_M / 2);
+ vect_mask_0pi.is_closed_polygon = true;
+ vect_mask_0pi.nbr_of_lines = 4;
- vect_mask_0pi.points[1].x = ROBOT_AXIS_TO_FRONT_M;
+ vect_mask_0pi.points[0].x = - (ROBOT_AXIS_TO_BACK_M + 0 * MAP_CELL_SIZE_M);
+ vect_mask_0pi.points[0].y = (double) ( - (ROBOT_WIDTH_M / 2 + 0 * MAP_CELL_SIZE_M));
+
+ vect_mask_0pi.points[1].x = ROBOT_AXIS_TO_FRONT_M + 0 * MAP_CELL_SIZE_M;
vect_mask_0pi.points[1].y = vect_mask_0pi.points[0].y;
vect_mask_0pi.points[2].x = vect_mask_0pi.points[1].x;
- vect_mask_0pi.points[2].y = (double)(ROBOT_WIDTH_M / 2);
+ vect_mask_0pi.points[2].y = (double)(ROBOT_WIDTH_M / 2 + 0 * MAP_CELL_SIZE_M);
vect_mask_0pi.points[3].x = vect_mask_0pi.points[0].x;
vect_mask_0pi.points[3].y = vect_mask_0pi.points[2].y;
+ // init line before mask, it represented move forward in rot mask
+ vect_mask_0pi_front_line.is_closed_polygon = false;
+ vect_mask_0pi_front_line.nbr_of_lines = 1;
+ vect_mask_0pi_front_line.points[0].x = vect_mask_0pi.points[1].x + MAP_CELL_SIZE_M;
+ vect_mask_0pi_front_line.points[0].y = vect_mask_0pi.points[1].y;// - MAP_CELL_SIZE_M;
+
+ vect_mask_0pi_front_line.points[1].x = vect_mask_0pi.points[2].x + MAP_CELL_SIZE_M;
+ vect_mask_0pi_front_line.points[1].y = vect_mask_0pi.points[2].y;// + MAP_CELL_SIZE_M;
+
+ vect_mask_pi4.is_closed_polygon = vect_mask_0pi.is_closed_polygon;
+ vect_mask_pi4.nbr_of_lines = vect_mask_0pi.nbr_of_lines;
+
// init vect_mask_pi4 as rotation vect_mask_0pi
- int i;
- vect_coordinates_t new_coor;
- for(i = 0; (i < sizeof(vect_mask_0pi.points) / sizeof(vect_mask_0pi.points[0])); i++) {
- new_coor.x = cos(PI/4) * vect_mask_0pi.points[i].x - sin(PI/4) * vect_mask_0pi.points[i].y;
- new_coor.y = sin(PI/4) * vect_mask_0pi.points[i].x + cos(PI/4) * vect_mask_0pi.points[i].y;
- vect_mask_pi4.points[i] = new_coor;
-
-#ifdef AALGORITHM_DEBUG
- printf("PATHPLAN: init_aalgorithm : vect mask point %d, 0pi mask, pi4 mask are ([x, y]): [%2.2f, %2.2f], [%2.2f, %2.2f] \n", i, vect_mask_0pi.points[i].x, vect_mask_0pi.points[i].y, vect_mask_pi4.points[i].x, vect_mask_pi4.points[i].y);
-#endif
-
- }
+ transform_vect_mask(&vect_mask_0pi, &vect_mask_pi4, PI/4);
+
+ // init line before mask, it represented move forward in rot mask
+ vect_mask_pi4_front_line.is_closed_polygon = vect_mask_0pi_front_line.is_closed_polygon;
+ vect_mask_pi4_front_line.nbr_of_lines = vect_mask_0pi_front_line.nbr_of_lines;
+ transform_vect_mask(&vect_mask_0pi_front_line, &vect_mask_pi4_front_line, PI/4);
//init mask_0pi dynamic array
int init_size = 0;
init_size = abs(2 * (int) ceil((vect_mask_0pi.points[1].x - vect_mask_0pi.points[0].x) / MAP_CELL_SIZE_M) + 1); // 24
- init_size += abs(2 * (int) ceil((vect_mask_0pi.points[2].y - vect_mask_0pi.points[1].y) / MAP_CELL_SIZE_M) + 1); // 24 + 26
+ init_size += abs(4 * (int) ceil((vect_mask_0pi.points[2].y - vect_mask_0pi.points[1].y) / MAP_CELL_SIZE_M) + 1); // 24 + 26
//init mask_pi4 and mask_rot dynamic array
init_size += 4 * init_size; // 50 + 250 = 300
#ifdef AALGORITHM_DEBUG
printf("PATHPLAN: init_aalgorithm : init size of mask is (number * sizeof()): %d * %d \n", init_size, sizeof(cell_coordinates_t));
#endif
-
+ // init memmory for all mask
mask_rot.p_first_cell = (cell_coordinates_t *) calloc((size_t)init_size, (size_t)sizeof(cell_coordinates_t));
if(mask_rot.p_first_cell == NULL) {
- printf("init_aalgorithm: malloc error!\n");
+ fprintf( stderr, "\nERROR: PATHPLAN: init_aalgorithm: malloc error!\n" );
return 1;
}
#ifdef AALGORITHM_DEBUG
printf("PATHPLAN: init_aalgorithm : memory for mask is alocated, pointer to map is : %X \n", (unsigned int) mask_rot.p_first_cell);
#endif
- vect_coordinates_t ref_point;
- ref_point.x = 0;
- ref_point.y = 0;
-
- double r_f = sqrt(vect_mask_0pi.points[1].x * vect_mask_0pi.points[1].x + vect_mask_0pi.points[1].y * vect_mask_0pi.points[1].y); // radius to front of robot
mask_rot.nbr_mask_cells = 0;
- arc_2_grid(r_f, vect_mask_0pi.points[1].x, vect_mask_0pi.points[1].y, vect_mask_pi4.points[1].x, vect_mask_pi4.points[1].y, ref_point.x, ref_point.y, &mask_rot, init_size);
+ mask_t tmp_mask_rot;
-#ifdef AALGORITHM_DEBUG
- printf("PATHPLAN: init_aalgorithm : number of mask cells, rot, 0pi, pi4: %d, %d, %d \n", mask_rot.nbr_mask_cells, mask_0pi.nbr_mask_cells, mask_pi4.nbr_mask_cells);
-#endif
+ tmp_mask_rot.p_first_cell = mask_rot.p_first_cell + mask_rot.nbr_mask_cells;
+ tmp_mask_rot.nbr_mask_cells = 0;
+ vect_m_2_grid_m(&vect_mask_0pi_front_line, &tmp_mask_rot, init_size - mask_rot.nbr_mask_cells);
+ mask_rot.nbr_mask_cells += tmp_mask_rot.nbr_mask_cells;
- mask_t tmp_mask_rot;
tmp_mask_rot.p_first_cell = mask_rot.p_first_cell + mask_rot.nbr_mask_cells;
tmp_mask_rot.nbr_mask_cells = 0;
- arc_2_grid(r_f, vect_mask_0pi.points[2].x, vect_mask_0pi.points[2].y, vect_mask_pi4.points[2].x, vect_mask_pi4.points[2].y, ref_point.x, ref_point.y, &tmp_mask_rot, init_size - mask_rot.nbr_mask_cells);
+ vect_m_2_grid_m(&vect_mask_pi4_front_line, &tmp_mask_rot, init_size - mask_rot.nbr_mask_cells);
+ mask_rot.nbr_mask_cells += tmp_mask_rot.nbr_mask_cells;
+ int i;
+ for(i = 0; i < vect_mask_0pi.nbr_of_lines; i++) {
+
+ tmp_mask_rot.p_first_cell = mask_rot.p_first_cell + mask_rot.nbr_mask_cells;
+ tmp_mask_rot.nbr_mask_cells = 0;
+
+ arc_2_grid(vect_mask_0pi.points[i].x, vect_mask_0pi.points[i].y, vect_mask_pi4.points[i].x, vect_mask_pi4.points[i].y, &tmp_mask_rot, init_size - mask_rot.nbr_mask_cells);
+
+ mask_rot.nbr_mask_cells += tmp_mask_rot.nbr_mask_cells;
+
#ifdef AALGORITHM_DEBUG
- printf("PATHPLAN: init_aalgorithm : number of mask cells, rot, 0pi, pi4: %d, %d, %d \n", mask_rot.nbr_mask_cells, mask_0pi.nbr_mask_cells, mask_pi4.nbr_mask_cells);
+ printf("PATHPLAN: init_aalgorithm : arc nbr. %d has %d members\n",i ,tmp_mask_rot.nbr_mask_cells);
#endif
+
+ }
- double r_b = sqrt(vect_mask_0pi.points[0].x * vect_mask_0pi.points[0].x + vect_mask_0pi.points[0].y * vect_mask_0pi.points[0].y); // radius to back of robot
- mask_rot.nbr_mask_cells += tmp_mask_rot.nbr_mask_cells;
- tmp_mask_rot.nbr_mask_cells = 0;
- tmp_mask_rot.p_first_cell = mask_rot.p_first_cell + mask_rot.nbr_mask_cells;
- arc_2_grid(r_b, vect_mask_0pi.points[0].x, vect_mask_0pi.points[0].y, vect_mask_pi4.points[0].x, vect_mask_pi4.points[0].y, ref_point.x, ref_point.y, &tmp_mask_rot, init_size - mask_rot.nbr_mask_cells );
-#ifdef AALGORITHM_DEBUG
- printf("PATHPLAN: init_aalgorithm : number of mask cells, rot, 0pi, pi4: %d, %d, %d \n", mask_rot.nbr_mask_cells, mask_0pi.nbr_mask_cells, mask_pi4.nbr_mask_cells);
-#endif
-
- mask_rot.nbr_mask_cells += tmp_mask_rot.nbr_mask_cells;
- tmp_mask_rot.nbr_mask_cells = 0;
- tmp_mask_rot.p_first_cell = mask_rot.p_first_cell + mask_rot.nbr_mask_cells;
- arc_2_grid(r_b, vect_mask_0pi.points[3].x, vect_mask_0pi.points[3].y, vect_mask_pi4.points[3].x, vect_mask_pi4.points[3].y, ref_point.x, ref_point.y, &tmp_mask_rot, init_size - mask_rot.nbr_mask_cells);
-
- mask_rot.nbr_mask_cells += tmp_mask_rot.nbr_mask_cells;
- tmp_mask_rot.nbr_mask_cells = 0;
-
#ifdef AALGORITHM_DEBUG
printf("PATHPLAN: init_aalgorithm : number of mask cells, rot, 0pi, pi4: %d, %d, %d \n", mask_rot.nbr_mask_cells, mask_0pi.nbr_mask_cells, mask_pi4.nbr_mask_cells);
#endif
mask_0pi.nbr_mask_cells = 0;
mask_0pi.p_first_cell = mask_rot.p_first_cell + mask_rot.nbr_mask_cells;
- vect_m_2_grid_m(&vect_mask_0pi, sizeof(vect_mask_0pi.points)/ sizeof(vect_mask_0pi.points[0]), &mask_0pi, init_size - mask_rot.nbr_mask_cells);
+ vect_m_2_grid_m(&vect_mask_0pi, &mask_0pi, init_size - mask_rot.nbr_mask_cells);
#ifdef AALGORITHM_DEBUG
printf("PATHPLAN: init_aalgorithm : number of mask cells, rot, 0pi, pi4: %d, %d, %d \n", mask_rot.nbr_mask_cells, mask_0pi.nbr_mask_cells, mask_pi4.nbr_mask_cells);
mask_pi4.nbr_mask_cells = 0;
mask_pi4.p_first_cell = mask_0pi.p_first_cell + mask_0pi.nbr_mask_cells;
- vect_m_2_grid_m(&vect_mask_pi4, sizeof(vect_mask_pi4.points)/ sizeof(vect_mask_pi4.points[0]), &mask_pi4, init_size - mask_rot.nbr_mask_cells - mask_0pi.nbr_mask_cells);
+ vect_m_2_grid_m(&vect_mask_pi4, &mask_pi4, init_size - mask_rot.nbr_mask_cells - mask_0pi.nbr_mask_cells);
#ifdef AALGORITHM_DEBUG
printf("PATHPLAN: init_aalgorithm : number of mask cells, rot, 0pi, pi4: %d, %d, %d \n", mask_rot.nbr_mask_cells, mask_0pi.nbr_mask_cells, mask_pi4.nbr_mask_cells);
printf("PATHPLAN: init_aalgorithm : mask for angle PI4 inited, pointer is %X , number of inited cells is %d \n", (unsigned int) mask_pi4.p_first_cell, mask_pi4.nbr_mask_cells);
#endif
- init_size = mask_0pi.nbr_mask_cells + mask_pi4.nbr_mask_cells + mask_rot.nbr_mask_cells;
-
-#ifdef AALGORITHM_DEBUG
- printf("PATHPLAN: init_aalgorithm : final size of mask is : %d \n", init_size);
-#endif
-
- tmp_mask_rot.p_first_cell = (cell_coordinates_t *) realloc( mask_rot.p_first_cell, (unsigned int)(init_size * sizeof(cell_coordinates_t)));
+ tmp_mask_rot.nbr_mask_cells = mask_0pi.nbr_mask_cells + mask_pi4.nbr_mask_cells + mask_rot.nbr_mask_cells;
+ tmp_mask_rot.p_first_cell = (cell_coordinates_t *) realloc( mask_rot.p_first_cell, (unsigned int)(tmp_mask_rot.nbr_mask_cells * sizeof(cell_coordinates_t)));
if(tmp_mask_rot.p_first_cell == NULL) {
- printf("init_aalgorithm: malloc error!\n");
+ fprintf( stderr, "\nERROR: PATHPLAN: init_aalgorithm: malloc error!\n" );
return 1;
}
mask_0pi.p_first_cell = mask_rot.p_first_cell + mask_rot.nbr_mask_cells;
mask_pi4.p_first_cell = mask_0pi.p_first_cell + mask_0pi.nbr_mask_cells;
+
+ mask_rot.nbr_mask_cells = tmp_mask_rot.nbr_mask_cells; // - mask_0pi.nbr_mask_cells - mask_pi4.nbr_mask_cells;
+
+ mask_rot.type_of_mask = ROTATE_MASK;
+ mask_0pi.type_of_mask = DIRECT_MASK;
+ mask_pi4.type_of_mask = DIRECT_MASK;
#ifdef AALGORITHM_DEBUG
printf("PATHPLAN: init_aalgorithm : pointer to rotation mask is %X \n", (unsigned int) mask_rot.p_first_cell);
printf("PATHPLAN: init_aalgorithm : pointer to angle 0PI mask is %X \n", (unsigned int) mask_0pi.p_first_cell);
printf("PATHPLAN: init_aalgorithm : pointer to angle PI4 mask is %X \n", (unsigned int) mask_pi4.p_first_cell);
+ printf("PATHPLAN: init_aalgorithm : number of all inited cells is %d \n", mask_rot.nbr_mask_cells);
#endif
return 0;
}
+/**
+ * @name A* Free memory function
+ * @{
+ */
+
+/**
+ * @brief Free mask memory alocated in init_aalgorithm function
+ * @return 1 mask memory is not free, 0 if success.
+ *
+ * This is an implementation of extended A* Algorithm
+ * diploma thesis Collison-free Pathplannig for Mobile Robots, CTU
+ * Matous Pokorny 2012
+ */
+
+// free alocated memory,
+int freemem_aalgorithm(void) {
+ free((void*) mask_rot.p_first_cell);
+
+ mask_rot.p_first_cell = NULL;
+ mask_0pi.p_first_cell = NULL;
+ mask_pi4.p_first_cell = NULL;
+
+ mask_rot.nbr_mask_cells = 0;
+ mask_0pi.nbr_mask_cells = 0;
+ mask_pi4.nbr_mask_cells = 0;
+
+ return 0;
+}
+
+// tranform point to cell, only for init_aalgorithm, not for map
void point_2_cell(double x, double y ,cell_coordinates_t *p_new_coor) {
- p_new_coor->x = (int)floor(x / MAP_CELL_SIZE_M);
- p_new_coor->y = (int)( - floor(y / MAP_CELL_SIZE_M)); // y axis in cells has reverse orientation
+ p_new_coor->x = (int)(round(x / MAP_CELL_SIZE_M));
+ p_new_coor->y = (int)( - round(y / MAP_CELL_SIZE_M)); // y axis in cells has reverse orientation
}
-void vect_m_2_grid_m(vect_mask_t *p_vect_mask, int vect_mask_dim, mask_t *p_grid_mask, int nbr_grid_mask_cells) {
+// transform vect mask to mask
+void vect_m_2_grid_m(vect_mask_t *p_vect_mask, mask_t *p_grid_mask, int nbr_grid_mask_cells) {
int i_1, i_2;
int nbr_of_steps = 0;
double step = MAP_CELL_SIZE_M;
double dx12 = 0;
+ double dy12 = 0;
double k = 0;
double q = 0;
cell_coordinates_t *p_cell = p_grid_mask->p_first_cell;
#ifdef AALGORITHM_DEBUG
- printf("PATHPLAN: vect_m_2_grid_m : pointer to tmp cell : %X \n", (unsigned int) p_cell);
+ printf("PATHPLAN: vect_m_2_grid_m : vect. mask has %d members and is closed polygon %d\n", p_vect_mask->nbr_of_lines, p_vect_mask->is_closed_polygon);
#endif
-
- for(i_1 = 0; i_1 < vect_mask_dim; i_1++) {
+ // for all lines in polygon
+ for(i_1 = 0; i_1 < p_vect_mask->nbr_of_lines; i_1++) {
- i_2 = (i_1 == (vect_mask_dim - 1)) ? 0 : (i_1 + 1);
+ if( p_vect_mask->is_closed_polygon) {
+ i_2 = (i_1 == (p_vect_mask->nbr_of_lines - 1)) ? 0 : (i_1 + 1);
+ }
+ else {
+ i_2 = i_1 + 1;
+ if(i_2 > (sizeof(p_vect_mask->points) / sizeof(p_vect_mask->points[0]))){
+ printf("PATHPLAN: vect_m_2_grid_m : index i_2 %d in vect. mask is out of range 0 - %d\n", i_2, (sizeof(p_vect_mask->points) / sizeof(p_vect_mask->points[0])));
+ return;
+ }
+ }
#ifdef AALGORITHM_DEBUG
printf("PATHPLAN: vect_m_2_grid_m : index in vect_mask, point B, point A: %d, %d \n", i_2, i_1);
#endif
dx12 = (p_vect_mask->points[i_2].x - p_vect_mask->points[i_1].x); // dx12 = x2 - x1
+ dy12 = (p_vect_mask->points[i_2].y - p_vect_mask->points[i_1].y); // dy12 = y2 - y1
if(dx12 != 0) {
k = (p_vect_mask->points[i_2].y - p_vect_mask->points[i_1].y) / dx12; // k = (y2 - y1)/(x2 - x1)
q = (p_vect_mask->points[i_2].x * p_vect_mask->points[i_1].y - p_vect_mask->points[i_1].x * p_vect_mask->points[i_2].y) / dx12; // q = (x2 * y1 - x1 * y2) / (x2 -x1)
- nbr_of_steps = abs((int) ceil(dx12 / step));
+
+ nbr_of_steps = abs((int) round(dx12 / step));
}
- else {
- nbr_of_steps = abs((int) ceil((p_vect_mask->points[i_2].y - p_vect_mask->points[i_1].y) / step));
+ else {
+ nbr_of_steps = abs((int) round(dy12 / step));
}
+ nbr_of_steps += 1;
#ifdef AALGORITHM_DEBUG
- printf("PATHPLAN: vect_m_2_grid_m : vect equation in mask is : y = %2.2f x + %2.2f \n", k, q);
+ printf("PATHPLAN: vect_m_2_grid_m : vect equation in mask is : y = %2.2f x + %2.2f \n", k, q);
#endif
#ifdef AALGORITHM_DEBUG
- printf("PATHPLAN: vect_m_2_grid_m : number of steps in one vect is : %d \n", nbr_of_steps);
+ printf("PATHPLAN: vect_m_2_grid_m : number of steps in one vect is : %d \n", nbr_of_steps);
#endif
int i;
- for(i = 0; i <= (nbr_of_steps + 1); i++) {
+ // for every sample (step) on the line
+ for(i = 0; i < nbr_of_steps; i++) {
if(dx12 != 0) {
if(p_vect_mask->points[i_2].x > p_vect_mask->points[i_1].x) {
point_2_cell(tmp_coor.x, tmp_coor.y , p_cell);
#ifdef AALGORITHM_DEBUG
- printf("PATHPLAN: vect_m_2_grid_m : cell added, coordinates are x, y: %d, %d \n", p_cell->x, p_cell->y);
+ printf("PATHPLAN: vect_m_2_grid_m : cell added, coordinates are x, y: %d, %d \n", p_cell->x, p_cell->y);
#endif
p_cell++;
p_grid_mask->nbr_mask_cells++;
+ // error if memory is bad alocated
if(p_cell > (p_grid_mask->p_first_cell + nbr_grid_mask_cells)) {
- printf("PATHPLAN: vect_m_2_grid_m : end of alocated memory, not possible write after that \n");
+ fprintf( stderr, "\nERROR: PATHPLAN: vect_m_2_grid_m : end of alocated memory, not possible write after that!\n" );
break;
}
}
}
}
-// point x1 is asigned to smaller angle
-void arc_2_grid(double r, double x1, double y1, double x2, double y2, double x_r, double y_r, mask_t *p_grid_mask, int nbr_grid_mask_cells) {
-
- double a_step = MAP_CELL_SIZE_M / 2;
- // vectors end point x1, ref point and end point x2, ref point
- vect_coordinates_t vect_sx1, vect_sx2;
-
- vect_sx1.x = (double) (x1 - x_r);
- vect_sx1.y = (double) (y1 - y_r);
- vect_sx2.x = (double) (x2 - x_r);
- vect_sx2.y = (double) (x2 - y_r);
-
- // radius r is size of vector
- double radius = sqrt(vect_sx1.x * vect_sx1.x + vect_sx1.y * vect_sx1.y);
+// transform arc to grid, point x1 is asigned to smaller angle
+void arc_2_grid(double x1, double y1, double x2, double y2, mask_t *p_grid_mask, int nbr_grid_mask_cells) {
+
+ double alpha_1 = atan2(y1, x1);
+ alpha_1 = (alpha_1 < 0) ? (alpha_1 + (2 * PI)) : alpha_1 ;
+ double alpha_2 = atan2(y2, x2);
+ alpha_2 = (alpha_2 < 0) ? (alpha_2 + (2 * PI)) : alpha_2 ;
+ double da12 = alpha_2 - alpha_1;
- // angle between two vectors, alpha = arccos((vect_sx1 * vect_sx2) / (|vect_sx1| * |vect_sx2|))
- double alpha = 0;
- double acos_arg = (vect_sx1.x * vect_sx2.x + vect_sx1.y * vect_sx2.y) / ( radius * radius );
+ if (da12 == 0) {
+ fprintf( stderr, "\nERROR: PATHPLAN: arc_2_grid : no arc to transform!\n" );
+ return;
+ }
- if((acos_arg <= 1.0) && (acos_arg >= -1.0)) {
- alpha = acos((vect_sx1.x * vect_sx2.x + vect_sx1.y * vect_sx2.y) / ( radius * radius ));
+ double alpha = 0, alpha_0 = 0;
+ double x0 = 0, y0 = 0;
+ if(da12 > 0) {
+ // alpha_2 is greater
+ if(da12 >= PI) {
+ alpha = 2*PI - da12;
+ alpha_0 = alpha_2;
+ x0 = x2;
+ y0 = y2;
+ }
+ else {
+ alpha = da12;
+ alpha_0 = alpha_1;
+ x0 = x1;
+ y0 = y1;
+ }
+
}
- else {
- printf("PATHPLAN: arc_2_grid : argument of acos is out of range < -1, 1 > \n");
+ else {
+ // alpha_1 is greater
+ if(da12 <= - PI) {
+ alpha = 2*PI + da12;
+ alpha_0 = alpha_1;
+ x0 = x1;
+ y0 = y1;
+ }
+ else {
+ alpha = - da12;
+ alpha_0 = alpha_2;
+ x0 = x2;
+ y0 = y2;
+ }
}
+
+#ifdef AALGORITHM_DEBUG
+ printf("PATHPLAN: arc_2_grid: alpha 1, 2, 0, x0, y0, x1, y1, x2, y2: %3f, %3f, %3f, %1.3f, %1.3f, %1.3f, %1.3f, %1.3f, %1.3f \n", alpha_1*(180 / PI), alpha_2*(180 / PI), alpha_0*(180 / PI), x1, y1, x2, y2, x0, y0);
+#endif
- double alpha_0 = atan2(y1, x1);
- double alpha_i = 0;
-
+ double radius = sqrt(x0 * x0 + y0 * y0);
// lenght of arc
double a = radius * alpha;
-
+ double a_step = MAP_CELL_SIZE_M / 2;
double alpha_step = alpha / (a / a_step);
int nbr_of_angle_steps = (int) ceil(a / a_step);
+#ifdef AALGORITHM_DEBUG
+ printf("PATHPLAN: arc_2_grid: arc lenght, step, angle step, nbr of angle steps: %1.3f,%1.3f, %3f, %d \n", a, a_step, alpha_step, nbr_of_angle_steps);
+#endif
cell_coordinates_t *p_cell = NULL;
-
+ double alpha_i = 0;
int i;
+ double x, y;
+ // for all sample on the arc
for(i = 0; i < nbr_of_angle_steps; i++) {
- alpha_i = alpha_0 + i * alpha_step;
+ alpha_i = i * alpha_step;
- double x = x1 * cos(alpha_i) - y1 * sin(alpha_i);
- double y = y1 * sin(alpha_i) + y1 * cos(alpha_i);
+ x = x0 * cos(alpha_i) - (y0) * sin(alpha_i);
+ y = x0 * sin(alpha_i) + (y0) * cos(alpha_i);
p_cell = p_grid_mask->p_first_cell + i;
p_grid_mask->nbr_mask_cells++;
}
else {
- printf("PATHPLAN: arc_2_grid : end of alocated memory, not possible write after that \n");
- printf("PATHPLAN: arc_2_grid : pointer to cell > max address: %X > %X \n", (unsigned int) p_cell, (unsigned int) (p_grid_mask->p_first_cell + nbr_grid_mask_cells));
+ fprintf( stderr, "\nERROR: PATHPLAN: arc_2_grid : end of alocated memory, not possible write after that!\n" );
break;
}
*x = (c - &graph[0][0]) % MAP_WIDTH;
}
-static bool is_pos_reach(int *p_x_ref, int *p_y_ref, mask_t *p_mask, int angle_index)
+// is position reachable, test expanded cell, if is free, using mask
+static bool is_pos_reach(int x_ref, int y_ref, mask_t *p_mask, int angle_index)
{
cell_coordinates_t *p_cell_coor;
cell_coordinates_t coor;
p_cell_coor = p_mask->p_first_cell;
-
+
int i;
for(i = 0; i < p_mask->nbr_mask_cells; i++) {
//transform mask to right orientation (mirror and rotation)
- coor = transform(*p_cell_coor, angle_index);
-
- coor.x += *p_x_ref;
- coor.y += *p_y_ref;
+ coor = transform(*(p_cell_coor + i), angle_index, p_mask->type_of_mask);
+ coor.x += x_ref;
+ coor.y += y_ref;
-#ifdef AALGORITHM_DEBUG
- ShmapSetCellFlag(coor.x,coor.y,MAP_FLAG_PLAN_MASK);
+#ifdef AALGORITHM_DEBUG_PNG
+ ShmapSetCellFlag(coor.x, coor.y, MAP_FLAG_PLAN_MASK);
#endif
- if ((!ShampIsPointInMap(coor.x,coor.y)) || (!ShmapIsFreeCell(coor.x, coor.y))) {
+ if(ShmapIsFreeCell(coor.x, coor.y) == false) {
+#ifdef AALGORITHM_DEBUG
+ printf("PATHPLAN: is_pos_reach: point in map is not free \n");
+#endif
return false;
}
-
- p_cell_coor = p_mask->p_first_cell + (sizeof(cell_coordinates_t)) * i;
-
+
}
return true;
}
+// is angle index even
static bool is_angle_idx_even(int angle_index)
{
return (((angle_index) % 2) == 0);
}
-static cell_coordinates_t transform( cell_coordinates_t coor, int angle_index)
+// tranform one cell of mask with rotation matrix
+cell_coordinates_t transform( cell_coordinates_t coor, int angle_index, TYPE_OF_MASK type_of_mask)
{
cell_coordinates_t new_coor;
- // mirror for rotation mask, other masks are immune
- if (!is_angle_idx_even(angle_index)) {
- coor.y = -coor.y;
- }
+
+ bool odd_angle_index = !(is_angle_idx_even(angle_index));
+ angle_index = (int) floor((double)angle_index / 2.0);
+
+#ifdef AALGORITHM_DEBUG
+ printf("PATHPLAN: transform: angle_index is %d, its odd %d\n", angle_index, odd_angle_index);
+#endif
- angle_index = angle_index % 2;
+#ifdef AALGORITHM_DEBUG
+ printf("PATHPLAN: transform: rotation matrix is %d %d %d %d\n", angle_idx_2_rot_mat[angle_index].r1, angle_idx_2_rot_mat[angle_index].r2, angle_idx_2_rot_mat[angle_index].r3, angle_idx_2_rot_mat[angle_index].r4);
+#endif
+
+
+ if ((type_of_mask == ROTATE_MASK) && odd_angle_index ) {
+ // mirror for rotation mask, other masks are immune
+ coor.y = - coor.y;
+ angle_index++;
+ int last_index = (sizeof(angle_idx_2_rot_mat) / sizeof(angle_idx_2_rot_mat[0])) - 1;
+ angle_index = (angle_index > last_index) ? 0 : angle_index;
+ new_coor.x = coor.x * angle_idx_2_rot_mat[angle_index].r1 + coor.y * (1) * angle_idx_2_rot_mat[angle_index].r2;
+ new_coor.y = coor.x * angle_idx_2_rot_mat[angle_index].r3 + coor.y * (1) * angle_idx_2_rot_mat[angle_index].r4;
+ }
+ else {
+ new_coor.x = coor.x * angle_idx_2_rot_mat[angle_index].r1 + coor.y * angle_idx_2_rot_mat[angle_index].r2;
+ new_coor.y = coor.x * angle_idx_2_rot_mat[angle_index].r3 + coor.y * angle_idx_2_rot_mat[angle_index].r4;
+ }
- new_coor.x = coor.x * angle_idx_2_rot_mat[angle_index].r1 + coor.y * angle_idx_2_rot_mat[angle_index].r2;
- new_coor.y = coor.x * angle_idx_2_rot_mat[angle_index].r3 + coor.y * angle_idx_2_rot_mat[angle_index].r4;
-
return new_coor;
}
+
+#ifndef HOLOMIC
int aAlgorithm(double xstart_real, double ystart_real, double xgoal_real, double ygoal_real, double start_angle, double goal_angle, GraphMapCell **original_path)
{
/**
double angle;
mask_t mask;
- init_aalgorithm();
+#ifdef AALGORITHM_DEBUG_PNG
+ int file_counter = 0;
+#endif
+
+#ifdef AALGORITHM_DEBUG_EXP_CELLS
+ clear_flag(map, MAP_FLAG_PLAN_EXPANDED_CELLS);
+#endif
if (!map) return -1;
-#ifdef AALGORITHM_DEBUG
- // Clear previous masks flags
- for (y=0;y<MAP_HEIGHT;y++){
- for(x=0;x<MAP_WIDTH;x++){
- map->cells[y][x].flags &= ~(MAP_FLAG_PLAN_MASK);
- }
- }
+#ifdef AALGORITHM_DEBUG_PNG
+ clear_flag(map, MAP_FLAG_PLAN_MASK);
#endif
// Transform real data in cell data
return -1;
start = &graph[ystart][xstart];
+#ifdef AALGORITHM_DEBUG
+ printf("PATHPLAN: aAlgorithm: START point has real coordinates %1.2f, %1.2f and is in cell %d, %d \n",xstart_real , ystart_real, xstart, ystart);
+#endif
+
ShmapPoint2Cell(xgoal_real, ygoal_real, &xgoal, &ygoal, &valid);
if (!valid)
return -1;
goal = &graph[ygoal][xgoal];
+#ifdef AALGORITHM_DEBUG
+ printf("PATHPLAN: aAlgorithm: GOAL point has real coordinates %1.2f, %1.2f and is in cell %d, %d \n",xgoal_real , ygoal_real, xgoal, ygoal);
+#endif
/**@{*/
/// <ol>
/// <li> Init graph structure with default values.
#endif
GraphCell2XY(nbest, &x, &y);
-/* map->cells[y][x].detected_obstacle = 255; */
-/* usleep(100000); */
/// <li> add to processed
nbest->processed = 1;
/// <li> IF nbest == goal ; EXIT
if (nbest == goal) {
+#ifdef AALGORITHM_DEBUG
+ printf("PATHPLAN: aAlgorithm: NBEST point is GOAL %d, %d \n",x , y);
+#endif
break;
}
// determines angle between cells
if (nbest == start) {
angle_index = get_angle_index(start_angle);
+#ifdef AALGORITHM_DEBUG
+ printf("PATHPLAN: aAlgorithm: START angle is %3.0f and start angle index %d \n",start_angle, angle_index);
+#endif
}
else {
GraphCell2XY(nbest->backpointer, &x_back, &y_back); // determines coordinates of previous cell
- angle = PI + atan2((double)(y - y_back), (double)(x - x_back)); // angle between cells, atan2 return angle in the close range < -pi, pi >
+ angle = atan2((double)(y - y_back), (double)(x - x_back)); // angle between cells, atan2 return angle in the close range < -pi, pi >
+ angle = (angle < 0) ? (angle + (2 * PI)) : angle;
angle_index = get_angle_index(angle);
}
- //check position direct before robot, one cell forward
+ //check position direct before robot, one cell forward, i = 0
if (is_angle_idx_even(angle_index)) {
mask = mask_0pi;
}
cell_coordinates_t coor_contig;
bool rot_left = true;
bool rot_right = true;
-
- for (i = 0; i < 7; i++) {
-
- angle_index = angle_index + (int) pow((double) (-1), (double) i); // if angle_index before for() is for example 3, then for i = <0..7>, angle_index is 3, 2, 4, 1, 5, 0, 6
- angle_index = (angle_index < 0) ? (8 - angle_index) : angle_index;
- angle_index = (angle_index > 8) ? (angle_index - 8) : angle_index;
-
- coor_contig = angle_idx_2_xy[angle_index]; // get relative coordinates from nbest to contiguous cell
-
- if(ShmapIsCellInMap(coor_contig.x, coor_contig.y) && (graph[coor_contig.x][coor_contig.y].processed == 0)) {
+ for (i = 0; i < 9; i++) {
+ // if angle_index before for() is for example 3, then for i = <0..7>, angle_index is 3, 2, 4, 1, 5, 0, 6
+ angle_index += ((int) pow((double) (-1), (double) i)) * i;
+ angle_index = (angle_index < 0) ? (8 + angle_index) : angle_index;
+ angle_index = (angle_index > 7) ? (angle_index - 8) : angle_index;
+#ifdef AALGORITHM_DEBUG
+ printf("PATHPLAN: aAlgorithm: angle_index for mask choice is %d\n", angle_index);
+#endif
+ // get relative coordinates from nbest to contiguous cell
+ coor_contig = angle_idx_2_xy[angle_index];
+ coor_contig.x += x;
+ coor_contig.y += y;
+#ifdef AALGORITHM_DEBUG
+ printf("PATHPLAN: aAlgorithm: coordinates of CONTIG point are %d, %d, of NBEST point are %d, %d\n", coor_contig.x, coor_contig.y, x, y);
+#endif
+ if(ShmapIsCellInMap(coor_contig.x, coor_contig.y) && (graph[coor_contig.y][coor_contig.x].processed == 0)) {
double cost = cFunction(x, y, coor_contig.x, coor_contig.y);
+#ifdef AALGORITHM_DEBUG
+ printf("PATHPLAN: aAlgorithm: cost to CONTIG point is %1.3f\n", cost);
+#endif
contig = &graph[coor_contig.y][coor_contig.x];
/// <li> IF is not in Priorityset add to prioritySet.
if (!contig->in_queue) {
-
- if((i == 0) && is_pos_reach(&x, &y, &mask, angle_index)) {
- contig->g=nbest->g + cost;
- contig->f = contig->g + contig->h;
- contig->backpointer=nbest;
- pushNodeInOrder(contig);
- mask = mask_rot;
- }
- else {
- if(rot_left && ((int) pow((double) (-1), (double) i) < 0 ) && is_pos_reach(&x, &y, &mask, angle_index)) {
+#ifdef AALGORITHM_DEBUG
+ printf("PATHPLAN: aAlgorithm: cycle index is %d and angle index %d \n", i, angle_index);
+#endif
+ // check position one cell forward
+ if(i == 0) {
+// if(ShmapIsFreeCell(coor_contig.x, coor_contig.y)) {
+#ifdef AALGORITHM_DEBUG_EXP_CELLS
+ ShmapSetCellFlag(coor_contig.x, coor_contig.y, MAP_FLAG_PLAN_EXPANDED_CELLS);
+#endif
+ if(is_pos_reach(coor_contig.x, coor_contig.y, &mask, angle_index)) {
contig->g=nbest->g + cost;
contig->f = contig->g + contig->h;
contig->backpointer=nbest;
- pushNodeInOrder(contig);
- }
- else {
- rot_left = false;
+ pushNodeInOrder(contig);
+ mask = mask_rot;
+#ifdef AALGORITHM_DEBUG
+ printf("PATHPLAN: aAlgorithm: CONTIG point is reachable, angle index %d and func. f %3.0f = %3.0f + %3.0f\n", angle_index, contig->f, contig->g, contig->h);
+#endif
}
-
- if(rot_right && ((int) pow((double) (-1), (double) i) > 0 ) && is_pos_reach(&x, &y, &mask, angle_index)) {
- contig->g=nbest->g + cost;
- contig->f = contig->g + contig->h;
- contig->backpointer=nbest;
- pushNodeInOrder(contig);
+ }
+ // check rotation
+ else {
+#ifdef AALGORITHM_DEBUG
+ printf("PATHPLAN: aAlgorithm: rot_left %d, rot_right %d, (int) pow((double) (-1), (double) i) %d\n", rot_left, rot_right, (int) pow((double) (-1), (double) i));
+#endif
+ // rotation to left
+ if(rot_left && ((int) pow((double) (-1), (double) i) > 0 )) {
+// if(ShmapIsFreeCell(coor_contig.x, coor_contig.y)) {
+#ifdef AALGORITHM_DEBUG_EXP_CELLS
+ ShmapSetCellFlag(coor_contig.x, coor_contig.y, MAP_FLAG_PLAN_EXPANDED_CELLS);
+#endif
+ int tmp_angle_index = angle_index - 1;
+ tmp_angle_index = (tmp_angle_index < 0) ? (8 + tmp_angle_index) : tmp_angle_index;
+ if(is_pos_reach(x, y, &mask, tmp_angle_index)) {
+ contig->g=nbest->g + cost;
+ contig->f = contig->g + contig->h;
+ contig->backpointer=nbest;
+ pushNodeInOrder(contig);
+#ifdef AALGORITHM_DEBUG
+ printf("PATHPLAN: aAlgorithm: CONTIG point is reachable, angle index %d and func. f %3.0f = %3.0f + %3.0f\n", tmp_angle_index, contig->f, contig->g, contig->h);
+#endif
+ }
+ else {
+ rot_left = false;
+ }
}
- else {
- rot_right = false;
+
+ // rotation to right
+ if(rot_right && ((int) pow((double) (-1), (double) i) < 0 )) {
+// if(ShmapIsFreeCell(coor_contig.x, coor_contig.y)) {
+#ifdef AALGORITHM_DEBUG_EXP_CELLS
+ ShmapSetCellFlag(coor_contig.x, coor_contig.y, MAP_FLAG_PLAN_EXPANDED_CELLS);
+#endif
+ if(is_pos_reach(x, y, &mask, angle_index)) {
+ contig->g=nbest->g + cost;
+ contig->f = contig->g + contig->h;
+ contig->backpointer=nbest;
+ pushNodeInOrder(contig);
+#ifdef AALGORITHM_DEBUG
+ printf("PATHPLAN: aAlgorithm: CONTIG point is reachable, angle index %d and func. f %3.0f = %3.0f + %3.0f\n", angle_index, contig->f, contig->g, contig->h);
+#endif
+ }
+ else {
+ rot_right = false;
+ }
}
- }
+ }
+
+#ifdef AALGORITHM_DEBUG_PNG
+ // create png picture in every step
+ ShmapSetCellFlag(xstart , ystart, MAP_FLAG_START);
+ ShmapSetCellFlag(xgoal , ygoal, MAP_FLAG_GOAL);
+
+ char file_name[50];
+ sprintf(file_name,"%.5i_map_x%3d_y%3d_angle%1d_step%d.png", file_counter, coor_contig.x,coor_contig.y, angle_index, i);
+ map_2_png(map, file_name);
+
+ clear_flag(map, MAP_FLAG_PLAN_MASK);
+#endif
+
} else if (nbest->g + cost < contig->g) {
/// <li> ELSE update backpointer to point to nbest.
contig->backpointer=nbest;
contig->g = nbest->g + cost;
+#ifdef AALGORITHM_DEBUG
+ printf("PATHPLAN: aAlgorithm: CONTIG point backpointer was updated \n");
+#endif
+
}
+#ifdef AALGORITHM_DEBUG
+ printf("PATHPLAN: aAlgorithm: CONTIG %d %d point wais in quene.\n", coor_contig.x, coor_contig.y);
+#endif
}
-
-#ifdef AALGORITHM_DEBUG
- char file_name[20];
- sprintf(file_name,"map_x%3d_y%3d_a%1d", coor_contig.x,coor_contig.y, angle_index);
-
- if(map_2_png(map, file_name)) {
- printf("PATHPLAN: aAlgorithm : PNG file sucessfull created");
- }
- else {
- printf("PATHPLAN: aAlgorithm : PNG file not created!");
+
+#ifdef AALGORITHM_DEBUG
+ else {
+
+ printf("PATHPLAN: aAlgorithm: is point CONTIG %d %d in map %d, is proccessed %d \n", coor_contig.x, coor_contig.y, ShmapIsCellInMap(coor_contig.x, coor_contig.y), graph[coor_contig.y][coor_contig.x].processed);
+
}
-#endif
-
+#endif
+
+#ifdef AALGORITHM_DEBUG
+ print_quene(graph_queue);
+#endif
+
+#ifdef AALGORITHM_DEBUG
+ //stop in every step
+ //fprintf( stderr, "\npress ENTER key to continue!\n" ); // better form
+ //getchar();
+#endif
+
+#ifdef AALGORITHM_DEBUG_PNG
+ file_counter++;
+#endif
if ((!rot_left) && (!rot_right)) {
break;
}
return -1;
}
}
+#endif
+
/**
* @name A* Main function
* @{
* This is an implementation of A* Algorithm (Algorithm 24, p531)
* defined in the book "Principles of Robot Motion" by
* H. Choset&others
+ *
+ * Function was extended. See
+ * diploma thesis Collison-free Pathplannig for Mobile Robots, CTU
+ * for more information.
+ * Matous Pokorny 2012
*/
#ifdef HOLOMIC
if (!contig->in_queue) {
if(ShmapIsFreeCell(xcontig, ycontig)) {
// add to prioritySet.
+#ifdef AALGORITHM_DEBUG_EXP_CELLS
+ ShmapSetCellFlag(xcontig, ycontig, MAP_FLAG_PLAN_EXPANDED_CELLS);
+#endif
contig->g=nbest->g + cost;
contig->f = contig->g + contig->h;
contig->backpointer=nbest;
projects / eurobot / public.git / blobdiff