6 #include <movehelper.h>
16 #include <shape_detect.h>
19 #include "actuators.h"
20 #include "match-timing.h"
21 #include "common-states.h"
22 #include "sub-states.h"
24 UL_LOG_CUST(ulogd_common_states); /* Log domain name = ulogd + name of the file */
26 /************************************************************************
27 * Functions used in and called from all the (almost identical)
28 * "wait for start" states in particular strategies.
29 ************************************************************************/
32 #define DBG_FSM_STATE(name) do { if (fsm->debug_states) ul_loginf("fsm %s %.1f: %s(%s)\n", \
33 fsm->debug_name, robot_current_time(), \
34 name, fsm_event_str(fsm->events[fsm->ev_head])); } while(0)
36 /************************************************************************
37 * Trajectory constraints used; They are initialized in the main() function in competition.cc
38 ************************************************************************/
40 struct TrajectoryConstraints tcFast, tcSlow, tcVerySlow;
43 * Vector where all absolute positions of all detected targets are stored.
45 std::vector<robot_pos_type> detected_target;
48 * Safe distance for target recognition
50 const double approach_radius = TARGET_RADIUS_M + 2.0*MAP_CELL_SIZE_M + ROBOT_DIAGONAL_RADIUS_M;
52 void set_initial_position()
54 robot_set_est_pos_trans(ROBOT_START_X_M, ROBOT_START_Y_M, DEG2RAD(ROBOT_START_ANGLE_DEG));
62 /* Check if the new point is within the playground scene */
63 bool goal_is_in_playground(double goalx, double goaly)
65 if ((goalx < 0) || (goalx > PLAYGROUND_WIDTH_M) || (goaly < 0) || (goaly > PLAYGROUND_HEIGHT_M))
71 /* Check if the new point is close to the robot */
72 bool close_goal(double goalx, double goaly)
74 const double close = 0.5;
76 robot_get_est_pos(&x, &y, &phi);
78 if ((abs(goalx - x) < close) && (abs(goaly - y) < close) )
85 * Take data from hokuyo and run shape detection on it.
87 * Absolute positions of all detected targets centers are stored in alobal variable (vector).
89 * @return True if at least one target detected, else false.
91 static bool detect_target()
93 struct hokuyo_scan_type hokuyo = robot.hokuyo;
96 std::vector<Shape_detect::Arc> arcs;
97 sd.prepare(hokuyo.data);
101 detected_target.clear();
103 if (arcs.size() > 0) {
104 robot_pos_type e, target, hok;
106 robot_get_est_pos(&e.x, &e.y, &e.phi);
108 double sinus = sin(e.phi);
109 double cosinus = cos(e.phi);
111 // save absolute positions of all detected targets
112 for (int i = 0; i < arcs.size(); i++) {
113 Shape_detect::Arc *a = &arcs[i];
115 hok.x = HOKUYO_CENTER_OFFSET_M + (double)a->center.x / 1000.0;
116 hok.y = (double)a->center.y / 1000.0;
118 /* transform target position which is relative to Hokuyo
119 center to absolute position in space */
120 target.x = (hok.x * cosinus) - (hok.y * sinus) + e.x;
121 target.y = (hok.x * sinus) + (hok.y * cosinus) + e.y;
123 // filter those targets not in playground range
124 //if (goal_is_in_playground(target.x, target.y))
125 // detected_target.push_back(target);
128 return detected_target.size();
132 * Calculates point to approach the target.
134 * @param target Position of the center of the target.
135 * @param approach Pointer to the the intersection point of circle around
136 * the target and line between robot center and target.
138 void get_approach_point(double xtarget, double ytarget, double *xapproach, double *yapproach, double *phi_approach)
140 double xrobot, yrobot, phi;
143 robot_get_est_pos(&xrobot, &yrobot, &phi);
145 delta = distance(xrobot, yrobot, xtarget, ytarget);
147 *xapproach = xtarget - (approach_radius * (xtarget - xrobot) / delta);
148 *yapproach = ytarget - (approach_radius * (ytarget - yrobot) / delta);
150 *phi_approach = get_approach_angle(xtarget, ytarget);
154 * Calculates point to approach the target.
156 * @param target Position of the center of the target.
157 * @return Angle to approach the target form.
159 double get_approach_angle(double xtarget, double ytarget)
161 double xrobot, yrobot,phi;
163 robot_get_est_pos(&xrobot, &yrobot, &phi);
165 return atan2((ytarget - yrobot), (xtarget - xrobot));
170 * FSM state for neighborhood observation.
172 * Detect targets using shape_detect.
173 * If no target detected, turn 120deg and try again.
174 * Scan all 360deg and then go back to move_around state.
176 * If target detected, go to approach_target state.
180 static char turn_cntr = 0;
185 DBG_PRINT_EVENT("survey");
186 #if 1 // FIXME just for test
187 if (detect_target()) {
190 robot_pos_type target;
191 detected_target.clear();
192 for (double i = 1; i < 5; i++) {
195 detected_target.push_back(target);
198 // target detected, go to the target
199 FSM_TRANSITION(approach_target);
200 DBG_PRINT_EVENT("Target detected!");
202 // no target detected in this heading, turn 120°
203 robot_get_est_pos(&x, &y, &phi);
204 robot_goto_notrans(x, y, TURN(DEG2RAD(120)+phi), &tcSlow);
206 DBG_PRINT_EVENT("no target");
211 FSM_TRANSITION(move_around);
213 FSM_TRANSITION(survey);
219 case EV_MOTION_ERROR:
220 FSM_TRANSITION(move_around);
229 * FSM state for approaching all detected targets.
231 * Try to approach target.
232 * If approach OK - go to subautomaton and do target recognition, touch and load.
233 * On subautomaton return check if target loaded/valid.
235 * If target loaded, go home.
236 * If target not valid, try next target if any.
237 * If approach not succesfull - go to move_around state.
239 FSM_STATE(approach_target)
241 double x_approach, y_approach, phi_approach;
245 DBG_PRINT_EVENT("approaching target");
247 get_approach_point(robot.target_pos.x, robot.target_pos.y, &x_approach, &y_approach, &phi_approach);
248 robot_goto_notrans(x_approach, y_approach, ARRIVE_FROM(phi_approach, 0.2), &tcFast);
251 DBG_PRINT_EVENT("target approached");
252 SUBFSM_TRANSITION(get_target_turn, NULL);
255 if (robot.target_loaded) {
256 FSM_TRANSITION(go_home);
258 DBG_PRINT_EVENT("target not loaded");
259 FSM_TRANSITION(move_around);
262 case EV_MOTION_ERROR:
263 DBG_PRINT_EVENT("can not approach target");
264 FSM_TRANSITION(move_around);
271 FSM_STATE(move_around)
277 // TODO dodelat aby jel takovyho sneka pres hriste, ale nejezdil tam nahodne
279 goalx = ((rand()%1000)/1000.0) + 0.2;
280 goaly = ((rand()%1000)/1000.0) + 1.6;
281 } while (!ShmapIsFreePoint(goalx, goaly) && close_goal(goalx, goaly));
283 robot_goto_notrans(goalx, goaly, NO_TURN(), &tcFast);
284 DBG_PRINT_EVENT("new survey point");
286 case EV_MOTION_ERROR:
287 DBG_PRINT_EVENT("can not access survey point");
289 FSM_TRANSITION(move_around);
291 case EV_TARGET_DETECTED:
292 robot.target_localization_enab = false;
293 FSM_TRANSITION(approach_target);
302 FSM_STATE(approach_arena)
306 DBG_PRINT_EVENT("approaching arena");
307 robot_goto_notrans(1.15, 1.35, ARRIVE_FROM(DEG2RAD(90), 0.1), &tcFast);
308 robot.target_localization_enab = true;
310 case EV_MOTION_ERROR:
311 DBG_PRINT_EVENT("ERROR: position is not reachable!");
312 FSM_TRANSITION(approach_arena);
315 FSM_TRANSITION(move_around);
317 case EV_TARGET_DETECTED:
318 robot.target_localization_enab = false;
319 FSM_TRANSITION(approach_target);
324 DBG_PRINT_EVENT("Unhandled event!");
333 DBG_PRINT_EVENT("homing");
334 robot_goto_notrans(ROBOT_START_X_M, ROBOT_START_Y_M + 0.3, ARRIVE_FROM(DEG2RAD(270), 0.1), &tcFast);
336 case EV_MOTION_ERROR:
337 DBG_PRINT_EVENT("ERROR: home position is not reachable!");
342 DBG_PRINT_EVENT("Mission completed!");
359 case EV_MOTION_ERROR:
360 case EV_SWITCH_STRATEGY:
361 DBG_PRINT_EVENT("unhandled event");