当前位置: 首页>>代码示例>>C++>>正文

C++ Trajectory::push_back方法代码示例

本文整理汇总了C++中Trajectory::push_back方法的典型用法代码示例。如果您正苦于以下问题：C++ Trajectory::push_back方法的具体用法？C++ Trajectory::push_back怎么用？C++ Trajectory::push_back使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Trajectory的用法示例。


在下文中一共展示了Trajectory::push_back方法的6个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。

示例1: gotoBridgeEntryNear

// --------------------------------------------------------------------------
// 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;
}
开发者ID:BackupTheBerlios，项目名称:robotm6，代码行数:38，代码来源:attackBridge.cpp

示例2: gotoBridgeEntryFar

// --------------------------------------------------------------------------
// 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;
}
开发者ID:BackupTheBerlios，项目名称:robotm6，代码行数:33，代码来源:attackBridge.cpp

示例3: gotoBridgeDetection

// --------------------------------------------------------------------------
// 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;
}
开发者ID:BackupTheBerlios，项目名称:robotm6，代码行数:54，代码来源:attackBridge.cpp

示例4: lineStart

// ---------------------------------------------------------------
// 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?
//.........这里部分代码省略.........
开发者ID:BackupTheBerlios，项目名称:robotm6，代码行数:101，代码来源:attackLargeExplore.cpp

示例5: 

// ---------------------------------------------------------------
// 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;
}
开发者ID:BackupTheBerlios，项目名称:robotm6，代码行数:86，代码来源:attackLargeExplore.cpp

示例6: optimize_trajectory

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;
//.........这里部分代码省略.........
开发者ID:racinmat，项目名称:AutonomousSurveillanceBachelorThesis，代码行数:101，代码来源:rrt.cpp


注：本文中的Trajectory::push_back方法示例由纯净天空整理自Github/MSDocs等开源代码及文档管理平台，相关代码片段筛选自各路编程大神贡献的开源项目，源码版权归原作者所有，传播和使用请参考对应项目的License；未经允许，请勿转载。