// --------------------------------------------------------------------------
// commence le movement qui va vers l'entree d'un pont juste un peu decalle en y (~15cm)
// --------------------------------------------------------------------------
bool StrategyAttackCL::gotoBridgeEntryNear(Millimeter y)
{
Trajectory t;
disableBridgeCaptors();
LOG_COMMAND("gotoBridgeEntry Hard Near:%d\n", (int)y);
// on ne va pas loin en y donc on fait un joli mouvement en S
// met les servos en position
Millimeter y2 = RobotPos->y();
if (y>1800) { // pont du bord
Move->go2Target(Point(BRIDGE_ENTRY_NAVIGATE_X, y2), 3, 30);
} else if (y<1500) { // pont du bord ou pont contre le milieu
Move->go2Target(Point(BRIDGE_ENTRY_NAVIGATE_X, y2), 3, 30);
} else { // ponts au milieu
t.push_back(Point(BRIDGE_DETECT_BUMP_X-100, y2));
t.push_back(Point(BRIDGE_ENTRY_NAVIGATE_X, (2*y+y2)/3));
Move->followTrajectory(t, TRAJECTORY_RECTILINEAR, 3, 30);
}
Events->wait(evtEndMove);
Move->enableAccelerationController(false);
if (checkEndEvents()) return false;
if(!Events->isInWaitResult(EVENTS_MOVE_END)) {
// collision
Move->backward(100);
Events->wait(evtEndMoveNoCollision);
return false;
}
// va en face du pont
enableBridgeCaptors();
MvtMgr->setRobotDirection(MOVE_DIRECTION_FORWARD);
Move->go2Target(BRIDGE_DETECT_BUMP_X, y, 2, 30);
// dont need to wait, it is done by the upper function
return true;
}
// --------------------------------------------------------------------------
// commence le movement qui va vers l'entree d'un pont tres decalle en y (>20cm)
// --------------------------------------------------------------------------
bool StrategyAttackCL::gotoBridgeEntryFar(Millimeter y)
{
disableBridgeCaptors();
Millimeter y2 = RobotPos->y();
LOG_COMMAND("gotoBridgeEntry Hard Far:%d\n", (int)y);
// on va loin: on recule bettement puit on prend un point cible
// on s'eloigne un peu du bord, pour aller en x qui nous permet
//de nous promener tranquillement le long de la riviere
Move->go2Target(BRIDGE_ENTRY_NAVIGATE_X, y2, 2, 40); // norotation
Events->wait(evtEndMove);
if (checkEndEvents() ||
!Events->isInWaitResult(EVENTS_MOVE_END)) {
Move->enableAccelerationController(false);
return false;
}
// va en face du pont
enableBridgeCaptors();
Trajectory t;
// Pour eviter les rotations finales on
if (y2 < y)
t.push_back(Point(BRIDGE_ENTRY_NAVIGATE_X, y-75));
else
t.push_back(Point(BRIDGE_ENTRY_NAVIGATE_X, y+75));
t.push_back(Point(BRIDGE_DETECT_BUMP_X, y));
MvtMgr->setRobotDirection(MOVE_DIRECTION_FORWARD);
Move->followTrajectory(t, TRAJECTORY_RECTILINEAR, 2, 30, true); // norotation
// dont wait events, it is done by the upper function
return true;
}
// --------------------------------------------------------------------------
// va a l'endroit ou on detecte les pont par capteurs sharps
// --------------------------------------------------------------------------
bool StrategyAttackCL::gotoBridgeDetection(bool gotoSiouxFirst)
{
Events->disable(EVENTS_NO_BRIDGE_BUMP_LEFT);
Events->disable(EVENTS_NO_BRIDGE_BUMP_RIGHT);
while(true) {
LOG_INFO("gotoBridgeDetection(%s)\n",
gotoSiouxFirst?"Passe par le milieu":"Passe par un pont normal");
Trajectory t;
t.push_back(Point(RobotPos->x(), RobotPos->y()));
// if looping (while(true)..) be careful not to go too close.
if (RobotPos->x() < BRIDGE_DETECT_SHARP_X - 500)
t.push_back(Point(RobotPos->x()+250, RobotPos->y()));
if (gotoSiouxFirst) {
// va vers le pont du milieu
t.push_back(Point(BRIDGE_DETECT_SHARP_X-250, BRIDGE_ENTRY_SIOUX_Y));
t.push_back(Point(BRIDGE_DETECT_SHARP_X, BRIDGE_ENTRY_SIOUX_Y));
} else {
// va vers la gauche du terrain pour detecter la position du pont d'un coup
t.push_back(Point(BRIDGE_DETECT_SHARP_X-250, BRIDGE_ENTRY_MIDDLE_BORDURE_Y));
t.push_back(Point(BRIDGE_DETECT_SHARP_X, BRIDGE_ENTRY_MIDDLE_BORDURE_Y));
}
Move->followTrajectory(t, TRAJECTORY_RECTILINEAR, -1, 40); // gain, vitesse max
Events->wait(evtEndMove);
if (checkEndEvents()) return false;
if (Events->isInWaitResult(EVENTS_MOVE_END)) {
Move->rotate(0);
Move->stop();
Events->wait(evtEndMove);
if (checkEndEvents()) return false;
if (Events->isInWaitResult(EVENTS_MOVE_END)) {
return true;
}
return true;
}
if (RobotPos->x()>600 || Timer->time() > 6000) {
// collision: on recule et on essaye de repartir par un autre endroit...
Move->backward(150);
Events->wait(evtEndMoveNoCollision);
gotoSiouxFirst = !gotoSiouxFirst;
}
if (isZeroAngle(RobotPos->thetaAbsolute() - M_PI, M_PI_2) &&
RobotPos->x() < 600)
{
Move->backward(700);
Events->wait(evtEndMoveNoCollision);
gotoSiouxFirst = !gotoSiouxFirst;
}
}
return false;
}
// ---------------------------------------------------------------
// strategie exploration en passant par la rangee en y = 1350
// ---------------------------------------------------------------
bool StrategyLargeAttackCL::preDefinedSkittleExploration2()
{
LOG_COMMAND("preDefinedLargeSkittleExploration2\n");
MvtMgr->setRobotDirection(MOVE_DIRECTION_FORWARD);
bool noRotation = true;
Point lineStart(2500, 1500);
Point lineEnd(3200, 1500);
Move->go2Target(lineStart,
MOVE_USE_DEFAULT_GAIN,
MOVE_USE_DEFAULT_SPEED,
noRotation);
Events->wait(evtEndMove);
if (!Events->isInWaitResult(EVENTS_MOVE_END)) {
// on n'a pas reussi
if (checkEndEvents()) return false; // c'est la fin du match?
CollisionEnum collision = checkCollisionEvents();
if (collision != COLLISION_NONE) {
if (!handleCollision(collision, lineStart, lineEnd))
return false;
} else {
LOG_WARNING("don't know what caused abort of movement. -> abort predefined exploration\n");
return false;
}
} else {
// go2Target succeeded its movement.
Move->rotate(0); // face right border
Events->wait(evtEndMove);
if (!Events->isInWaitResult(EVENTS_MOVE_END)) {
if (checkEndEvents()) return false; // end of match
// ok. normally the collision can only be on the left side...
CollisionEnum collision = checkCollisionEvents();
if (collision == COLLISION_LEFT) {
if (!handleCollision(collision, lineStart, lineEnd))
return false;
} else if (collision == COLLISION_NONE) {
LOG_WARNING("unhandled event. leaving function\n");
return false;
} else {
LOG_WARNING("collision, but most likely not a support\n");
return false;
}
}
}
bool endOfLine = false;
while (!endOfLine) {
Move->go2Target(lineEnd);
Events->wait(evtEndMove);
if (!Events->isInWaitResult(EVENTS_MOVE_END)) {
// let's hope it's a support.
if (checkEndEvents()) return false; // c'est la fin du match?
CollisionEnum collision = checkCollisionEvents();
if (collision != COLLISION_NONE) {
if (!handleCollision(collision, lineStart, lineEnd))
return false;
} else {
LOG_WARNING("don't know what caused abort of movement. -> abort predefined exploration\n");
return false;
}
if (!RobotPos->isTargetForward(lineEnd))
endOfLine = true;
} else {
// ok. no more supports detected
endOfLine = true;
}
}
LOG_INFO("predefined-large finished.\n");
//alignBorder();
// on recule un petit peu car on ne sais pas ce qu'on va se prendre en
// approchant du robot adverse!, mieux vaut tenir que courir
MvtMgr->setRobotDirection(MOVE_DIRECTION_BACKWARD);
Trajectory t;
t.clear();
t.push_back(Point(3144, 1350));
t.push_back(Point(3190, 1700));
Move->followTrajectory(t, TRAJECTORY_BASIC);
Events->wait(evtEndMove);
if (!Events->isInWaitResult(EVENTS_MOVE_END)) {
// on n'a pas reussi
// c'est la fin du match?
if (checkEndEvents()) return false;
// TODO manage collisions
return false;
}
//alignBorder();
// on va droit sur l'adversaire
MvtMgr->setRobotDirection(MOVE_DIRECTION_FORWARD);
Move->go2Target(Point(3190, 650));
Events->wait(evtEndMove);
if (!Events->isInWaitResult(EVENTS_MOVE_END)) {
// on n'a pas reussi
// c'est la fin du match?
if (checkEndEvents()) return false;
// TODO manage collisions
return false;
}
// on recule un peu
MvtMgr->setRobotDirection(MOVE_DIRECTION_BACKWARD);
Move->go2Target(Point(3190, 1050));
Events->wait(evtEndMove);
if (!Events->isInWaitResult(EVENTS_MOVE_END)) {
// on n'a pas reussi
// c'est la fin du match?
if (checkEndEvents()) return false;
// TODO manage collisions
return false;
}
t.clear();
t.push_back(Point(2550, 1050));
t.push_back(Point(2594, 1650));
t.push_back(Point(3340, 1650));
MvtMgr->setRobotDirection(MOVE_DIRECTION_FORWARD);
Move->followTrajectory(t, TRAJECTORY_BASIC);
Events->wait(evtEndMove);
if (!Events->isInWaitResult(EVENTS_MOVE_END)) {
// on n'a pas reussi
// c'est la fin du match?
if (checkEndEvents()) return false;
// TODO manage collisions
return false;
}
MvtMgr->setRobotDirection(MOVE_DIRECTION_BACKWARD);
Move->go2Target(Point(3190, 1650));
Events->wait(evtEndMove);
if (!Events->isInWaitResult(EVENTS_MOVE_END)) {
// on n'a pas reussi
// c'est la fin du match?
if (checkEndEvents()) return false;
// TODO manage collisions
return false;
}
return true;
}
// ---------------------------------------------------------------
// strategie exploration en passant par la rangee en y = 1650
// ---------------------------------------------------------------
bool StrategyLargeAttackCL::preDefinedSkittleExploration1()
{
return preDefinedSkittleExploration2();
LOG_COMMAND("preDefinedLargeSkittleExploration1\n");
Trajectory t;
t.push_back(Point(2640, 1650));
t.push_back(Point(3240, 1650));
MvtMgr->setRobotDirection(MOVE_DIRECTION_FORWARD);
Move->followTrajectory(t, TRAJECTORY_BASIC);
Events->wait(evtEndMove);
if (!Events->isInWaitResult(EVENTS_MOVE_END)) {
// on n'a pas reussi
// c'est la fin du match?
if (checkEndEvents()) return false;
// TODO manage collisions
return false;
}
//alignBorder();
MvtMgr->setRobotDirection(MOVE_DIRECTION_FORWARD);
Move->rotate(-M_PI_2);
Events->wait(evtEndMove);
if (!Events->isInWaitResult(EVENTS_MOVE_END)) {
// on n'a pas reussi
// c'est la fin du match?
if (checkEndEvents()) return false;
// TODO manage collisions
return false;
}
//alignBorder();
MvtMgr->setRobotDirection(MOVE_DIRECTION_FORWARD);
Move->go2Target(Point(3190, 650));
Events->wait(evtEndMove);
if (!Events->isInWaitResult(EVENTS_MOVE_END)) {
// on n'a pas reussi
// c'est la fin du match?
if (checkEndEvents()) return false;
// TODO manage collisions
return false;
}
MvtMgr->setRobotDirection(MOVE_DIRECTION_BACKWARD);
Move->go2Target(Point(3190, 1050));
Events->wait(evtEndMove);
if (!Events->isInWaitResult(EVENTS_MOVE_END)) {
// on n'a pas reussi
// c'est la fin du match?
if (checkEndEvents()) return false;
// TODO manage collisions
return false;
}
t.clear();
t.push_back(Point(2550, 1050));
t.push_back(Point(2594, 1400));
t.push_back(Point(3340, 1350));
MvtMgr->setRobotDirection(MOVE_DIRECTION_FORWARD);
Move->followTrajectory(t, TRAJECTORY_BASIC);
Events->wait(evtEndMove);
if (!Events->isInWaitResult(EVENTS_MOVE_END)) {
// on n'a pas reussi
// c'est la fin du match?
if (checkEndEvents()) return false;
// TODO manage collisions
return false;
}
MvtMgr->setRobotDirection(MOVE_DIRECTION_BACKWARD);
Move->go2Target(Point(3190, 1350));
Events->wait(evtEndMove);
if (!Events->isInWaitResult(EVENTS_MOVE_END)) {
// on n'a pas reussi
// c'est la fin du match?
if (checkEndEvents()) return false;
// TODO manage collisions
return false;
}
return true;
}
void RRT::optimize_trajectory(){
Trajectory traj;
double length, new_length;
double v_max = world.leader->get_limits().vMax*0.8;
double c_max = world.leader->get_limits().cMax;
double c_min = world.leader->get_limits().cMin;
State st = states[0],
st2;
for (int i = 0; i < out_trajectory.size(); ++i) {
traj.push_back(Desc(st,out_trajectory[i]));
st2 = world.leader->calculate_state(st, out_trajectory[i]);
st = st2;
}
traj.push_back(Desc(st,Control()));
#ifdef DEBUG_RRT
length = 0;
for (int i = 0; i < traj.size(); ++i) {
length += traj[i].control.t * traj[i].control.v;
}
TimeMeasurement tm;
tm.start();
std::cout << "RRT: original trajectory length: " << length << std::endl;
/*
std::cout << std::endl << "LEADER PLANNED POSITIONS:" << std::endl;
for (int i = 0; i < traj.size(); ++i) {
std::cout << " X = [ " << traj[i].state.X(0) << ", " << traj[i].state.X(1) << ", " << traj[i].state.X(2) << " ]" << std::endl;
}
*/
#endif
int loop = 0;
bool isValid;
bool endPoint;
while(loop < 100){
++loop;
int n_first = rand() % traj.size();
int n_second = rand() % traj.size();
while(n_first == n_second){
n_second = rand() % traj.size();
}
if (n_first > n_second){
const int p = n_first;
n_first = n_second;
n_second = p;
}
length = 0;
for (int i = n_first; i < n_second; ++i) {
length += traj[i].control.t * fabs(traj[i].control.v);
}
endPoint = false;
if (n_second != traj.size()-1){
dubins = new geom::Dubins(geom::Position(geom::Point(traj[n_first].state.X(0),traj[n_first].state.X(1)), traj[n_first].state.phi),
geom::Position(geom::Point(traj[n_second].state.X(0),traj[n_second].state.X(1)), traj[n_second].state.phi),
1/world.leader->cMax());
}else{
endPoint = true;
dubins = new geom::Dubins(geom::Position(geom::Point(traj[n_first].state.X(0),traj[n_first].state.X(1)), traj[n_first].state.phi),
geom::Point(goal[0],goal[1]),
1/world.leader->cMax());
length += dist2D(traj.back().state.X,Vector(goal[0],goal[1],0));
}
new_length = dubins->getLength();
if (new_length < length){
#ifdef DEBUG_RRT_DUBINS
if (endPoint){
std::cout << "DUBINS: original part length : " << length << std::endl;
std::cout << " simplified part length : " << new_length << std::endl;
std::cout << " type of Dubins maneuver: " << dubins->getTypeOfManeuver() << " [RSR, LSL, LSR, RSL, RLR, LRL, LS, RS]" << std::endl;
}
#endif
Trajectory new_traj;
for (int i = 0; i < n_first; ++i) {
new_traj.push_back(traj[i]);
}
double t1, t2, t3, c1, c2, c3;
State state_first = traj[n_first].state;
Control cnt;
switch (dubins->getTypeOfManeuver()) {
case geom::RSR:
t1 = dubins->getRadius()*fabs(dubins->getLen1())/v_max;
t2 = dubins->getLen2()/v_max;
t3 = dubins->getRadius()*fabs(dubins->getLen3())/v_max;
c1 = c_min;
c2 = 0;
c3 = c_min;
break;
case geom::LSL:
t1 = dubins->getRadius()*fabs(dubins->getLen1())/v_max;
t2 = dubins->getLen2()/v_max;
t3 = dubins->getRadius()*fabs(dubins->getLen3())/v_max;
c1 = c_max;
c2 = 0;
c3 = c_max;
break;
case geom::LSR:
t1 = dubins->getRadius()*fabs(dubins->getLen1())/v_max;
t2 = dubins->getLen2()/v_max;
t3 = dubins->getRadius()*fabs(dubins->getLen3())/v_max;
c1 = c_max;
c2 = 0;
c3 = c_min;
break;
case geom::RSL:
t1 = dubins->getRadius()*fabs(dubins->getLen1())/v_max;
t2 = dubins->getLen2()/v_max;
t3 = dubins->getRadius()*fabs(dubins->getLen3())/v_max;
c1 = c_min;
c2 = 0;
c3 = c_max;
break;
case geom::RLR:
t1 = dubins->getRadius()*fabs(dubins->getLen1())/v_max;
t2 = dubins->getRadius()*fabs(dubins->getLen2())/v_max;
t3 = dubins->getRadius()*fabs(dubins->getLen3())/v_max;
c1 = c_min;
c2 = c_max;
c3 = c_min;
break;
case geom::LRL:
t1 = dubins->getRadius()*fabs(dubins->getLen1())/v_max;
t2 = dubins->getRadius()*fabs(dubins->getLen2())/v_max;
t3 = dubins->getRadius()*fabs(dubins->getLen3())/v_max;
c1 = c_max;
c2 = c_min;
c3 = c_max;
break;
default:
break;
}
State s_new, s_old = state_first;
int n_added = 0;
if (t1 > eps){
while (t1-RRT_TIME_STEP>eps){
cnt = Control(v_max,0,c1,RRT_TIME_STEP);
new_traj.push_back(Desc(s_old,cnt));
t1 -= RRT_TIME_STEP;
s_new = world.leader->calculate_state(s_old, cnt);
s_old = s_new;
++n_added;
}
cnt = Control(v_max,0,c1,t1);
new_traj.push_back(Desc(s_old,cnt));
s_new = world.leader->calculate_state(s_old, cnt);
s_old = s_new;
++n_added;
}
if (t2 > eps){
while (t2-RRT_TIME_STEP>eps){
cnt = Control(v_max,0,c2,RRT_TIME_STEP);
new_traj.push_back(Desc(s_old,cnt));
t2 -= RRT_TIME_STEP;
s_new = world.leader->calculate_state(s_old, cnt);
s_old = s_new;
++n_added;
}
cnt = Control(v_max,0,c2,t2);
new_traj.push_back(Desc(s_old,cnt));
s_new = world.leader->calculate_state(s_old, cnt);
s_old = s_new;
++n_added;
}
if (t3 > eps){
while (t3-RRT_TIME_STEP>eps){
cnt = Control(v_max,0,c3,RRT_TIME_STEP);
new_traj.push_back(Desc(s_old,cnt));
t3 -= RRT_TIME_STEP;
s_new = world.leader->calculate_state(s_old, cnt);
s_old = s_new;
++n_added;
}
cnt = Control(v_max,0,c3,t3);
new_traj.push_back(Desc(s_old,cnt));
++n_added;
}
isValid = true;
for (int i = 0; i < n_added; ++i) {
const int index = new_traj.size()-n_added+i;
Points pts = world.leader->getSampledTrajectory(new_traj[index].state,&new_traj[index].control,1);
for (int j = 0; j < pts.size(); ++j) {
isValid &= world.map->wall_distance(pts[j]) > world.leader->get_ra();
if (!isValid)
break;
}
if (!isValid)
break;
}
if (!isValid){
continue;
}
loop = 0;
if (!endPoint){
for (int i = n_second; i < traj.size(); ++i) {
new_traj.push_back(traj[i]);
}
}else{
s_new = world.leader->calculate_state(new_traj.back().state, new_traj.back().control);
new_traj.push_back(Desc(State(goal[0],goal[1],0,0),Control()));
}
traj = new_traj;
}
}
out_trajectory.clear();
for (int i = 0; i < traj.size()-1; ++i) {
out_trajectory.push_back(traj[i].control);
}
#ifdef DEBUG_RRT
length = 0;
for (int i = 0; i < traj.size(); ++i) {
length += traj[i].control.t * traj[i].control.v;
}
tm.end();
std::cout << "RRT: optimized trajectory length: " << length << std::endl;
std::cout << " size: " << traj.size()-1 << std::endl;
/*
std::cout << std::endl;
for (int i = 0; i < traj.size(); ++i) {
std::cout << " v = " << traj[i].control.v << ", w = " << traj[i].control.w << ", c = " << traj[i].control.c << ", t = " << traj[i].control.t << std::endl;
}
*/
#endif
}