C++ ArRobot::stop方法代码示例

    void manualControlHandler(){


    	if(firstCall){
    		firstCall=false;
    		showMenu();
    	}

    	if(keyPressedBefore && !driver->estaEjecutando()){
    		keyPressedBefore=false;
    		showMenu();
    	}

    	if(mrpt::system::os::kbhit() ){
    		char c = mrpt::system::os::getch();
    		keyPressedBefore=true;


    		switch(c){

    		case '\033':

    			c=mrpt::system::os::getch(); // skip the [
    			cout << "Siguiente caracter" << (int)c << endl;
    			double v;
    			switch(mrpt::system::os::getch()) { // the real value
    			case 'A':
    				// code for arrow up
    				v=robot->getVel();
    				robot->setVel(v+50);
    				break;
    			case 'B':
    				// code for arrow down
    				v=robot->getVel();
    				robot->setVel(v-50);
    				break;
    			case 'C':
    				// code for arrow right
    				v=robot->getRotVel();
    				robot->setRotVel(v-5);
    				break;
    			case 'D':
    				// code for arrow left
    				v=robot->getRotVel();
    				robot->setRotVel(v+5);
    				break;
    			}
    			break;

    			case ' ':
    				robot->stop();
    				break;


    		case 'x':
    			//Aria::shutdown();
    			driver->stopRunning();
    			robot->stopRunning();
    			break;

    		case 'c':
    			guardarContinuo();
    			break;

    		case 'p':
    			start();
    			break;

    		case 'g':
    			guardar();
    			break;


    		case 'w':
    			guardarTrayectoria();
    			break;

    		case 't':
    			testParado();
    			break;

    		case 's':
    			stop();
    			break;
    		}

    	}
    }

