From: Petr Silhavik Date: Wed, 27 Mar 2013 18:12:56 +0000 (+0100) Subject: robofsm : map_handling X-Git-Url: http://rtime.felk.cvut.cz/gitweb/eurobot/public.git/commitdiff_plain/35d71e781859f9c617d5c8b75ccca0f174c135e7 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 78cf67f7..ec043000 100644 --- 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 &team) { +void MapHandling::get_checkerboard(std::vector &team) { Shape_detect::Point tmp, newPoint, start; unsigned int i; @@ -180,11 +173,7 @@ void get_checkerboard(std::vector &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 +#include #include +#include +#include +#include -#ifdef __cplusplus -extern "C" { -#endif +#include -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 &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 668ec1a8..faee7590 100644 --- a/src/robofsm/motion-control.cc +++ b/src/robofsm/motion-control.cc @@ -34,6 +34,7 @@ #include #include #include "events.h" +#include 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 b56af5ba..6090fed1 100644 --- a/src/robofsm/robot.cc +++ b/src/robofsm/robot.cc @@ -12,11 +12,11 @@ #include #include #include -#include "map_handling.h" #include #include #include #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 ce8aa4ce..4f599e5a 100644 --- a/src/robofsm/robot.h +++ b/src/robofsm/robot.h @@ -4,14 +4,13 @@ #include #include #include -#include #include #include #include #include #include -#include "movehelper.h" -#include "scheduler.hpp" +#include +#include //#include #include @@ -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 592823c6..10e11d02 100644 --- 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; }