summaryrefslogtreecommitdiff
path: root/digital
diff options
context:
space:
mode:
authorOlivier Lanneluc2013-04-12 23:54:46 +0200
committerNicolas Schodet2013-04-27 00:14:57 +0200
commitedba4c1f20ca40734a0fe69699fba7d06e8afc59 (patch)
tree7726fcd5614e6d87a8749dc1344004079d804e92 /digital
parentc4f4fcd21a3440014c35152e6258033351ed7dc0 (diff)
First commit - testing the escape factor
Diffstat (limited to 'digital')
-rw-r--r--digital/io-hub/src/common-cc/path.cc252
-rw-r--r--digital/io-hub/src/common-cc/path.hh118
2 files changed, 347 insertions, 23 deletions
diff --git a/digital/io-hub/src/common-cc/path.cc b/digital/io-hub/src/common-cc/path.cc
index ef7787cc..78011eb3 100644
--- a/digital/io-hub/src/common-cc/path.cc
+++ b/digital/io-hub/src/common-cc/path.cc
@@ -22,23 +22,265 @@
// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
//
// }}}
+
#include "path.hh"
+#include "bot.hh"
+#include "robot.hh"
+#include "playground.hh"
extern "C" {
-#include "modules/path/astar/astar.h"
+#include "modules/math/fixed/fixed.h"
+#include "modules/math/geometry/distance.h"
+#include "modules/utils/utils.h"
+}
+
+#define PATH_DEBUG 1
+
+#ifdef HOST
+#include "../../digital/ai/src/common/debug.host.h"
+#endif
+
+#define PATH_PLOT_ID 2
+
+enum {
+ PATH_POINT_SRC_IDX = 0,
+ PATH_POINT_DST_IDX
+};
+
+Path::Path() :
+ border_xmin(pg_border_distance),
+ border_ymin(pg_border_distance),
+ border_xmax(pg_width - pg_border_distance),
+ border_ymax(pg_length - pg_border_distance),
+ escape_factor(0),
+ obstacles_nb(0),
+ points_nb(0),
+ next_node(0)
+{
+ DPRINTF("Path constructor\n");
+}
+
+void Path::reset()
+{
+ vect_t nul = {0,0};
+ vect_t cake = {pg_width>>1, pg_length};
+
+ /* Reset everything */
+ DPRINTF( "Path reset\n");
+ obstacles_nb = 0;
+ points_nb = PATH_FIXED_POINTS_NB;
+ points[PATH_POINT_SRC_IDX] = nul;
+ points[PATH_POINT_DST_IDX] = nul;
+ next_node = 0;
+ escape_factor = 0;
+
+ /* Declare the cake as an obstacle */
+ add_obstacle(cake, 500 + BOT_SIZE_SIDE, 0, PATH_CAKE_POINTS_NB);
+}
+
+void Path::add_obstacle(const vect_t &c, const int r, const int factor, const int num)
+{
+ uint32_t rot_a, rot_b, fact_r;
+ uint32_t x, y, nx;
+ int j;
+
+ DPRINTF("Add obstacle c=(%u;%u) r=%u factor=%u points_nb=%u\n",c.x,c.y,r,factor,num);
+
+ /* Store obstacle */
+ //assert(obstacles_nb < PATH_OBSTACLES_NB);
+ obstacles[obstacles_nb].c = c;
+ obstacles[obstacles_nb].r = r;
+ obstacles[obstacles_nb].factor = factor;
+ obstacles_nb++;
+
+ /* Complex number of modulus 1 and rotation angle */
+ rot_a = fixed_cos_f824(PATH_OBSTACLES_POINTS_ANGLE(num));
+ rot_b = fixed_sin_f824(PATH_OBSTACLES_POINTS_ANGLE(num));
+
+ /* Additional radius factor to allow the robot to go */
+ /* from one obstacle point to its neighbor point */
+ /* without collision with the obstacle avoidance circle */
+ /* Radius factor is sqrt( sin(angle)^2 + 1^2 ) */
+ fact_r = fixed_sqrt_uf248(
+ (fixed_mul_f824(rot_b,rot_b) >> 16) +
+ (1L<<8) );
+ fact_r += 1; /* To counter rounding operations */
+ fact_r += 13; /* Add 10% margin */
+
+ /* First point is at the relative position (r;0) */
+ x = ((r * fact_r) >> 8);
+ y = 0;
+
+ /* Compute obstacle points positions around a circle */
+ for (j = 0; j<num; j++)
+ {
+ /* Compute the point absolute position */
+ points[points_nb].x = c.x + (vect_value_t)x;
+ points[points_nb].y = c.y + (vect_value_t)y;
+
+ /* Check it is in playground */
+ if (points[points_nb].x >= border_xmin
+ && points[points_nb].y >= border_ymin
+ && points[points_nb].x <= border_xmax
+ && points[points_nb].y <= border_ymax)
+ {
+ /* Accept point */
+ DPRINTF("Add point %u (%u;%u)\n", points_nb, points[points_nb].x, points[points_nb].y);
+ points_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;
+ }
+
+#ifdef HOST
+ // Plot obstacle points
+ robot->hardware.simu_report.pos( &points[PATH_FIXED_POINTS_NB], points_nb-PATH_FIXED_POINTS_NB, PATH_PLOT_ID);
+#if 0
+ // Draw the last obstacle
+ points[points_nb] = points[points_nb - num];
+ robot->hardware.simu_report.path( &points[points_nb - num], points_nb + 1);
+#endif
+#endif
+}
+
+void Path::obstacle(int index, const vect_t &c, int r, int factor)
+{
+ add_obstacle(c, r, factor, PATH_OBSTACLES_POINTS_NB);
+}
+
+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);
+ points[PATH_POINT_SRC_IDX] = src;
+ points[PATH_POINT_DST_IDX] = dst;
+}
+
+void Path::compute(int factor)
+{
+ DPRINTF("** Path compute(start) factor=%u\n", factor);
+
+ /* Store the escape factor */
+ escape_factor = factor;
+
+ /* Call the A* algorithm */
+ path_found = (bool)astar(astar_nodes, PATH_NODES_NB, PATH_POINT_DST_IDX, PATH_POINT_SRC_IDX);
+ if (path_found)
+ {
+ /* Store next node to go to */
+ next_node = astar_nodes[0].prev;
+
+#if PATH_DEBUG
+ /* Log and display the path found */
+ vect_t path[PATH_POINTS_NB];
+ uint8_t node = PATH_POINT_SRC_IDX;
+ uint8_t path_nb = 0;
+
+ DPRINTF(">> Path found: ");
+ while(node!=PATH_POINT_DST_IDX)
+ {
+ DPRINTF("%u, ", node);
+ path[path_nb++] = points[node];
+ node = astar_nodes[node].prev;
+ }
+ path[path_nb++] = points[node];
+ robot->hardware.simu_report.path(path, path_nb);
+ DPRINTF("%u\n", node);
+ }
+#endif
+
+ DPRINTF("** Path compute(end) found=%u\n", path_found);
+}
+
+bool Path::get_next(vect_t &p)
+{
+ if (path_found)
+ {
+ p = points[next_node];
+ next_node = astar_nodes[next_node].prev;
+ }
+ return path_found;
+}
+
+vect_t& Path::get_point(uint8_t index)
+{
+ return points[index];
+}
+
+uint8_t Path::find_neighbors(uint8_t cur_point, struct astar_neighbor_t *neighbors)
+{
+ uint8_t i, j;
+ uint8_t neighbors_nb = 0;
+
+ /* Parse all nodes */
+ for(i=0; i<points_nb; i++)
+ {
+ /* Except the current one */
+ if (i!=cur_point)
+ {
+ /* Get segment length */
+ uint32_t weight = distance_point_point(&points[cur_point], &points[i]);
+ DPRINTF("- Node %u (%u;%u) w=%u ", i, points[i].x, points[i].y, weight);
+
+ /* Check every obstacle */
+ for(j=0; j<obstacles_nb; j++)
+ {
+ // Check for intersection with obstacle
+ uint16_t d = distance_segment_point(&points[cur_point], &points[i], &obstacles[j].c);
+ if (d < obstacles[j].r)
+ {
+ /* Collision: apply the escape factor if node is the source point, invalidate node otherwise */
+ weight *= (i==PATH_POINT_SRC_IDX || cur_point==PATH_POINT_DST_IDX ? escape_factor : 0);
+ DPRINTF("in collision with c=(%u;%u) r=%u w=%u ",
+ obstacles[j].c.x, obstacles[j].c.y, obstacles[j].r, weight);
+ }
+ }
+
+ // Add neighbor if valid
+ if (weight)
+ {
+ DPRINTF("=> validated w=%u\n", weight);
+ neighbors[neighbors_nb].node = i;
+ neighbors[neighbors_nb].weight = (uint16_t)UTILS_MIN(weight,0xFFFF);
+ neighbors_nb++;
+ }
+ else
+ {
+ DPRINTF("=> skipped\n");
+ }
+ }
+ }
+
+#if PATH_DEBUG
+ DPRINTF("\tFound %u neighbors: ", neighbors_nb);
+ for(i=0;i<neighbors_nb;i++)
+ DPRINTF("%u (%u) ", neighbors[i].node, neighbors[i].weight);
+ DPRINTF("\n");
+#endif
+
+ return neighbors_nb;
}
extern "C" uint8_t
AC_ASTAR_NEIGHBOR_CALLBACK (uint8_t node, struct astar_neighbor_t *neighbors)
{
- // TODO: dummy implementation.
- return 0;
+#if PATH_DEBUG
+ vect_t node_v = robot->path.get_point(node);
+ DPRINTF("AC_ASTAR_NEIGHBOR_CALLBACK node=%u (%u;%u)\n", node, node_v.x, node_v.y);
+#endif
+ return robot->path.find_neighbors(node, neighbors);
}
extern "C" uint16_t
AC_ASTAR_HEURISTIC_CALLBACK (uint8_t node)
{
- // TODO: dummy implementation.
- return 0;
+ vect_t node_a = robot->path.get_point(node);
+ vect_t node_b = robot->path.get_point(PATH_POINT_SRC_IDX);
+ int16_t dist = distance_point_point(&node_a, &node_b);
+ DPRINTF("Heuristic node=%u dist=%u\n", node, dist);
+ return dist;
}
diff --git a/digital/io-hub/src/common-cc/path.hh b/digital/io-hub/src/common-cc/path.hh
index 3c281e46..19533593 100644
--- a/digital/io-hub/src/common-cc/path.hh
+++ b/digital/io-hub/src/common-cc/path.hh
@@ -25,30 +25,112 @@
// }}}
#include "defs.hh"
-/// Path finding class.
-/// TODO: dummy implementation.
+extern "C" {
+#include "modules/path/astar/astar.h"
+}
+
+/** Number of possible obstacles. */
+#define PATH_OBSTACLES_NB (4+1/*cake*/)
+
+/** Number of points for the cake */
+#define PATH_CAKE_POINTS_NB 10
+
+/** Number of points per standard obstacle. */
+#define PATH_OBSTACLES_POINTS_NB 6
+
+#define PATH_FIXED_POINTS_NB 2
+
+/** Number of points. */
+#define PATH_POINTS_NB (PATH_FIXED_POINTS_NB + \
+ (PATH_OBSTACLES_NB * PATH_OBSTACLES_POINTS_NB) + \
+ (PATH_CAKE_POINTS_NB - PATH_OBSTACLES_POINTS_NB) + \
+ 1 /*debug*/)
+
+/** Number of nodes. */
+#define PATH_NODES_NB PATH_POINTS_NB
+
+/** Angle between obstacles points. */
+#define PATH_OBSTACLES_POINTS_ANGLE(pOINTS_NB) ((1L << 24) / (pOINTS_NB))
+
+/** Obstacle */
+typedef struct path_obstacle_t
+{
+ /** Center. */
+ vect_t c;
+ /** Radius. */
+ uint16_t r;
+ /** factor */
+ int factor;
+} path_obstacle_t;
+
+// Path finding class.
class Path
{
public:
- /// Reset path computation, remove every obstacles.
- void reset () { }
- /// Set an obstacle position, radius and factor.
- void obstacle (int index, const vect_t &c, int r, int factor = 0) { }
- /// Set path source and destination.
- void endpoints (const vect_t &src, const vect_t &dst) { dst_ = dst; }
- /// Compute path with the given escape factor.
- void compute (int factor = 0) { }
- /// Get next point in computed path, return false if none (no path found
- /// or last point given yet).
- bool get_next (vect_t &p) { p = dst_; return true; }
- /// Prepare score compuration for the given source, with given escape
- /// factor.
+ // Initialise path
+ Path();
+
+ // Reset path computation, remove every obstacles.
+ void reset(void);
+
+ // Add an obstacle on the field
+ void add_obstacle(const vect_t &c, const int r, const int factor, const int points_nb);
+
+ // Set a moving obstacle position, radius and factor.
+ void obstacle(int index, const vect_t &c, int r, int factor = 0);
+
+ // Set path source and destination.
+ void endpoints(const vect_t &src, const vect_t &dst);
+
+ // Compute path with the given escape factor.
+ void compute(int factor = 0);
+
+ // Return a point by index
+ vect_t& get_point(uint8_t index);
+
+ // Get next point in computed path, return false if none (no path found
+ // or last point given yet).
+ bool get_next(vect_t &p);
+
+ //Find all neighbors of a given node, fill the astar_neighbor structure
+ uint8_t find_neighbors(uint8_t node, struct astar_neighbor_t *neighbors);
+
+#if 0
+ // Prepare score computation for the given source, with given escape factor.
void prepare_score (const vect_t &src, int factor = 0);
- /// Return score for a given destination, using a previous preparation
- /// (also reuse previously given escape factor).
+
+ // Return score for a given destination, using a previous preparation
+ // (also reuse previously given escape factor).
int get_score (const vect_t &dst);
+#endif
+
private:
- vect_t dst_;
+
+ /** Borders, any point outside borders is eliminated. */
+ const int16_t border_xmin, border_ymin, border_xmax, border_ymax;
+
+ /** Escape factor, 0 if none. */
+ uint8_t escape_factor;
+
+ /** List of obstacles. */
+ path_obstacle_t obstacles[PATH_OBSTACLES_NB];
+
+ uint8_t obstacles_nb;
+
+ /** List of navigation points */
+ vect_t points[PATH_POINTS_NB];
+
+ /** Number of navigation points */
+ uint8_t points_nb;
+
+ /** List of nodes used for A*. */
+ struct astar_node_t astar_nodes[PATH_NODES_NB];
+
+ /** Which node to look at for next step. */
+ uint8_t next_node;
+
+ /** TRUE when a path has been found */
+ bool path_found;
};
#endif // path_hh