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

int main (int argc, char** argv) {
	
	Aria::init();
	Arnl::init();
	ArRobot robot;
	ArArgumentParser parser(&argc, argv);
	parser.loadDefaultArguments();
	ArRobotConnector robotConnector(&parser, &robot);
	//ArAnalogGyro gyro = new 
	ArAnalogGyro gyro(&robot);
	if (!robotConnector.connectRobot()) {
		ArLog::log(ArLog::Terse, "Could not connect to the robot.");
		if(parser.checkHelpAndWarnUnparsed())
		{
			// -help not given, just exit.
			Aria::logOptions();
			Aria::exit(1);
			return 1;
		}
	}
	
	ArSonarDevice sonarDev;
	robot.addRangeDevice(&sonarDev);
	
	robot.runAsync(true);
	
	ArMap map("office.map");
	// set it up to ignore empty file names (otherwise if a configuration omits
	// the map file, the whole configuration change will fail)
	map.setIgnoreEmptyFileName(true);
	// ignore the case, so that if someone is using MobileEyes or
	// MobilePlanner from Windows and changes the case on a map name,
	// it will still work.
	map.setIgnoreCase(true);
	
	ArSonarLocalizationTask locTask(&robot, &sonarDev, &map);
	
	locTask.localizeRobotAtHomeBlocking();
	
	robot.runAsync(true);
	robot.enableMotors();
	//robot.lock();
	robot.setVel(200);
	//robot.unlock();
	ArPose pose;
	locTask.forceUpdatePose(pose);
	while(true) {
	//while (robot.blockingConnect()){
		//robot.lock();
		//ArPose pose  = robot.getPose();
		//pose.setX(100);
		//robot.moveTo(pose);
		//t = robot.getLastOdometryTime();
		//int a = interp.getPose(t, &pose);
		ArLog::log(ArLog::Normal, "x = %f \t y = %f\n", pose.getX(), pose.getY());
		//robot.unlock();
	} 
}

int main(int argc, char **argv) 
{
  Aria::init();
  ArRobot robot;
  ArArgumentParser argParser(&argc, argv);
  ArSimpleConnector con(&argParser);
  if(!Aria::parseArgs() || !argParser.checkHelpAndWarnUnparsed())
  {
    Aria::logOptions();
    return 1;
  }

  // Create a connection handler object, defined above, then try to connect to the
  // robot.
  ConnHandler ch(&robot);
  con.connectRobot(&robot);
  robot.runAsync(true);

  // Sleep for 10 seconds, then request that ArRobot stop its thread.
  ArLog::log(ArLog::Normal, "Sleeping for 10 seconds...");
  ArUtil::sleep(10000);
  ArLog::log(ArLog::Normal, "...requesting that the robot thread exit, then shutting down ARIA and exiting.");
  robot.stopRunning();
  Aria::shutdown();
  return 0;
}

int main(int argc, char **argv)
{
  Aria::init();
  ArRobot robot;

  ArSerialConnection serialConnection;
  ArTcpConnection tcpConnection;
    
  if (tcpConnection.open("localhost", 8101)) {
    robot.setDeviceConnection(&tcpConnection);
  } else {
    serialConnection.setPort("/dev/ttyUSB0");
    robot.setDeviceConnection(&serialConnection);
  }
  robot.blockingConnect();
   
  printf("Setting robot to run async\n");
  robot.runAsync(false);

  printf("Turning off sound\n");
  robot.comInt(ArCommands::SOUNDTOG, 0);

  printf("Enabling motors\n");
  robot.enableMotors();

  // add a set of actions that combine together to effect the wander behavior
  /*ArActionStallRecover recover;
  ArActionBumpers bumpers;
  ArActionAvoidFront avoidFrontNear("Avoid Front Near", 225, 0);
  ArActionAvoidFront avoidFrontFar;
  ArActionConstantVelocity constantVelocity("Constant Velocity", 400);
  robot.addAction(&recover, 100);
  robot.addAction(&bumpers, 75);
  robot.addAction(&avoidFrontNear, 50);
  robot.addAction(&avoidFrontFar, 49);
  robot.addAction(&constantVelocity, 25);*/

  printf("Locking\n");
  robot.lock();
  robot.setVel(100.0);
  robot.unlock();
  printf("Sleeping\n");
  ArUtil::sleep(3*1000);
  printf("Awake\n");

  
  // wait for robot task loop to end before exiting the program
  //while (true);
  //robot.waitForRunExit();
  

  Aria::exit(0);
  return 0;
}

