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

void Joydrive::drive(void)
{
  int trans, rot;

  // print out some data about the robot
  printf("\rx %6.1f  y %6.1f  tth  %6.1f vel %7.1f mpacs %3d   ", 
	 myRobot->getX(), myRobot->getY(), myRobot->getTh(), 
	 myRobot->getVel(), myRobot->getMotorPacCount());
  fflush(stdout);

  // see if a joystick butotn is pushed, if so drive
  if (myJoyHandler.haveJoystick() && (myJoyHandler.getButton(1) ||
				    myJoyHandler.getButton(2)))
  {
    // get the values out of the joystick handler
    myJoyHandler.getAdjusted(&rot, &trans);
    // drive the robot
    myRobot->setVel(trans);
    myRobot->setRotVel(-rot);
  }
  // if a button isn't pushed, stop the robot
  else
  {
    myRobot->setVel(0);
    myRobot->setRotVel(0);
  }
}

void Joydrive::drive(void)
{
    int trans, rot;
    int pan, tilt;
    int zoom, nothing;

    // if both buttons are pushed, zoom the joystick
    if (myJoyHandler.haveJoystick() && myJoyHandler.getButton(1) &&
            myJoyHandler.getButton(2))
    {
        // set its speed so we get desired value range, we only care about y
        myJoyHandler.setSpeeds(0, myZoomValCamera);
        // get the values
        myJoyHandler.getAdjusted(&nothing, &zoom);
        // zoom the camera
        myCam.zoomRel(zoom);
    }
    // if both buttons aren't pushed, see if one button is pushed
    else
    {
        // if button one is pushed, drive the robot
        if (myJoyHandler.haveJoystick() && myJoyHandler.getButton(1))
        {
            // set the speed on the joystick so we get the values we want
            myJoyHandler.setSpeeds(myRotValRobot, myTransValRobot);
            // get the values
            myJoyHandler.getAdjusted(&rot, &trans);
            // set the robots speed
            myRobot->setVel(trans);
            myRobot->setRotVel(-rot);
        }
        // if button one isn't pushed, stop the robot
        else
        {
            myRobot->setVel(0);
            myRobot->setRotVel(0);
        }
        // if button two is pushed, move the camera
        if (myJoyHandler.haveJoystick() && myJoyHandler.getButton(2))
        {
            // set the speeds on the joystick so we get desired value range
            myJoyHandler.setSpeeds(myPanValCamera, myTiltValCamera);
            // get the values
            myJoyHandler.getAdjusted(&pan, &tilt);
            // drive the camera
            myCam.panTiltRel(pan, tilt);
        }
    }

}

// this is the function called in the new thread
void *Joydrive::runThread(void *arg)
{
  threadStarted();

  int trans, rot;

  // only run while running, ie play nice and pay attention to the thread 
  //being shutdown
  while (myRunning)
  {
    // lock the robot before touching it
    myRobot->lock();
    if (!myRobot->isConnected())
    {
      myRobot->unlock();
      break;
    }
    // print out some information about the robot
    printf("\rx %6.1f  y %6.1f  tth  %6.1f vel %7.1f mpacs %3d   ", 
	   myRobot->getX(), myRobot->getY(), myRobot->getTh(), 
	   myRobot->getVel(), myRobot->getMotorPacCount());
    fflush(stdout);
    // if one of the joystick buttons is pushed, drive the robot
    if (myJoyHandler.haveJoystick() && (myJoyHandler.getButton(1) ||
					myJoyHandler.getButton(2)))
    {
      // get out the values from the joystick
      myJoyHandler.getAdjusted(&rot, &trans);
      // drive the robot
      myRobot->setVel(trans);
      myRobot->setRotVel(-rot);
    }
    // if no buttons are pushed stop the robot
    else
    {
      myRobot->setVel(0);
      myRobot->setRotVel(0);
    }
    // unlock the robot, so everything else can run
    myRobot->unlock();
    // now take a little nap
    ArUtil::sleep(50);
  }
  // return out here, means the thread is done
  return NULL;
}

