#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 100000 /**< The period of thread_update_map() */
-
-#define OBS_FORGET_SEC 15 /**< Time to completely forget detected obstacle. */
-
-#define OBS_OFFSET 0.6
/*******************************************************************************
* Controller thread and helper functions for that thread
*******************************************************************************/
// Global varibles
static pthread_t thr_trajctory_follower;
static pthread_t thr_trajctory_recalc;
-static pthread_t thr_update_map;
static struct timeval tv_start; /**< Absolute time, when trajectory started. */
/** Stores the actually followed trajectory object */
static Trajectory *actual_trajectory = NULL;
-
-//static Trajectory *switch_to_trajectory = NULL;
-//static double switch_time;
-
-// function definition
-static int new_goal(void);
-static void stop();
-
bool actual_trajectory_reverse;
pthread_mutex_t actual_trajectory_lock = PTHREAD_MUTEX_INITIALIZER;
pthread_mutex_t switch_to_trajectory_lock = PTHREAD_MUTEX_INITIALIZER;
-// static void odometry(double l, r) {
-// static double last_l, last_r;
-// robot.act_pos.x = (double)ROBOT_AXIS_TO_BACK_MM/1000;
-// robot.act_pos.y = ((double)PLAYGROUND_HEIGHT_MM - ROBOT_WIDTH_MM/2)/1000;
-// robot.act_pos.phi = 0;
+// Target position of the current movement
+static struct move_target current_target;
+
+
+//static Trajectory *switch_to_trajectory = NULL;
+//static double switch_time;
-// ROBOT_LOCK(act_pos);
-// ROBOT_UNLOCK(act_pos);
-// }
+static void stop();
static void delete_actual_trajectory()
{
}
}
+static void do_control()
+{
+ double speedl, speedr;
+ pthread_mutex_lock(&actual_trajectory_lock);
+ Trajectory *w = actual_trajectory;
+ if (w) {
+ Pos ref_pos, est_pos, balet_out;
+ bool done;
+ double t;
+ struct timeval tv;
+
+ // Calculate reference position
+ gettimeofday(&tv, NULL);
+ t = (double)(tv.tv_usec - tv_start.tv_usec) / 1000000.0;
+ t += (tv.tv_sec - tv_start.tv_sec);
+
+ done = w->getRefPos(t, ref_pos);
+
+ ROBOT_LOCK(ref_pos);
+ robot.ref_pos.x = ref_pos.x;
+ robot.ref_pos.y = ref_pos.y;
+ robot.ref_pos.phi = ref_pos.phi;
+ ROBOT_UNLOCK(ref_pos);
+
+ ROBOT_LOCK(est_pos);
+ est_pos.x = robot.est_pos.x;
+ est_pos.y = robot.est_pos.y;
+ est_pos.phi = robot.est_pos.phi;
+ ROBOT_UNLOCK(est_pos);
+
+#ifdef CONFIG_OPEN_LOOP
+ // We don't use any feedback now. It is
+ // supposed that the estimated position is
+ // equal to the reference position.
+ robot_set_est_pos_notrans(ref_pos.x, ref_pos.y, ref_pos.phi);
+ est_pos.x = robot.est_pos.x;
+ est_pos.y = robot.est_pos.y;
+ est_pos.phi = robot.est_pos.phi;
+#endif
+
+ DBG("rx=%5.02f ry=%5.02f, rphi=%4.0f v=%-4.02f omega=%-4.02f\r", ref_pos.x, ref_pos.y, ref_pos.phi/M_PI*180, ref_pos.v, ref_pos.omega);
+#ifdef DEBUG
+ fflush(stdout);
+#endif
+ // Call the controller
+ double error;
+ error = balet(ref_pos, est_pos, k, balet_out);
+ speedl = balet_out.v - ROBOT_ROTATION_RADIUS_M*balet_out.omega;
+ speedr = balet_out.v + ROBOT_ROTATION_RADIUS_M*balet_out.omega;
+ notify_fsm(done, error);
+ } else {
+ speedl = 0;
+ speedr = 0;
+ }
+
+
+ // Apply controller output
+ robot_send_speed(speedl, speedr);
+ pthread_mutex_unlock(&actual_trajectory_lock);
+}
+
/**
* A thread running the controller.
*
}
while (1) {
- double speeds[2];
int sig;
// Wait for start of new period or new data
sigwait(&sigset, &sig);
//DBG("signal %d ", sig - SIGRTMIN);
- pthread_mutex_lock(&actual_trajectory_lock);
- Trajectory *w = actual_trajectory;
- if (w) {
- Pos ref_pos, est_pos, balet_out;
- bool done;
- double t;
- struct timeval tv;
-
- // Calculate reference position
- gettimeofday(&tv, NULL);
- t = (double)(tv.tv_usec - tv_start.tv_usec) / 1000000.0;
- t += (tv.tv_sec - tv_start.tv_sec);
-
- done = w->getRefPos(t, ref_pos);
-
- ROBOT_LOCK(ref_pos);
- robot.ref_pos.x = ref_pos.x;
- robot.ref_pos.y = ref_pos.y;
- robot.ref_pos.phi = ref_pos.phi;
- ROBOT_UNLOCK(ref_pos);
-
- ROBOT_LOCK(est_pos);
- est_pos.x = robot.est_pos.x;
- est_pos.y = robot.est_pos.y;
- est_pos.phi = robot.est_pos.phi;
- ROBOT_UNLOCK(est_pos);
-
-#ifdef CONFIG_OPEN_LOOP
- // We don't use any feedback now. It is
- // supposed that the estimated position is
- // equal to the reference position.
- robot_set_est_pos_notrans(ref_pos.x, ref_pos.y, ref_pos.phi);
- est_pos.x = robot.est_pos.x;
- est_pos.y = robot.est_pos.y;
- est_pos.phi = robot.est_pos.phi;
-#endif
-
- DBG("rx=%5.02f ry=%5.02f, rphi=%4.0f v=%-4.02f omega=%-4.02f\r", ref_pos.x, ref_pos.y, ref_pos.phi/M_PI*180, ref_pos.v, ref_pos.omega);
-#ifdef DEBUG
- fflush(stdout);
-#endif
- // Call the controller
- double error;
- error = balet(ref_pos, est_pos, k, balet_out);
- speeds[0] = balet_out.v - ROBOT_ROTATION_RADIUS_M*balet_out.omega;
- speeds[1] = balet_out.v + ROBOT_ROTATION_RADIUS_M*balet_out.omega;
- notify_fsm(done, error);
- } else {
- speeds[0] = 0;
- speeds[1] = 0;
- }
-
-
- // Apply controller output
- robot_send_speed(speeds[0], speeds[1]);
- pthread_mutex_unlock(&actual_trajectory_lock);
- }
-}
-
-static 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;
- bool obs_already_seen;
-
- if (!map)
- return;
-
- ShmapPoint2Cell(x, y, &xcell, &ycell);
- if (!ShmapIsCellInMap(xcell, ycell))
- return;
- obs_already_seen = (map->cells[ycell][xcell].flags & (MAP_FLAG_DET_OBST)) != 0;
-
- DBG("New obstacle detected at cell [%f,%f] \n", x,y);
- bool needs_replan = false; // This flag indicates if the path should be recalculated
- bool obstacle_at_goal = false; // This flag indicates that the goal is an obstacle
- //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;
- if (map->cells[j][i].flags & MAP_FLAG_PATH) needs_replan = true;
- if (map->cells[j][i].flags & MAP_FLAG_GOAL) obstacle_at_goal = true;
- }
- }
- }
-
- // Path recalculation if the goal is not an obstacle and the path should be recalc
- if (obstacle_at_goal) {
- FSM_SIGNAL(MOTION, EV_MOVE_STOP, NULL);
- FSM_SIGNAL(MAIN, EV_GOAL_NOT_REACHABLE, NULL);
- }
- else if (needs_replan && !obs_already_seen) {
- if (new_goal() == 0)
- DBG("Path recalc\n");
- else {
- // TODO:
- }
- }
-}
-
-/**
- * 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
- */
-static void * trajctory_recalc(void * arg)
-{
-#define NUM_SHARPS 3
- double val[NUM_SHARPS], x, y;
- //Pos p;
- struct est_pos_type p;
- int i;
-
- while (1) {
- /** Updates Map. */
- //ShmapUpdateTmpObstacles(OBS_UPDATE_MAP_SPEED);
- /* FIXME: here should be data from opponent sharp sensors */
- ROBOT_LOCK(sharps);
- val[0] = robot.sharps.front_left;
- val[1] = robot.sharps.front_right;
- ROBOT_UNLOCK(sharps);
-
- const float ang[NUM_SHARPS] = {DEG2RAD(22), DEG2RAD(0), DEG2RAD(-22)};
- for (i=0; i<NUM_SHARPS; i++) {
- if(val[i] < OBS_OFFSET) {
- /** Get actual position. */
- ROBOT_LOCK(est_pos);
- p = robot.est_pos;
- ROBOT_UNLOCK(est_pos);
- /**
- * The obstacle cell its calulated as (val*cos(p.phi)) + p.x) and ((val*sin(p.phi)) + p.y)
- * where p is actual position and val is the value of the sensors in milimiters. Then ShmapPoint2Cell_X() and
- * ShmapPoint2Cell_Y() funcions are used to get cell coordonate.
- */
- val[i] += ROBOT_AXIS_TO_BELT_M;
- x = val[i]*cos(p.phi+ang[i]) + p.x;
- y = val[i]*sin(p.phi+ang[i]) + p.y;
- obstacle_detected_at(x, y);
-
- //waits between detection
- usleep(OBS_PERIOD_BETWEEN_DET);
- }
- }
-
- /** This process repeats with a period of #OBS_PERIOD_WATCH. */
- usleep(OBS_PERIOD_WATCH);
- }
-}
-
-/**
- * A thread updating the map
- */
-static void * thread_update_map(void * arg)
-{
- int val = (long long)MAP_NEW_OBSTACLE/(OBS_FORGET_SEC*1000000/OBS_FORGET_PERIOD);
- if (val == 0) val = 1;
- while (1) {
- /** Updates Map. */
- ShmapUpdateTmpObstacles(val);
- usleep(OBS_FORGET_PERIOD);
+ do_control();
}
}
pthread_kill(thr_trajctory_follower, SIG_NEWDATA);
}
-
+enum target_status {
+ TARGET_OK,
+ TARGET_NOT_REACHABLE,
+ TARGET_ERROR
+};
/**
- * @brief Calculates and simplify a path to goal position avoiding obstacles. In case of error it sends the proper event to MAIN FSM.
- * @return Zero on succes, non-zero on error.
- * @note Only call this function from this file.
+ * @brief Calculates and simplify a path to goal position avoiding obstacles.
+ * @return True on succes, false on error.
*/
-static int new_goal(void)
+static enum target_status new_goal(struct move_target *target)
{
- struct ref_pos_type goal;
+ enum target_status ret;
double startx, starty, angle;
PathPoint * path = NULL;
PathPoint * tmp_point = NULL;
start.phi = robot.est_pos.phi;
tc = robot.goal_trajectory_constraints;
ROBOT_UNLOCK(est_pos);
- /// Get goal position.
- ROBOT_LOCK(goal);
- goal = robot.goal;
- ROBOT_UNLOCK(goal);
//DBG("Going from (%.2f, %.2f) to (%.2f, %.2f)\n", startx, starty, goalx, goaly);
/// Calculates the path between actual position to goal position. If the goal is not reachable returns -1.
- switch (path_planner(startx, starty, goal.x, goal.y, &path, &angle)) {
- case 1:
- break;
+ switch (path_planner(startx, starty, target->x, target->y, &path, &angle)) {
case PP_ERROR_MAP_NOT_INIT:
- printf("ERROR: Map not initialized\n");
- FSM_SIGNAL(MOTION, EV_MOVE_STOP, NULL);
- FSM_SIGNAL(MAIN, EV_GOAL_NOT_REACHABLE, NULL);
- return -1;
- break;
+ ret = TARGET_ERROR; break;
case PP_ERROR_GOAL_IS_OBSTACLE:
- printf("ERROR: Goal is obstacle\n");
- FSM_SIGNAL(MOTION, EV_MOVE_STOP, NULL);
- FSM_SIGNAL(MAIN, EV_GOAL_NOT_REACHABLE, NULL);
- return -1;
- break;
case PP_ERROR_GOAL_NOT_REACHABLE:
- FSM_SIGNAL(MOTION, EV_MOVE_STOP, NULL);
- FSM_SIGNAL(MAIN, EV_GOAL_NOT_REACHABLE, NULL);
- return -1;
- break;
+ ret = TARGET_NOT_REACHABLE; break;
default:
- break;
- // shouldn't we call freePathMemory() here? No, because no memory has been allocanted for the path, path is only a pointer to NULL
+ ret = TARGET_OK; break;
}
+ if (ret != TARGET_OK)
+ return ret;
+
bool backward = false;
Trajectory *t = new Trajectory(tc, backward);
t->addPoint(tmp_point->x, tmp_point->y);
}
- // add the path to the robot trajectory
- t->finalHeading = robot.goal_final_heading; // NAN should be OK here, if not, it is error.
-
- // Free path allocated memory
+ t->finalHeading = target->h;
freePathMemory(path);
if (t->prepare(start)) {
go(t);
} else {
delete(t);
- FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
- return -1;
+ ret = TARGET_ERROR;
}
-
- return 0;
+ return ret;
}
* movehelper.cc. In case of error it sends the proper event to MAIN
* FSM.
*
- * @return Zero on succes, non-zero on error.
+ * @return true on succes, false on error.
*/
-static int new_trajectory(void)
+static enum target_status new_trajectory(Trajectory *t)
{
- Trajectory *t;
Pos pos;
- ROBOT_LOCK(new_trajectory);
- t = (Trajectory*) robot.new_trajectory;
- if (t) robot.new_trajectory = NULL;
- else {
- ROBOT_UNLOCK(new_trajectory);
- DBG("No trajectory\n");
- FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
- return -1;
- }
- ROBOT_UNLOCK(new_trajectory);
-
+ if (!t) {
+ printf("ERROR: No trajectory\n");
+ return TARGET_ERROR;
+ }
ROBOT_LOCK(est_pos);
pos.x = robot.est_pos.x;
pos.y = robot.est_pos.y;
if (t->prepare(pos)) {
go(t);
+ return TARGET_OK;
} else {
- FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL); // FIXME: Is this correct?
- return -1;
+ delete(t);
+ return TARGET_ERROR;
}
- return 0;
+}
+
+static enum target_status new_target(struct move_target *target)
+{
+ enum target_status ret;
+ if (target->trajectory) {
+ ret = new_trajectory((Trajectory*)target->trajectory);
+ // Trajectory is deleted by somebody else
+ target->trajectory = NULL;
+ } else {
+ ret = new_goal(target);
+ }
+ if (ret != TARGET_ERROR) {
+ current_target = *target;
+ }
+ return ret;
}
/*******************************************************************************
FSM_STATE_DECL(movement);
FSM_STATE_DECL(close_to_target);
FSM_STATE_DECL(wait_for_command);
+FSM_STATE_DECL(reset_mcl);
+FSM_STATE_DECL(wait_and_try_again);
FSM_STATE(init)
}
}
-FSM_STATE(delay)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY:
- FSM_TIMER(FSM_EVENT_INT);
- break;
- case EV_TIMER:
- SUBFSM_RET(NULL);
- break;
- }
-}
-
FSM_STATE(wait_for_command)
{
+ enum target_status ret;
switch (FSM_EVENT) {
case EV_ENTRY:
stop();
break;
- case EV_NEW_GOAL:
- if (new_goal() != 0) {
- // The event must be the return value of new_goal()
- DBG("Goal error\n");
- break;
- }
- FSM_TRANSITION(movement);
- break;
- case EV_NEW_TRAJECTORY:
- if (new_trajectory() != 0) {
- break;
+ case EV_NEW_TARGET:
+ ret = new_target((struct move_target*)FSM_EVENT_PTR);
+ switch (ret) {
+ case TARGET_OK:
+ FSM_TRANSITION(movement);
+ break;
+ case TARGET_ERROR:
+ printf("Target error\n");
+ FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
+ break;
+ case TARGET_NOT_REACHABLE:
+ FSM_TRANSITION(wait_and_try_again);
+ break;
}
- FSM_TRANSITION(movement);
+ free(FSM_EVENT_PTR);
break;
default:
break;
FSM_TRANSITION(wait_for_command);
break;
case EV_TRAJECTORY_LOST:
- FSM_SIGNAL(LOC, EV_RESET, NULL);
- // FIXME: delay and try again
- FSM_TRANSITION(wait_for_command);
+ FSM_TRANSITION(reset_mcl);
break;
case EV_MOVE_STOP:
FSM_TRANSITION(wait_for_command);
case EV_ENTRY:
case EV_EXIT:
break;
- case EV_NEW_TRAJECTORY:
- case EV_LOC_UPDATED:
-
+ case EV_NEW_TARGET:
case EV_TIMER:
case EV_RETURN:
- case EV_NEW_GOAL:
-// DBG_PRINT_EVENT("unhaldled event");
+ case EV_FOUND_AFTER_RESET:
+ DBG_PRINT_EVENT("unhandled event");
break;
}
}
-
FSM_STATE(close_to_target)
{
- FSM_TIMER(MAX_WAIT_FOR_CLOSE_MS);
switch (FSM_EVENT) {
+ case EV_RETURN:
+ if (FSM_EVENT_INT == EV_MOVE_STOP)
+ FSM_TRANSITION(wait_for_command);
+ //
case EV_ENTRY:
+ FSM_TIMER(MAX_WAIT_FOR_CLOSE_MS);
break;
case EV_TRAJECTORY_DONE_AND_CLOSE:
FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
break;
case EV_TRAJECTORY_LOST:
case EV_TIMER:
- FSM_SIGNAL(LOC, EV_RESET, NULL);
+ FSM_TRANSITION(reset_mcl);
break;
case EV_MOVE_STOP:
FSM_TRANSITION(wait_for_command);
stop();
break;
case EV_TRAJECTORY_DONE:
- case EV_NEW_TRAJECTORY:
- case EV_LOC_UPDATED:
- case EV_NEW_GOAL:
+ case EV_NEW_TARGET:
+ case EV_FOUND_AFTER_RESET:
+ DBG_PRINT_EVENT("unhandled event");
+ break;
+ }
+}
+
+FSM_STATE(reset_mcl)
+{
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ stop();
+ FSM_SIGNAL(LOC, EV_RESET, NULL);
+ break;
+ case EV_FOUND_AFTER_RESET:
+ SUBFSM_RET((void*)EV_FOUND_AFTER_RESET);
+ printf("TODO: start motion again\n");
+ break;
+ case EV_MOVE_STOP:
+ SUBFSM_RET((void*)EV_MOVE_STOP);
+ case EV_EXIT:
+ break;
+ case EV_TRAJECTORY_DONE:
+ case EV_NEW_TARGET:
+ case EV_TRAJECTORY_DONE_AND_CLOSE:
+ case EV_TRAJECTORY_LOST:
case EV_RETURN:
+ case EV_TIMER:
DBG_PRINT_EVENT("unhandled event");
+ }
+}
+
+
+FSM_STATE(wait_and_try_again)
+{
+ enum target_status ret;
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+ stop();
+ FSM_TIMER(1000);
break;
+ case EV_TIMER:
+ ret = new_goal(¤t_target);
+ switch (ret) {
+ case TARGET_OK:
+ FSM_TRANSITION(movement);
+ break;
+ case TARGET_ERROR:
+ printf("Target error\n");
+ FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
+ break;
+ case TARGET_NOT_REACHABLE:
+ FSM_TRANSITION(wait_and_try_again);
+ break;
+ }
+ case EV_MOVE_STOP:
+ FSM_TRANSITION(wait_for_command);
+ break;
+ case EV_TRAJECTORY_DONE:
+ case EV_NEW_TARGET:
+ case EV_TRAJECTORY_DONE_AND_CLOSE:
+ case EV_TRAJECTORY_LOST:
+ case EV_RETURN:
+ case EV_FOUND_AFTER_RESET:
+ case EV_EXIT:
+ DBG_PRINT_EVENT("unhandled event");
}
}
projects / eurobot / public.git / commitdiff