int main(void)
{
  ArTcpConnection con;
  ArRobot robot;
  int ret;
  std::string str;
  JoydriveAction jdAct;
  FillerThread ft;

  ft.create();

  FillerThread ft2;

  ft2.create();

  Aria::init();
  /*
  if (!jdAct.joystickInited())
  {
    printf("Do not have a joystick, set up the joystick then rerun the program\n\n");
    Aria::shutdown();
    return 1;
  }
  */
  if ((ret = con.open()) != 0)
  {
    str = con.getOpenMessage(ret);
    printf("Open failed: %s\n", str.c_str());
    Aria::shutdown();
    return 1;
  }
  
  robot.setDeviceConnection(&con);
  if (!robot.blockingConnect())
  {
    printf("Could not connect to robot... exiting\n");
    Aria::shutdown();
    return 1;
  }

  robot.comInt(ArCommands::SONAR, 0);
  robot.comInt(ArCommands::ENABLE, 1);
  robot.comInt(ArCommands::SOUNDTOG, 0);

  lastLoopTime.setToNow();
  loopTime = robot.getCycleTime();

  robot.addAction(&jdAct, 100);
  robot.runAsync(true);
  
  robot.waitForRunExit();
  Aria::shutdown();
  return 0;
}

int main(int argc, char **argv) 
{
  std::string str;
  int ret;

  // connection to the robot
  ArTcpConnection con;
  // the robot
  ArRobot robot;
  
  // ake the joydrive object, which also creates its own thread
  Joydrive joyd(&robot);
  
  // the connection handler
  ConnHandler ch(&robot, &joyd);

  // init aria, which will make a dedicated signal handling thread
  Aria::init(Aria::SIGHANDLE_THREAD);

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


  // set the connection on the robot
  robot.setDeviceConnection(&con);

  // run the robot in its own thread
  robot.runAsync(false);
  
  // have the robot connect asyncronously (so its loop is still running)
  // if this fails it means that the robot isn't running in its own thread
  if (!robot.asyncConnect())
  {
    printf(
    "asyncConnect failed because robot is not running in its own thread.\n");
    Aria::shutdown();
    return 1;
  }

  // now we just wait for the robot to be done running
  printf("Waiting for the robot's run to exit.\n");
  robot.waitForRunExit();
  // then we exit
  printf("exiting main\n");
  Aria::exit(0);  // exit program
  return 0;
}

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

  int i;
  while (Aria::getRunning())
  {
    robot.lock();
    robot.comStr(ArCommands::TTY3, "1234567890");
    robot.unlock();
  }

  robot.disconnect();
  // shutdown and ge tout
  Aria::shutdown();
  return 0;
}

