コード例 #1
0
 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");
 }
コード例 #2
0
ファイル: mathHelper.cpp プロジェクト: xufango/contrib_bk
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;
}
コード例 #3
0
// ---------------------------------------------------------------
// 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;
}
コード例 #4
0
// ---------------------------------------------------------------
// 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;
}