robot.c: fill_in_known_areas_in_map(): add all corns to map

diff --git a/src/robofsm/Makefile.omk b/src/robofsm/Makefile.omk
index c0bfdca231d7a6afdb6900df767fa2c376ebb11a..513df044d4f792e43e60d9bc9c5c20c70a26a375 100644 (file)
--- a/src/robofsm/Makefile.omk
+++ b/src/robofsm/Makefile.omk
@@ -31,7 +31,7 @@ cornslib_SOURCES = corns_configs.c
 # Libraries linked to all programs in this Makefile
 lib_LOADLIBES = robot mcl robomath roboorte robottype          \
                pthread rt m orte pathplan sharp map fsm        \
-               rbtree motion robodim actlib
+               rbtree motion robodim actlib cornslib
 
 # Automatic generation of event definition files
 include-pass_HOOKS = roboevent.c roboevent.h

diff --git a/src/robofsm/robot.c b/src/robofsm/robot.c
index 3ea026d33a8220cc224c95bbed5e298871a1436e..fcee589933f8188e2fdaf163f55891e87b0a569e 100644 (file)
--- a/src/robofsm/robot.c
+++ b/src/robofsm/robot.c
@@ -23,6 +23,8 @@
 #include "map_handling.h"
 #include <string.h>
 #include "actuators.h"
+#include "corns_configs.h"
+#include <robodim.h>
 
 #ifdef ROBOT_DEBUG
    #define DBG(format, ...) printf(format, ##__VA_ARGS__)
@@ -60,10 +62,14 @@ static void int_handler(int sig)
 void fill_in_known_areas_in_map()
 {
        /* Do not plan path close to edges */
-       //ShmapSetRectangleFlag(0.0, 0.0, 0.199, 2.1, MAP_FLAG_WALL, 0); /* left */
-       //ShmapSetRectangleFlag(0.0, 0.0, 3.0, 0.199, MAP_FLAG_WALL, 0); /* bottom */
-       //ShmapSetRectangleFlag(0.0, 1.901, 3.0, 2.1, MAP_FLAG_WALL, 0); /* top */
-       //ShmapSetRectangleFlag(2.801, 0.0, 3.0, 2.1, MAP_FLAG_WALL, 0); /* right */
+       ShmapSetRectangleFlag(0.0, 0.0, 0.26, 2.1, MAP_FLAG_WALL, 0); /* left */
+       ShmapSetRectangleFlag(0.0, 0.0, 3.0, 0.26, MAP_FLAG_WALL, 0); /* bottom */
+       ShmapSetRectangleFlag(0.0, 1.84, 3.0, 2.1, MAP_FLAG_WALL, 0); /* top */
+       ShmapSetRectangleFlag(2.74, 0.0, 3.0, 2.1, MAP_FLAG_WALL, 0); /* right */
+
+       /* Container surroundings */
+       ShmapSetRectangleFlag(0.0, 0.0, 0.4, 0.2, 0, MAP_FLAG_WALL); /* left container */
+       ShmapSetRectangleFlag(3.0, 0.0, 2.6, 0.2, 0, MAP_FLAG_WALL); /* right container */
 
        /* Ignore other obstacles at edges */
        //ShmapSetRectangleFlag(0.0, 0.0, 0.09, 2.1, MAP_FLAG_IGNORE_OBST, 0); /* left */
@@ -71,8 +77,16 @@ void fill_in_known_areas_in_map()
        //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 */
 
-       ShmapSetRectangleFlag(0.74, 1.6, 2.259, 2.1, MAP_FLAG_WALL, 0); /* plateau, slopes */
-
+       ShmapSetRectangleFlag(0.6, 1.45, 2.45, 2.1, MAP_FLAG_WALL, 0); /* plateau, slopes */
+       
+       /* corns */
+       struct corns_group *corns = get_all_corns(0, 0);
+       struct corn * corn;
+       for(corn = corns->corns; corn < &corns->corns[corns->corns_count]; corn++) {
+               printf("drawing corn at %.3f, %.3f with radius of %.3f\n", corn->position.x, corn->position.y, CORN_NEIGHBOURHOOD_RADIUS_M);
+               ShmapSetCircleFlag(corn->position.x, corn->position.y, CORN_NEIGHBOURHOOD_RADIUS_M, MAP_FLAG_WALL, 0);
+       }
+       dispose_corns_group(corns); // robot.corns will be set later, when the corns' configuration is known
 }
 
 static void trans_callback(struct robo_fsm *fsm)