summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Schodet2011-05-30 23:42:52 +0200
committerNicolas Schodet2011-05-30 23:42:52 +0200
commit850cc292ac7ea847f6be2afcdc5bae89418cabda (patch)
tree7d943f8700fbc7db79fe773662f75b615673f987
parent683d37717c83cd8ec8b752c194cb8c0bbc19af6d (diff)
digital/io-hub: add path finding
-rw-r--r--digital/io-hub/src/robospierre/Makefile2
-rw-r--r--digital/io-hub/src/robospierre/avrconfig.h6
-rw-r--r--digital/io-hub/src/robospierre/path.c398
-rw-r--r--digital/io-hub/src/robospierre/simu.host.c16
-rw-r--r--digital/io-hub/tools/io_hub/mex.py19
-rw-r--r--host/simu/robots/robospierre/model/bag.py1
-rw-r--r--host/simu/robots/robospierre/view/bag.py1
7 files changed, 440 insertions, 3 deletions
diff --git a/digital/io-hub/src/robospierre/Makefile b/digital/io-hub/src/robospierre/Makefile
index 43970369..714541dc 100644
--- a/digital/io-hub/src/robospierre/Makefile
+++ b/digital/io-hub/src/robospierre/Makefile
@@ -16,7 +16,7 @@ test_element_SOURCES = test_element.c logistic.c element.c
# Modules needed for IO.
MODULES = proto uart twi utils \
adc devices/usdist \
- math/fixed math/geometry
+ math/fixed math/geometry path/astar
AI_MODULES = twi_master common utils fsm move
test_element_MODULES = host math/fixed math/geometry
# Configuration file.
diff --git a/digital/io-hub/src/robospierre/avrconfig.h b/digital/io-hub/src/robospierre/avrconfig.h
index c3f107c5..581fa136 100644
--- a/digital/io-hub/src/robospierre/avrconfig.h
+++ b/digital/io-hub/src/robospierre/avrconfig.h
@@ -120,6 +120,12 @@
/** Number of possible obstacles. */
#define AC_PATH_OBSTACLES_NB 2
+/* astar - A* path finding module. */
+/** Neighbor callback. */
+#define AC_ASTAR_NEIGHBOR_CALLBACK path_astar_neighbor_callback
+/** Heuristic callback. */
+#define AC_ASTAR_HEURISTIC_CALLBACK path_astar_heuristic_callback
+
/* io-hub - io/ai board. */
/** TWI address of the io board. */
#define AC_IO_TWI_ADDRESS 10
diff --git a/digital/io-hub/src/robospierre/path.c b/digital/io-hub/src/robospierre/path.c
index b3c91ac8..1122c684 100644
--- a/digital/io-hub/src/robospierre/path.c
+++ b/digital/io-hub/src/robospierre/path.c
@@ -23,18 +23,235 @@
*
* }}} */
#include "common.h"
+#include "defs.h"
#include "path.h"
+#include "bot.h"
+#include "playground.h"
+
+#include "modules/path/astar/astar.h"
+#include "modules/utils/utils.h"
+#include "modules/math/geometry/distance.h"
+
+#define PATH_DEBUG 0
+
+#if PATH_DEBUG
+#include "debug.host.h"
+#endif
+
+/**
+ * This year, due to the large number of obstacles, a grid like structure is
+ * used for path finding on the playground. The A* algorithm is used to find
+ * path along nodes.
+ *
+ * The grid is composed of 11 columns of 4 node each. They are numbered by
+ * column. Even columns are aligned with center of squares, while odd columns
+ * are at squares intersections. Therefore, odd columns have a offset of
+ * 352/2 mm, and that is the reason why code should handle odd and even
+ * columns differently.
+ *
+ * All those tricks are used to reduce the number of nodes.
+ */
+
+/** Number of possible obstacles. */
+#define PATH_OBSTACLES_NB AC_PATH_OBSTACLES_NB
+
+/** Number of nodes in a column. */
+#define PATH_COLUMN_NODES_NB 4
+
+/** Number of columns. */
+#define PATH_COLUMNS_NB 11
+
+/** Number of nodes in the grid. */
+#define PATH_GRID_NODES_NB (PATH_COLUMNS_NB * PATH_COLUMN_NODES_NB)
+
+/** Number of nodes in search graph, last two nodes are destination and source
+ * nodes. */
+#define PATH_NODES_NB (PATH_GRID_NODES_NB + 2)
+
+/** Index of destination node. */
+#define PATH_DST_NODE_INDEX PATH_GRID_NODES_NB
+
+/** Index of source node. */
+#define PATH_SRC_NODE_INDEX (PATH_DST_NODE_INDEX + 1)
+
+/** Information on a node. */
+struct path_node_t
+{
+ /** Whether this node can be used. */
+ uint8_t usable;
+};
/** Context. */
struct path_t
{
+ /** List of obstacles. */
+ struct path_obstacle_t obstacles[PATH_OBSTACLES_NB];
+ /** Escape factor, 0 if none. */
+ uint8_t escape_factor;
+ /** List of nodes used for A*. */
+ struct astar_node_t astar_nodes[PATH_NODES_NB];
+ /** Cache of whether a node is blocked. */
+ uint8_t valid[PATH_GRID_NODES_NB];
/** Position of end points. */
vect_t endpoints[2];
/** Whether the last update was a success. */
uint8_t found;
+ /** Which node to look at for next step. */
+ uint8_t get;
};
static struct path_t path;
+/** Static information on nodes. */
+static const struct path_node_t path_nodes[PATH_GRID_NODES_NB] = {
+ /* {{{ */
+ { 1 }, /* 0 column 0. */
+ { 1 }, /* 1 */
+ { 1 }, /* 2 */
+ { 1 }, /* 3 */
+ { 1 }, /* 4 column 1. */
+ { 1 }, /* 5 */
+ { 1 }, /* 6 */
+ { 1 }, /* 7 */
+ { 1 }, /* 8 column 2. */
+ { 1 }, /* 9 */
+ { 1 }, /* 10 */
+ { 1 }, /* 11 */
+ { 1 }, /* 12 column 3. */
+ { 1 }, /* 13 */
+ { 1 }, /* 14 */
+ { 1 }, /* 15 */
+ { 1 }, /* 16 column 4. */
+ { 1 }, /* 17 */
+ { 1 }, /* 18 */
+ { 1 }, /* 19 */
+ { 1 }, /* 20 column 5. */
+ { 1 }, /* 21 */
+ { 1 }, /* 22 */
+ { 1 }, /* 23 */
+ { 1 }, /* 24 column 6. */
+ { 1 }, /* 25 */
+ { 1 }, /* 26 */
+ { 1 }, /* 27 */
+ { 1 }, /* 28 column 7. */
+ { 1 }, /* 29 */
+ { 1 }, /* 30 */
+ { 1 }, /* 31 */
+ { 1 }, /* 32 column 8. */
+ { 1 }, /* 33 */
+ { 1 }, /* 34 */
+ { 1 }, /* 35 */
+ { 1 }, /* 36 column 9. */
+ { 1 }, /* 37 */
+ { 1 }, /* 38 */
+ { 1 }, /* 39 */
+ { 1 }, /* 40 column 10. */
+ { 1 }, /* 41 */
+ { 1 }, /* 42 */
+ { 1 }, /* 43 */
+ /* }}} */
+};
+
+/** Compute position of a node. */
+static void
+path_pos (uint8_t node, vect_t *pos)
+{
+ assert (node < PATH_NODES_NB);
+ if (node < PATH_GRID_NODES_NB)
+ {
+ uint8_t col = node / PATH_COLUMN_NODES_NB;
+ uint8_t line = node - col * PATH_COLUMN_NODES_NB;
+ pos->x = 400 + 50 + 350 / 2 + col * 350 / 2;
+ pos->y = 2100 - 350 - 350 / 2
+ + (col % 2 ? 350 / 2 : 0)
+ - line * 350;
+ }
+ else
+ {
+ *pos = path.endpoints[node - PATH_GRID_NODES_NB];
+ }
+}
+
+/** Return 1 if the direct path between a and b nodes is blocked, also compute
+ * distance. */
+static uint8_t
+path_blocking (uint8_t a, uint8_t b, int16_t *dp)
+{
+ uint8_t i;
+ vect_t va;
+ vect_t vb;
+ uint8_t escape_factor = 0;
+ if (a == PATH_SRC_NODE_INDEX || b == PATH_SRC_NODE_INDEX)
+ escape_factor = path.escape_factor;
+ path_pos (a, &va);
+ path_pos (b, &vb);
+ /* Test for a blocking obstacle. */
+ for (i = 0; i < PATH_OBSTACLES_NB; i++)
+ {
+ if (path.obstacles[i].valid)
+ {
+ uint16_t d = distance_segment_point (&va, &vb,
+ &path.obstacles[i].c);
+ if (d < path.obstacles[i].r)
+ {
+ if (escape_factor)
+ {
+ int16_t d = distance_point_point (&va, &vb);
+ *dp = d * escape_factor;
+ return 0;
+ }
+ else
+ return 1;
+ }
+ }
+ }
+ /* Compute distance. */
+ int16_t d = distance_point_point (&va, &vb);
+ if (d == 0)
+ {
+ *dp = 0;
+ return 0;
+ }
+ /* No blocking. */
+ *dp = d;
+ return 0;
+}
+
+/** Update the cache of blocked nodes. */
+static void
+path_blocked_update (void)
+{
+ uint8_t i, j;
+ for (i = 0; i < PATH_GRID_NODES_NB; i++)
+ {
+ uint8_t valid = 1;
+ /* First, gather information from tables. */
+ if (!path_nodes[i].usable)
+ valid = 0;
+ else
+ {
+ vect_t pos;
+ path_pos (i, &pos);
+ /* Then, test for obstacles. */
+ for (j = 0; j < PATH_OBSTACLES_NB; j++)
+ {
+ if (path.obstacles[j].valid)
+ {
+ vect_t v = pos; vect_sub (&v, &path.obstacles[j].c);
+ uint32_t dsq = vect_dot_product (&v, &v);
+ uint32_t r = path.obstacles[j].r;
+ if (dsq <= r * r)
+ {
+ valid = 0;
+ break;
+ }
+ }
+ }
+ }
+ /* Update cache. */
+ path.valid[i] = valid;
+ }
+}
+
void
path_init (void)
{
@@ -50,23 +267,51 @@ path_endpoints (vect_t s, vect_t d)
void
path_escape (uint8_t factor)
{
+ path.escape_factor = factor;
}
void
path_obstacle (uint8_t i, vect_t c, uint16_t r, uint8_t factor,
uint16_t valid)
{
+ assert (i < AC_PATH_OBSTACLES_NB);
+ assert (factor == 0);
+ path.obstacles[i].c = c;
+ path.obstacles[i].r = r;
+ path.obstacles[i].valid = valid;
}
void
path_decay (void)
{
+ uint8_t i;
+ for (i = 0; i < PATH_OBSTACLES_NB; i++)
+ {
+ if (path.obstacles[i].valid
+ && path.obstacles[i].valid != PATH_OBSTACLE_VALID_ALWAYS)
+ path.obstacles[i].valid--;
+ }
}
void
path_update (void)
{
- path.found = 1;
+ path_blocked_update ();
+ path.found = astar (path.astar_nodes, PATH_NODES_NB, PATH_DST_NODE_INDEX,
+ PATH_SRC_NODE_INDEX);
+ path.get = PATH_SRC_NODE_INDEX;
+#if AC_PATH_REPORT
+ if (path.found)
+ {
+ uint8_t n, len = 0;
+ vect_t points[PATH_NODES_NB];
+ for (n = path.get; n != PATH_DST_NODE_INDEX; n = path.astar_nodes[n].prev)
+ path_pos (n, &points[len++]);
+ path_pos (n, &points[len++]);
+ AC_PATH_REPORT_CALLBACK (points, len, path.obstacles,
+ PATH_OBSTACLES_NB);
+ }
+#endif
}
uint8_t
@@ -74,10 +319,159 @@ path_get_next (vect_t *p)
{
if (path.found)
{
- *p = path.endpoints[0];
+ assert (path.get != PATH_DST_NODE_INDEX);
+ uint8_t prev = path.get;
+ vect_t pp;
+ path_pos (prev, &pp);
+ uint8_t next = path.astar_nodes[path.get].prev;
+ path.get = next;
+ path_pos (next, p);
+ while (next != 0xff)
+ {
+ /* Try to remove useless points. */
+ uint8_t next = path.astar_nodes[path.get].prev;
+ if (next == 0xff || next == PATH_DST_NODE_INDEX)
+ break;
+ vect_t np;
+ path_pos (next, &np);
+ vect_t vnp = np; vect_sub (&vnp, &pp);
+ vect_t vp = *p; vect_sub (&vp, &pp);
+ if (vect_normal_dot_product (&vp, &vnp) == 0)
+ {
+ path.get = next;
+ *p = np;
+ }
+ else
+ break;
+ }
return 1;
}
else
return 0;
}
+/** Neighbors callback for nodes in grid. */
+static uint8_t
+path_astar_neighbor_callback_grid (uint8_t node,
+ struct astar_neighbor_t *neighbors)
+{
+ uint8_t neighbors_nb = 0;
+ uint8_t i;
+ /* Add neighbors in all 8 directions. */
+ static const struct {
+ /** Column offset of this neighbor. */
+ int8_t column_offset;
+ /** Line offset of this neighbor, for even columns. */
+ int8_t even_line_offset;
+ /** Line offset for odd columns. */
+ int8_t odd_line_offset;
+ /** Distance to this neighbor. */
+ uint16_t weight;
+ } star_n[] = {
+ { 0, -1, -1, 350 }, /* N */
+ { -1, 0, -1, 248 }, /* NW */
+ { -2, 0, 0, 350 }, /* W */
+ { -1, 1, 0, 248 }, /* SW */
+ { 0, 1, 1, 350 }, /* S */
+ { 1, 1, 0, 248 }, /* SE */
+ { 2, 0, 0, 350 }, /* E */
+ { 1, 0, -1, 248 }, /* NE */
+ };
+ uint8_t col = node / PATH_COLUMN_NODES_NB;
+ uint8_t line = node - col * PATH_COLUMN_NODES_NB;
+ uint8_t odd = col % 2;
+ for (i = 0; i < UTILS_COUNT (star_n); i++)
+ {
+ int8_t new_col = col + star_n[i].column_offset;
+ int8_t new_line = line + (odd ? star_n[i].odd_line_offset
+ : star_n[i].even_line_offset);
+ int8_t new_node = new_col * PATH_COLUMN_NODES_NB + new_line;
+ if (new_col >= 0 && new_col < PATH_COLUMNS_NB
+ && new_line >= 0 && new_line < PATH_COLUMN_NODES_NB)
+ {
+ uint8_t valid = path.valid[new_node];
+ if (valid)
+ {
+ neighbors[neighbors_nb].node = new_node;
+ neighbors[neighbors_nb].weight = star_n[i].weight + 1;
+ neighbors_nb++;
+ }
+ }
+ }
+ /* Check if direct path OK. */
+ int16_t d;
+ if (!path_blocking (node, PATH_SRC_NODE_INDEX, &d))
+ {
+ /* Add this neighbor. */
+ neighbors[neighbors_nb].node = PATH_SRC_NODE_INDEX;
+ neighbors[neighbors_nb].weight = d + 1;
+ neighbors_nb++;
+ }
+#if PATH_DEBUG
+ for (i = 0; i < neighbors_nb; i++)
+ DPRINTF (" n %d %d\n", neighbors[i].node, neighbors[i].weight);
+#endif
+ return neighbors_nb;
+}
+
+/** Neighbors callback for endpoints. */
+static uint8_t
+path_astar_neighbor_callback_endpoints (uint8_t node,
+ struct astar_neighbor_t *neighbors)
+{
+ uint8_t neighbors_nb = 0;
+ uint8_t i;
+ assert (node == PATH_DST_NODE_INDEX);
+ /* Select neighbors in the grid. */
+ for (i = 0; i < PATH_GRID_NODES_NB; i++)
+ {
+ /* Discard blocking nodes. */
+ if (!path.valid[i])
+ continue;
+ /* Check if there is an obstacle along the path. */
+ int16_t d;
+ if (path_blocking (PATH_DST_NODE_INDEX, i, &d))
+ continue;
+ /* Add this neighbor. */
+ neighbors[neighbors_nb].node = i;
+ neighbors[neighbors_nb].weight = d + 1;
+ neighbors_nb++;
+ }
+ /* Check if direct path OK. */
+ int16_t d;
+ if (!path_blocking (PATH_DST_NODE_INDEX, PATH_SRC_NODE_INDEX, &d))
+ {
+ /* Add this neighbor. */
+ neighbors[neighbors_nb].node = PATH_SRC_NODE_INDEX;
+ neighbors[neighbors_nb].weight = d + 1;
+ neighbors_nb++;
+ }
+#if PATH_DEBUG
+ for (i = 0; i < neighbors_nb; i++)
+ DPRINTF (" n %d %d\n", neighbors[i].node, neighbors[i].weight);
+#endif
+ return neighbors_nb;
+}
+
+uint8_t
+path_astar_neighbor_callback (uint8_t node,
+ struct astar_neighbor_t *neighbors)
+{
+#if PATH_DEBUG
+ DPRINTF ("neighbor %d\n", node);
+#endif
+ if (node < PATH_GRID_NODES_NB)
+ return path_astar_neighbor_callback_grid (node, neighbors);
+ else
+ return path_astar_neighbor_callback_endpoints (node, neighbors);
+}
+
+uint16_t
+path_astar_heuristic_callback (uint8_t node)
+{
+ /* TODO: a better and faster heuristic can be found, considering that
+ * movement is only allowed on the grid. */
+ vect_t pos;
+ path_pos (node, &pos);
+ return distance_point_point (&pos, &path.endpoints[0]);
+}
diff --git a/digital/io-hub/src/robospierre/simu.host.c b/digital/io-hub/src/robospierre/simu.host.c
index 7ca7364b..70055d7d 100644
--- a/digital/io-hub/src/robospierre/simu.host.c
+++ b/digital/io-hub/src/robospierre/simu.host.c
@@ -29,6 +29,7 @@
#include "modules/host/host.h"
#include "modules/host/mex.h"
#include "modules/adc/adc.h"
+#include "modules/path/path.h"
#include "io.h"
/** AVR registers. */
@@ -36,6 +37,7 @@ uint8_t PORTA, DDRA, PINA, PINE, PINF;
/** Message types. */
uint8_t simu_mex_pos_report;
+uint8_t simu_mex_path;
static void
simu_adc_handle (void *user, mex_msg_t *msg)
@@ -56,6 +58,7 @@ simu_init (void)
uint8_t mtype = mex_node_reservef ("%s:adc", mex_instance);
mex_node_register (mtype, simu_adc_handle, 0);
simu_mex_pos_report = mex_node_reservef ("%s:pos-report", mex_instance);
+ simu_mex_path = mex_node_reservef ("%s:path", mex_instance);
}
/** Make a simulation step. */
@@ -78,6 +81,19 @@ timer_wait (void)
return 0;
}
+/** Send computed path. */
+void
+simu_send_path (vect_t *points, uint8_t len,
+ struct path_obstacle_t *obstacles, uint8_t obstacles_nb)
+{
+ int i;
+ mex_msg_t *m;
+ m = mex_msg_new (simu_mex_path);
+ for (i = 0; i < len; i++)
+ mex_msg_push (m, "hh", points[i].x, points[i].y);
+ mex_node_send (m);
+}
+
void
simu_send_pos_report (vect_t *pos, uint8_t pos_nb, uint8_t id)
{
diff --git a/digital/io-hub/tools/io_hub/mex.py b/digital/io-hub/tools/io_hub/mex.py
index 66060ea1..8d758382 100644
--- a/digital/io-hub/tools/io_hub/mex.py
+++ b/digital/io-hub/tools/io_hub/mex.py
@@ -121,6 +121,24 @@ class Mex:
m.push ('L', self.contacts)
self.node.send (m)
+ class Path (Observable):
+ """Path finding algorithm report.
+
+ - path: sequence of (x, y) coordinates (millimeters).
+
+ """
+
+ def __init__ (self, node, instance):
+ Observable.__init__ (self)
+ self.path = [ ]
+ node.register (instance + ':path', self.__handle)
+
+ def __handle (self, msg):
+ self.path = [ ]
+ while len (msg) >= 4:
+ self.path.append (msg.pop ('hh'))
+ self.notify ()
+
class PosReport (Observable):
"""General purpose position report.
@@ -149,5 +167,6 @@ class Mex:
self.__contact_pack = self.Contact.Pack (node, instance)
self.contact = tuple (self.Contact (self.__contact_pack, i)
for i in range (CONTACT_NB))
+ self.path = self.Path (node, instance)
self.pos_report = self.PosReport (node, instance)
diff --git a/host/simu/robots/robospierre/model/bag.py b/host/simu/robots/robospierre/model/bag.py
index b98dafd9..284e5bb8 100644
--- a/host/simu/robots/robospierre/model/bag.py
+++ b/host/simu/robots/robospierre/model/bag.py
@@ -55,5 +55,6 @@ class Bag:
DistanceSensorSensopart (link_bag.io_hub.adc[3], scheduler, table,
(-20, 20), pi - pi * 10 / 180, (self.position, ), 2),
]
+ self.path = link_bag.io_hub.path
self.pos_report = link_bag.io_hub.pos_report
diff --git a/host/simu/robots/robospierre/view/bag.py b/host/simu/robots/robospierre/view/bag.py
index b718f6fe..d87cecb9 100644
--- a/host/simu/robots/robospierre/view/bag.py
+++ b/host/simu/robots/robospierre/view/bag.py
@@ -45,5 +45,6 @@ class Bag:
ClampSide.height), model_bag.clamp))
self.distance_sensor = [DistanceSensorUS (self.robot, ds)
for ds in model_bag.distance_sensor]
+ self.path = Path (table, model_bag.path)
self.pos_report = PosReport (table, model_bag.pos_report)