int main(int argc, char **argv)
{

  Aria::init();
  ArRobot robot;
  ArArgumentParser parser(&argc, argv);
  parser.loadDefaultArguments();

  ArLog::log(ArLog::Terse, "WARNING: this program does no sensing or avoiding of obstacles, the robot WILL collide with any objects in the way! Make sure the robot has approximately 3 meters of free space on all sides.");

  // ArRobotConnector connects to the robot, get some initial data from it such as type and name,
  // and then loads parameter files for this robot.
  ArRobotConnector robotConnector(&parser, &robot);
  if(!robotConnector.connectRobot())
  {
    ArLog::log(ArLog::Terse, "simpleMotionCommands: Could not connect to the robot.");
    if(parser.checkHelpAndWarnUnparsed())
    {
        Aria::logOptions();
        Aria::exit(1);
        return 1;
    }
  }
  if (!Aria::parseArgs())
  {
    Aria::logOptions();
    Aria::exit(1);
    return 1;
  }
  
  ArLog::log(ArLog::Normal, "simpleMotionCommands: Connected.");

  // Start the robot processing cycle running in the background.
  // True parameter means that if the connection is lost, then the 
  // run loop ends.
  robot.runAsync(true);

  // Print out some data from the SIP.  

  // We must "lock" the ArRobot object
  // before calling its methods, and "unlock" when done, to prevent conflicts
  // with the background thread started by the call to robot.runAsync() above.
  // See the section on threading in the manual for more about this.
  // Make sure you unlock before any sleep() call or any other code that will
  // take some time; if the robot remains locked during that time, then
  // ArRobot's background thread will be blocked and unable to communicate with
  // the robot, call tasks, etc.
  
  robot.lock();
  ArLog::log(ArLog::Normal, "simpleMotionCommands: Pose=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Rot. Vel=%.2f, Battery=%.2fV",
    robot.getX(), robot.getY(), robot.getTh(), robot.getVel(), robot.getRotVel(), robot.getBatteryVoltage());
  robot.unlock();

  // Sleep for 3 seconds.
  ArLog::log(ArLog::Normal, "simpleMotionCommands: Will start driving in 3 seconds...");
  ArUtil::sleep(3000);

  // Set forward velocity to 50 mm/s
  ArLog::log(ArLog::Normal, "simpleMotionCommands: Driving forward at 250 mm/s for 5 sec...");
  robot.lock();
  robot.enableMotors();
  robot.setVel(250);
  robot.unlock();
  ArUtil::sleep(5000);

  ArLog::log(ArLog::Normal, "simpleMotionCommands: Stopping.");
  robot.lock();
  robot.stop();
  robot.unlock();
  ArUtil::sleep(1000);

  ArLog::log(ArLog::Normal, "simpleMotionCommands: Rotating at 10 deg/s for 5 sec...");
  robot.lock();
  robot.setRotVel(10);
  robot.unlock();
  ArUtil::sleep(5000);

  ArLog::log(ArLog::Normal, "simpleMotionCommands: Rotating at -10 deg/s for 10 sec...");
  robot.lock();
  robot.setRotVel(-10);
  robot.unlock();
  ArUtil::sleep(10000);

  ArLog::log(ArLog::Normal, "simpleMotionCommands: Driving forward at 150 mm/s for 5 sec...");
  robot.lock();
  robot.setRotVel(0);
  robot.setVel(150);
  robot.unlock();
  ArUtil::sleep(5000);

  ArLog::log(ArLog::Normal, "simpleMotionCommands: Stopping.");
  robot.lock();
  robot.stop();
  robot.unlock();
  ArUtil::sleep(1000);


  // Other motion command functions include move(), setHeading(),
  // setDeltaHeading().  You can also adjust acceleration and deceleration
  // values used by the robot with setAccel(), setDecel(), setRotAccel(),
//.........这里部分代码省略.........

//.........这里部分代码省略.........
	robot.lock();
	Follow follow = Follow(&robot,&sonar);
	GoTo goTo(&robot,&pathTask,&arMap);
	Search s(&robot,&sonar);

	// Bumpers.
  	ArBumpers bumpers;
  	robot.addRangeDevice(&bumpers);
  	pathTask.addRangeDevice(&bumpers, ArPathPlanningTask::CURRENT);

  	// Forbidden regions from the map
  	ArForbiddenRangeDevice forbidden(&arMap);
  	robot.addRangeDevice(&forbidden);
  	pathTask.addRangeDevice(&forbidden, ArPathPlanningTask::CURRENT);

  	// Mode To stop and remain stopped:
  	ArServerModeStop modeStop(&moServer, &robot);

  	// Action to slow down robot when localization score drops but not lost.
  	ArActionSlowDownWhenNotCertain actionSlowDown(&locTask);
  	pathTask.getPathPlanActionGroup()->addAction(&actionSlowDown, 140);

  	// Action to stop the robot when localization is "lost" (score too low)
  	ArActionLost actionLostPath(&locTask, &pathTask);
  	pathTask.getPathPlanActionGroup()->addAction(&actionLostPath, 150);

	// These provide various kinds of information to the client:
	ArServerInfoRobot serverInfoRobot(&moServer, &robot);
	ArServerInfoSensor serverInfoSensor(&moServer, &robot);
	ArServerInfoPath serverInfoPath(&moServer, &robot, &pathTask);
	
	// Provide the map to the client (and related controls):
	// This uses both lines and points now, since everything except
	// sonar localization uses both (path planning with sonar still uses both)
  	ArServerHandlerMap serverMap(&moServer, &arMap);
	
	// Provides localization info and allows the client (MobileEyes) to relocalize at a given
	// pose:
	ArServerInfoLocalization serverInfoLocalization(&moServer, &robot, &locTask);
	ArServerHandlerLocalization serverLocHandler(&moServer, &robot, &locTask);

	robot.unlock();

	moServer.runAsync();

	//Main loop
	while (true){
		//The socket to accept connection
		ServerSocket new_sock;
		server.accept ( new_sock );
		int state = 1;		//1 = Follow, 2 = Search, 3 = GoTo
		int lastPos[2];		//Storing last position of BB to search the target
		int data[2];		//matrix with X,Y of BB
		try{
			while ( true ){
				//receive data from tld
				new_sock >> data;
				//cout << data[0] << "," << data[1] << endl;

				if(data[0] != -1)
					lastPos[0] = data[0];

				//cout << state <<endl; //for debugging
				//Main logic
				switch(state){

					case 1:
						cout << "Following target\n";
						state = follow.run(data);
					break;
					case 2:
						cout << "Searching for target\n";
						state = s.seek(lastPos, data);
					break;
					case 3:
						cout << "Going to ...\n";
						state = goTo.run(data);
					break;
					default:
						cout << "Not a case for state\n";
					break;
				}
				std::cout << "Loc score: " << locTask.getLocalizationScore() << std::endl;
			}
		}
		catch ( SocketException& ) {
			cout << "Lost Connection" << endl;
			robot.lock();
			robot.stop();
			robot.unlock();
		}
	}
}
catch ( SocketException& e ){
	std::cout << "Exception was caught:" << e.description() << "\nExiting.\n";
}

