int i, x, y;
int xcontig, ycontig, xstart, ystart, xgoal, ygoal;
struct map *map = ShmapIsMapInit();
+ bool valid;
if (!map) return -1;
// Transform real data in cell data
- ShmapPoint2Cell(xstart_real, ystart_real, &xstart, &ystart);
+ ShmapPoint2Cell(xstart_real, ystart_real, &xstart, &ystart, &valid);
+ if (!valid)
+ return -1;
start = &graph[ystart][xstart];
- ShmapPoint2Cell(xgoal_real, ygoal_real, &xgoal, &ygoal);
+ ShmapPoint2Cell(xgoal_real, ygoal_real, &xgoal, &ygoal, &valid);
+ if (!valid)
+ return -1;
goal = &graph[ygoal][xgoal];
/**@{*/
#include <sys/stat.h>
#include <math.h>
-
struct map* map = NULL;
int shmap_id;
/** @addtogroup maplib */
struct map_cell *ShmapCellAtPoint(double x_m, double y_m)
{
int i, j;
- ShmapPoint2Cell(x_m, y_m, &i, &j);
- return &(map->cells[j][i]);
+ bool valid;
+ ShmapPoint2Cell(x_m, y_m, &i, &j, &valid);
+ if (valid)
+ return &(map->cells[j][i]);
+ else
+ return NULL;
}
/**
int ShmapIsFreePoint(double x_m, double y_m)
{
int i,j;
- ShmapPoint2Cell(x_m, y_m, &i, &j);
+ bool valid;
+ ShmapPoint2Cell(x_m, y_m, &i, &j, &valid);
return ShmapIsFreeCell(i, j);
}
int ShmapSetRectangleFlag(double x1, double y1, double x2, double y2, map_cell_flag_t set_flags, map_cell_flag_t clear_flags)
{
int i,j, i1, i2, j1, j2;
- ShmapPoint2Cell(x1, y1, &i1, &j1);
- ShmapPoint2Cell(x2, y2, &i2, &j2);
+ bool valid;
+ ShmapPoint2Cell(x1, y1, &i1, &j1, &valid);
+ if (!valid)
+ return 0;
+ ShmapPoint2Cell(x2, y2, &i2, &j2, &valid);
+ if (!valid)
+ return 0;
if (i1 > i2) {
i = i2;
i2 = i1;
* @param x Coodonte of X
*/
-void ShmapPoint2Cell(double x, double y, int *ix, int *iy){
- if (ix) *ix = (int)floor(x/MAP_CELL_SIZE_M);
- if (iy) *iy = MAP_HEIGHT-1-(int)floor(y/MAP_CELL_SIZE_M);
+void ShmapPoint2Cell(double x, double y, int *ix, int *iy, bool *valid){
+ int xx, yy;
+ xx = (int)floor(x/MAP_CELL_SIZE_M);
+ yy = MAP_HEIGHT-1-(int)floor(y/MAP_CELL_SIZE_M);
+
+ if (valid != NULL) {
+ *valid = ShmapIsCellInMap(xx, yy);
+ }
+ if (ix) *ix = xx;
+ if (iy) *iy = yy;
}
/**
#ifndef __MAP_H
#define __MAP_H
+
+#include <stdbool.h>
+
/**
* @defgroup maplib Library to manage the map
* This libary implements a discrete map use wich can be use in robotics.
void ShmapDt(void);
struct map * ShmapIsMapInit(void);
-void ShmapPoint2Cell(double x, double y, int *cx, int *cy);
+void ShmapPoint2Cell(double x, double y, int *cx, int *cy, bool *valid);
int ShmapCell2Point(int ix, int iy, double *x, double *y);
*/
static inline int ShampIsPointInMap(double x_m, double y_m){
int x, y;
- ShmapPoint2Cell(x_m, y_m, &x, &y);
+ bool valid;
+ ShmapPoint2Cell(x_m, y_m, &x, &y, &valid);
return ShmapIsCellInMap(x, y);
}
* ROBOT DIMENSIONS
* S0
* S4 +--------------------------+ S2
- * ^ | | | |---|
+ * ^ | | | |--,
* ^ | | ------- ABE | |
* RR| | | :<--------------------->|
- * v | | : | |
- * W| | + Center (L) | |
- * | | AB : AF | |
+ * v | | : |--'
+ * W| | + Center (L) |
+ * | | AB : AF |--,
* | |<---->:<----------------->| |
* | | ------- | |
- * v | | | |---|
+ * v | | | |--'
* S5 +--------------------------+ S3
* <--> S1
* WR
#define ROBOT_AXIS_TO_BACK_M (ROBOT_AXIS_TO_BACK_MM/1000.0)
#define ROBOT_AXIS_TO_FRONT_MM 183 /* AF */
#define ROBOT_AXIS_TO_FRONT_M (ROBOT_AXIS_TO_FRONT_MM/1000.0)
-#define ROBOT_AXIS_TO_BELT_MM 280 /* ABE */ /* FIXME: no belt in the Eurobot 2008 */
-#define ROBOT_AXIS_TO_BELT_M (ROBOT_AXIS_TO_BELT_MM/1000.0)
+#define ROBOT_AXIS_TO_BRUSH_MM 280 /* ABE */
+#define ROBOT_AXIS_TO_BRUSH_M (ROBOT_AXIS_TO_BRUSH_MM/1000.0)
/**
#include "robot_config.h"
#include <robomath.h>
-
/*******************************************************************************
* Controller thread and helper functions for that thread
*******************************************************************************/
static pthread_t thr_trajctory_follower;
static struct timeval tv_start; /**< Absolute time, when trajectory started. */
+// Trajectory recalculation thread and synchoronization
+pthread_t thr_trajctory_recalculation;
+sem_t start_recalculation;
+sem_t recalculation_not_running;
+
/** Stores the actually followed trajectory object */
static Trajectory *actual_trajectory = NULL;
bool actual_trajectory_reverse;
}
}
+static void check_for_collision_in_future(Trajectory *traj, double current_time)
+{
+ Pos future_pos;
+ struct map *map = robot.map;
+ int xcell, ycell;
+ double x, y;
+ bool valid;
+
+ traj->getRefPos(current_time+1/*s*/, future_pos);
+
+ /* Ignore obstacles when turning */
+ if (fabs(future_pos.v) < 0.01)
+ return;
+
+ x = future_pos.x + cos(future_pos.phi)*ROBOT_AXIS_TO_BRUSH_M;
+ y = future_pos.y + sin(future_pos.phi)*ROBOT_AXIS_TO_BRUSH_M;
+
+ ShmapPoint2Cell(x, y, &xcell, &ycell, &valid);
+ if (!valid)
+ return;
+ if (map->cells[ycell][xcell].detected_obstacle > 0) {
+ if (sem_trywait(&recalculation_not_running) == 0) {
+ sem_post(&start_recalculation);
+ }
+ }
+}
+
static void do_control()
{
double speedl, speedr;
t = (double)(tv.tv_usec - tv_start.tv_usec) / 1000000.0;
t += (tv.tv_sec - tv_start.tv_sec);
+ check_for_collision_in_future(w, t);
+
done = w->getRefPos(t, ref_pos);
ROBOT_LOCK(ref_pos);
*
* @return
*/
-static void *trajectory_follower(void *arg)
+static void *thread_trajectory_follower(void *arg)
{
struct itimerspec ts;
sigset_t sigset;
return ret;
}
+/* This recalculation must only be triggered from mvoement state. */
+void *thread_trajectory_realculation(void *arg)
+{
+ while (1) {
+ sem_wait(&start_recalculation);
+ /* TODO: Different start for planning than esitmated position */
+ enum target_status ret = new_goal(¤t_target);
+ switch (ret) {
+ case TARGET_OK:
+ break;
+ case TARGET_ERROR:
+ printf("Target error on recalculation_in_progress\n");
+ FSM_SIGNAL(MOTION, EV_RECALCULATION_FAILED, NULL);
+ break;
+ case TARGET_NOT_REACHABLE:
+ FSM_SIGNAL(MOTION, EV_RECALCULATION_FAILED, NULL);
+ break;
+ }
+ sem_post(&recalculation_not_running);
+ }
+}
+
/*******************************************************************************
* The automaton
*******************************************************************************/
// Trajectory follower thread
pthread_attr_init (&tattr);
pthread_attr_getschedparam (&tattr, ¶m);
- param.sched_priority = TRAJ_FOLLOWER_PRIO;
+ param.sched_priority = THREAD_PRIO_TRAJ_FOLLOWER;
ret = pthread_attr_setschedparam (&tattr, ¶m);
if(ret) {
perror("move_init: pthread_attr_setschedparam(follower)");
exit(1);
}
- ret = pthread_create(&thr_trajctory_follower, &tattr, trajectory_follower, NULL);
+ ret = pthread_create(&thr_trajctory_follower, &tattr, thread_trajectory_follower, NULL);
+ if(ret) {
+ perror("move_init: pthread_create");
+ exit(1);
+ }
+
+ // Trajectory follower thread
+ sem_init(&recalculation_not_running, 0, 1);
+ sem_init(&start_recalculation, 0, 0);
+ pthread_attr_init (&tattr);
+ pthread_attr_getschedparam (&tattr, ¶m);
+ param.sched_priority = THREAD_PRIO_TRAJ_RECLAC;
+ ret = pthread_attr_setschedparam (&tattr, ¶m);
+ if(ret) {
+ perror("move_init: pthread_attr_setschedparam(follower)");
+ exit(1);
+ }
+ ret = pthread_create(&thr_trajctory_recalculation, &tattr, thread_trajectory_realculation, NULL);
if(ret) {
perror("move_init: pthread_create");
exit(1);
}
}
-
-
FSM_STATE(movement)
{
switch (FSM_EVENT) {
FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
FSM_TRANSITION(wait_for_command);
break;
- case EV_OBSTACLE_ON_ROAD:
- FIND_NEW_WAY(¤t_target);
+ case EV_RECALCULATION_FAILED:
+ FSM_TRANSITION(wait_and_try_again);
break;
case EV_TRAJECTORY_LOST:
FSM_TRANSITION(reset_mcl);
case EV_MOVE_STOP:
FSM_TRANSITION(wait_for_command);
break;
- case EV_OBSTACLE_ON_ROAD:
- /* ignore it */
- break;
case EV_EXIT:
stop();
break;
case EV_TRAJECTORY_DONE:
case EV_NEW_TARGET:
case EV_FOUND_AFTER_RESET:
+ case EV_RECALCULATION_FAILED:
DBG_PRINT_EVENT("unhandled event");
break;
}
case EV_TRAJECTORY_LOST:
case EV_RETURN:
case EV_TIMER:
- case EV_OBSTACLE_ON_ROAD:
+ case EV_RECALCULATION_FAILED:
DBG_PRINT_EVENT("unhandled event");
}
}
{
switch (FSM_EVENT) {
case EV_ENTRY:
- stop();
+ stop(); //FIXME: Not hard stop
FSM_TIMER(1000);
break;
case EV_TIMER:
case EV_TRAJECTORY_LOST:
case EV_RETURN:
case EV_FOUND_AFTER_RESET:
- case EV_OBSTACLE_ON_ROAD:
+ case EV_RECALCULATION_FAILED:
DBG_PRINT_EVENT("unhandled event");
break;
case EV_EXIT:
struct est_pos_type est_pos;
struct map *map = robot.map;
double xx, yy;
+ bool valid;
if (!map)
return;
- ShmapPoint2Cell(x, y, &xcell, &ycell);
- if (!ShmapIsCellInMap(xcell, ycell))
+ ShmapPoint2Cell(x, y, &xcell, &ycell, &valid);
+ if (!valid)
return;
/* The obstacle was detected here */
FSM_SIGNAL(MOTION, EV_NEW_TARGET, target);
}
+
+
+/**
+ * Move robot forward or backward
+ *
+ * @param distance Distance in meters ( >0 forward, <0 backward)
+ * @param heading Final heading
+ * @param tc Trajectory constrains
+ */
+void robot_move_by(double distance, struct final_heading *heading, struct TrajectoryConstraints *tc)
+{
+ double x, y, phi;
+
+ ROBOT_LOCK(est_pos);
+ x = robot.est_pos.x;
+ y = robot.est_pos.y ;
+ phi = robot.est_pos.phi;
+ ROBOT_UNLOCK(est_pos);
+
+ x += distance*cos(phi);
+ y += distance*sin(phi);
+
+ Trajectory *t;
+ bool backward = distance < 0;
+ t = new Trajectory(*tc, backward);
+ t->addPoint(x, y);
+ t->finalHeading = *heading;
+
+ struct move_target *target;
+ target = (struct move_target*)malloc(sizeof(*target));
+ memset(target, 0, sizeof(*target));
+ target->x = x;
+ target->y = y;
+ target->h = *heading;
+ target->trajectory = t;
+ FSM_SIGNAL(MOTION, EV_NEW_TARGET, target);
+}
void robot_goto_notrans(double x, double y, struct final_heading *heading, struct TrajectoryConstraints *tc);
#define robot_goto_trans(x, y, heading, tc) robot_goto_notrans(TRANS_X(x), TRANS_Y(y), TRANS_HEADING(heading), tc)
+void robot_move_by(double distance, struct final_heading *heading, struct TrajectoryConstraints *tc);
+
static inline void robot_translate_coordinates(double *x, double *y, double *phi)
{
if (x) {
"EV_TRAJECTORY_DONE" : "Reference position is at the end of the previously submitted trajectory",
"EV_TRAJECTORY_DONE_AND_CLOSE" : "::EV_TRAJECTORY_DONE + distance of actual position to the target is less than XXX",
"EV_FOUND_AFTER_RESET" : "MCL estimated position after reset",
- "EV_OBSTACLE_ON_ROAD" : "An obstacle is detected on the current trajectory",
+ "EV_RECALCULATION_FAILED" : "Trajectory recalculation caused by a detected obstcale has failed (error or target not reachable)",
},
"joy" : {
},
#endif
/*Thread priorities*/
-#define TRAJ_FOLLOWER_PRIO 255 /* As high as possible */
+#define THREAD_PRIO_TRAJ_FOLLOWER 255 /* As high as possible */
#define THREAD_PRIO_MOTION 30
#define THREAD_PRIO_MAIN 25
#define THREAD_PRIO_LOC 20
+#define THREAD_PRIO_TRAJ_RECLAC 18
#define OBST_FORGETING_PRIO 17 /* Priority of the thread for forgeting detected obstacles. */
#define THREAD_PRIO_DISP 15
#define THREAD_PRIO_JOY 10
#include <laser-nav.h>
#include "map_handling.h"
#include <oledlib.h>
+#include <string.h>
/* ----------------------------------------------------------------------
* PUBLISHER CALLBACKS - GENERIC
* ---------------------------------------------------------------------- */
void *recvCallBackParam)
{
struct cmu_type *instance = (struct cmu_type *)vinstance;
- static last_color = NO_BALL;
+ static enum ball_color last_color = NO_BALL;
static unsigned char first = 1;
switch (info->status) {
ShmapSetRectangleFlag(0.0, 0.0, 3.0, 0.199, MAP_FLAG_WALL, 0);
ShmapSetRectangleFlag(0.0, 1.901, 3.0, 2.1, MAP_FLAG_WALL, 0);
ShmapSetRectangleFlag(2.801, 0.0, 3.0, 2.1, MAP_FLAG_WALL, 0);
- ShmapSetRectangleFlag(1.3, 0.7, 1.7, 1.0, MAP_FLAG_SIMULATED_WALL, 0);
+ //ShmapSetRectangleFlag(1.3, 0.7, 1.7, 1.0, MAP_FLAG_SIMULATED_WALL, 0);
//ShmapSetRectangleFlag(1.8, 1.5, 2.3, 1.8, MAP_FLAG_SIMULATED_WALL, 0);
do {
goalx = (rand()%(MAP_PLAYGROUND_WIDTH_MM-50)+25)/1000.0;
projects / eurobot / public.git / commitdiff