robofsm: Insert shape_detect for visualization obstacle in map.

diff --git a/src/robofsm/Makefile.omk b/src/robofsm/Makefile.omk
index 924a30d0862030b348ccb4e1e77670c761a505bc..c7795d9f82e0158389fd3908ee27993394e765aa 100644 (file)
--- a/src/robofsm/Makefile.omk
+++ b/src/robofsm/Makefile.omk
@@ -13,7 +13,7 @@ robot_config_DEFINES = CONFIG_LOCK_CHECKING HAVE_PRIO_INHERIT
 # Library with general support functions for the robot
 lib_LIBRARIES += robot
 robot_SOURCES = robot_orte.c robot.c fsmmove.cc movehelper.cc  \
-               motion-control.cc map_handling.c        \
+               motion-control.cc map_handling.cc       \
                match-timing.c
 robot_GEN_SOURCES = roboevent.c
 include_GEN_HEADERS += roboevent.h
@@ -26,7 +26,7 @@ actlib_SOURCES = actuators.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 ulut
+               actlib ulut shape_detect
 
 # Automatic generation of event definition files
 include-pass_HOOKS = roboevent.c roboevent.h

diff --git a/src/robofsm/map_handling.cc b/src/robofsm/map_handling.cc
index 7b01cc113940748dde71fcc6675cef64dcaf89ae..77b54176d1c4ddb1aed28b5d47f260ba4cec2c3b 100644 (file)
--- a/src/robofsm/map_handling.cc
+++ b/src/robofsm/map_handling.cc
@@ -4,6 +4,10 @@
 #include <robomath.h>
 #include <hokuyo.h>
 
+#include <shape_detect.h>
+
+#include "map_handling.h"
+
 /*******************************************************************************
  * Parameters of Obstacle detection
  *******************************************************************************/
@@ -61,6 +65,54 @@ void obstacle_detected_at(double x, double y, bool real_obstacle)
                }
        }
 }
+
+void figure_detected_at(double x, double y, bool real_obstacle)
+{
+       int i,j, xcell, ycell;
+       struct robot_pos_type est_pos;
+       struct map *map = robot.map;
+       double xx, yy;
+       bool valid;
+
+       if (!map)
+               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;
+
+       if (real_obstacle) {
+               /* The obstacle was detected here */
+               map->cells[ycell][xcell].flags |= MAP_FLAG_IGNORE_OBST;
+       }
+
+       /** Then all the cells arround obstacle cell are set as
+        * #MAP_NEW_OBSTACLE. Cells of current robot position are not
+        * set to avoid path planning deadlock. If there are a path
+        * cell between them, the path will be recalculated. @see
+        * #OBS_CSPACE. */
+
+       /* Mark "protected" area around the obstacle */
+       robot_get_est_pos(&est_pos.x, &est_pos.y, &est_pos.phi);
+
+       int obst_size_cell = (int)ceil(OBS_SIZE_M/MAP_CELL_SIZE_M);
+       for (i=(xcell-obst_size_cell); i <= xcell+obst_size_cell; i++) {
+               for (j=(ycell- obst_size_cell); j <=ycell + obst_size_cell; j++) {
+                       if (!ShmapIsCellInMap(i, j)) continue;
+                       ShmapCell2Point(i, j, &xx, &yy);
+                       if ((distance(xx, yy, est_pos.x, est_pos.y) > IGNORE_CLOSER_THAN_M) &&
+                           (distance(xx, yy, x, y) < OBS_SIZE_M)) { /* We expect cirtular area around obstacle */
+                               map->cells[j][i].detected_obstacle = MAP_NEW_OBSTACLE;
+                       }
+               }
+       }
+}
+
 /**
  * A thread running the trajectory recalc
  *
@@ -71,7 +123,6 @@ void obstacle_detected_at(double x, double y, bool real_obstacle)
  *
  * @return
  */
-
 void obst_coord(struct robot_pos_type *e, const struct sharp_pos  *s, double v, double *x, double *y)
 {
        double sx, sy, sa;
@@ -88,10 +139,10 @@ void update_map_hokuyo(struct hokuyo_scan_type *s)
        double x, y;
        //Pos p;
        struct robot_pos_type e;
-       int i;
+       unsigned int i;
        struct sharp_pos beam;
        u_int16_t *data;
-
+       
        robot_get_est_pos(&e.x, &e.y, &e.phi);
 
        beam.x = HOKUYO_CENTER_OFFSET_M;
@@ -99,6 +150,17 @@ void update_map_hokuyo(struct hokuyo_scan_type *s)
 
        data = s->data;
 
+       Shape_detect shapeDet;
+       std::vector<Shape_detect::Arc> arcs;
+
+       shapeDet.prepare(data);
+       shapeDet.arc_detect(arcs);
+
+       for (i = 0; i < arcs.size(); i++) {
+               figure_detected_at(arcs[i].center.x, arcs[i].center.y, true);
+       }
+
+       /*
        for (i = 0; i < HOKUYO_ARRAY_SIZE; i++) {
                beam.ang = HOKUYO_INDEX_TO_RAD(i);
                if((beam.ang<(-HOKUYO_RANGE_ANGLE_LEFT/180.0*M_PI))||((beam.ang>(HOKUYO_RANGE_ANGLE_RIGHT/180.0*M_PI))))
@@ -112,6 +174,7 @@ void update_map_hokuyo(struct hokuyo_scan_type *s)
                }
 
        }
+       */
 }
 
 /**

diff --git a/src/robofsm/map_handling.h b/src/robofsm/map_handling.h
index b45082fff9a0b179754b29cc9599915e32c8899b..8383753f83344439898f9fe7406ba013b2ff0ee5 100644 (file)
--- a/src/robofsm/map_handling.h
+++ b/src/robofsm/map_handling.h
@@ -3,8 +3,16 @@
 
 #include <robodim.h>
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 void * thread_obstacle_forgeting(void * arg);
 /*void update_map(struct sharps_type *s);*/
 void update_map_hokuyo(struct hokuyo_scan_type *s);
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif