From cb38861f65201edce85a93bcb69ec7b07b96635d Mon Sep 17 00:00:00 2001 From: Olivier Lanneluc Date: Tue, 16 Apr 2013 18:06:12 +0200 Subject: digital/io-hub/src/common-cc: add possibility for multiple navigation points layers --- digital/io-hub/src/common-cc/path.cc | 207 ++++++++++++++++++----------------- digital/io-hub/src/common-cc/path.hh | 18 +-- 2 files changed, 120 insertions(+), 105 deletions(-) (limited to 'digital/io-hub/src/common-cc') diff --git a/digital/io-hub/src/common-cc/path.cc b/digital/io-hub/src/common-cc/path.cc index cc5e290a..99b9b3f1 100644 --- a/digital/io-hub/src/common-cc/path.cc +++ b/digital/io-hub/src/common-cc/path.cc @@ -5,7 +5,7 @@ // Copyright (C) 2013 Olivier Lanneluc // // APBTeam: -// Web: http://apbteam.org/ +// Web: http://apbteam.org/ // Email: team AT apbteam DOT org // // This program is free software; you can redistribute it and/or modify @@ -82,12 +82,13 @@ void Path::reset() add_obstacle(pg_cake_pos, pg_cake_radius, PATH_CAKE_NAVPOINTS_NB); } -void Path::add_obstacle(const vect_t &c, uint16_t r, int num) +void Path::add_obstacle(const vect_t &c, uint16_t r, const int nodes) { uint32_t rot_a, rot_b, nr; uint32_t x, y, nx; + int npt, weight; - DPRINTF("Add obstacle c=(%u;%u) r=%u num=%u\n",c.x, c.y, r, num); + DPRINTF("Add obstacle c=(%u;%u) r=%u num=%u\n",c.x, c.y, r, nodes); /* Enlarge the obstacle radius by the robot size and clearance area width */ r += BOT_SIZE_SIDE + Obstacles::clearance_mm; @@ -99,39 +100,48 @@ void Path::add_obstacle(const vect_t &c, uint16_t r, int num) obstacles_nb++; /* Complex number of modulus 1 and rotation angle */ - nr = PATH_ANGLE_824(num); + nr = PATH_ANGLE_824(nodes); rot_a = fixed_cos_f824(nr); rot_b = fixed_sin_f824(nr); /* Extend the points radius to allow the robot to go */ /* from one to another without collision with the */ /* obstacle circle. New radius is r / cos(angle/2) */ - nr = PATH_ANGLE_824(2*num); + nr = PATH_ANGLE_824(nodes*2); x = fixed_div_f824(r, fixed_cos_f824(nr)) + 1 /* margin */; y = 0; - /* Compute obstacle points positions around a circle */ - for (; num>0; num--) + /* Add 2 sets of navigation points with different weights */ + for(weight=PATH_NAVPOINTS_LAYERS; weight>0; weight--) { - /* Compute the point absolute position */ - navpoints[navpoints_nb].x = c.x + (vect_value_t)x; - navpoints[navpoints_nb].y = c.y + (vect_value_t)y; - - /* Check it is in playground */ - if (navpoints[navpoints_nb].x >= border_xmin - && navpoints[navpoints_nb].y >= border_ymin - && navpoints[navpoints_nb].x <= border_xmax - && navpoints[navpoints_nb].y <= border_ymax) + /* Compute obstacle points positions around a circle */ + for (npt=0; npt= border_xmin + && navpoints[navpoints_nb].y >= border_ymin + && navpoints[navpoints_nb].x <= border_xmax + && navpoints[navpoints_nb].y <= border_ymax) + { + /* Accept point */ + DPRINTF("Add point %u (%u;%u) w=%u\n", + navpoints_nb, navpoints[navpoints_nb].x, navpoints[navpoints_nb].y, weight); + navweights[navpoints_nb] = weight; + navpoints_nb++; + } + + /* Complex multiply with A = cos(angle) + i sin(angle) */ + nx = fixed_mul_f824(x, rot_a) - fixed_mul_f824(y, rot_b); + y = fixed_mul_f824(y, rot_a) + fixed_mul_f824(x, rot_b); + x = nx; } - /* Complex multiply with A = cos(angle) + i sin(angle) */ - nx = fixed_mul_f824(x, rot_a) - fixed_mul_f824(y, rot_b); - y = fixed_mul_f824(y, rot_a) + fixed_mul_f824(x, rot_b); - x = nx; + /* Enlarge the circle */ + x += BOT_SIZE_RADIUS; } #ifdef HOST @@ -145,20 +155,70 @@ void Path::add_obstacle(const vect_t &c, uint16_t r, int num) #endif } -void Path::obstacle(int index, const vect_t &c, uint16_t r, int f) +int Path::find_neighbors(int cur_point, struct astar_neighbor_t *neighbors) { - add_obstacle(c, r, PATH_OBSTACLES_NAVPOINTS_NB); -} + int neighbors_nb = 0; -void Path::endpoints(const vect_t &src, const vect_t &dst) -{ - DPRINTF("Set path endpoints src=(%u;%u) dst=(%u;%u)\n", - src.x, src.y, dst.x, dst.y); - navpoints[PATH_NAVPOINT_SRC_IDX] = src; - navpoints[PATH_NAVPOINT_DST_IDX] = dst; + /* Parse all nodes */ + for(int i=0; i validated w=%u\n", weight); + neighbors[neighbors_nb].node = i; + neighbors[neighbors_nb].weight = weight; + neighbors_nb++; + } + else + { + DPRINTF("=> skipped\n"); + } + } + } + +#if PATH_DEBUG + DPRINTF("\tFound %u neighbors: ", neighbors_nb); + for(int i=0;i validated w=%u\n", weight); - neighbors[neighbors_nb].node = i; - neighbors[neighbors_nb].weight = weight; - neighbors_nb++; - } - else - { - DPRINTF("=> skipped\n"); - } - } - } - -#if PATH_DEBUG - DPRINTF("\tFound %u neighbors: ", neighbors_nb); - for(int i=0;i