int main (int argc, char** argv) {
	
	Aria::init();
	Arnl::init();
	ArRobot robot;
	ArArgumentParser parser(&argc, argv);
	parser.loadDefaultArguments();
	ArRobotConnector robotConnector(&parser, &robot);
	//ArAnalogGyro gyro = new 
	ArAnalogGyro gyro(&robot);
	if (!robotConnector.connectRobot()) {
		ArLog::log(ArLog::Terse, "Could not connect to the robot.");
		if(parser.checkHelpAndWarnUnparsed())
		{
			// -help not given, just exit.
			Aria::logOptions();
			Aria::exit(1);
			return 1;
		}
	}
	
	ArSonarDevice sonarDev;
	

//	robot.runAsync(true);
	
//	ArMap map("office.map");
	// set it up to ignore empty file names (otherwise if a configuration omits
	// the map file, the whole configuration change will fail)
//	map.setIgnoreEmptyFileName(true);
	// ignore the case, so that if someone is using MobileEyes or
	// MobilePlanner from Windows and changes the case on a map name,
	// it will still work.
//	map.setIgnoreCase(true);


	robot.runAsync(true);
	robot.enableMotors();
	robot.moveTo(ArPose(0,0,0));
	//robot.lock();
	robot.comInt(ArCommands::ENABLE, 1);
	robot.addRangeDevice(&sonarDev);
	//robot.unlock();
//	ArSonarLocalizationTask locTask(&robot, &sonarDev, &map);
<<<<<<< HEAD

int main(int argc, char **argv){
	Aria::init();
	ArRobot robot;
	ArArgumentParser parser(&argc, argv);
	ArSimpleConnector connector(& parser);

	parser.loadDefaultArguments();
	Aria::logOptions();
	if (!connector.parseArgs()){
		cout << "Unknown settings\n";
		Aria::exit(0);
		exit(1);
	}

	if (!connector.connectRobot(&robot)){
		cout << "Unable to connect\n";
		Aria::exit(0);
		exit(1);
	}

	robot.runAsync(true);
	robot.lock();
	robot.comInt(ArCommands::ENABLE, 1);
	robot.unlock();



	ArSonarDevice sonar;
	robot.addRangeDevice(&sonar);

	G_id = 0;
	G_SONAR_FD = fopen("../sensors/sonars","w");
	G_pose_fd  = fopen("../sensors/pose","w");
	int numSonar = robot.getNumSonar();
	while(1){
		readPosition(robot);
		readSonars(robot, 8);
		setMotors(robot);
		usleep(20000);
	}

	fclose(G_SONAR_FD);
	fclose(G_pose_fd);
	Aria::exit(0);
}

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

  Aria::init();

  ArArgumentParser parser(&argc, argv);
  // set up our simple connector
  ArSimpleConnector simpleConnector(&parser);  
  ArRobot robot;

  // set up the robot for connecting
  if (!simpleConnector.connectRobot(&robot))
  {
    printf("Could not connect to robot... exiting\n");
    Aria::exit(1);
  }

  robot.runAsync(true);

  ArModuleLoader::Status status;
  std::string str;

  ArLog::log(ArLog::Terse, "moduleActionExample: Loading the module \"moduleActionExample_Mod\" with a string argument...");
  status=ArModuleLoader::load("./moduleActionExample_Mod", &robot, (char *)"You've loaded a module!");
  printStatus(status);

  ArLog::log(ArLog::Terse, "moduleActionExample: Loading the module \"moduleActionExample_Mod2\" with a string argument...");
  status=ArModuleLoader::load("./moduleActionExample2_Mod", &robot, (char *)"You've loaded a second module!");
  printStatus(status);

  //ArLog::log(ArLog::Terse, "moduleActionExample: Reloading \"moduleActionExample_Mod\" with no argument...");
  //status=ArModuleLoader::reload("./moduleActionExample_Mod", &robot);
  //printStatus(status);

  //ArLog::log(ArLog::Terse, "moduleActionExample: Closing  \"moduleActionExample_Mod\"...");
  //status=ArModuleLoader::close("./moduleActionExample_Mod");
  //printStatus(status);

  ArUtil::sleep(3000);

  Aria::exit(0);
  return(0);
}

int RosAriaNode::Setup()
{
	ArArgumentBuilder *args;

	args = new ArArgumentBuilder();
	args->add("-rp"); //pass robot's serial port to Aria
	args->add(serial_port.c_str());
	conn = new ArSimpleConnector(args);


	robot = new ArRobot();
	robot->setCycleTime(1);

	ArLog::init(ArLog::File, ArLog::Verbose, "aria.log", true);

	//parse all command-line arguments
	if (!Aria::parseArgs())
	{
		Aria::logOptions();
		return 1;
	}

	// Connect to the robot
	if (!conn->connectRobot(robot)) {
		ArLog::log(ArLog::Terse, "rotate: Could not connect to robot! Exiting.");
		return 1;
	}

	//Sonar sensor
	sonar.setMaxRange(Max_Range);
	robot->addRangeDevice(&sonar);
	robot->enableSonar();

	// Enable the motors
	robot->enableMotors();

	robot->runAsync(true);

	return 0;
}

