robofsm : map_handling

Remove pointer to map from class robot. Map is now stored in new class MapHandling that represents all functions that updates map.

diff --git a/src/robofsm/map_handling.cc b/src/robofsm/map_handling.cc
index 78cf67f70dffd6caf978ce393e5e6f6bfa0ea41c..ec0430009028c6ea4c6483308444c09c666dfdbd 100644 (file)
--- a/src/robofsm/map_handling.cc
+++ b/src/robofsm/map_handling.cc
@@ -12,21 +12,15 @@
  * Parameters of Obstacle detection
  *******************************************************************************/
 
-const double OBS_SIZE_M = 0.2;  /**< Expected size of detected obstacle  */
-const double IGNORE_CLOSER_THAN_M = 0.2; /**< Do not mark any obstacle closer than this to center of the robot (avoid path planning deadlock) */
-const double IGNORE_FURTHER_THAN_M = 0.5; /**< Ignore data from sharp if further than this */
-const int OBS_FORGET_PERIOD = 100;  /**< The period of thread_obstacle_forgeting [ms] */
-const int OBS_FORGET_SEC = 1;  /**< Time to completely forget detected obstacle. */
-const double OBS_OFFSET = 0.6;
-
-void obstacle_detected_at(double x, double y, bool real_obstacle) {
+MapHandling map_handle;
+
+void MapHandling::obstacle_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)
+       if (!sh_map)
                return;
 
        ShmapPoint2Cell(x, y, &xcell, &ycell, &valid);
@@ -35,12 +29,12 @@ void obstacle_detected_at(double x, double y, bool real_obstacle) {
                return;
 
        /* Ignore obstacles at marked places */
-       if (map->cells[ycell][xcell].flags & MAP_FLAG_IGNORE_OBST)
+       if (sh_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_DET_OBST;
+               sh_map->cells[ycell][xcell].flags |= MAP_FLAG_DET_OBST;
        }
 
        /** Then all the cells arround obstacle cell are set as
@@ -59,21 +53,20 @@ void obstacle_detected_at(double x, double y, bool real_obstacle) {
                        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;
+                               sh_map->cells[j][i].detected_obstacle = MAP_NEW_OBSTACLE;
                        }
                }
        }
 
 }
 
-void figure_detected_at(double x, double y, const bool state) {
+void MapHandling::figure_detected_at(double x, double y, const bool state) {
        int i,j, xcell, ycell;
        struct robot_pos_type est_pos;
-       struct map *map = robot.map;
        double xx, yy;
        bool valid;
 
-       if (!map)
+       if (!sh_map)
                return;
 
        ShmapPoint2Cell(x, y, &xcell, &ycell, &valid);
@@ -82,11 +75,11 @@ void figure_detected_at(double x, double y, const bool state) {
                return;
 
        /* Ignore obstacles at marked places */
-       if (map->cells[ycell][xcell].flags & MAP_FLAG_IGNORE_OBST)
+       if (sh_map->cells[ycell][xcell].flags & MAP_FLAG_IGNORE_OBST)
                return;
 
        if (state) {
-               map->cells[ycell][xcell].flags |= MAP_FLAG_DET_OBST;
+               sh_map->cells[ycell][xcell].flags |= MAP_FLAG_DET_OBST;
 
                /* Mark "protected" area around the obstacle */
                robot.get_est_pos(est_pos.x, est_pos.y, est_pos.phi);
@@ -98,7 +91,7 @@ void figure_detected_at(double x, double y, const bool state) {
                                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;
+                                   sh_map->cells[j][i].detected_obstacle = MAP_NEW_OBSTACLE;
                                }
                        }
                }
