From 3fb9771de0461fa14682995f624b4244ccf2bcb3 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 23 Apr 2010 00:33:43 +0200 Subject: digital/io/src, host/simu: US sensors are looking at the beacon only --- digital/io/src/radar.c | 2 +- digital/io/src/radar.h | 6 +++++- digital/io/tools/test_simu.py | 24 ++++++++++++++++++++++-- 3 files changed, 28 insertions(+), 4 deletions(-) (limited to 'digital') diff --git a/digital/io/src/radar.c b/digital/io/src/radar.c index 31da7c58..ca815006 100644 --- a/digital/io/src/radar.c +++ b/digital/io/src/radar.c @@ -98,7 +98,7 @@ radar_update (const position_t *robot_pos, vect_t *obs_pos) robot_pos->a + radar_sensors[i].a); vect_translate (&hit[i], &ray); valid[i] = radar_valid (hit[i]); - vect_from_polar_uf016 (&ray, RADAR_OBSTACLE_RADIUS_MM, + vect_from_polar_uf016 (&ray, RADAR_OBSTACLE_EDGE_RADIUS_MM, robot_pos->a + radar_sensors[i].a); vect_translate (&hit[i], &ray); } diff --git a/digital/io/src/radar.h b/digital/io/src/radar.h index 1e541a28..b4611ec7 100644 --- a/digital/io/src/radar.h +++ b/digital/io/src/radar.h @@ -35,8 +35,12 @@ * - determining if an obstacle should make the robot stop. */ -/** Estimated obstacle radius. As the sensors detect obstacle edge, this is +/** Estimated obstacle edge radius. As the sensors detect obstacle edge, this is * added to position obstacle center. */ +#define RADAR_OBSTACLE_EDGE_RADIUS_MM 40 + +/** Estimated obstacle radius. The obstacle may be larger than at the + * detected edge. */ #define RADAR_OBSTACLE_RADIUS_MM 150 /** Stop distance. Distance under which an obstacle is considered harmful when diff --git a/digital/io/tools/test_simu.py b/digital/io/tools/test_simu.py index 9123569b..8411b0d8 100644 --- a/digital/io/tools/test_simu.py +++ b/digital/io/tools/test_simu.py @@ -45,6 +45,22 @@ import simu.robots.marcel.view.bag as robot_view from simu.inter.inter_node import InterNode from Tkinter import * +class ObstacleWithBeacon (obstacle_view.RoundObstacle): + + def __init__ (self, onto, model, beacon_model): + obstacle_view.RoundObstacle.__init__ (self, onto, model) + self.beacon_model = beacon_model + + def __notified (self): + self.pos = self.model.pos + self.update () + + def draw (self): + obstacle_view.RoundObstacle.draw (self) + if self.pos: + self.draw_circle ((0, 0), self.beacon_model.radius, + fill = '#505050') + class TestSimu (InterNode): """Interface, with simulated programs.""" @@ -75,8 +91,10 @@ class TestSimu (InterNode): self.table = table.Table (self.table_view, self.table_model) self.obstacle = obstacle_model.RoundObstacle (150) self.table_model.obstacles.append (self.obstacle) - self.obstacle_view = obstacle_view.RoundObstacle (self.table, - self.obstacle) + self.obstacle_beacon = obstacle_model.RoundObstacle (40, 2) + self.table_model.obstacles.append (self.obstacle_beacon) + self.obstacle_view = ObstacleWithBeacon (self.table, self.obstacle, + self.obstacle_beacon) self.table_view.bind ('<2>', self.place_obstacle) # Add robot. self.robot_link = robot_link.Bag (self.node) @@ -114,7 +132,9 @@ class TestSimu (InterNode): def place_obstacle (self, ev): pos = self.table_view.screen_coord ((ev.x, ev.y)) self.obstacle.pos = pos + self.obstacle_beacon.pos = pos self.obstacle.notify () + self.obstacle_beacon.notify () if __name__ == '__main__': app = TestSimu (('../../asserv/src/asserv/asserv.host', '-m9', 'marcel'), -- cgit v1.2.3