#include "robodim_eb2008.h"
+#include <robomath.h>
/* Beacon positions. For some version of MCL, this must be sorted in
* the same order as laser rotation. */
/* S3 */ { 0, 0 },
};
+
+ const struct sharp_pos sharp[SHARP_COUNT] = {
+ { ROBOT_AXIS_TO_FRONT_M-0.02, +ROBOT_WIDTH_M/2.0, DEG2RAD(+90) }, /* Left */
+ { ROBOT_AXIS_TO_FRONT_M-0.02, -ROBOT_WIDTH_M/2.0, DEG2RAD(-90) }, /* Right */
+ { ROBOT_AXIS_TO_FRONT_M, +ROBOT_WIDTH_M/2.0, DEG2RAD(0) }, /* Front left */
+ { ROBOT_AXIS_TO_FRONT_M, -ROBOT_WIDTH_M/2.0, DEG2RAD(0) }, /* Front right */
+ { ROBOT_AXIS_TO_BACK_M, +ROBOT_WIDTH_M/2.0, DEG2RAD(180) }, /* Back left */
+ { ROBOT_AXIS_TO_BACK_M, -ROBOT_WIDTH_M/2.0, DEG2RAD(180) }, /* Back right */
+};
--- /dev/null
+#include <robot_eb2008.h>
+#include <robodim_eb2008.h>
+#include <map.h>
+#include <robomath.h>
+
+/*******************************************************************************
+ * Parameters of Obstacle detection
+ *******************************************************************************/
+
+#define OBS_PERIOD_WATCH 100000 /**< Time period for watching the sensors in us*/
+#define OBS_PERIOD_BETWEEN_DET 1000000 /**< Minimal time period between two detections in us. */
+#define OBS_AREA (200/MAP_CELL_SIZE_MM) /**< Configuration Space of the obstacle in cell units. In this case 200mm = 2 cells. @warning It should be an integer */
+#define OBS_AREA_M (OBS_AREA*MAP_CELL_SIZE_MM)
+#define OBS_FORGET_PERIOD 500 /**< The period of thread_obstacle_forgeting [ms] */
+
+#define OBS_FORGET_SEC 15 /**< Time to completely forget detected obstacle. */
+
+#define OBS_OFFSET 0.6
+
+
+void obstacle_detected_at(double x, double y)
+{
+ int i,j, xcell, ycell;
+ struct est_pos_type est_pos;
+ struct map *map = robot.map;
+ double xx, yy;
+
+ if (!map)
+ return;
+
+ ShmapPoint2Cell(x, y, &xcell, &ycell);
+ if (!ShmapIsCellInMap(xcell, ycell))
+ return;
+
+ //DBG("Cell %d, %d\n", xcell, ycell);
+
+ /** 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. */
+
+ ROBOT_LOCK(est_pos);
+ est_pos = robot.est_pos;
+ ROBOT_UNLOCK(est_pos);
+
+ for (i=(xcell-OBS_AREA); i <= xcell+OBS_AREA; i++) {
+ for (j=(ycell- OBS_AREA); j <=ycell + OBS_AREA; j++) {
+ if (!ShmapIsCellInMap(i, j)) continue;
+ ShmapCell2Point(i, j, &xx, &yy);
+ if (distance(xx, yy, est_pos.x, est_pos.y) > 0.2) {
+ if (i==xcell && j==ycell)
+ map->cells[ycell][xcell].flags |= MAP_FLAG_DET_OBST;
+ map->cells[j][i].detected_obstacle = MAP_NEW_OBSTACLE;
+ }
+ }
+ }
+}
+/**
+ * A thread running the trajectory recalc
+ *
+ * This (low-medium priority) thread updates the map with sensors information.
+ * If it is necesary, it recalculate the path.
+ *
+ * @param arg
+ *
+ * @return
+ */
+
+void obst_coord(struct est_pos_type *e, const struct 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);
+ sa = e->phi + s->ang;
+
+ *x = sx+v*cos(sa);
+ *y = sy+v*sin(sa);
+}
+
+void update_map(struct sharps_type *s)
+{
+ double val[SHARP_COUNT] = { s->left, s->right, s->front_left, s->front_right,
+ s->back_left, s->back_right };
+ int sharp80[] = {0,1,2,3};
+ double x, y;
+ //Pos p;
+ struct est_pos_type e;
+ int i, j;
+
+ ROBOT_LOCK(est_pos);
+ e = robot.est_pos;
+ ROBOT_UNLOCK(est_pos);
+
+ for (i=0; i<sizeof(sharp80)/sizeof(sharp80[0]); i++) {
+ j = sharp80[i];
+ if (val[j] < 0.7) {
+ obst_coord(&e, &sharp[j], val[j], &x, &y);
+ obstacle_detected_at(x, y);
+
+ /* The second end of oponents robot */
+ obst_coord(&e, &sharp[j], val[j]+0.3, &x, &y);
+ obstacle_detected_at(x, y);
+ }
+ }
+}
+
+/**
+ * Decrease map.detected_obstacle by val with saturation on zero. It
+ * also clears #MAP_FLAG_DET_OBST if value reaches zero.
+ *
+ * @param val Value to decrease obstacle life.
+ * @see map #MAP_NEW_OBSTACLE
+ * @todo Do faster this process. Use a cell's list to update.
+ */
+static void forget_obstacles(map_cell_detobst_t val){
+ int i,j;
+ struct map *map = robot.map;
+
+ for (j=0;j<MAP_HEIGHT;j++){
+ for(i=0;i<MAP_WIDTH;i++){
+ struct map_cell *cell = &map->cells[j][i];
+ if (val < cell->detected_obstacle) cell->detected_obstacle -= val;
+ else {
+ cell->detected_obstacle = 0;
+ cell->flags &= ~MAP_FLAG_DET_OBST;
+ }
+ }
+ }
+}
+
+static void gettimeofday_ts(struct timespec *ts)
+{
+ struct timeval tv;
+ gettimeofday(&tv, NULL);
+ /* Convert from timeval to timespec */
+ ts->tv_sec = tv.tv_sec;
+ ts->tv_nsec = tv.tv_usec * 1000;
+}
+
+/**
+ * A thread updating the map
+ */
+static void * thread_obstacle_forgeting(void * arg)
+{
+ struct timespec ts;
+ sem_t timer;
+ int val = (long long)MAP_NEW_OBSTACLE/(OBS_FORGET_SEC*1000/OBS_FORGET_PERIOD);
+ if (val == 0) val = 1;
+
+ gettimeofday_ts(&ts);
+
+ sem_init(&timer, 0, 0);
+ while (1) {
+ __fsm_timespec_add_ms(&ts, NULL, OBS_FORGET_PERIOD);
+ sem_timedwait(&timer, &ts);
+
+ forget_obstacles(val);
+ }
+}
#include <math.h>
#include <robomath.h>
#include <laser-nav.h>
+#include "map_handling.h"
/* ----------------------------------------------------------------------
* PUBLISHER CALLBACKS - GENERIC
}
}
+void rcv_sharps_cb(const ORTERecvInfo *info, void *vinstance,
+ void *recvCallBackParam)
+{
+ struct sharps_type *instance = (struct sharps_type *)vinstance;
+ //struct sharps_type *for_mcl;
+
+ switch (info->status) {
+ case NEW_DATA: {
+ update_map(instance);
+ break;
+ }
+ case DEADLINE:
+ DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
+ break;
+ }
+}
+
void robot_init_orte()
{
robot.orte.strength = 20;
eb2008_subscriber_laser_data_create(&robot.orte, rcv_laser_data_cb, &robot.orte);
eb2008_subscriber_cmu_create(&robot.orte, rcv_cmu_cb, &robot.orte);
eb2008_subscriber_bumper_create(&robot.orte, rcv_bumper_cb, &robot.orte);
+ eb2008_subscriber_sharps_create(&robot.orte, rcv_sharps_cb, &robot.orte);
}
projects / eurobot / public.git / commitdiff