void
RosAriaNode::cmdvel_cb( const geometry_msgs::TwistConstPtr &msg)
{
  veltime = ros::Time::now();
#ifdef SPEW
  ROS_INFO( "new speed: [%0.2f,%0.2f](%0.3f)", msg->linear.x*1e3, msg->angular.z, veltime.toSec() );
#endif

  robot->lock();
  robot->setVel(msg->linear.x*1e3);
  robot->setRotVel(msg->angular.z*180/M_PI);
  robot->unlock();
  ROS_DEBUG("RosAria: sent vels to to aria (time %f): x vel %f mm/s, y vel %f mm/s, ang vel %f deg/s", veltime.toSec(),
    (double) msg->linear.x * 1e3, (double) msg->linear.y * 1.3, (double) msg->angular.z * 180/M_PI);
}

void RosAriaNode::spin()
{
  //ros::spin();

    while(ros::ok()){
               if(ros::WallTime::now()-last_command_time_ > ros::WallDuration(1)){
                   robot->lock();
                   robot->setVel(0.0);
                   if(robot->hasLatVel())
                     robot->setLatVel(0.0);
                   robot->setRotVel(0.0);
                   robot->unlock();
               }
               ros::spinOnce();
           }
}

void setMotors(ArRobot& robot){
	TiObj G_motor;
	G_motor.loadFile("../motors/motors");

	if ( G_motor.has("speed") || G_motor.has("rotation") ){
		cout << G_motor;

		robot.lock();
		if ( G_motor.has("speed") )
			robot.setVel( G_motor.atInt("speed") );
		if ( G_motor.has("rotation") ) 
			robot.setRotVel( G_motor.atInt("rotation") );
		robot.unlock();

		FILE* fd = fopen("../motors/motors","w");
		fclose(fd);
	}
	//
}

void RosAriaNode::cmdvel_cb( const geometry_msgs::TwistConstPtr &msg)
{
  veltime = ros::WallTime::now();
  last_command_time_=veltime;
  ROS_INFO( "new speed: [%0.2f,%0.2f](%0.3f)", msg->linear.x*1e3, msg->angular.z, veltime.toSec() );

  twistMsg.linear.x=msg->linear.x;
  twistMsg.angular.z=msg->angular.z;
  twist_pub.publish(twistMsg);


  robot->lock();
  robot->setVel(msg->linear.x*1e3);
  if(robot->hasLatVel())
    robot->setLatVel(msg->linear.y*1e3);
  robot->setRotVel(msg->angular.z*180/M_PI);
  robot->unlock();
  ROS_DEBUG("RosAria: sent vels to to aria (time %f): x vel %f mm/s, y vel %f mm/s, ang vel %f deg/s", veltime.toSec(),
    (double) msg->linear.x * 1e3, (double) msg->linear.y * 1.3, (double) msg->angular.z * 180/M_PI);
}

    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;
    		}

    	}
    }

