// Generate next movement
bool BufferingPlayer::move(double deadline, Motion& motion_output) {
initialize();
CGAL::Timer timer;
timer.start();
// Make sure the motion buffer is not empty before we proceed
while ((timer.time() < deadline) && !has_buffered_motion()) {
TIMED_TRACE_ACTION("move", "no pending movement");
plan(deadline);
}
if (timer.time() < deadline) {
TIMED_TRACE_ACTION("move", "additional motion found");
} else {
TIMED_TRACE_ACTION("move", "could not find additional movement");
return true;
}
// Attempt to perform the first motion in the buffer
Motion& motion = motion_buffer.front();
MS_base* step = NULL;
bool blocked = false;
while (!motion.empty() && !blocked && (deadline - timer.time() > 0)) {
// Get the next step
motion.cut_step(deadline - timer.time(), motion_output);
step = motion_output.back();
deadline -= Motion::step_time(step);
// Validate it
Extended_polygon robot_a = env->get_robot_a();
robot_a.move_absolute(step->source());
Extended_polygon robot_b = env->get_robot_b();
robot_b.move_absolute(dynamic_obstacle);
VALIDATION vResult = planner.validate_step(*step, robot_a, robot_b);
switch (vResult) {
case OK:
// Step is valid
continue;
case PATH_BLOCKED:
// Path is blocked but destination is free
case DST_BLOCKED:
// Destination is blocked
// Return it to the buffer
motion.add_motion_step_front(step);
// Don't output this invalid step
motion_output.pop_back();
// TODO find the entire gap and find a path around it
// Right now we can't do anything useful with our remaining time
blocked = true;
break;
}
}
if (step == NULL) {
return false;
}
if (motion.empty()) {
// Executed the entire motion
buffered_targets.pop_front();
motion_buffer.pop_front();
}
return !blocked;
}