int main(int argc, char **argv) 
{
    Aria::init();
    ArArgumentParser parser(&argc, argv);
    parser.loadDefaultArguments();
    ArRobot robot;
    ArRobotConnector robotConnector(&parser, &robot);
    if(!robotConnector.connectRobot())
    {
      ArLog::log(ArLog::Terse, "dpptuExample: Could not connect to the robot.");
      if(parser.checkHelpAndWarnUnparsed())
      {
          Aria::logOptions();
          Aria::exit(1);
      }
    }
    if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
    {
      Aria::logOptions();
      Aria::exit(1);
    }
    ArLog::log(ArLog::Normal, "dpptuExample: Connected to robot.");

  robot.runAsync(true);

  // an object for keyboard control, class defined above, this also adds itself as a user task
  KeyPTU ptu(&robot);


  // turn off the sonar
  robot.comInt(ArCommands::SONAR, 0);

  printf("Press '?' for available commands\r\n");

  // run, if we lose connection to the robot, exit
  robot.waitForRunExit();

  Aria::exit(0);
}

int main(int argc, char** argv)
{
  Aria::init();
  ArLog::init(ArLog::StdErr, ArLog::Normal);

  ArArgumentParser argParser(&argc, argv);
  argParser.loadDefaultArguments();
  ArSimpleConnector connector(&argParser);
  ArGPSConnector gpsConnector(&argParser);

  if(!Aria::parseArgs() || !argParser.checkHelpAndWarnUnparsed())
  {
    Aria::logOptions();
    ArLog::log(ArLog::Terse, "gpsExample options:\n  -printTable   Print data to standard output in regular columns rather than a refreshing terminal display, and print more digits of precision");
    return 1;
  }

  // Try connecting to robot 
  ArRobot robot;
  if(!connector.connectRobot(&robot))
  {
    ArLog::log(ArLog::Terse, "gpsExample: Warning: Could not connect to robot.  Will not be able to switch GPS power on, or load GPS options from this robot's parameter file.");
  }
  else
  {
    ArLog::log(ArLog::Normal, "gpsExample: Connected to robot.");
    robot.runAsync(true);
  }

  // check command line arguments for -printTable
  bool printTable = argParser.checkArgument("printTable");

  // On the Seekur, power to the GPS receiver is switched on by this command.
  // (A third argument of 0 would turn it off). On other robots this command is
  // ignored.
  robot.com2Bytes(116, 6, 1);

  // Try connecting to a GPS. We pass the robot pointetr to the connector so it
  // can check the robot parameters for this robot type for default values for
  // GPS device connection information (receiver type, serial port, etc.)
  ArLog::log(ArLog::Normal, "gpsExample: Connecting to GPS, it may take a few seconds...");
  ArGPS *gps = gpsConnector.createGPS(&robot);
  if(!gps || !gps->connect())
  {
    ArLog::log(ArLog::Terse, "gpsExample: Error connecting to GPS device.  Try -gpsType, -gpsPort, and/or -gpsBaud command-line arguments. Use -help for help.");
    return -1;
  }


  ArLog::log(ArLog::Normal, "gpsExample: Reading data...");
  ArTime lastReadTime;
  if(printTable)
    gps->printDataLabelsHeader();
  while(true)
  {
    int r = gps->read();
    if(r & ArGPS::ReadError)
    {
      ArLog::log(ArLog::Terse, "gpsExample: Warning: error reading GPS data.");
      ArUtil::sleep(1000);
      continue;
    }


    if(r & ArGPS::ReadUpdated)
    {
      if(printTable)
      {
        gps->printData(false);
        printf("\n");
      }
      else
      {
        gps->printData();
        printf("\r");
      }
      fflush(stdout);
      ArUtil::sleep(500);
      lastReadTime.setToNow();
      continue;
    } else {
      if(lastReadTime.secSince() >= 5) {
        ArLog::log(ArLog::Terse, "gpsExample: Warning: haven't recieved any data from GPS for more than 5 seconds!");
      }
      ArUtil::sleep(1000);
      continue;
    }

  }
  return 0;
}