void PeoplebotTest::userTask(void)
{
  switch (myState)
  {
    case IDLE:
      // start wandering
      printf("Starting to wander for the first time\n");
      myStateTime.setToNow();
      myState = WANDERING;
      myRobot->addAction(myConstantVelocity, 25);
      printf("Opening up ACTS\n");
      myCmd = "DISPLAY=";
      myCmd += myHostname.c_str();
      myCmd += ":0; /usr/local/acts/bin/acts -G bttv -n 0 &> /dev/null &";
      system(myCmd.c_str());
      break;
    case WANDERING:
      if (timeout(myWanderingTimeout))
      {
	myRobot->comInt(ArCommands::SONAR,0);
	myRobot->remAction(myConstantVelocity);
	myRobot->setVel(0);
	myRobot->setRotVel(0);
	myState = RESTING;
	mySonar = 0;
	printf("Going to rest now\n");
	printf("Killing ACTS\n");
	system("killall -9 acts &> /dev/null");
	myStateTime.setToNow();
	myTotalWanderTime += myWanderingTimeout;
      }
      else if (myRobot->getVel() > 0 && mySonar != 1)
      {
	// ping front sonar
	//printf("Enabling front sonar\n");
	mySonar = 1;
	myRobot->comInt(ArCommands::SONAR, 0);
	myRobot->comInt(ArCommands::SONAR, 1);
	myRobot->comInt(ArCommands::SONAR, 4);
      }
      else if (myRobot->getVel() < 0 && mySonar != -1)
      {
	// ping rear sonar
	//printf("Enabling rear sonar\n");
	mySonar = -1;
	myRobot->comInt(ArCommands::SONAR, 0);
	myRobot->comInt(ArCommands::SONAR, 5);
      }
      break;
    case RESTING:
      if (timeout(myRestingTimeout))
      {
	printf("Going to wander now\n");
	myState = WANDERING;
	myStateTime.setToNow();
	myTotalRestTime += myRestingTimeout;
	myRobot->clearDirectMotion();
	myRobot->addAction(myConstantVelocity, 25);
	printf("Opening up ACTS\n");
	myCmd = "DISPLAY=";
	myCmd += myHostname.c_str();
	myCmd += ":0; /usr/local/acts/bin/acts -G bttv -n 0 &> /dev/null &";
	system(myCmd.c_str());
      }
      break;
    case OTHER:
    default:
      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(),
//.........这里部分代码省略.........

//.........这里部分代码省略.........

  //ArAnalogGyro gyro(&robot);


  // if we're not using the sim, make a serial connection and set it up
  if (!useSim)
  {
    ArSerialConnection *serCon;
    serCon = new ArSerialConnection;
    serCon->setPort();
    //serCon->setBaud(38400);
    con = serCon;
  }
  // if we are using the sim, set up a tcp connection
  else
  {
    ArTcpConnection *tcpCon;
    tcpCon = new ArTcpConnection;
    tcpCon->setPort();
    con = tcpCon;
  }

  // set the connection on the robot
  robot.setDeviceConnection(con);
  // try to connect, if we fail exit
  if (!robot.blockingConnect())
  {
    printf("Could not connect to robot... exiting\n");
    Aria::shutdown();
    return 1;
  }


  // set up a key handler so escape exits and attach to the robot
  ArKeyHandler keyHandler;
  robot.attachKeyHandler(&keyHandler);

  // run the robot, true here so that the run will exit if connection lost
  robot.runAsync(true);



  // if we're not using the sim, set up the port for the laser
  if (!useSim)
  {
    laserCon.setPort(ArUtil::COM3);
    sick.setDeviceConnection(&laserCon);
  }


  // now that we're connected to the robot, connect to the laser
  sick.runAsync();


  if (!sick.blockingConnect())
  {
    printf("Could not connect to SICK laser... exiting\n");
    robot.disconnect();
    Aria::shutdown();
    return 1;
  }

#ifdef WIN32
  // wait until someone pushes the motor button to go
  while (1)
  {
    robot.lock();
    if (!robot.isRunning())
      exit(0);
    if (robot.areMotorsEnabled())
    {
      robot.unlock();
      break;
    }
    robot.unlock();
    ArUtil::sleep(100);
  }
#endif

  // basically from here on down the robot just cruises around a bit

  robot.lock();
  // enable the motors, disable amigobot sounds
  robot.comInt(ArCommands::ENABLE, 1);

  // move a couple meters
  robot.setRotVel(3000);
  robot.unlock();

  ArUtil::sleep(15 * 1000);

  robot.lock();
  robot.disconnect();
  robot.unlock();
  keyHandler.restore();
  exit(1);

  // now exit
  return 0;
}

int main(int argc, char **argv) 
{
  Aria::init();
  
  ArArgumentParser argParser(&argc, argv);

  ArSimpleConnector con(&argParser);

  ArRobot robot;

  // the connection handler from above
  ConnHandler ch(&robot);

  if(!Aria::parseArgs())
  {
    Aria::logOptions();
    Aria::shutdown();
    return 1;
  }

  if(!con.connectRobot(&robot))
  {
    ArLog::log(ArLog::Normal, "directMotionExample: Could not connect to the robot. Exiting.");
    return 1;
  }

  ArLog::log(ArLog::Normal, "directMotionExample: Connected.");

  // Run the robot processing cycle in its own thread. Note that after starting this
  // thread, we must lock and unlock the ArRobot object before and after
  // accessing it.
  robot.runAsync(false);


  // Send the robot a series of motion commands directly, sleeping for a 
  // few seconds afterwards to give the robot time to execute them.
  printf("directMotionExample: Setting rot velocity to 100 deg/sec then sleeping 3 seconds\n");
  robot.lock();
  robot.setRotVel(100);
  robot.unlock();
  ArUtil::sleep(3*1000);
  printf("Stopping\n");
  robot.lock();
  robot.setRotVel(0);
  robot.unlock();
  ArUtil::sleep(200);

  printf("directMotionExample: Telling the robot to go 300 mm on left wheel and 100 mm on right wheel for 5 seconds\n");
  robot.lock();
  robot.setVel2(300, 100);
  robot.unlock();
  ArTime start;
  start.setToNow();
  while (1)
  {
    robot.lock();
    if (start.mSecSince() > 5000)
    {
      robot.unlock();
      break;
    }   
    robot.unlock();
    ArUtil::sleep(50);
  }
  
  printf("directMotionExample: Telling the robot to move forwards one meter, then sleeping 5 seconds\n");
  robot.lock();
  robot.move(1000);
  robot.unlock();
  start.setToNow();
  while (1)
  {
    robot.lock();
    if (robot.isMoveDone())
    {
      printf("directMotionExample: Finished distance\n");
      robot.unlock();
      break;
    }
    if (start.mSecSince() > 5000)
    {
      printf("directMotionExample: Distance timed out\n");
      robot.unlock();
      break;
    }   
    robot.unlock();
    ArUtil::sleep(50);
  }
  printf("directMotionExample: Telling the robot to move backwards one meter, then sleeping 5 seconds\n");
  robot.lock();
  robot.move(-1000);
  robot.unlock();
  start.setToNow();
  while (1)
  {
    robot.lock();
    if (robot.isMoveDone())
    {
      printf("directMotionExample: Finished distance\n");
      robot.unlock();
//.........这里部分代码省略.........

int main(int argc, char **argv) 
{
  std::string str;
  int ret;
  ArTime start;
  
  // connection to the robot
  ArSerialConnection con;
  // the robot
  ArRobot robot;
  // the connection handler from above
  ConnHandler ch(&robot);

  // init area with a dedicated signal handling thread
  Aria::init(Aria::SIGHANDLE_THREAD);

  // open the connection with the defaults, exit if failed
  if ((ret = con.open()) != 0)
  {
    str = con.getOpenMessage(ret);
    printf("Open failed: %s\n", str.c_str());
    Aria::shutdown();
    return 1;
  }

  // set the robots connection
  robot.setDeviceConnection(&con);
  // try to connect, if we fail, the connection handler should bail
  if (!robot.blockingConnect())
  {
    // this should have been taken care of by the connection handler
    // but just in case
    printf(
    "asyncConnect failed because robot is not running in its own thread.\n");
    Aria::shutdown();
    return 1;
  }
  // run the robot in its own thread, so it gets and processes packets and such
  robot.runAsync(false);

  // just a big long set of printfs, direct motion commands and sleeps,
  // it should be self-explanatory
  printf("Telling the robot to go 300 mm for 5 seconds\n");
  robot.lock();
  robot.setVel(500);
  robot.unlock();
  start.setToNow();
  while (1)
  {
    robot.lock();
    if (start.mSecSince() > 5000)
    {
      robot.unlock();
      break;
    }   
    printf("Trans: %10g Rot: %10g\n", robot.getVel(), robot.getRotVel());
    robot.unlock();
    ArUtil::sleep(100);
  }
  
  printf("Telling the robot to turn at 50 deg/sec for 10 seconds\n");
  robot.lock();
  robot.setVel(0);
  robot.setRotVel(50);
  robot.unlock();
  start.setToNow();
  while (1)
  {
    robot.lock();
    if (start.mSecSince() > 10000)
    {
      robot.unlock();
      break;
    }   
    printf("Trans: %10g Rot: %10g\n", robot.getVel(), robot.getRotVel());
    robot.unlock();
    ArUtil::sleep(100);
  }

  printf("Telling the robot to turn at 100 deg/sec for 10 seconds\n");
  robot.lock();
  robot.setVel(0);
  robot.setRotVel(100);
  robot.unlock();
  start.setToNow();
  while (1)
  {
    robot.lock();
    if (start.mSecSince() > 10000)
    {
      robot.unlock();
      break;
    }   
    printf("Trans: %10g Rot: %10g\n", robot.getVel(), robot.getRotVel());
    robot.unlock();
    ArUtil::sleep(100);
  }

  printf("Done with tests, exiting\n");
  robot.disconnect();
//.........这里部分代码省略.........

void Joydrive::drive(void)
{
  int trans, rot;
  ArPose pose;
  ArPose rpose;
  ArTransform transform;
  ArRangeDevice *dev;
  ArSensorReading *son;

  if (!myRobot->isConnected())
  {
    printf("Lost connection to the robot, exiting\n");
    exit(0);
  }
  printf("\rx %6.1f  y %6.1f  th  %6.1f", 
	 myRobot->getX(), myRobot->getY(), myRobot->getTh());
  fflush(stdout);
  if (myJoyHandler.haveJoystick() && myJoyHandler.getButton(1))
  {
    if (ArMath::fabs(myRobot->getVel()) < 10.0)
      myRobot->comInt(ArCommands::ENABLE, 1);
    myJoyHandler.getAdjusted(&rot, &trans);
    myRobot->setVel(trans);
    myRobot->setRotVel(-rot);
  }
  else
  {
    myRobot->setVel(0);
    myRobot->setRotVel(0);
  }
  if (myJoyHandler.haveJoystick() && myJoyHandler.getButton(2) &&
      time(NULL) - myLastPress > 1)
  {
    myLastPress = time(NULL);
    printf("\n");
    switch (myTest)
    {
    case 1:
      printf("Moving back to the origin.\n");
      pose.setPose(0, 0, 0);
      myRobot->moveTo(pose);
      break;
    case 2:
      printf("Moving over a meter.\n");
      pose.setPose(myRobot->getX() + 1000, myRobot->getY(), 0);
      myRobot->moveTo(pose);
      break;
    case 3:
      printf("Doing a transform test....\n");
      printf("\nOrigin should be transformed to the robots coords.\n");
      transform = myRobot->getToGlobalTransform();
      pose.setPose(0, 0, 0);
      pose = transform.doTransform(pose);
      rpose = myRobot->getPose();
      printf("Pos:  ");
      pose.log();
      printf("Robot:  ");
      rpose.log();

      if (pose.findDistanceTo(rpose) < .1)
	printf("Success\n");
      else
	printf("#### FAILURE\n");
    
      printf("\nRobot coords should be transformed to the origin.\n");
      transform = myRobot->getToLocalTransform();
      pose = myRobot->getPose();
      pose = transform.doTransform(pose);
      rpose.setPose(0, 0, 0);
      printf("Pos:  ");
      pose.log();
      printf("Robot:  ");
      rpose.log();
      if (pose.findDistanceTo(rpose) < .1)
	printf("Success\n");
      else
	printf("#### FAILURE\n");
      break;
    case 4:
      printf("Doing a tranform test...\n");
      printf("A point 1 meter to the -x from the robot (in local coords) should be transformed into global coordinates.\n");
      transform = myRobot->getToGlobalTransform();
      pose.setPose(-1000, 0, 0);
      pose = transform.doTransform(pose);
      rpose = myRobot->getPose();
      printf("Pos:  ");
      pose.log();
      printf("Robot:  ");
      rpose.log();

      if (ArMath::fabs(pose.findDistanceTo(rpose) - 1000.0) < .1)
	printf("Probable Success\n");
      else
	printf("#### FAILURE\n");
      break;
    case 5:
      printf("Doing a transform test on range devices..\n");
      printf("Moving the robot +4 meters x and +4 meters y and seeing if the moveTo will move the sonar readings along with it.\n");
      dev = myRobot->findRangeDevice("sonar");
      if (dev == NULL)
//.........这里部分代码省略.........