4 * @author Michal Sojka, Jose Maria Martin Laguna, Petr Beneš
16 #include <path_planner.h>
18 #include <movehelper.h>
22 #include "robot_config.h"
24 #include "motion-control.h"
28 #include <boost/mpl/list.hpp>
29 #include <boost/statechart/transition.hpp>
30 #include <boost/statechart/custom_reaction.hpp>
32 UL_LOG_CUST(ulogd_fsmmove); /* Log domain name = ulogd + name of the file */
35 #define DBG(format, ...) ul_logdeb(format, ##__VA_ARGS__)
37 #define DBG(format, ...)
40 const int MAX_WAIT_FOR_CLOSE_MS = 2000;
42 // time to calculate new trajectory [seconds]
43 const float TRGEN_DELAY = 0.3;
46 // Target position of the current movement
47 static move_target current_target;
50 TARGET_OK, // We can go the the target
51 TARGET_INACC, // Target is inaccessible because of an obstacle
52 TARGET_ERROR // Fatal errror during planning
55 static double free_space_in_front_of_robot()
59 i1 = HOKUYO_DEG_TO_INDEX(-45);
60 i2 = HOKUYO_DEG_TO_INDEX(+45);
61 for (i=MIN(i1, i2); i < MAX(i1, i2); i++) {
62 m = robot.orte.hokuyo_scan.data[i];
63 if (m > 19 && m < min)
66 return min/1000.0 - (ROBOT_AXIS_TO_FRONT_M - HOKUYO_CENTER_OFFSET_M);
69 static bool obstackle_in_front_if_turn(Trajectory *t)
74 f = free_space_in_front_of_robot();
76 if (fabs(t->initialTurnBy) < DEG2RAD(95) &&
77 f > 1.2*(ROBOT_WIDTH_M/2.0)) {
79 } else if (f > 1.5*ROBOT_AXIS_TO_BACK_M) {
88 * @brief FIXME Calculates and simplifies a path to goal position avoiding obstacles.
89 * @param start_in_future is delay utilised to calculate the trajectory while robot
90 * is moving (zero means to start movement from the current position)
91 * @return True on succes, false on error.
93 static target_status new_goal(struct move_target *move_target, double start_in_future)
97 PathPoint * path = NULL;
98 PathPoint * tmp_point = NULL;
99 Pos future_traj_point;
100 Point target(move_target->x, move_target->y);
101 Point center(PLAYGROUND_WIDTH_M/2.0, PLAYGROUND_HEIGHT_M/2.0);
103 bool backward = false;
105 // Where to start trajectory planning?
106 get_future_pos(start_in_future, future_traj_point, time_ts);
107 Point start(future_traj_point.x, future_traj_point.y);
109 Trajectory *t = new Trajectory(move_target->tc, backward);
111 if (move_target->heading.operation == TOP_ARRIVE_FROM) {
112 robot.move_helper.get_arrive_from_point(move_target->x, move_target->y, move_target->heading,
116 if (move_target->use_planning) {
117 /// Calculates the path between actual position to goal position. If the goal is not reachable returns -1.
118 switch (path_planner(start.x, start.y, target.x, target.y, &path, &angle)) {
119 case PP_ERROR_MAP_NOT_INIT:
120 ret = TARGET_ERROR; break;
121 case PP_ERROR_GOAL_IS_OBSTACLE:
122 case PP_ERROR_GOAL_NOT_REACHABLE:
123 ret = TARGET_INACC; break;
125 ret = TARGET_OK; break;
128 if (ret != TARGET_OK)
130 // Add this path to the trajectory.
131 for (tmp_point = path; tmp_point!=NULL; tmp_point=tmp_point->next) {
132 DBG("ADDING POINT (%f, %f)\n", tmp_point->x, tmp_point->y);
133 t->addPoint(tmp_point->x, tmp_point->y);
135 freePathMemory(path);
137 t->addPoint(target.x, target.y);
140 if (move_target->heading.operation == TOP_ARRIVE_FROM) {
141 t->addPoint(move_target->x, move_target->y);
143 t->finalHeading = target2final_heading(move_target->heading);
145 if (t->prepare(future_traj_point)) {
146 if (obstackle_in_front_if_turn(t))
149 if (start_in_future) {
150 /* not to switch immediately
151 switch_trajectory_at(t, time_ts); */
152 // new test - appending trajectories
168 * Starts moving on trajectory prepared by functions in
169 * movehelper.cc. In case of error it sends the proper event to MAIN
172 * @return TARGET_OK on succes, TARGET_ERROR on error.
174 static target_status new_trajectory(Trajectory *t)
179 ul_logerr("ERROR: No trajectory\n");
182 // FIXME: This is duplicite code with new_goal. Remove this
183 // function and use always new_goal.
184 robot.get_est_pos(pos.x, pos.y, pos.phi);
188 if (t->prepare(pos)) {
189 if (robot.check_turn_safety && obstackle_in_front_if_turn(t)) {
202 * Handles new target signal from main FSM
208 static target_status new_target(struct move_target *target)
211 if (target->trajectory) {
212 Trajectory *t = (Trajectory*)target->trajectory;
213 target->tc = t->constr;
214 ret = new_trajectory(t);
215 target->trajectory = NULL;
216 // Trajectory is deleted by somebody else
218 ret = new_goal(target, 0);
220 if (ret != TARGET_ERROR) {
221 current_target = *target;
227 * Recalculates trajectory to previous move target specified by
232 target_status recalculate_trajectory(void)
234 /* TODO: Different start for planning than esitmated position */
236 current_target.use_planning = true;
237 ret = new_goal(¤t_target, TRGEN_DELAY);
242 ul_logerr("Target error on recalculation_in_progress\n");
249 sem_getvalue(&recalculation_not_running, &val);
251 sem_post(&recalculation_not_running);
255 /*******************************************************************************
257 *******************************************************************************/
260 struct wait_for_command;
262 struct wait_and_try_again;
265 struct FSMMotion : boost::statechart::asynchronous_state_machine< FSMMotion, MotionBase, Scheduler > {
266 FSMMotion(my_context ctx) : my_base(ctx) {}
269 struct MotionBase : sc::simple_state< MotionBase, FSMMotion, wait_for_command > {
270 sc::result react(const sc::event_base &) {
271 // DBG_PRINT_EVENT("unhandled event");
272 return discard_event();
274 typedef sc::custom_reaction< sc::event_base > reactions;
278 struct wait_for_command : TimedSimpleState< wait_for_command, MotionBase > {
283 sc::result react(const evNewTarget &event) {
284 ret = new_target((move_target*)event.ev_ptr);
287 case TARGET_OK: return transit<movement>();
288 case TARGET_INACC: return transit<wait_and_try_again>();
290 robot.sched.queue_event(robot.MAIN, new evMotionDone());
291 ul_logerr("Target error\n");
293 return discard_event();
295 typedef sc::custom_reaction< evNewTarget > reactions;
298 struct movement : TimedSimpleState< movement, MotionBase > {
303 sc::result react(const evTrajectoryDone &) {
304 robot.sched.queue_event(robot.MAIN, new evMotionDone());
305 return transit<wait_for_command>();
307 sc::result react(const evObstacle &) {
308 if (obstacle_cntr++ > 5) {
309 robot.sched.queue_event(robot.MAIN, new evMotionError());
310 return transit<wait_for_command>();
312 switch (recalculate_trajectory()) {
313 // We can go to the target:
314 case TARGET_OK: return discard_event();
315 // Target inaccessible because of an obstacle
316 case TARGET_INACC: return transit<wait_and_try_again>();
317 // Fatal error during planning
319 robot.sched.queue_event(robot.MAIN, new evMotionDone());
320 ul_logerr("Target error\n");
321 /* FIXME (Filip): shouldn't transition to wait_for_command be here?
322 No, probably not, if an error occurs, robot won't move any more
323 and we can recognize the error. */
325 return discard_event();
327 sc::result react(const evObstacleBehind &) {
328 if (robot.moves_backward && robot.use_rear_bumper)
329 return transit<wait_and_try_again>();
330 return discard_event();
332 sc::result react(const evObstacleSide &) {
333 if ((robot.use_left_bumper && robot.orte.robot_bumpers.bumper_left) ||
334 (robot.use_right_bumper && robot.orte.robot_bumpers.bumper_right))
335 return transit<wait_and_try_again>();
336 return discard_event();
339 sc::transition< evTrajectoryLost, lost >,
340 sc::transition< evMoveStop, wait_for_command >,
341 sc::custom_reaction< evObstacleSide >,
342 sc::custom_reaction< evObstacleBehind >,
343 sc::custom_reaction< evObstacle >,
344 sc::custom_reaction< evTrajectoryDone > > reactions;
348 struct lost : TimedState< lost, MotionBase > {
350 lost(my_context ctx) : base_state(ctx) {
352 runTimer(lost_tout, 1000, new evTimer());
354 sc::result react(const evTimer &) {
355 switch (recalculate_trajectory()) {
356 case TARGET_OK: return transit<movement>();
357 case TARGET_INACC: return transit<wait_and_try_again>();
359 robot.sched.queue_event(robot.MAIN, new evMotionDone());
360 ul_logerr("Target error\n");
362 return discard_event();
365 sc::custom_reaction< evTimer >,
366 sc::transition< evMoveStop, wait_for_command > > reactions;
369 struct wait_and_try_again : TimedState< wait_and_try_again, MotionBase > {
371 wait_and_try_again(my_context ctx) : base_state(ctx) {
373 runTimer(wait_tout, 1000, new evTimer());
375 sc::result react(const evTimer &) {
376 switch (recalculate_trajectory()) {
377 // We can go to the target:
378 case TARGET_OK: return transit<movement>();
379 // Target inaccessible because of an obstacle
381 ul_logerr("Inaccessible\n");
382 return transit<wait_and_try_again>();
383 /* FIXME (Filip): this could happen forever */
384 // Fatal error during planning
386 robot.sched.queue_event(robot.MAIN, new evMotionDone());
387 ul_logerr("Target error\n");
389 return discard_event();
391 sc::result react(const evTrajectoryDone &) {
392 robot.sched.queue_event(robot.MAIN, new evTrajectoryDone());
393 return transit<wait_for_command>();
396 sc::custom_reaction< evTimer >,
397 sc::custom_reaction< evTrajectoryDone >,
398 sc::transition< evMoveStop, wait_for_command > > reactions;