//.........这里部分代码省略.........
    robot.addRangeDevice(&sonarDev);

    ArIRs irs;
    robot.addRangeDevice(&irs);

    ArBumpers bumpers;
    robot.addRangeDevice(&bumpers);

    // a laser in case one is used
    ArSick sick(361, 180);
    // add the laser to the robot
    robot.addRangeDevice(&sick);




    // attach stuff to the server
    ArServerInfoRobot serverInfoRobot(&server, &robot);
    ArServerInfoSensor serverInfoSensor(&server, &robot);
    ArServerInfoDrawings drawings(&server);
    drawings.addRobotsRangeDevices(&robot);

    // ways of moving the robot
    ArServerModeStop modeStop(&server, &robot);
    ArServerModeDrive modeDrive(&server, &robot);
    ArServerModeRatioDrive modeRatioDrive(&server, &robot);
    ArServerModeWander modeWander(&server, &robot);
    modeStop.addAsDefaultMode();
    modeStop.activate();

    // set up the simple commands
    ArServerHandlerCommands commands(&server);
    // add the commands for the microcontroller
    ArServerSimpleComUC uCCommands(&commands, &robot);
    // add the commands for logging
    ArServerSimpleComMovementLogging loggingCommands(&commands, &robot);
    // add the commands for the gyro
    ArServerSimpleComGyro gyroCommands(&commands, &robot, &gyro);

    // add the commands to enable and disable safe driving to the simple commands
    modeDrive.addControlCommands(&commands);

    // Forward any video if we have some to forward.. this will forward
    // from SAV or ACTS, you can find both on our website
    // http://robots.activmedia.com, ACTS is for color tracking and is
    // charged for but SAV just does software A/V transmitting and is
    // free to all our customers... just run ACTS or SAV before you
    // start this program and this class here will forward video from it
    // to MobileEyes
    ArHybridForwarderVideo videoForwarder(&server, "localhost", 7070);

    // make a camera to use in case we have video
    ArPTZ *camera = NULL;
    ArServerHandlerCamera *handlerCamera = NULL;
    // if we have video then set up a camera
    if (videoForwarder.isForwardingVideo())
    {
        bool invertedCamera = false;
        camera = new ArVCC4(&robot,	invertedCamera,
                            ArVCC4::COMM_UNKNOWN, true, true);
        camera->init();
        handlerCamera = new ArServerHandlerCamera(&server, &robot, camera);
    }

    server.logCommandGroups();
    server.logCommandGroupsToFile("userServerTest.commandGroups");

    // now let it spin off in its own thread
    server.runAsync();

    // set up the robot for connecting
    if (!simpleConnector.connectRobot(&robot))
    {
        printf("Could not connect to robot... exiting\n");
        Aria::shutdown();
        return 1;
    }

    // set up the laser before handing it to the laser mode
    simpleConnector.setupLaser(&sick);

    robot.enableMotors();
    // start the robot running, true so that if we lose connection the run stops
    robot.runAsync(true);

    sick.runAsync();

    // connect the laser if it was requested
    if (!simpleConnector.connectLaser(&sick))
    {
        printf("Could not connect to laser... exiting\n");
        Aria::shutdown();
        return 1;
    }

    robot.waitForRunExit();
    // now exit
    Aria::shutdown();
    exit(0);
}

