예제 #1
0
/*!
 * Interact with user on the terminal.
 */
void
interact()
{
  ArMap* ariamap = advancedptr->myMap;
  sleep(1);
  advancedptr->getAllGoals(ariamap);
  advancedptr->getAllRobotHomes(ariamap);

  /// MPL
//  lkeyCB();
  advancedptr->myLocaTask->localizeRobotAtHomeNonBlocking();
  //
  // Interact with user using keyboard.
  //
  ArGlobalFunctor lCB(&lkeyCB);
  ArGlobalFunctor pCB(&pkeyCB);
  ArGlobalFunctor hCB(&hkeyCB);
  ArGlobalFunctor rCB(&rkeyCB);
  ArGlobalFunctor qCB(&quitCB);
  ArGlobalFunctor escapeCB(&quitCB);

  keyHandler.addKeyHandler('l', &lCB);
  keyHandler.addKeyHandler('p', &pCB);
  keyHandler.addKeyHandler('h', &hCB);
  keyHandler.addKeyHandler('r', &rCB);
  keyHandler.addKeyHandler('q', &qCB);
  keyHandler.addKeyHandler(ArKeyHandler::ESCAPE, &escapeCB);

  printf("Put robot at RobotHome and press 'l' to localize first.\n\   
Press 'p' to move to the next goal\n\
Press 'h' to move to the first home\n\
Press 'r' to move to the goals in order\n\
Press 'q' to quit\n");   
  while (advancedptr->myLocaTask->getRunning() &&
     advancedptr->myPathPlanningTask->getRunning()){

    keyHandler.checkKeys();
    ArUtil::sleep(250);

    advancedptr->myRobot->lock();
    ArPose rpose = advancedptr->myRobot->getPose();
    double lvel = advancedptr->myRobot->getVel();
    double avel = advancedptr->myRobot->getRotVel();
    double volts = advancedptr->myRobot->getBatteryVoltage();
    advancedptr->myRobot->unlock();
    if(advancedptr->myLocaTask->getInitializedFlag()){
      printf("\r%5.2f %5.2f %5.2f: %5.2f %5.2f: %4.1f\r",
         rpose.getX(), rpose.getY(), rpose.getTh(), lvel, avel, volts);
      fflush(stdout);
    }
  }
}
int main(int argc, char **argv)
{
  char* host = "localhost";
  if(argc > 1)
    host = argv[1];
  Aria::init();
  ArClientBase client;
  ArGlobalFunctor escapeCB(&escape);
  ArKeyHandler keyHandler;
  Aria::setKeyHandler(&keyHandler);


  printf("Connecting to standaloneServerDemo at %s:%d...\n", host, 7272);
  if (!client.blockingConnect(host, 7272))
  {
    printf("Could not connect to server, exiting\n");
    exit(1);
  } 
  InputHandler inputHandler(&client, &keyHandler);
  OutputHandler outputHandler(&client);
  keyHandler.addKeyHandler(ArKeyHandler::ESCAPE, &escapeCB);
  client.runAsync();
  while (client.getRunningWithLock())
  {
    keyHandler.checkKeys();
    ArUtil::sleep(1);
  }
  keyHandler.restore();
  Aria::shutdown();
  return 0;
}
예제 #3
0
    void addKeyHandlers()
    {

        ArKeyHandler *keyHandler = Aria::getKeyHandler();
        if(keyHandler == NULL)
        {
            keyHandler = new ArKeyHandler();
            Aria::setKeyHandler(keyHandler);
            robot->attachKeyHandler(keyHandler);
        }
        //keyHandler->addKeyHandler('g', &myGoCB);
        //keyHandler->addKeyHandler('c', &myGoHomeCB);
        keyHandler->addKeyHandler('p', &myStartCB);
        keyHandler->addKeyHandler('s', &myStopCB);
        keyHandler->addKeyHandler('m', &myPrintCB);
    }
int main(int argc, char **argv)
{
	ros::init(argc, argv, "ariaClientDriverNode");	//ROS Initialization


	Aria::init();										//Aria Initialization
	ArClientBase client;								//setup client
	ArArgumentParser parser(&argc, argv);				//command line argument handler
	ArClientSimpleConnector clientConnector(&parser);	//connect to Arserver

	parser.loadDefaultArguments();
	if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
	{
		Aria::logOptions();
		exit(0);
	}

	if (!clientConnector.connectClient(&client))
	{
		if (client.wasRejected())
			printf("Server '%s' rejected connection, exiting\n", client.getHost());
		else
			printf("Could not connect to server '%s', exiting\n", client.getHost());
		exit(1);
	}
	printf("Connected to server.\n");

	client.setRobotName(client.getHost()); // include server name in log messages
	ArKeyHandler keyHandler;
	Aria::setKeyHandler(&keyHandler);
	ArGlobalFunctor escapeCB(&escape);
	keyHandler.addKeyHandler(ArKeyHandler::ESCAPE, &escapeCB);
	client.runAsync();

	if(!client.dataExists("ratioDrive") )
		printf("Warning: server does not have ratioDrive command, can not use drive commands!\n");
	else
		printf("Keys are:\nUP: Forward\nDOWN: Backward\nLEFT: Turn Left\nRIGHT: Turn Right\n");
	printf("s: Enable safe drive mode (if supported).\nu: Disable safe drive mode (if supported).\nl: list all data requests on server\n\nDrive commands use 'ratioDrive'.\nt: logs the network tracking tersely\nv: logs the network tracking verbosely\nr: resets the network tracking\n\n");


	AriaClientDriver ariaClientDriver(&client,&keyHandler,"");

	//while (ros::ok() && client.getRunningWithLock()) //the main loop
	while (client.getRunningWithLock()) //the main loop
	{
		keyHandler.checkKeys();  //addthis if teleop from node required
		ariaClientDriver.controlloop();
		//Input output handling callback threads implemented in ariaClientDriver Class
		ArUtil::sleep(100);	//noneed

	}

	client.disconnect();
	Aria::shutdown();
	return 0;
}
예제 #5
0
int main(int argc, char **argv)
{
  Aria::init();

  ArClientBase client;

  ArArgumentParser parser(&argc, argv);

  ArClientSimpleConnector clientConnector(&parser);

  parser.loadDefaultArguments();

  if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
  {
    Aria::logOptions();
    exit(0);
  }

  if (!clientConnector.connectClient(&client))
  {
    if (client.wasRejected())
      printf("Server '%s' rejected connection, exiting\n", client.getHost());
    else
      printf("Could not connect to server '%s', exiting\n", client.getHost());
    exit(1);
  } 

  printf("Connected to server.\n");


  /* Create a key handler and also tell Aria about it */
  ArKeyHandler keyHandler;
  Aria::setKeyHandler(&keyHandler);

  /* Global escape-key handler to shut everythnig down */
  ArGlobalFunctor escapeCB(&escape);
  keyHandler.addKeyHandler(ArKeyHandler::ESCAPE, &escapeCB);

  client.runAsync();

  while (client.getRunningWithLock())
  {
    keyHandler.checkKeys();
    ArUtil::sleep(100);
  }

  Aria::shutdown();
  return 0;
}
예제 #6
0
AREXPORT void ArActionKeydrive::takeKeys(void)
{
  ArKeyHandler *keyHandler;
  if ((keyHandler = Aria::getKeyHandler()) == NULL)
  {
    ArLog::log(ArLog::Terse, 
	       "ArActionKeydrive::takeKeys: There is no key handler, keydrive will not work.");
  }
  // now that we have one, add our keys as callbacks, print out big
  // warning messages if they fail
  if (!keyHandler->addKeyHandler(ArKeyHandler::UP, &myUpCB))
    ArLog::log(ArLog::Terse, "The key handler already has a key for up, keydrive will not work correctly.");
  if (!keyHandler->addKeyHandler(ArKeyHandler::DOWN, &myDownCB))
    ArLog::log(ArLog::Terse, "The key handler already has a key for down, keydrive will not work correctly.");
  if (!keyHandler->addKeyHandler(ArKeyHandler::LEFT, &myLeftCB))
    ArLog::log(ArLog::Terse,  
	       "The key handler already has a key for left, keydrive will not work correctly.");
  if (!keyHandler->addKeyHandler(ArKeyHandler::RIGHT, &myRightCB))
    ArLog::log(ArLog::Terse,  
	       "The key handler already has a key for right, keydrive will not work correctly.");
  if (!keyHandler->addKeyHandler(ArKeyHandler::SPACE, &mySpaceCB))
    ArLog::log(ArLog::Terse,  
	       "The key handler already has a key for space, keydrive will not work correctly.");
}
AREXPORT void ArRatioInputKeydrive::takeKeys(void)
{
  myHaveKeys = true;
  ArKeyHandler *keyHandler;
  if ((keyHandler = Aria::getKeyHandler()) == NULL)
  {
    ArLog::log(ArLog::Terse, 
	       "ArRatioInputKeydrive::takeKeys: There is no key handler, keydrive will not work.");
  }
  // now that we have one, add our keys as callbacks, print out big
  // warning messages if they fail
  if (!keyHandler->addKeyHandler(ArKeyHandler::UP, &myUpCB))
    ArLog::log(ArLog::Terse, "The key handler already has a key for up, keydrive will not work correctly.");
  if (!keyHandler->addKeyHandler(ArKeyHandler::DOWN, &myDownCB))
    ArLog::log(ArLog::Terse, "The key handler already has a key for down, keydrive will not work correctly.");
  if (!keyHandler->addKeyHandler(ArKeyHandler::LEFT, &myLeftCB))
    ArLog::log(ArLog::Terse,  
	       "The key handler already has a key for left, keydrive will not work correctly.");
  if (!keyHandler->addKeyHandler(ArKeyHandler::RIGHT, &myRightCB))
    ArLog::log(ArLog::Terse,  
	       "The key handler already has a key for right, keydrive will not work correctly.");
  if (!keyHandler->addKeyHandler(ArKeyHandler::SPACE, &mySpaceCB))
    ArLog::log(ArLog::Terse,  
	       "The key handler already has a key for space, keydrive will not work correctly.");
  if (myRobot != NULL && myRobot->hasLatVel())
  {
    if (!keyHandler->addKeyHandler('z', &myZCB))
      ArLog::log(ArLog::Terse,  
		 "The key handler already has a key for z, keydrive will not work correctly.");
    if (!keyHandler->addKeyHandler('Z', &myZCB))
      ArLog::log(ArLog::Terse,  
		 "The key handler already has a key for Z, keydrive will not work correctly.");
    if (!keyHandler->addKeyHandler('x', &myXCB))
      ArLog::log(ArLog::Terse,  
		 "The key handler already has a key for x, keydrive will not work correctly.");
    if (!keyHandler->addKeyHandler('X', &myXCB))
      ArLog::log(ArLog::Terse,  
		 "The key handler already has a key for x, keydrive will not work correctly.");
  }
}
/**
   @param robot the robot we're attaching to
   
   @param name the name of this mode

   @param key the primary key to switch to this mode on... it can be
   '\\0' if you don't want to use this

   @param key2 an alternative key to switch to this mode on... it can be
   '\\0' if you don't want a second alternative key
**/
AREXPORT ArMode::ArMode(ArRobot *robot, const char *name, char key, 
			char key2) :
  myActivateCB(this, &ArMode::activate),
  myDeactivateCB(this, &ArMode::deactivate),
  myUserTaskCB(this, &ArMode::userTask)
{
  ArKeyHandler *keyHandler;
  myName = name;
  myRobot = robot;
  myKey = key;
  myKey2 = key2;
  // see if there is already a keyhandler, if not make one for ourselves
  if ((keyHandler = Aria::getKeyHandler()) == NULL)
  {
    keyHandler = new ArKeyHandler;
    Aria::setKeyHandler(keyHandler);
    if (myRobot != NULL)
      myRobot->attachKeyHandler(keyHandler);
    else
      ArLog::log(ArLog::Terse, "ArMode: No robot to attach a keyHandler to, keyHandling won't work... either make your own keyHandler and drive it yourself, make a keyhandler and attach it to a robot, or give this a robot to attach to.");
  }  
  if (ourHelpCB == NULL)
  {
    ourHelpCB = new ArGlobalFunctor(&ArMode::baseHelp);
    if (!keyHandler->addKeyHandler('h', ourHelpCB))
      ArLog::log(ArLog::Terse, "The key handler already has a key for 'h', ArMode will not be invoked on an 'h' keypress.");
    if (!keyHandler->addKeyHandler('H', ourHelpCB))
      ArLog::log(ArLog::Terse, "The key handler already has a key for 'H', ArMode will not be invoked on an 'H' keypress.");
    if (!keyHandler->addKeyHandler('?', ourHelpCB))
      ArLog::log(ArLog::Terse, "The key handler already has a key for '?', ArMode will not be invoked on an '?' keypress.");
    if (!keyHandler->addKeyHandler('/', ourHelpCB))
      ArLog::log(ArLog::Terse, "The key handler already has a key for '/', ArMode will not be invoked on an '/' keypress.");

  }

  // now that we have one, add our keys as callbacks, print out big
  // warning messages if they fail
  if (myKey != '\0')
    if (!keyHandler->addKeyHandler(myKey, &myActivateCB))
      ArLog::log(ArLog::Terse, "The key handler already has a key for '%c', ArMode will not work correctly.", myKey);
  if (myKey2 != '\0')
    if (!keyHandler->addKeyHandler(myKey2, &myActivateCB))
      ArLog::log(ArLog::Terse, "The key handler already has a key for '%c', ArMode will not work correctly.", myKey2);

  // toss this mode into our list of modes
  ourModes.push_front(this);
}
 void addKeyHandlers(ArRobot *robot)
 {
   ArKeyHandler *keyHandler = Aria::getKeyHandler();
   if(keyHandler == NULL)
   {
     keyHandler = new ArKeyHandler();
     Aria::setKeyHandler(keyHandler);
     robot->attachKeyHandler(keyHandler);
   }
   keyHandler->addKeyHandler(ArKeyHandler::PAGEUP, &myUpCB);
   keyHandler->addKeyHandler('u', &myUpCB);
   keyHandler->addKeyHandler(ArKeyHandler::PAGEDOWN, &myDownCB);
   keyHandler->addKeyHandler('d', &myDownCB);
   keyHandler->addKeyHandler('o', &myOpenCB);
   keyHandler->addKeyHandler('c', &myCloseCB);
   keyHandler->addKeyHandler('s', &myStopCB);
 }
예제 #10
0
/*
  Constructor, sets the robot pointer, and some initial values, also note the
  use of constructor chaining on myPTU and myDriveCB.
*/
KeyPTU::KeyPTU(ArRobot *robot) :
  myUpCB(this, &KeyPTU::up),
  myDownCB(this, &KeyPTU::down),
  myLeftCB(this, &KeyPTU::left),
  myRightCB(this, &KeyPTU::right),
  myPlusCB(this, &KeyPTU::plus),
  myMinusCB(this, &KeyPTU::minus),
  myGreaterCB(this, &KeyPTU::greater),
  myLessCB(this, &KeyPTU::less),
  myQuestionCB(this, &KeyPTU::question),
  mySCB(this, &KeyPTU::status),
  myECB(this, &KeyPTU::exercise),
  myACB(this, &KeyPTU::autoupdate),
  myCCB(this, &KeyPTU::c),
  myHCB(this, &KeyPTU::h),
  myICB(this, &KeyPTU::i),
  myPCB(this, &KeyPTU::p),
  myXCB(this, &KeyPTU::x),
  myZCB(this, &KeyPTU::z),

  myPTU(robot),
  myDriveCB(this, &KeyPTU::drive)
{
  // set the robot pointer and add the KeyPTU as user task
  ArKeyHandler *keyHandler;
  myRobot = robot;
  myRobot->addSensorInterpTask("KeyPTU", 50, &myDriveCB);

  myExerciseTime.setToNow();
  myExercise = true;

//  SETPORT Uncomment the following to run the camera off
//  of the computer's serial port, rather than the microcontroller

// uncomment below here
/*
#ifdef WIN32
  myCon.setPort("COM2");
#else
  myCon.setPort("/dev/ttyS0");
#endif
  myPTU.setDeviceConnection(&myCon);
*/
// to here

  // or use this next line to set the aux port 
 //myPTU.setAuxPort(2);


  if ((keyHandler = Aria::getKeyHandler()) == NULL)
  {
    keyHandler = new ArKeyHandler;
    Aria::setKeyHandler(keyHandler);
    myRobot->attachKeyHandler(keyHandler);
  }

  if (!keyHandler->addKeyHandler(ArKeyHandler::UP, &myUpCB))
    ArLog::log(ArLog::Terse, "The key handler already has a key for up, keydrive will not work correctly.");
  if (!keyHandler->addKeyHandler(ArKeyHandler::DOWN, &myDownCB))
    ArLog::log(ArLog::Terse, "The key handler already has a key for down, keydrive will not work correctly.");
  if (!keyHandler->addKeyHandler(ArKeyHandler::LEFT, &myLeftCB))
    ArLog::log(ArLog::Terse,  
"The key handler already has a key for left, keydrive will not work correctly.");
  if (!keyHandler->addKeyHandler(ArKeyHandler::RIGHT, &myRightCB))
    ArLog::log(ArLog::Terse,  
"The key handler already has a key for right, keydrive will not work correctly.");

  if (!keyHandler->addKeyHandler('+', &myPlusCB))
    ArLog::log(ArLog::Terse,
"The key handler already has a key for '+', keydrive will not work correctly.");
  if (!keyHandler->addKeyHandler('-', &myMinusCB))
    ArLog::log(ArLog::Terse,
"The key handler already has a key for '-', keydrive will not work correctly.");
  if (!keyHandler->addKeyHandler('>', &myGreaterCB))
    ArLog::log(ArLog::Terse,
"The key handler already has a key for '>', keydrive will not work correctly.");
  if (!keyHandler->addKeyHandler('<', &myLessCB))
    ArLog::log(ArLog::Terse,
"The key handler already has a key for '<', keydrive will not work correctly.");
  if (!keyHandler->addKeyHandler('?', &myQuestionCB))
    ArLog::log(ArLog::Terse,
"The key handler already has a key for '?', keydrive will not work correctly.");

  if (!keyHandler->addKeyHandler('c', &myCCB))
    ArLog::log(ArLog::Terse,
"The key handler already has a key for 'C', keydrive will not work correctly.");
  if (!keyHandler->addKeyHandler('h', &myHCB))
    ArLog::log(ArLog::Terse,
"The key handler already has a key for 'H', keydrive will not work correctly.");
  if (!keyHandler->addKeyHandler('i', &myICB))
    ArLog::log(ArLog::Terse,
"The key handler already has a key for 'I', keydrive will not work correctly.");
  if (!keyHandler->addKeyHandler('p', &myPCB))
    ArLog::log(ArLog::Terse,
"The key handler already has a key for 'P', keydrive will not work correctly.");
  if (!keyHandler->addKeyHandler('s', &mySCB))
    ArLog::log(ArLog::Terse,
"The key handler already has a key for 'S', keydrive will not work correctly.");
  if (!keyHandler->addKeyHandler('x', &myXCB))
    ArLog::log(ArLog::Terse,
"The key handler already has a key for 'X', keydrive will not work correctly.");
  if (!keyHandler->addKeyHandler('z', &myZCB))
    ArLog::log(ArLog::Terse,
"The key handler already has a key for 'Z', keydrive will not work correctly.");
  if (!keyHandler->addKeyHandler('a', &myACB))
    ArLog::log(ArLog::Terse, "The key handler already has a key for 'A', keydrive will not work correctly.");
  if (!keyHandler->addKeyHandler('e', &myECB))
    ArLog::log(ArLog::Terse, "The key handler already has a key for 'E', keydrive will not work correctly.");



  // initialize some variables

  myPTUInitRequested = false;
  myPosIncrement = 1;
  mySlewIncrement = 5;
  myZoomIncrement = 50;

}
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;
}
예제 #12
0
int main(int argc, char **argv)
{
  Aria::init();
  ArArgumentParser parser(&argc, argv);
  parser.loadDefaultArguments();
  ArRobot robot;
  ArRobotConnector robotConnector(&parser, &robot);
  ArLaserConnector laserConnector(&parser, &robot, &robotConnector);

  if(!robotConnector.connectRobot())
  {
    ArLog::log(ArLog::Terse, "lineFinderExample: Could not connect to the robot.");
    if(parser.checkHelpAndWarnUnparsed())
    {
        // -help not given
        Aria::logOptions();
        Aria::exit(1);
    }
  }

  if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
  {
    Aria::logOptions();
    Aria::exit(1);
  }

  ArLog::log(ArLog::Normal, "lineFinderExample: Connected to robot.");

  robot.runAsync(true);

  // Connect to laser(s) as defined in parameter files.
  // (Some flags are available as arguments to connectLasers() to control error behavior and to control which lasers are put in the list of lasers stored by ArRobot. See docs for details.)
  if(!laserConnector.connectLasers())
  {
    ArLog::log(ArLog::Terse, "Could not connect to configured lasers. Exiting.");
    Aria::exit(3);
    return 3;
  }

  ArLog::log(ArLog::Normal, "lineFinderExample: Connected to laser");

  ArKeyHandler keyHandler;
  Aria::setKeyHandler(&keyHandler);
  robot.attachKeyHandler(&keyHandler);


  // Create the ArLineFinder object. Set it to log lots of information about its
  // processing.

  ArLaser *laser = robot.findLaser(1);
  if(!laser)
  {
    ArLog::log(ArLog::Terse, "lineFinderExample: No laser device connected, exiting.");
    Aria::exit(4);
    return 4;
  }

  ArLineFinder lineFinder(laser);
  lineFinder.setVerbose(true);

  // Add key callbacks that simply call the ArLineFinder::getLinesAndSaveThem()
  // function, which searches for lines in the current set of laser sensor
  // readings, and saves them in files with the names 'points' and 'lines'.
  ArFunctorC<ArLineFinder> findLineCB(&lineFinder, 
				  &ArLineFinder::getLinesAndSaveThem);
  keyHandler.addKeyHandler('f', &findLineCB);
  keyHandler.addKeyHandler('F', &findLineCB);

  
  printf("If you press the 'f' key the points and lines found will be saved\n");
  printf("Into the 'points' and 'lines' file in the current working directory\n");

  robot.waitForRunExit();
  Aria::exit(0);
  return 0;
}
예제 #13
0
/**
   Make sure you have called ArSick::configure or
   ArSick::configureShort on your laser before you make this class

   @param robot The robot to attach to

   @param sick the laser to log from

   @param distDiff the distance traveled at which to take a new reading

   @param degDiff the degrees turned at which to take a new reading

   @param fileName the file name in which to put the log

   @param addGoals whether to add goals automatically or... if true
   then the sick logger puts hooks into places it needs this to
   happen, into any keyhandler thats around (for a keypress of G), it
   pays attention to the flag bit of the robot, and it puts in a
   button press callback for the joyhandler passed in (if any)
**/
AREXPORT ArSickLogger::ArSickLogger(ArRobot *robot, ArSick *sick,
                                    double distDiff, double degDiff,
                                    const char *fileName, bool addGoals,
                                    ArJoyHandler *joyHandler,
                                    const char *baseDirectory,
                                    bool useReflectorValues,
                                    ArRobotJoyHandler *robotJoyHandler) :
    mySectors(18),
    myTaskCB(this, &ArSickLogger::robotTask),
    myGoalKeyCB(this, &ArSickLogger::goalKeyCallback),
    myLoopPacketHandlerCB(this, &ArSickLogger::loopPacketHandler)
{
    ArKeyHandler *keyHandler;

    ArSick::Degrees degrees;
    ArSick::Increment increment;
    double deg, incr;

    myOldReadings = false;
    myNewReadings = true;
    myUseReflectorValues = useReflectorValues;
    myWrote = false;
    myRobot = robot;
    mySick = sick;
    if (baseDirectory != NULL && strlen(baseDirectory) > 0)
        myBaseDirectory = baseDirectory;
    else
        myBaseDirectory = "";
    std::string realFileName;
    if (fileName[0] == '/' || fileName[0] == '\\')
    {
        realFileName = fileName;
    }
    else
    {
        realFileName = myBaseDirectory;
        realFileName += fileName;
    }
    myFileName = realFileName;

    myFile = fopen(realFileName.c_str(), "w+");
    degrees = mySick->getDegrees();
    increment = mySick->getIncrement();
    if (degrees == ArSick::DEGREES180)
        deg = 180;
    else
        deg = 100;
    if (increment == ArSick::INCREMENT_ONE)
        incr = 1;
    else
        incr = .5;
    if (myFile != NULL)
    {
        const ArRobotParams *params;
        params = robot->getRobotParams();
        fprintf(myFile, "LaserOdometryLog\n");
        fprintf(myFile, "#Created by ARIA's ArSickLogger\n");
        fprintf(myFile, "version: 2\n");
        fprintf(myFile, "sick1pose: %d %d %.2f\n", params->getLaserX(),
                params->getLaserY(), params->getLaserTh());
        fprintf(myFile, "sick1conf: %d %d %d\n",
                ArMath::roundInt(0.0 - deg / 2.0),
                ArMath::roundInt(deg / 2.0), ArMath::roundInt(deg / incr + 1.0));
    }
    else
        ArLog::log(ArLog::Terse, "ArSickLogger cannot write to file %s",
                   myFileName.c_str());

    myDistDiff = distDiff;
    myDegDiff = degDiff;
    myFirstTaken = false;
    myScanNumber = 0;
    myLastVel = 0;
    myStartTime.setToNow();
    myRobot->addUserTask("Sick Logger", 1, &myTaskCB);

    char uCFileName[15];
    strncpy(uCFileName, fileName, 14);
    uCFileName[14] = '\0';
    myRobot->comStr(94, uCFileName);

    myLoopPacketHandlerCB.setName("ArSickLogger");
    myRobot->addPacketHandler(&myLoopPacketHandlerCB);

    myAddGoals = addGoals;
    myJoyHandler = joyHandler;
    myRobotJoyHandler = robotJoyHandler;
    myTakeReadingExplicit = false;
    myAddGoalExplicit = false;
    myAddGoalKeyboard = false;
    myLastAddGoalKeyboard = false;
    myLastJoyButton = false;
    myLastRobotJoyButton = false;
    myFirstGoalTaken = false;
    myNumGoal = 1;
    myLastLoops = 0;
    // only add goals from the keyboard if there's already a keyboard handler
    if (myAddGoals && (keyHandler = Aria::getKeyHandler()) != NULL)
    {
        // now that we have a key handler, add our keys as callbacks, print out big
        // warning messages if they fail
        if (!keyHandler->addKeyHandler('g', &myGoalKeyCB))
            ArLog::log(ArLog::Terse, "The key handler already has a key for g, sick logger goal handling will not work correctly.");
        if (!keyHandler->addKeyHandler('G', &myGoalKeyCB))
            ArLog::log(ArLog::Terse, "The key handler already has a key for g, sick logger goal handling will not work correctly.");
    }
}
예제 #14
0
/*
  Constructor, sets the robot pointer, and some initial values, also note the
  use of constructor chaining on myPTU and myDriveCB.
*/
KeyPTU::KeyPTU(ArRobot *robot) :
  myUpCB(this, &KeyPTU::up),
  myDownCB(this, &KeyPTU::down),
  myLeftCB(this, &KeyPTU::left),
  myRightCB(this, &KeyPTU::right),
  mySpaceCB(this, &KeyPTU::space),
  myICB(this, &KeyPTU::i),
  myPlusCB(this, &KeyPTU::plus),
  myMinusCB(this, &KeyPTU::minus),
  myGreaterCB(this, &KeyPTU::greater),
  myLessCB(this, &KeyPTU::less),
  myQuestionCB(this, &KeyPTU::question),
  mySCB(this, &KeyPTU::status),
  myACB(this, &KeyPTU::a),
  myZCB(this, &KeyPTU::z),
  myMCB(this, &KeyPTU::m),
  myHCB(this, &KeyPTU::h),
  myRCB(this, &KeyPTU::r),
  myPTU(robot),
  myDriveCB(this, &KeyPTU::drive),
  mySerialConnection(NULL)
{
#ifdef SERIAL_PORT
  mySerialConnection = new ArSerialConnection;
  ArLog::log(ArLog::Normal, "dpptuExample: connecting to DPPTU over computer serial port %s.", SERIAL_PORT);
  if(mySerialConnection->open(SERIAL_PORT) != 0)
  {
	ArLog::log(ArLog::Terse, "dpptuExample: Error: Could not open computer serial port %s for DPPTU!", SERIAL_PORT);
    Aria::exit(5);
  }
  myPTU.setDeviceConnection(mySerialConnection);
#endif

  // set the robot pointer and add the KeyPTU as user task
  ArKeyHandler *keyHandler;
  myRobot = robot;
  myRobot->addSensorInterpTask("KeyPTU", 50, &myDriveCB);

  if ((keyHandler = Aria::getKeyHandler()) == NULL)
  {
    keyHandler = new ArKeyHandler;
    Aria::setKeyHandler(keyHandler);
    myRobot->attachKeyHandler(keyHandler);
  }

  if (!keyHandler->addKeyHandler(ArKeyHandler::UP, &myUpCB))
    ArLog::log(ArLog::Terse, "The key handler already has a key for up, keydrive will not work correctly.");
  if (!keyHandler->addKeyHandler(ArKeyHandler::DOWN, &myDownCB))
    ArLog::log(ArLog::Terse, "The key handler already has a key for down, keydrive will not work correctly.");
  if (!keyHandler->addKeyHandler(ArKeyHandler::LEFT, &myLeftCB))
    ArLog::log(ArLog::Terse,  
"The key handler already has a key for left, keydrive will not work correctly.");
  if (!keyHandler->addKeyHandler(ArKeyHandler::RIGHT, &myRightCB))
    ArLog::log(ArLog::Terse,  
"The key handler already has a key for right, keydrive will not work correctly.");
  if (!keyHandler->addKeyHandler(ArKeyHandler::SPACE, &mySpaceCB))
    ArLog::log(ArLog::Terse,
"The key handler already has a key for space, keydrive will not work correctly.");
  if (!keyHandler->addKeyHandler('i', &myICB))
    ArLog::log(ArLog::Terse,
"The key handler already has a key for 'i', keydrive will not work correctly.");
  if (!keyHandler->addKeyHandler('+', &myPlusCB))
    ArLog::log(ArLog::Terse,
"The key handler already has a key for '+', keydrive will not work correctly.");
  if (!keyHandler->addKeyHandler('-', &myMinusCB))
    ArLog::log(ArLog::Terse,
"The key handler already has a key for '-', keydrive will not work correctly.");
  if (!keyHandler->addKeyHandler('>', &myGreaterCB))
    ArLog::log(ArLog::Terse,
"The key handler already has a key for '>', keydrive will not work correctly.");
  if (!keyHandler->addKeyHandler('<', &myLessCB))
    ArLog::log(ArLog::Terse,
"The key handler already has a key for '<', keydrive will not work correctly.");
  if (!keyHandler->addKeyHandler('?', &myQuestionCB))
    ArLog::log(ArLog::Terse,
"The key handler already has a key for '?', keydrive will not work correctly.");
  if (!keyHandler->addKeyHandler('s', &mySCB))
    ArLog::log(ArLog::Terse,
"The key handler already has a key for 'S', keydrive will not work correctly.");
  if (!keyHandler->addKeyHandler('a', &myACB))
    ArLog::log(ArLog::Terse,
"The key handler already has a key for 'A', keydrive will not work correctly.");
  if (!keyHandler->addKeyHandler('z', &myZCB))
    ArLog::log(ArLog::Terse,
"The key handler already has a key for 'Z', keydrive will not work correctly.");
  if (!keyHandler->addKeyHandler('m', &myMCB))
    ArLog::log(ArLog::Terse,
"The key handler already has a key for 'M', keydrive will not work correctly.");
  if (!keyHandler->addKeyHandler('h', &myHCB))
    ArLog::log(ArLog::Terse,
"The key handler already has a key for 'H', keydrive will not work correctly.");
  if (!keyHandler->addKeyHandler('r', &myRCB))
    ArLog::log(ArLog::Terse,
"The key handler already has a key for 'R', keydrive will not work correctly.");

  // initialize some variables
  myReset = false;
  myInit = true;
  myDesiredPanPos = 0;
  myDesiredTiltPos = 0;
  myPosIncrement = 1;
  mySlewIncrement = 1;
  myPTUInited = false;
  posIncIncrement = 1;
  myMonitor = false;

}
예제 #15
0
int main(int argc, char **argv)
{
  bool done;
  double distToTravel = 3000;
  int spinTime = 0;

  // set up our simpleConnector
  ArSimpleConnector simpleConnector(&argc, argv);
  // set up a key handler so escape exits and attach to the robot
  ArKeyHandler keyHandler;

  Aria::init();

  robot = new ArRobot;

  printf("You can press the escape key to exit this program\n");

  // parse its arguments
  if (simpleConnector.parseArgs())
  {
    simpleConnector.logOptions();
    exit(1);
  }

  // if there are more arguments left then it means we didn't
  // understand an option
  /*
  if (argc > 1)
  {    
    simpleConnector.logOptions();
    keyHandler.restore();
    exit(1);
  }
  */

  ArGlobalFunctor exitCB(&hardExit);
  ArGlobalFunctor printerCB(&printer);

  keyHandler.addKeyHandler(ArKeyHandler::ESCAPE, &exitCB);
  robot->attachKeyHandler(&keyHandler);
  Aria::setKeyHandler(&keyHandler);



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

  //robot->addUserTask("printer", 50, &printerCB);
  // run the robot, true here so that the run will exit if connection lost
  robot->runAsync(true);

#ifdef WIN32
  // wait until someone pushes the motor button to go
  printf("Press the motor button to start the robot moving\n");
  while (1)
  {
    robot->lock();
    if (!robot->isRunning())
      hardExit();
    if (robot->areMotorsEnabled())
    {
      robot->unlock();
      break;
    }
    robot->unlock();
    ArUtil::sleep(100);
  }
#endif
  ArAnalogGyro *gyro;

  if (argc == 1)
  {
    printf("Gyro\n");
    gyro = new ArAnalogGyro(robot);
  }
  printf("Waiting for inertial to stabilize for 5 seconds.\n");
  // wait a bit for the inertial to warm up
  ArUtil::sleep(5000);
  // basically from here on down the robot just cruises around a bit
  robot->lock();
  // enable the motors, disable amigobot sounds
  robot->comInt(ArCommands::SONAR, 0);
  robot->comInt(ArCommands::ENABLE, 1);
  robot->setMoveDoneDist(200);

  // move a couple meters
  printf("Driving out\n");
  robot->move(distToTravel);
  robot->setHeading(0);
  robot->unlock();
  do {
    ArUtil::sleep(100);
    robot->lock();
    //robot->setHeading(0);
    done = robot->isMoveDone(200);
    robot->unlock();
  } while (!done);

  if (spinTime != 0)
  {
    printf("Spinning a while\n");
    // rotate a few times
    robot->lock();
    robot->setRotVel(200);
    robot->unlock();
    ArUtil::sleep(spinTime * 1000);
  }

  printf("Pointing back\n");
  robot->lock();
  robot->setHeading(180);
  robot->unlock();
  do {
    ArUtil::sleep(100);
    robot->lock();
    //robot->setHeading(180);
    done = robot->isHeadingDone(5);
    robot->unlock();
  } while (!done);

  printf("Driving back\n");
  // move a couple meters
  robot->lock();
  robot->move(distToTravel);
  robot->setHeading(180);
  robot->unlock();
  do {
    ArUtil::sleep(100);
    robot->lock();
    //robot->setHeading(180);
    done = robot->isMoveDone(200);
    robot->unlock();
  } while (!done);

  printf("Pointing back in original direction.\n");
  robot->lock();
  robot->setHeading(0);
  robot->unlock();
  do {
    ArUtil::sleep(100);
    robot->lock();
    //robot->setHeading(0);
    done = robot->isHeadingDone(5);
    robot->unlock();
  } while (!done);


  robot->lock();
  printf("Final heading %.2f\n", robot->getTh());
  robot->disconnect();
  robot->unlock();
  // now exit
  Aria::shutdown();
  return 0;
}
예제 #16
0
/*
  Constructor, sets the robot pointer, and some initial values, also note the
  use of constructor chaining on myCam and myDriveCB.
*/
Joydrive::Joydrive(ArRobot *robot, int LRAmount, int UDAmount, int zoomAmount) :
    myCam(robot),
    myDriveCB(this, &Joydrive::drive),
    myUpCB(this, &Joydrive::up),
    myDownCB(this, &Joydrive::down),
    myLeftCB(this, &Joydrive::left),
    myRightCB(this, &Joydrive::right),
    myInCB(this, &Joydrive::in),
    myOutCB(this, &Joydrive::out),
    myCenterCB(this, &Joydrive::center)

