void traj_cb(const occgrid_planner::TrajectoryConstPtr & msg) {
frame_id_ = msg->header.frame_id;
delay_ = 0.0;
traj_.clear();
for (unsigned int i=0;i<msg->Ts.size();i++) {
traj_.insert(Trajectory::value_type(msg->Ts[i].header.stamp.toSec(), msg->Ts[i]));
}
ROS_INFO("Trajectory received");
}
bool MathHelper::generateMinJerkTrajectory(const yarp::sig::Vector &start, const yarp::sig::Vector &goal, const double duration,
const double deltaT, Trajectory &trajectory)
{
int trajectoryDimension = trajectory.getDimension();
if ((trajectoryDimension % POS_VEL_ACC) != 0)
{
printf("ERROR: Trajectory dimension (%i) must be a multiple of 3 to contain position, velocity, and acceleration information for each trace.\n",
trajectoryDimension);
return false;
}
trajectoryDimension /= POS_VEL_ACC;
if ((trajectoryDimension != start.size()) || (trajectoryDimension != goal.size()))
{
printf("ERROR: Trajectory dimension (%i), does not match start (%i) and goal (%i).\n", trajectoryDimension, start.size(), goal.size());
return false;
}
trajectory.clear();
int numSteps = static_cast<int> (duration / deltaT);
yarp::sig::Vector tmpCurrent = yarp::math::zeros(POS_VEL_ACC);
yarp::sig::Vector tmpGoal = yarp::math::zeros(POS_VEL_ACC);
yarp::sig::Vector tmpNext = yarp::math::zeros(POS_VEL_ACC);
yarp::sig::Vector next = yarp::math::zeros(trajectoryDimension * POS_VEL_ACC);
Eigen::VectorXd eigenNext;
//add first trajectory point
for (int j = 0; j < trajectoryDimension; j++)
{
next(j * POS_VEL_ACC + 0) = start(j);
next(j * POS_VEL_ACC + 1) = 0;
next(j * POS_VEL_ACC + 2) = 0;
}
yarpToEigenVector(next, eigenNext);
if (!trajectory.add(eigenNext))
{
printf("ERROR: Could not add first trajectory point.\n");
return false;
}
for (int i = 1; i < numSteps; i++)
{
for (int j = 0; j < trajectoryDimension; j++)
{
if (i == 1)
{
tmpCurrent(0) = start(j);
}
else
{
//update the current state
for (int k = 0; k < POS_VEL_ACC; k++)
{
tmpCurrent(k) = next(j * POS_VEL_ACC + k);
}
}
tmpGoal(0) = goal(j);
if (!MathHelper::calculateMinJerkNextStep(tmpCurrent, tmpGoal, duration - ((i - 1) * deltaT), deltaT, tmpNext))
{
printf("ERROR: Could not compute next minimum jerk trajectory point.\n");
return false;
}
for (int k = 0; k < POS_VEL_ACC; k++)
{
next(j * POS_VEL_ACC + k) = tmpNext(k);
}
}
yarpToEigenVector(next, eigenNext);
if (!trajectory.add(eigenNext))
{
printf("ERROR: Could not add next trajectory point.\n");
return false;
}
}
if(!trajectory.writeTrajectoryToFile("data/minJerkTrajectory.txt")) return false;
return true;
}
// ---------------------------------------------------------------
// 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;
}