ArLog::log(ArLog::Normal, "RobotServer: Exiting.");
return 0;
}

//.........这里部分代码省略.........
  {
    robot.lock();
    if (robot.isHeadingDone(5))
    {
      printf("directMotionExample: Finished turn\n");
      robot.unlock();
      break;
    }
    if (start.mSecSince() > 5000)
    {
      printf("directMotionExample: Turn timed out\n");
      robot.unlock();
      break;
    }
    robot.unlock();
    ArUtil::sleep(100);
  }
  printf("directMotionExample: Telling the robot to turn to 90, then sleeping 2 seconds\n");
  robot.lock();
  robot.setHeading(90);
  robot.unlock();
  start.setToNow();
  while (1)
  {
    robot.lock();
    if (robot.isHeadingDone(5))
    {
      printf("directMotionExample: Finished turn\n");
      robot.unlock();
      break;
    }
    if (start.mSecSince() > 5000)
    {
      printf("directMotionExample: turn timed out\n");
      robot.unlock();
      break;
    }
    robot.unlock();
    ArUtil::sleep(100);
  }
  printf("directMotionExample: Setting vel2 to 200 mm/sec on both wheels, then sleeping 3 seconds\n");
  robot.lock();
  robot.setVel2(200, 200);
  robot.unlock();
  ArUtil::sleep(3000);
  printf("directMotionExample: Stopping the robot, then sleeping for 2 seconds\n");
  robot.lock();
  robot.stop();
  robot.unlock();
  ArUtil::sleep(2000);
  printf("directMotionExample: Setting velocity to 200 mm/sec then sleeping 3 seconds\n");
  robot.lock();
  robot.setVel(200);
  robot.unlock();
  ArUtil::sleep(3000);
  printf("directMotionExample: Stopping the robot, then sleeping for 2 seconds\n");
  robot.lock();
  robot.stop();
  robot.unlock();
  ArUtil::sleep(2000);
  printf("directMotionExample: Setting vel2 with 0 on left wheel, 200 mm/sec on right, then sleeping 5 seconds\n");
  robot.lock();
  robot.setVel2(0, 200);
  robot.unlock();
  ArUtil::sleep(5000);
  printf("directMotionExample: Telling the robot to rotate at 50 deg/sec then sleeping 5 seconds\n");
  robot.lock();
  robot.setRotVel(50);
  robot.unlock();
  ArUtil::sleep(5000);
  printf("directMotionExample: Telling the robot to rotate at -50 deg/sec then sleeping 5 seconds\n");
  robot.lock();
  robot.setRotVel(-50);
  robot.unlock();
  ArUtil::sleep(5000);
  printf("directMotionExample: Setting vel2 with 0 on both wheels, then sleeping 3 seconds\n");
  robot.lock();
  robot.setVel2(0, 0);
  robot.unlock();
  ArUtil::sleep(3000);
  printf("directMotionExample: Now having the robot change heading by -125 degrees, then sleeping for 6 seconds\n");
  robot.lock();
  robot.setDeltaHeading(-125);
  robot.unlock();
  ArUtil::sleep(6000);
  printf("directMotionExample: Now having the robot change heading by 45 degrees, then sleeping for 6 seconds\n");
  robot.lock();
  robot.setDeltaHeading(45);
  robot.unlock();
  ArUtil::sleep(6000);
  printf("directMotionExample: Setting vel2 with 200 on left wheel, 0 on right wheel, then sleeping 5 seconds\n");
  robot.lock();
  robot.setVel2(200, 0);
  robot.unlock();
  ArUtil::sleep(5000);

  printf("directMotionExample: Done, shutting down Aria and exiting.\n");
  Aria::shutdown();
  return 0;
}