{
    // set the robot pointer and add the joydrive as a user task
    myRobot = robot;
    myRobot->addUserTask("joydrive", 50, &myDriveCB);




    // initalize some variables
    myRotValRobot = 100;
    myTransValRobot = 700;
    myPanValCamera = 8;
    myTiltValCamera = 3;
    myZoomValCamera = 50;
    myLRAmount = LRAmount;
    myUDAmount = UDAmount;
    myZoomAmount = zoomAmount;

    ArKeyHandler *keyHandler;
    myRobot = robot;
    // see if there is already a keyhandler, if not make one for ourselves
    if ((keyHandler = Aria::getKeyHandler()) == NULL)
    {
        keyHandler = new ArKeyHandler;
        Aria::setKeyHandler(keyHandler);
        myRobot->attachKeyHandler(keyHandler);
    }
    // now that we have one, add our keys as callbacks, print out big
    // warning messages if they fail
    if (!keyHandler->addKeyHandler(ArKeyHandler::UP, &myUpCB))
        ArLog::log(ArLog::Terse, "The key handler already has a key for up, keydrive will not work correctly.");
    if (!keyHandler->addKeyHandler(ArKeyHandler::DOWN, &myDownCB))
        ArLog::log(ArLog::Terse, "The key handler already has a key for down, keydrive will not work correctly.");
    if (!keyHandler->addKeyHandler(ArKeyHandler::LEFT, &myLeftCB))
        ArLog::log(ArLog::Terse,
                   "The key handler already has a key for left, keydrive will not work correctly.");
    if (!keyHandler->addKeyHandler(ArKeyHandler::RIGHT, &myRightCB))
        ArLog::log(ArLog::Terse,
                   "The key handler already has a key for right, keydrive will not work correctly.");
    if (!keyHandler->addKeyHandler('z', &myInCB))
        ArLog::log(ArLog::Terse,
                   "The key handler already has a key for 'z', keydrive will not work correctly.");
    if (!keyHandler->addKeyHandler('Z', &myInCB))
        ArLog::log(ArLog::Terse,
                   "The key handler already has a key for 'Z', keydrive will not work correctly.");
    if (!keyHandler->addKeyHandler('x', &myOutCB))
        ArLog::log(ArLog::Terse,
                   "The key handler already has a key for 'x', keydrive will not work correctly.");
    if (!keyHandler->addKeyHandler('X', &myOutCB))
        ArLog::log(ArLog::Terse,
                   "The key handler already has a key for 'X', keydrive will not work correctly.");
    if (!keyHandler->addKeyHandler('c', &myCenterCB))
        ArLog::log(ArLog::Terse,
                   "The key handler already has a key for 'c', keydrive will not work correctly.");
    if (!keyHandler->addKeyHandler('C', &myCenterCB))
        ArLog::log(ArLog::Terse,
                   "The key handler already has a key for 'C', keydrive will not work correctly.");


    // initialize the joystick
    myJoyHandler.init();
    // see if we have a joystick, and let the usre know the results
    if (myJoyHandler.haveJoystick())
    {
        printf("Have a joystick\n\n");
    }
    // we don't have a joystic, so get out
    else
    {
        printf("Do not have a joystick, only the keyboard will work.\n");
    }
}
void ArMode::addKeyHandler(int keyToHandle, ArFunctor *functor)
{
  ArKeyHandler *keyHandler;
  std::string charStr;

  // see if there is already a keyhandler, if not something is wrong
  // (since constructor should make one if there isn't one yet
  if ((keyHandler = Aria::getKeyHandler()) == NULL)
  {
    ArLog::log(ArLog::Terse,"ArMode '%s'::keyHandler: There should already be a key handler, but there isn't... mode won't work right.", getName());
    return;
  }
  if (!keyHandler->addKeyHandler(keyToHandle, functor))
  {
    switch (keyToHandle) {
    case ArKeyHandler::UP:
      charStr = "Up";
      break;
    case ArKeyHandler::DOWN:
      charStr = "Down";
      break;
    case ArKeyHandler::LEFT:
      charStr = "Left";
      break;
    case ArKeyHandler::RIGHT:
      charStr = "Right";
      break;
    case ArKeyHandler::ESCAPE:
      charStr = "Escape";
      break;
    case ArKeyHandler::F1:
      charStr = "F1";
      break;
    case ArKeyHandler::F2:
      charStr = "F2";
      break;
    case ArKeyHandler::F3:
      charStr = "F3";
      break;
    case ArKeyHandler::F4:
      charStr = "F4";
      break;
    case ArKeyHandler::SPACE:
      charStr = "Space";
      break;
    case ArKeyHandler::TAB:
      charStr = "Tab";
      break;
    case ArKeyHandler::ENTER:
      charStr = "Enter";
      break;
    case ArKeyHandler::BACKSPACE:
      charStr = "Backspace";
      break;
    default:
      charStr = (char)keyToHandle;
      break;
    }
    ArLog::log(ArLog::Terse,  
	       "ArMode '%s': The key handler has a duplicate key for '%s' so the mode may not work right.", getName(), charStr.c_str());
  }
  
}
예제 #18
0
int main(int argc, char **argv)
{

  desiredVels.resize(5);

  puts("Connecting... press ctrl-c to cancel");
  if(!arm.open())
  {
    puts("Error opening arm");
    return 1;
  }

  puts("\nConnected to arm.\nPress spacebar or exit program (ESC) to stop arm.\n");
  puts("  b/1/2      Select base joints (joint 1 rotation, joint 2 pivot)");
  puts("  e/2/3      Select elbow joints (joint 3 pivot, joint 4 rotation)");
  puts("  w/4/5      Select wrist joints (joint 5 pivot, joint 6 rotation)");
  puts("  UP/DOWN    Change pivot speed");
  puts("  LEFT/RIGHT Change rotate speed");
  puts("  SPACE      Stop");
  puts("  g/G        Gripper");
  puts("\npress ESC or q to exit program\n");

  ArKeyHandler keys;
  keys.addKeyHandler(ArKeyHandler::SPACE, new ArFunctorC<Arm>(&arm, &Arm::haltAll));
  keys.addKeyHandler(ArKeyHandler::UP,  new ArGlobalFunctor(&increasePivot));
  keys.addKeyHandler(ArKeyHandler::DOWN,  new ArGlobalFunctor(&decreasePivot));
  keys.addKeyHandler(ArKeyHandler::LEFT,  new ArGlobalFunctor(&decreaseRotate));
  keys.addKeyHandler(ArKeyHandler::RIGHT,  new ArGlobalFunctor(&increaseRotate));
  keys.addKeyHandler('G', new ArGlobalFunctor(&increaseGrip));
  keys.addKeyHandler('g', new ArGlobalFunctor(&decreaseGrip));
  keys.addKeyHandler('b', new ArGlobalFunctor(&selectBase));
  keys.addKeyHandler('e', new ArGlobalFunctor(&selectElbow));
  keys.addKeyHandler('w', new ArGlobalFunctor(&selectWrist));

  keys.addKeyHandler('1', new ArGlobalFunctor(&selectBase));
  keys.addKeyHandler('2', new ArGlobalFunctor(&selectBase));
  keys.addKeyHandler('3', new ArGlobalFunctor(&selectElbow));
  keys.addKeyHandler('4', new ArGlobalFunctor(&selectElbow));
  keys.addKeyHandler('5', new ArGlobalFunctor(&selectWrist));
  keys.addKeyHandler('6', new ArGlobalFunctor(&selectWrist));
  
  keys.addKeyHandler(ArKeyHandler::ESCAPE, new ArGlobalFunctor1<int>(&Aria::exit, 0));
  keys.addKeyHandler('q', new ArGlobalFunctor1<int>(&Aria::exit, 0));

  std::vector<float> pos;
  int ctr = 0;
  while(true)
  {
    ArUtil::sleep(20);
    keys.checkKeys();
    arm.sync();

    if(DISPLAY_POSITIONS && ++ctr > 500)
    {
      pos = arm.getJointPositions();
      for(std::vector<float>::const_iterator i = pos.begin(); i != pos.end(); ++i)
      {
        int n = (i - pos.begin());
        if(n == selectedPivotJoint || n == selectedRotateJoint)
          printf("[% 3.1f]", *i);
        else
          printf(" % 3.1f ", *i);
      }
      printf(" press ESC to exit, SPACE to halt arm\r");
      ctr = 0;
    }
    
  }

  return 0;
}
예제 #19
0
int main(int argc, char **argv)
{
  /* Aria initialization: */
  Aria::init();

  //ArLog::init(ArLog::StdErr, ArLog::Verbose);
 

  /* Create our client object. This is the object which connects with a remote
   * server over the network, and which manages all of our communication with it
   * once connected by sending data "requests".  Requests may be sent once, or
   * may be repeated at any frequency. Requests and replies to requsets contain 
   * payload "packets", into which various data types may be packed (when making a 
   * request), and from which they may also be extracted (when handling a reply). 
   * See the InputHandler and OutputHandler classes above for
   * examples of making requests and reading/writing the data in packets.
   */
  ArClientBase client;

  /* Aria components use this to get options off the command line: */
  ArArgumentParser parser(&argc, argv);

  /* This will be used to connect our client to the server, including
   * various bits of handshaking (e.g. sending a password, retrieving a list
   * of data requests and commands...)
   * It will get the hostname from the -host command line argument: */
  ArClientSimpleConnector clientConnector(&parser);

  parser.loadDefaultArguments();

  /* Check for -help, and unhandled arguments: */
  if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
  {
    Aria::logOptions();
    exit(0);
  }

  
  /* Connect our client object to the remote server: */
  if (!clientConnector.connectClient(&client))
  {
    if (client.wasRejected())
      printf("Server '%s' rejected connection, exiting\n", client.getHost());
    else
      printf("Could not connect to server '%s', exiting\n", client.getHost());
    exit(1);
  } 

  printf("Connected to server.\n");

  client.setRobotName(client.getHost()); // include server name in log messages

  /* Create a key handler and also tell Aria about it */
  ArKeyHandler keyHandler;
  Aria::setKeyHandler(&keyHandler);

  /* Global escape-key handler to shut everythnig down */
  ArGlobalFunctor escapeCB(&escape);
  keyHandler.addKeyHandler(ArKeyHandler::ESCAPE, &escapeCB);

  /* Now that we're connected, we can run the client in a background thread, 
   * sending requests and receiving replies. When a reply to a request arrives,
   * or the server makes a request of the client, a handler functor is invoked. 
   * The handlers for this program are registered with the client by the 
   * InputHandler and OutputHandler classes (in their constructors, above) */
  client.runAsync();

  /* Create the InputHandler object and request safe-drive mode */
  InputHandler inputHandler(&client, &keyHandler);
  inputHandler.safeDrive();

  /* Use ArClientBase::dataExists() to see if the "ratioDrive" request is available on the 
   * currently connected server.  */
  if(!client.dataExists("ratioDrive") )
      printf("Warning: server does not have ratioDrive command, can not use drive commands!\n");
  else
    printf("Keys are:\nUP: Forward\nDOWN: Backward\nLEFT: Turn Left\nRIGHT: Turn Right\n");
  printf("s: Enable safe drive mode (if supported).\nu: Disable safe drive mode (if supported).\nl: list all data requests on server\n\nDrive commands use 'ratioDrive'.\nt: logs the network tracking tersely\nv: logs the network tracking verbosely\nr: resets the network tracking\n\n");


  /* Create the OutputHandler object. It will begin printing out data from the
   * server. */
  OutputHandler outputHandler(&client);


  /* Begin capturing keys into the key handler. Callbacks will be called
   * asyncrosously from this main thread when pressed.  */

  /* While the client is still running (getRunningWithLock locks the "running"
   * flag until it returns), check keys on the key handler (which will call
   * our callbacks), then tell the input handler to send drive commands. 
   * Sleep a fraction of a second as well to avoid using
   * too much CPU time, and give other threads time to work.
   */
  while (client.getRunningWithLock())
  {
    keyHandler.checkKeys();
    inputHandler.sendInput();
    ArUtil::sleep(100);
  }

  /* The client stopped running, due to disconnection from the server, general
   * Aria shutdown, or some other reason. */
  client.disconnect();
  Aria::shutdown();
  return 0;
}
예제 #20
0
int main(int argc, char **argv)
{
  // Initialize Aria and Arnl global information
 
 /* Aria initialization: */
  Aria::init();

  ArLog::init(ArLog::File, ArLog::Verbose,"c:\\temp\\AmbifluxRobot.log",true);

  ArLog::log(ArLog::Verbose, "Ambiflux Starting");

  // Create the sound queue.
  ArSoundsQueue soundQueue;

  // Set WAV file callbacks 
  soundQueue.setPlayWavFileCallback(ArSoundPlayer::getPlayWavFileCallback());
  soundQueue.setInterruptWavFileCallback(ArSoundPlayer::getStopPlayingCallback());

  // Notifications when the queue goes empty or non-empty.
  soundQueue.addQueueEmptyCallback(new ArGlobalFunctor(&queueNowEmpty));
  soundQueue.addQueueNonemptyCallback(new ArGlobalFunctor(&queueNowNonempty));

  // Run the sound queue in a new thread
  soundQueue.runAsync();
  /* Pool de messages en provenance de la tablette
  Issu de l'implementation d'un modèle producteur/consommateur
  pour les messages entrants. Plusieurs thread y accèdent
  Tread-safe (mutex)*/
  //Pool<Frame> messagePool;
 /* Pool de messages en provenance d'un client TCP
  Issu de l'implementation d'un modèle producteur/consommateur
  pour les messages entrants. Plusieurs thread y accèdent
  Tread-safe (mutex)*/
  /*TODO : A remplacer par tcpReceivedPool */
  //Pool<Frame> tcpMessagePool;

  /* Pool de messages en provenance d'un client TCP
  Issu de l'implementation d'un modèle producteur/consommateur
  pour les messages entrants. Plusieurs thread y accèdent
  Tread-safe (mutex)*/
  Pool<TCPReceivedRequest> tcpReceivedPool;

  /*Create our thread to communicate with iPad
   Server start on port 7171 to receive requests from ipad
   A client is created on port 7474 to request iPad
   */
  //IhmCommunicationThread ihm(7171, &messagePool);

  IhmCommunicationThread ihm(7171, &tcpReceivedPool);
   //On s'abonne à la réception de message par la classe IhmCommunicationThread
  //Todo : A supprimer ?
  //ArGlobalFunctor1<Frame> functMessageReceived(&CallbackIhmReceived);
  //ihm.setCallback(&functMessageReceived);
  ihm.runAsync();

  //soundQueue.play("c:\\temp\\let_me_out.wav");

  

  //while(true);

  /* Create our client object. This is the object which connects with a remote
   * server over the network, and which manages all of our communication with it
   * once connected by sending data "requests".  Requests may be sent once, or
   * may be repeated at any frequency. Requests and replies to requsets contain 
   * payload "packets", into which various data types may be packed (when making a 
   * request), and from which they may also be extracted (when handling a reply). 
   * See the InputHandler and OutputHandler classes above for
   * examples of making requests and reading/writing the data in packets.
   */
  ArClientBase client;

  /* Aria components use this to get options off the command line: */
  ArArgumentParser parser(&argc, argv);

  /* This will be used to connect our client to the server, including
   * various bits of handshaking (e.g. sending a password, retrieving a list
   * of data requests and commands...)
   * It will get the hostname from the -host command line argument: */
  ArClientSimpleConnector clientConnector(&parser);

  parser.loadDefaultArguments();

  /* Check for -help, and unhandled arguments: */
  if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
  {
    Aria::logOptions();
    exit(0);
  }
  
  /* Connect our client object to the remote server: */
  if (!clientConnector.connectClient(&client))
  {
    if (client.wasRejected())
      printf("Server '%s' rejected connection, exiting\n", client.getHost());
    else
      printf("Could not connect to server '%s', exiting\n", client.getHost());
    exit(1);
  } 

  printf("Connected to server.\n");

  client.setRobotName(client.getHost()); // include server name in log messages


  ///* Create a key handler and also tell Aria about it */
  ArKeyHandler keyHandler;
  Aria::setKeyHandler(&keyHandler);

  /* Global escape-key handler to shut everythnig down */
  ArGlobalFunctor escapeCB(&escape);
  keyHandler.addKeyHandler(ArKeyHandler::ESCAPE, &escapeCB);

  /* Now that we're connected, we can run the client in a background thread, 
   * sending requests and receiving replies. When a reply to a request arrives,
   * or the server makes a request of the client, a handler functor is invoked. 
   * The handlers for this program are registered with the client by the 
   * InputHandler and OutputHandler classes (in their constructors, above) */
  client.runAsync();

  ///* Create the InputHandler object and request safe-drive mode */
  //InputHandler inputHandler(&client);
  //inputHandler.gotoGoal("215");
  ////inputHandler.safeDrive();
  

// Mode goto
if(!client.dataExists("gotoGoal") )
      printf("Warning: Pas de mode goto!\n");
  else
    printf("Mode goto accepte");


//ArFunctor1<ArNetPacket*>
//client.addHandler("pathPlannerStatus",);



  /* Create the OutputHandler object. It will begin printing out data from the
   * server. */
  OutputHandler outputHandler(&client);

   //On s'abonne à la réception de message par la classe IhmCommunicationThread
  //Todo : A supprimer ?
  //ArGlobalFunctor1<Frame> functMessageReceived(&CallbackIhmReceived);
  //ihm.setCallback(&functMessageReceived);
  //ihm.runAsync();

  //pour tester IHM
 // ArUtil::sleep(1000);
//  ihm.testCommunication();

	//SRMA object
	string strSRMA = DALRest::getResourceById("9");
	SRMA srma(strSRMA,client, outputHandler, ihm, &soundQueue);

	//Loop du mode Ambiant
	MainLoop myLoop(srma, &tcpReceivedPool);
	myLoop.runAsync();
	
	//Thread loop : TCP commands
	//Produces messages in tcpMessagePool
	//ServerLoop myServerLoop(srma, &tcpReceivedPool);
	//myServerLoop.runAsync();
 
	//Traitement des requetes TCP
	//Consulmes messages in tcpMessagePool
	//TCPRequestsLoop myTCPRequestsLoop(srma, &tcpReceivedPool);
	//myTCPRequestsLoop.runAsync();

 

  /* While the client is still running (getRunningWithLock locks the "running"
   * flag until it returns), check keys on the key handler (which will call
   * our callbacks), then tell the input handler to send drive commands. 
   * Sleep a fraction of a second as well to avoid using
   * too much CPU time, and give other threads time to work.
   */
  while (client.getRunningWithLock())
  {
    //keyHandler.checkKeys();
    //inputHandler.sendInput();
    ArUtil::sleep(100);
  }

  /* The client stopped running, due to disconnection from the server, general
   * Aria shutdown, or some other reason. */
  client.disconnect();
  Aria::shutdown();
  return 0;
}
int main(int argc, char** argv)
{
  Aria::init();
  ArArgumentParser argParser(&argc, argv);
  ArSimpleConnector con(&argParser);
  ArRobot robot;
  ArSonarDevice sonar;

  argParser.loadDefaultArguments();
  if(!Aria::parseArgs() || !argParser.checkHelpAndWarnUnparsed())
  {
    Aria::logOptions();
    return 1;
  }

  /* - the action group for teleoperation actions: */
  teleop = new ArActionGroup(&robot);

  // don't hit any tables (if the robot has IR table sensors)
  teleop->addAction(new ArActionLimiterTableSensor, 100);

  // limiter for close obstacles
  teleop->addAction(new ArActionLimiterForwards("speed limiter near", 
						300, 600, 250), 95);

  // limiter for far away obstacles
  teleop->addAction(new ArActionLimiterForwards("speed limiter far", 
					       300, 1100, 400), 90);

  // limiter so we don't bump things backwards
  teleop->addAction(new ArActionLimiterBackwards, 85);

  // the joydrive action (drive from joystick)
  ArActionJoydrive joydriveAct("joydrive", 400, 15);
  teleop->addAction(&joydriveAct, 50);

  // the keydrive action (drive from keyboard)
  teleop->addAction(new ArActionKeydrive, 45);
  


  /* - the action group for wander actions: */
  wander = new ArActionGroup(&robot);

  // if we're stalled we want to back up and recover
  wander->addAction(new ArActionStallRecover, 100);

  // react to bumpers
  wander->addAction(new ArActionBumpers, 75);

  // turn to avoid things closer to us
  wander->addAction(new ArActionAvoidFront("Avoid Front Near", 225, 0), 50);

  // turn avoid things further away
  wander->addAction(new ArActionAvoidFront, 45);

  // keep moving
  wander->addAction(new ArActionConstantVelocity("Constant Velocity", 400), 25);

  

  /* - use key commands to switch modes, and use keyboard
   *   and joystick as inputs for teleoperation actions. */

  // create key handler if Aria does not already have one
  ArKeyHandler *keyHandler = Aria::getKeyHandler();
  if (keyHandler == NULL)
  {
    keyHandler = new ArKeyHandler;
    Aria::setKeyHandler(keyHandler);
    robot.attachKeyHandler(keyHandler);
  }

  // set the callbacks
  ArGlobalFunctor teleopCB(&teleopMode);
  ArGlobalFunctor wanderCB(&wanderMode);
  keyHandler->addKeyHandler('w', &wanderCB);
  keyHandler->addKeyHandler('W', &wanderCB);
  keyHandler->addKeyHandler('t', &teleopCB);
  keyHandler->addKeyHandler('T', &teleopCB);

  // if we don't have a joystick, let 'em know
  if (!joydriveAct.joystickInited())
    printf("Note: Do not have a joystick, only the arrow keys on the keyboard will work.\n");
  
  // set the joystick so it won't do anything if the button isn't pressed
  joydriveAct.setStopIfNoButtonPressed(false);


  /* - connect to the robot, then enter teleoperation mode.  */

  robot.addRangeDevice(&sonar);
  if(!con.connectRobot(&robot))
  { 
    ArLog::log(ArLog::Terse, "actionGroupExample: Could not connect to the robot.");
    Aria::shutdown();
    return 1;
  }

  robot.enableMotors();
  teleopMode();
  robot.run(true);

  Aria::shutdown();
  return 0;
}
예제 #22
0
int main(int argc, char **argv)
{
  Aria::init();

  ArSimpleConnector connector(&argc, argv);
  ArRobot robot;
  ArSick sick;

  if (!Aria::parseArgs() || argc > 1)
  {
    Aria::logOptions();
    Aria::exit(1); // exit program with error code 1
    return 1;
  }
  
  ArKeyHandler keyHandler;
  Aria::setKeyHandler(&keyHandler);
  robot.attachKeyHandler(&keyHandler);


  robot.addRangeDevice(&sick);

  // Create the ArLineFinder object. Set it to log lots of information about its
  // processing.
  ArLineFinder lineFinder(&sick);
  lineFinder.setVerbose(true);

  // Add key callbacks that simply call the ArLineFinder::getLinesAndSaveThem()
  // function, which searches for lines in the current set of laser sensor
  // readings, and saves them in files with the names 'points' and 'lines'.
  ArFunctorC<ArLineFinder> findLineCB(&lineFinder, 
				  &ArLineFinder::getLinesAndSaveThem);
  keyHandler.addKeyHandler('f', &findLineCB);
  keyHandler.addKeyHandler('F', &findLineCB);

  
  ArLog::log(ArLog::Normal, "lineFinderExample: connecting to robot...");
  if (!connector.connectRobot(&robot))
  {
    printf("Could not connect to robot... exiting\n");
    Aria::exit(1);  // exit program with error code 1
    return 1;
  }
  robot.runAsync(true);

  // now set up the laser
  ArLog::log(ArLog::Normal, "lineFinderExample: connecting to SICK laser...");
  connector.setupLaser(&sick);
  sick.runAsync();
  if (!sick.blockingConnect())
  {
    printf("Could not connect to SICK laser... exiting\n");
    Aria::exit(1);
    return 1;
  }

  printf("If you press the 'f' key the points and lines found will be saved\n");
  printf("Into the 'points' and 'lines' file in the current working directory\n");

  robot.waitForRunExit();
  Aria::exit(0);
  return 0;
}