summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorNicolas Schodet2009-08-18 01:55:12 +0200
committerNicolas Schodet2009-08-18 01:55:12 +0200
commit3c8372708ae4c9676c7102358b6858117dae2c78 (patch)
tree797c890d61d5ff730c0346fbd6137dea4878bf4c
parent1145f47faf3c4841c89446e6730ecea2eea4c290 (diff)
* digital/avr/modules/path (closes #81):
- added escaping to avoid being blocked if inside an obstacle. - added obstacle factor to avoid a zone.
-rw-r--r--digital/avr/modules/path/path.c29
-rw-r--r--digital/avr/modules/path/path.h15
-rw-r--r--digital/avr/modules/path/path.txt17
-rw-r--r--digital/avr/modules/path/test/test_path.c21
-rw-r--r--digital/avr/modules/path/test/test_path.py24
-rw-r--r--digital/io/src/init_cb.c2
-rw-r--r--digital/io/src/move_cb.c2
7 files changed, 83 insertions, 27 deletions
diff --git a/digital/avr/modules/path/path.c b/digital/avr/modules/path/path.c
index 835fb30f..3b1982cb 100644
--- a/digital/avr/modules/path/path.c
+++ b/digital/avr/modules/path/path.c
@@ -72,6 +72,8 @@ struct path_t
uint32_t margin;
/** List of obstacles. */
struct path_obstacle_t obstacles[PATH_OBSTACLES_NB];
+ /** Escape factor, 0 if none. */
+ uint8_t escape_factor;
/** List of points. First two points are the destination and source
* points. Then comes the obstacles points. */
struct path_point_t points[PATH_POINTS_NB];
@@ -178,7 +180,15 @@ path_compute_weight (uint8_t a, uint8_t b)
- d * d);
if (!((m - f > 0 && m + f > 0 && m - f > ab && m + f > ab)
|| (m - f < 0 && m + f < 0 && m - f < ab && m + f < ab)))
- return 0xffff;
+ {
+ uint8_t factor = path.obstacles[i].factor;
+ if (path.escape_factor
+ && (a == 1 || b == 1)
+ && (a != 0 && b != 0)
+ && (factor == 0u || factor > path.escape_factor))
+ factor = path.escape_factor;
+ return factor == 0 ? 0xffffu : factor * (uint16_t) ab;
+ }
}
}
}
@@ -260,15 +270,25 @@ path_endpoints (int16_t sx, int16_t sy, int16_t dx, int16_t dy)
path.points[1].y = sy;
}
-/** Set up an obstacle at given position with the given radius and validity
- * period. */
+/** Try to escape from inside an obstacle. Bigger factor will shorten path
+ * followed inside the obstacle. Valid until the next update. */
+void
+path_escape (uint8_t factor)
+{
+ path.escape_factor = factor;
+}
+
+/** Set up an obstacle at given position with the given radius, factor and
+ * validity period. */
void
-path_obstacle (uint8_t i, int16_t x, int16_t y, uint16_t r, uint16_t valid)
+path_obstacle (uint8_t i, int16_t x, int16_t y, uint16_t r, uint8_t factor,
+ uint16_t valid)
{
assert (i < AC_PATH_OBSTACLES_NB);
path.obstacles[i].x = x;
path.obstacles[i].y = y;
path.obstacles[i].r = r;
+ path.obstacles[i].factor = factor;
path.obstacles[i].valid = valid;
}
@@ -291,6 +311,7 @@ path_update (void)
{
path_compute_points ();
path_compute_arcs ();
+ path.escape_factor = 0;
path_dijkstra ();
#if AC_PATH_REPORT
uint8_t len, i;
diff --git a/digital/avr/modules/path/path.h b/digital/avr/modules/path/path.h
index e3ec8322..24fc3e42 100644
--- a/digital/avr/modules/path/path.h
+++ b/digital/avr/modules/path/path.h
@@ -35,6 +35,9 @@ struct path_obstacle_t
int16_t x, y;
/** Radius. */
uint16_t r;
+ /** Factor. If not 0, obstacle is not blocking, but it add weight to path
+ * crossing it. Warning: if too big, it can make weight overflow. */
+ uint8_t factor;
/** Validity counter, when this is zero, the obstacle is ignored. */
uint16_t valid;
};
@@ -48,10 +51,16 @@ path_init (int16_t border_xmin, int16_t border_ymin,
void
path_endpoints (int16_t sx, int16_t sy, int16_t dx, int16_t dy);
-/** Set up an obstacle at given position with the given radius and validity
- * period. */
+/** Try to escape from inside an obstacle. Bigger factor will shorten path
+ * followed inside the obstacle. Valid until the next update. */
void
-path_obstacle (uint8_t i, int16_t x, int16_t y, uint16_t r, uint16_t valid);
+path_escape (uint8_t factor);
+
+/** Set up an obstacle at given position with the given radius, factor and
+ * validity period. */
+void
+path_obstacle (uint8_t i, int16_t x, int16_t y, uint16_t r, uint8_t factor,
+ uint16_t valid);
/** Slowly make the obstacles disappear. */
void
diff --git a/digital/avr/modules/path/path.txt b/digital/avr/modules/path/path.txt
index 5c779e23..3868ea77 100644
--- a/digital/avr/modules/path/path.txt
+++ b/digital/avr/modules/path/path.txt
@@ -24,9 +24,13 @@ First initialise the module with the area border (shrunken by the mobile
radius) using ``path_init``.
You can place obstacles using the ``path_obstacle`` function. Give it the
-obstacle index, its position and radius, and its life time. Use a life time
-of PATH_OBSTACLE_VALID_ALWAYS for infinite duration. Call ``path_decay``
-regularly to make obstacles disappear after the given delay.
+obstacle index, its position and radius, its factor and its life time. Use a
+life time of PATH_OBSTACLE_VALID_ALWAYS for infinite duration. Call
+``path_decay`` regularly to make obstacles disappear after the given delay.
+
+If factor is not 0 (2 or more), the obstacle can be entered, but it will cost
+more to do it. This can be used to mark an area which should be avoided, but
+might be entered if other paths are too long or inexistent.
You can also set the source and destination position using the
``path_endpoints`` function.
@@ -40,6 +44,13 @@ This interface only gives you the first point of the path because it is
assumed that the path will not be up to date any more once the mobile arrives
at the first point. This will change in the future.
+Sometime, the source position is located inside an obstacle, for example
+because we were too pessimistic about its size. In this case, the algorithm
+will not find any path. It can be directed to find a path anyway using the
+``path_escape`` function. The factor determines the will to escape quickly
+from the obstacle. If it is 1, the obstacle is completely ignored, bigger
+factors will make the algorithm find shorter path inside the obstacle.
+
When the path is updated, a user callback can be called by the path module to
report the found path. It receives the list of points of the path and the
list of obstacles. The callback is defined in the ``avrconfig.h`` file.
diff --git a/digital/avr/modules/path/test/test_path.c b/digital/avr/modules/path/test/test_path.c
index 87c049d7..218a3183 100644
--- a/digital/avr/modules/path/test/test_path.c
+++ b/digital/avr/modules/path/test/test_path.c
@@ -32,12 +32,14 @@ void
syntax (void)
{
fprintf (stderr,
- "test_path borders source destination [obstacles (0-%d)]\n"
+ "test_path borders source destination escape_factor"
+ " [obstacles (0-%d)]\n"
" borders: xmin,ymin,xmax,ymax\n"
" source, destination: x,y\n"
- " obstacles: x,y,r\n"
- "example: test_path 0,0,1500,1500 300,750 1200,750 600,680,100"
- " 900,820,100\n",
+ " obstacles: x,y,r,f\n"
+ "example: test_path 0,0,1500,1500 300,750 1200,750 0"
+ " 600,680,100,0"
+ " 900,820,100,10\n",
AC_PATH_OBSTACLES_NB);
exit (1);
}
@@ -67,7 +69,7 @@ read_tab (const char *s, int *tab, int n)
int
main (int argc, char **argv)
{
- if (argc < 4 || argc > 4 + AC_PATH_OBSTACLES_NB)
+ if (argc < 5 || argc > 5 + AC_PATH_OBSTACLES_NB)
syntax ();
int tab[4];
read_tab (argv[1], tab, 4);
@@ -75,11 +77,14 @@ main (int argc, char **argv)
read_tab (argv[2], tab, 2);
read_tab (argv[3], tab + 2, 2);
path_endpoints (tab[0], tab[1], tab[2], tab[3]);
+ read_tab (argv[4], tab, 1);
+ if (tab[0])
+ path_escape (tab[0]);
int i;
- for (i = 0; i + 4 < argc; i++)
+ for (i = 0; i + 5 < argc; i++)
{
- read_tab (argv[4 + i], tab, 3);
- path_obstacle (i, tab[0], tab[1], tab[2], 1);
+ read_tab (argv[5 + i], tab, 4);
+ path_obstacle (i, tab[0], tab[1], tab[2], tab[3], 1);
}
path_update ();
path_print_graph ();
diff --git a/digital/avr/modules/path/test/test_path.py b/digital/avr/modules/path/test/test_path.py
index f4654519..58939ff6 100644
--- a/digital/avr/modules/path/test/test_path.py
+++ b/digital/avr/modules/path/test/test_path.py
@@ -29,9 +29,10 @@ from simu.inter.drawable import *
from subprocess import Popen, PIPE
class Obstacle:
- def __init__ (self, pos, radius):
+ def __init__ (self, pos, radius, factor):
self.pos = pos
self.radius = radius
+ self.factor = factor
def move (self, pos):
self.pos = pos
@@ -45,6 +46,7 @@ class Area (Drawable):
self.border = None
self.src = None
self.dst = None
+ self.escape = 0
self.obstacles = [ ]
self.path = [ ]
self.points = { }
@@ -56,7 +58,8 @@ class Area (Drawable):
self.draw_rectangle (self.border_min, self.border_max, fill = 'white')
for o in self.obstacles:
if o.pos is not None:
- self.draw_circle (o.pos, o.radius, fill = 'gray25')
+ self.draw_circle (o.pos, o.radius,
+ fill = o.factor and 'gray50' or 'gray25')
if self.src is not None:
self.draw_circle (self.src, 10, fill = 'green')
if self.dst is not None:
@@ -72,14 +75,14 @@ class Area (Drawable):
def test (self):
self.src = (300, 750)
self.dst = (1200, 750)
- self.obstacles.append (Obstacle ((600, 680), 100))
- self.obstacles.append (Obstacle ((900, 820), 100))
+ self.obstacles.append (Obstacle ((600, 680), 100, 0))
+ self.obstacles.append (Obstacle ((900, 820), 100, 5))
def update (self):
args = [ [ self.border_min[0], self.border_min[1], self.border_max[0],
- self.border_max[1] ], self.src, self.dst ]
+ self.border_max[1] ], self.src, self.dst, (self.escape, ) ]
for o in self.obstacles:
- args.append ([o.pos[0], o.pos[1], o.radius])
+ args.append ([o.pos[0], o.pos[1], o.radius, o.factor])
args = [ ','.join (str (ai) for ai in a) for a in args ]
args[0:0] = [ './test_path.host' ]
p = Popen (args, stdout = PIPE)
@@ -141,6 +144,10 @@ class TestPath (Frame):
variable = self.arcsVar, command = self.arcs_toggle,
text = 'Arcs', indicatoron = True)
self.arcsButton.pack (side = 'top')
+ self.escapeScale = Scale (self.rightFrame, label = 'Escape',
+ orient = 'horizontal', from_ = 0, to = 16,
+ command = self.escape_changed)
+ self.escapeScale.pack (side = 'top')
self.areaview = AreaView (border_min, border_max, self)
self.areaview.pack (expand = True, fill = 'both')
self.areaview.bind ('<1>', self.click)
@@ -183,9 +190,12 @@ class TestPath (Frame):
def arcs_toggle (self):
self.areaview.area.draw_arcs = self.arcsVar.get () != 0
- print self.areaview.area.draw_arcs
self.areaview.area.draw ()
+ def escape_changed (self, value):
+ self.areaview.area.escape = value
+ self.update ()
+
if __name__ == '__main__':
app = TestPath ((0, 0), (1500, 1500))
app.mainloop ()
diff --git a/digital/io/src/init_cb.c b/digital/io/src/init_cb.c
index 349f6fec..d622cd68 100644
--- a/digital/io/src/init_cb.c
+++ b/digital/io/src/init_cb.c
@@ -244,7 +244,7 @@ init__WAIT_SECOND_JACK_OUT__jack_removed_from_bot (void)
/* Start the chrono. */
chrono_init ();
/* Set the central round to path module. */
- path_obstacle (0x01, PG_WIDTH / 2, PG_LENGTH / 2, 350,
+ path_obstacle (0x01, PG_WIDTH / 2, PG_LENGTH / 2, 350, 0,
PATH_OBSTACLE_VALID_ALWAYS);
return init_next (WAIT_SECOND_JACK_OUT, jack_removed_from_bot);
}
diff --git a/digital/io/src/move_cb.c b/digital/io/src/move_cb.c
index f34e441b..28ee70db 100644
--- a/digital/io/src/move_cb.c
+++ b/digital/io/src/move_cb.c
@@ -211,7 +211,7 @@ move_obstacle_here (void)
if (move_obstacle_in_table (move_data.obstacle))
{
path_obstacle (0, move_data.obstacle.x, move_data.obstacle.y,
- MOVE_OBSTACLE_RADIUS, MOVE_OBSTACLE_VALIDITY);
+ MOVE_OBSTACLE_RADIUS, 0, MOVE_OBSTACLE_VALIDITY);
DPRINTF ("Obstacle pos x : %d, pos y : %d\n", move_data.obstacle.x,
move_data.obstacle.y);
TRACE (TRACE_MOVE__OBSTACLE, move_data.obstacle.x,