From f5c72815286cde81af9d1f2a9d0a9aec7e81c41c Mon Sep 17 00:00:00 2001 From: Michal Sojka Date: Sat, 3 May 2008 04:07:26 +0200 Subject: [PATCH] Obstacles can be ignored at some places marked in map --- src/pathplan/map.c | 4 ---- src/pathplan/map.h | 7 ++++--- src/robofsm/eb2008/map_handling.c | 9 +++++++-- src/robofsm/eb2008/robot_eb2008.c | 24 ++++++++++++++++++++++++ src/robomon/src2/RobomonExplorer.cpp | 2 ++ 5 files changed, 37 insertions(+), 9 deletions(-) diff --git a/src/pathplan/map.c b/src/pathplan/map.c index a2aeba3c..1d38c92e 100644 --- a/src/pathplan/map.c +++ b/src/pathplan/map.c @@ -181,11 +181,7 @@ int ShmapSetRectangleFlag(double x1, double y1, double x2, double y2, map_cell_f int i,j, i1, i2, j1, j2; bool valid; ShmapPoint2Cell(x1, y1, &i1, &j1, &valid); - if (!valid) - return 0; ShmapPoint2Cell(x2, y2, &i2, &j2, &valid); - if (!valid) - return 0; if (i1 > i2) { i = i2; i2 = i1; diff --git a/src/pathplan/map.h b/src/pathplan/map.h index c924450c..87802a62 100644 --- a/src/pathplan/map.h +++ b/src/pathplan/map.h @@ -172,12 +172,13 @@ * @name Cell Flags * @{ */ -#define MAP_FLAG_WALL 1 +#define MAP_FLAG_WALL 1 /**< Known wall */ #define MAP_FLAG_PATH 2 #define MAP_FLAG_START 4 #define MAP_FLAG_GOAL 8 -#define MAP_FLAG_DET_OBST 16 -#define MAP_FLAG_SIMULATED_WALL 32 +#define MAP_FLAG_DET_OBST 16 /**< Set when an obstacle is detected, cleard on every "map forget cycle" */ +#define MAP_FLAG_SIMULATED_WALL 32 /**< Used by robomon to simulate obstacles */ +#define MAP_FLAG_IGNORE_OBST 64 /**< If obstacle is detected here, ignore it */ /** @}*/ /** @name Shared Memory macros */ diff --git a/src/robofsm/eb2008/map_handling.c b/src/robofsm/eb2008/map_handling.c index 4abc5476..791b9755 100644 --- a/src/robofsm/eb2008/map_handling.c +++ b/src/robofsm/eb2008/map_handling.c @@ -8,8 +8,8 @@ *******************************************************************************/ #define OBS_SIZE_M 0.2 /**< Expected size of detected obstacle */ -#define IGNORE_CLOSER_THAN_M 0.2 /**< Ignore detected which are closer than this to the robot (avoid path planning deadlock) */ -#define IGNORE_FURTHER_THAN_M 0.6 +#define IGNORE_CLOSER_THAN_M 0.2 /**< Do not mark any obstacle closer than this to center of the robot (avoid path planning deadlock) */ +#define IGNORE_FURTHER_THAN_M 0.5 /**< Ignore data from sharp if further than this */ #define OBS_FORGET_PERIOD 500 /**< The period of thread_obstacle_forgeting [ms] */ #define OBS_FORGET_SEC 5 /**< Time to completely forget detected obstacle. */ #define OBS_OFFSET 0.6 @@ -26,9 +26,14 @@ void obstacle_detected_at(double x, double y) return; ShmapPoint2Cell(x, y, &xcell, &ycell, &valid); + /* Ignore obstacles outside playground */ if (!valid) return; + /* Ignore obstacles at marked places */ + if (map->cells[ycell][xcell].flags & MAP_FLAG_IGNORE_OBST) + return; + /* The obstacle was detected here */ map->cells[ycell][xcell].flags |= MAP_FLAG_DET_OBST; diff --git a/src/robofsm/eb2008/robot_eb2008.c b/src/robofsm/eb2008/robot_eb2008.c index 27f356ed..d99834c4 100644 --- a/src/robofsm/eb2008/robot_eb2008.c +++ b/src/robofsm/eb2008/robot_eb2008.c @@ -63,6 +63,29 @@ static void int_handler(int sig) robot_exit(); } +void fill_in_known_areas_in_map() { + struct map *map = robot.map; + + /* Do not plan path close to edges */ +/* ShmapSetRectangleFlag(0.0, 0.0, 0.199, 2.1, MAP_FLAG_WALL, 0); */ +/* ShmapSetRectangleFlag(0.0, 0.0, 3.0, 0.199, MAP_FLAG_WALL, 0); */ +/* ShmapSetRectangleFlag(0.0, 1.901, 3.0, 2.1, MAP_FLAG_WALL, 0); */ +/* ShmapSetRectangleFlag(2.801, 0.0, 3.0, 2.1, MAP_FLAG_WALL, 0); */ + + + ShmapSetRectangleFlag(0, 1.25, 0.2, 1.45, MAP_FLAG_IGNORE_OBST, 0); /* Left white vert. dispenser */ + ShmapSetRectangleFlag(2.8, 1.25, 3, 1.45, MAP_FLAG_IGNORE_OBST, 0); /* Right white vert. dispenser */ + + ShmapSetRectangleFlag(0.65, 1.9, 0.85, 2.1, MAP_FLAG_IGNORE_OBST, 0); /* Blue vert. dispenser */ + ShmapSetRectangleFlag(2.35, 1.9, 2.55, 2.1, MAP_FLAG_IGNORE_OBST, 0); /* Red vert. dispenser */ + + /* Ignore other obstacles at edges */ + ShmapSetRectangleFlag(0.0, 0.0, 0.09, 2.1, MAP_FLAG_IGNORE_OBST, 0); /* left */ + ShmapSetRectangleFlag(0.0, 0.0, 3.0, 0.09, MAP_FLAG_IGNORE_OBST, 0); /* bottom */ + ShmapSetRectangleFlag(0.0, 2.01, 3.0, 2.1, MAP_FLAG_IGNORE_OBST, 0); /* top */ + ShmapSetRectangleFlag(2.91, 0.0, 3.0, 2.1, MAP_FLAG_IGNORE_OBST, 0); /* right */ +} + /** * Initializes the robot. * Setup fields in robot structure, initializes FSMs and ORTE. @@ -104,6 +127,7 @@ int robot_init() } robot.map = ShmapInit(1); + fill_in_known_areas_in_map(); signal(SIGINT, int_handler); block_signals(); diff --git a/src/robomon/src2/RobomonExplorer.cpp b/src/robomon/src2/RobomonExplorer.cpp index d021e74c..b16adcd0 100644 --- a/src/robomon/src2/RobomonExplorer.cpp +++ b/src/robomon/src2/RobomonExplorer.cpp @@ -733,6 +733,8 @@ void RobomonExplorer::paintMap() color = lightGray; if (cell->flags & MAP_FLAG_WALL) color = darkYellow; + if (cell->flags & MAP_FLAG_IGNORE_OBST) + color = darkGreen; if (cell->flags & MAP_FLAG_SIMULATED_WALL) color = yellow; if (cell->flags & MAP_FLAG_PATH) -- 2.39.2