int main(int argc, char *argv[])
{
    // Initialize location of Aria, Arnl and their args.
    Aria::init();
    Arnl::init();


    // The robot object
    ArRobot robot;

    // Parse the command line arguments.
    ArArgumentParser parser(&argc, argv);

    // Read data_index if exists
    int data_index;
    bool exist_data_index;
    parser.checkParameterArgumentInteger("dataIndex",&data_index,&exist_data_index);


    // Load default arguments for this computer (from /etc/Aria.args, environment
    // variables, and other places)
    parser.loadDefaultArguments();


    // Object that connects to the robot or simulator using program options
    ArRobotConnector robotConnector(&parser, &robot);


    // set up a gyro
    ArAnalogGyro gyro(&robot);

    ArLog::init(ArLog::File,ArLog::Normal,"run.log",false,true,true);

    // Parse arguments for the simple connector.
    if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
    {
        ArLog::log(ArLog::Normal, "\nUsage: %s -map mapfilename\n", argv[0]);
        Aria::logOptions();
        Aria::exit(1);
    }

    // Collision avoidance actions at higher priority
    ArActionAvoidFront avoidAction("avoid",200);
    ArActionLimiterForwards limiterAction("speed limiter near", 150, 500, 150);
    ArActionLimiterForwards limiterFarAction("speed limiter far", 300, 1100, 400);
    ArActionLimiterTableSensor tableLimiterAction;
    //robot.addAction(&tableLimiterAction, 100);
    //robot.addAction(&avoidAction,100);
    //robot.addAction(&limiterAction, 95);
    //robot.addAction(&limiterFarAction, 90);

    // Goto action at lower priority
    ArActionGoto gotoPoseAction("goto");
    //robot.addAction(&gotoPoseAction, 50);
    gotoPoseAction.setCloseDist(750);

    // Stop action at lower priority, so the robot stops if it has no goal
    ArActionStop stopAction("stop");
    //robot.addAction(&stopAction, 40);




    // Connect the robot
    if (!robotConnector.connectRobot())
    {
        ArLog::log(ArLog::Normal, "Could not connect to robot... exiting");
        Aria::exit(3);
    }

    if(!robot.isConnected())
    {
        ArLog::log(ArLog::Terse, "Internal error: robot connector succeeded but ArRobot::isConnected() is false!");
    }




    // Connector for laser rangefinders
    ArLaserConnector laserConnector(&parser, &robot, &robotConnector);


    // Sonar, must be added to the robot, used by teleoperation and wander to
    // detect obstacles, and for localization if SONARNL
    ArSonarDevice sonarDev;

    // Add the sonar to the robot
    robot.addRangeDevice(&sonarDev);

    // Start the robot thread.
    robot.runAsync(true);


    // Connect to the laser(s) if lasers were configured in this robot's parameter
    // file or on the command line, and run laser processing thread if applicable
    // for that laser class.  For the purposes of this demo, add all
    // possible lasers to ArRobot's list rather than just the ones that were
    // connected by this call so when you enter laser mode, you
    // can then interactively choose which laser to use from that list of all
    // lasers mentioned in robot parameters and on command line. Normally,
//.........这里部分代码省略.........

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

  // parse our args and make sure they were all accounted for
  ArSimpleConnector connector(&argc, argv);

  ArRobot robot;

  // the laser. ArActionTriangleDriveTo will use this laser object since it is
  // named "laser" when added to the ArRobot.
  ArSick sick;

  if (!connector.parseArgs() || argc > 1)
  {
    connector.logOptions();
    exit(1);
  }
  
  // a key handler so we can do our key handling
  ArKeyHandler keyHandler;
  // let the global aria stuff know about it
  Aria::setKeyHandler(&keyHandler);
  // toss it on the robot
  robot.attachKeyHandler(&keyHandler);

  // add the laser to the robot
  robot.addRangeDevice(&sick);

  ArSonarDevice sonar;
  robot.addRangeDevice(&sonar);
  
  ArActionTriangleDriveTo triangleDriveTo;
  ArFunctorC<ArActionTriangleDriveTo> lineGoCB(&triangleDriveTo, 
				      &ArActionTriangleDriveTo::activate);
  keyHandler.addKeyHandler('g', &lineGoCB);
  keyHandler.addKeyHandler('G', &lineGoCB);
  ArFunctorC<ArActionTriangleDriveTo> lineStopCB(&triangleDriveTo, 
					&ArActionTriangleDriveTo::deactivate);
  keyHandler.addKeyHandler('s', &lineStopCB);
  keyHandler.addKeyHandler('S', &lineStopCB);

  ArActionLimiterForwards limiter("limiter", 150, 0, 0, 1.3);
  robot.addAction(&limiter, 70);
  ArActionLimiterBackwards limiterBackwards;
  robot.addAction(&limiterBackwards, 69);

  robot.addAction(&triangleDriveTo, 60);

  ArActionKeydrive keydrive;
  robot.addAction(&keydrive, 55);


  ArActionStop stopAction;
  robot.addAction(&stopAction, 50);
  
  // try to connect, if we fail exit
  if (!connector.connectRobot(&robot))
  {
    printf("Could not connect to robot... exiting\n");
    Aria::shutdown();
    return 1;
  }

  robot.comInt(ArCommands::SONAR, 1);
  robot.comInt(ArCommands::ENABLE, 1);
  
  // start the robot running, true so that if we lose connection the run stops
  robot.runAsync(true);

  // now set up the laser
  connector.setupLaser(&sick);

  sick.runAsync();

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

  printf("If you press the 'g' key it'll go find a triangle, if you press 's' it'll stop.\n");

  robot.waitForRunExit();
  return 0;
}