@@ -116,7 +109,7 @@ void figure_detected_at(double x, double y, const bool state) {
  *
  * @return
  */
-void obst_coord(struct robot_pos_type *e, const sharp_pos  *s, double v, double &x, double &y) {
+void MapHandling::obst_coord(struct robot_pos_type *e, const sharp_pos  *s, double v, double &x, double &y) {
        double sx, sy, sa;
        sx = e->x + s->x*cos(e->phi) - s->y*sin(e->phi);
        sy = e->y + s->x*sin(e->phi) + s->y*cos(e->phi);
@@ -126,7 +119,7 @@ void obst_coord(struct robot_pos_type *e, const sharp_pos  *s, double v, double
        y = sy+v*sin(sa);
 }
 
-void get_checkerboard(std::vector<Shape_detect::Point> &team) {
+void MapHandling::get_checkerboard(std::vector<Shape_detect::Point> &team) {
        Shape_detect::Point tmp, newPoint, start;
        unsigned int i;
 
@@ -180,11 +173,7 @@ void get_checkerboard(std::vector<Shape_detect::Point> &team) {
        team.push_back(tmp);
 }
 
-inline float point_distance(Shape_detect::Point a, Shape_detect::Point b) {
-       return sqrt((a.x-b.x)*(a.x-b.x)+(a.y-b.y)*(a.y-b.y));
-}
-
-void update_map_hokuyo(struct hokuyo_scan_type *s) {
+void MapHandling::update_map_hokuyo(struct hokuyo_scan_type *s) {
        double x, y;
        struct robot_pos_type e;
        unsigned int i;
@@ -291,7 +280,7 @@ void update_map_hokuyo(struct hokuyo_scan_type *s) {
  */
 static void forget_obstacles(map_cell_detobst_t val) {
        int i,j;
-       struct map *map = robot.map;
+       struct map *map = map_handle.get_map();
 
         for (j=0;j<MAP_HEIGHT;j++){
                 for(i=0;i<MAP_WIDTH;i++){

diff --git a/src/robofsm/map_handling.h b/src/robofsm/map_handling.h
index 8383753f83344439898f9fe7406ba013b2ff0ee5..1b51f6fc2c7b80a3710f64658de02792184d1268 100644 (file)
--- a/src/robofsm/map_handling.h
+++ b/src/robofsm/map_handling.h
@@ -1,18 +1,43 @@
 #ifndef _MAP_HANDLING_H
 #define _MAP_HANDLING_H
 
+#include <cmath>
+#include <robot.h>
 #include <robodim.h>
+#include <map.h>
+#include <robomath.h>
+#include <hokuyo.h>
 
-#ifdef __cplusplus
-extern "C" {
-#endif
+#include <shape_detect.h>
 
-void * thread_obstacle_forgeting(void * arg);
-/*void update_map(struct sharps_type *s);*/
-void update_map_hokuyo(struct hokuyo_scan_type *s);
+const double OBS_SIZE_M = 0.2;  /**< Expected size of detected obstacle  */
+const double IGNORE_CLOSER_THAN_M = 0.2; /**< Do not mark any obstacle closer than this to center of the robot (avoid path planning deadlock) */
+const double IGNORE_FURTHER_THAN_M = 0.5; /**< Ignore data from sharp if further than this */
+const int OBS_FORGET_PERIOD = 100;  /**< The period of thread_obstacle_forgeting [ms] */
+const int OBS_FORGET_SEC = 1;  /**< Time to completely forget detected obstacle. */
+const double OBS_OFFSET = 0.6;
 
-#ifdef __cplusplus
-}
-#endif
+class MapHandling{
+       
+       map *sh_map;
+       void obstacle_detected_at(double x, double y, bool real_obstacle);
+       void figure_detected_at(double x, double y, const bool state);
+       void obst_coord(struct robot_pos_type *e, const sharp_pos  *s, double v, double &x, double &y);
+       void get_checkerboard(std::vector<Shape_detect::Point> &team);
+       float point_distance(Shape_detect::Point a, Shape_detect::Point b) {
+               return sqrt((a.x-b.x)*(a.x-b.x)+(a.y-b.y)*(a.y-b.y));
+       }
+       
+       public:
+       void update_map_hokuyo(struct hokuyo_scan_type *s);
+       map* get_map() {
+               return sh_map;
+       }
+       void set_map(map *map_shared) {
+               sh_map = map_shared;
+       }
+};
+extern MapHandling map_handle;
+void * thread_obstacle_forgeting(void * arg); 
 
 #endif

diff --git a/src/robofsm/motion-control.cc b/src/robofsm/motion-control.cc
index 668ec1a85b5c95a94bd0bc66a4547901eea80d4e..faee759075f7f1a87aba3f3788ec49f155ffc61c 100644 (file)
--- a/src/robofsm/motion-control.cc
+++ b/src/robofsm/motion-control.cc
@@ -34,6 +34,7 @@
 #include <robomath.h>
 #include <ul_log.h>
 #include "events.h"
+#include <map_handling.h>
 
 UL_LOG_CUST(ulogd_motion_control); /* Log domain name = ulogd + name of the file */
 
@@ -145,7 +146,7 @@ static void notify_fsm(bool done, double error) {
 
 static void check_for_collision_in_future(Trajectory *traj, double current_time) {
        Pos future_pos;
-       struct map *map = robot.map;
+       struct map *map = map_handle.get_map();
        int xcell, ycell;
        double x, y;
        bool valid;

diff --git a/src/robofsm/robot.cc b/src/robofsm/robot.cc
index b56af5ba5b5f346e22b198a307685db8c774c6b2..6090fed133f6a070e0054ff4d72798bb1ac393d7 100644 (file)
--- a/src/robofsm/robot.cc
+++ b/src/robofsm/robot.cc
@@ -12,11 +12,11 @@
 #include <sys/time.h>
 #include <time.h>
 #include <unistd.h>
-#include "map_handling.h"
 #include <string.h>
 #include <robodim.h>
 #include <ul_log.h>
 #include "fsmmove.cc"
+#include "map_handling.h"
 
 
 /* Subtract the `struct timespec' values X and Y,
@@ -104,7 +104,7 @@ int Robot::init() {
 
        ignore_hokuyo = false;
        
-       map = ShmapInit(1);
+       map_handle.set_map(ShmapInit(1));
 //     fill_in_known_areas_in_map();
 
        signal(SIGINT, int_handler);
@@ -190,7 +190,7 @@ err:
  * Signals all the robot threads to finish.
  */
 void Robot::robot_exit() {
-       destroy();
+       sched.terminate();
 }
 
 

diff --git a/src/robofsm/robot.h b/src/robofsm/robot.h
index ce8aa4ce06fc79db9d3ddd2b4a50e7c239f334fa..4f599e5a8fe5c1723ee389fc1823589de03cb975 100644 (file)
--- a/src/robofsm/robot.h
+++ b/src/robofsm/robot.h
@@ -4,14 +4,13 @@
 #include <stdio.h>
 #include <trgenconstr.h>
 #include <robottype.h>
-#include <robottype.h>
 #include <roboorte_robottype.h>
 #include <robodim.h>
 #include <robot_config.h>
 #include <actuators.h>
 #include <semaphore.h>
-#include "movehelper.h"
-#include "scheduler.hpp"
+#include <movehelper.h>
+#include <scheduler.hpp>
 //#include <ul_log.h>
 #include <boost/statechart/asynchronous_state_machine.hpp>
 
@@ -107,8 +106,6 @@ class Robot {
        hokuyo_scan_type hokuyo;
        bool ignore_hokuyo;
 
-       struct map *map;        /* Map for pathplanning (no locking) */
-
        robot_status status[ROBOT_COMPONENT_NUMBER];
 
        char corns_conf_center;

diff --git a/src/robofsm/robot_orte.cc b/src/robofsm/robot_orte.cc
index 592823c6b2b7ab2ebd14ae752e20fc46e7cf55d4..10e11d02d6ef77addd20b4de53982eeaadbdd875 100644 (file)
--- a/src/robofsm/robot_orte.cc
+++ b/src/robofsm/robot_orte.cc
@@ -328,7 +328,7 @@ void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
                        }
                        if(!robot.ignore_hokuyo)
                        {
-                               update_map_hokuyo(instance);
+                               map_handle.update_map_hokuyo(instance);
                        }
                        break;
                }