clientDemo.cpp

Go to the documentation of this file.

/*
MobileRobots Advanced Robotics Interface for Applications (ARIA)
Copyright (C) 2004, 2005 ActivMedia Robotics LLC
Copyright (C) 2006, 2007 MobileRobots Inc.

     This program is free software; you can redistribute it and/or modify
     it under the terms of the GNU General Public License as published by
     the Free Software Foundation; either version 2 of the License, or
     (at your option) any later version.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program; if not, write to the Free Software
     Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

If you wish to redistribute ARIA under different terms, contact 
MobileRobots for information about a commercial version of ARIA at 
robots@mobilerobots.com or 
MobileRobots Inc, 19 Columbia Drive, Amherst, NH 03031; 800-639-9481
*/

/* This is the ArNetworking example client. 
 * It connects to an ArNetworking server, and provides continuous 
 * information about the robot state (position, speed, server mode, etc.),
 * and, if the server supports the commands, lets you drive the robot with
 * the keyboard.  
 *
 * To see the example client in action, first run a server on the robot's 
 * onboard computer, for example, serverDemo, testServer, guiServer (from ARNL),
 * or ARAM. Make sure the robot computer is on a network, and run this
 * clientDemo with the hostname of the robot computer as an argument:
 *
 *    ./clientDemo -host myrobot
 *
 */

#include "Aria.h"
#include "ArNetworking.h"


/* This class provides callback functions which are called by the ArKeyHandler
 * object when the user presses keys. These callback functions adjust member
 * variables.  The sendInput() method sends a command with those values
 * to the server using the "ratioDrive" request.
 */

class InputHandler
{
public: 
  InputHandler(ArClientBase *client, ArKeyHandler *keyHandler);
  virtual ~InputHandler(void);

  void up(void);

  void down(void);

  void left(void);

  void right(void);

  void sendInput(void);

  void safeDrive();

  void unsafeDrive();

  void listData();

  void logTrackingTerse();
  void logTrackingVerbose();
  void resetTracking();
protected:
  ArClientBase *myClient;
  ArKeyHandler *myKeyHandler;

  bool myPrinting;

  double myTransRatio;

  double myRotRatio;


  ArFunctorC<InputHandler> myUpCB;
  ArFunctorC<InputHandler> myDownCB;
  ArFunctorC<InputHandler> myLeftCB;
  ArFunctorC<InputHandler> myRightCB;
  ArFunctorC<InputHandler> mySafeDriveCB;
  ArFunctorC<InputHandler> myUnsafeDriveCB;
  ArFunctorC<InputHandler> myListDataCB;
  ArFunctorC<InputHandler> myLogTrackingTerseCB;
  ArFunctorC<InputHandler> myLogTrackingVerboseCB;
  ArFunctorC<InputHandler> myResetTrackingCB;
};

InputHandler::InputHandler(ArClientBase *client, ArKeyHandler *keyHandler) : 
  myClient(client), myKeyHandler(keyHandler),
  myPrinting(false), myTransRatio(0.0), myRotRatio(0.0) ,

  /* Initialize functor objects with pointers to our handler methods: */
  myUpCB(this, &InputHandler::up),
  myDownCB(this, &InputHandler::down),
  myLeftCB(this, &InputHandler::left),
  myRightCB(this, &InputHandler::right),
  mySafeDriveCB(this, &InputHandler::safeDrive),
  myUnsafeDriveCB(this, &InputHandler::unsafeDrive),
  myListDataCB(this, &InputHandler::listData),
  myLogTrackingTerseCB(this, &InputHandler::logTrackingTerse),
  myLogTrackingVerboseCB(this, &InputHandler::logTrackingVerbose),
  myResetTrackingCB(this, &InputHandler::resetTracking)
{

  /* Add our functor objects to the key handler, associated with the appropriate
   * keys: */
  myKeyHandler->addKeyHandler(ArKeyHandler::UP, &myUpCB);
  myKeyHandler->addKeyHandler(ArKeyHandler::DOWN, &myDownCB);
  myKeyHandler->addKeyHandler(ArKeyHandler::LEFT, &myLeftCB);
  myKeyHandler->addKeyHandler(ArKeyHandler::RIGHT, &myRightCB);
  myKeyHandler->addKeyHandler('s', &mySafeDriveCB);
  myKeyHandler->addKeyHandler('u', &myUnsafeDriveCB);
  myKeyHandler->addKeyHandler('l', &myListDataCB);
  myKeyHandler->addKeyHandler('t', &myLogTrackingTerseCB);
  myKeyHandler->addKeyHandler('v', &myLogTrackingVerboseCB);
  myKeyHandler->addKeyHandler('r', &myResetTrackingCB);
}

InputHandler::~InputHandler(void)
{

}

void InputHandler::up(void)
{
  if (myPrinting)
    printf("Forwards\n");
  myTransRatio = 100;
}

void InputHandler::down(void)
{
  if (myPrinting)
    printf("Backwards\n");
  myTransRatio = -100;
}

void InputHandler::left(void)
{
  if (myPrinting)
    printf("Left\n");
  myRotRatio = 100;
}

void InputHandler::right(void)
{
  if (myPrinting)
    printf("Right\n");
  myRotRatio = -100;
}

void InputHandler::safeDrive()
{
  /* Construct a request packet. The data is a single byte, with value 
   * 1 to enable safe drive, 0 to disable. */
  ArNetPacket p;
  p.byteToBuf(1);

  /* Send the packet as a single request: */
  if(myPrinting)
    printf("Sending setSafeDrive 1.\n");
  myClient->requestOnce("setSafeDrive",&p);
  if(myPrinting)
    printf("\nSent enable safe drive.\n");
}

void InputHandler::unsafeDrive()
{
  /* Construct a request packet. The data is a single byte, with value 
   * 1 to enable safe drive, 0 to disable. */
  ArNetPacket p;
  p.byteToBuf(0);

  /* Send the packet as a single request: */
  if(myPrinting)
    printf("Sending setSafeDrive 0.\n");
  myClient->requestOnce("setSafeDrive",&p);
  if(myPrinting)
    printf("\nSent disable safe drive command. Your robot WILL run over things if you're not careful.\n");
}

void InputHandler::listData()
{
  myClient->logDataList();
}

void InputHandler::logTrackingTerse()
{
  myClient->logTracking(true);
}

void InputHandler::logTrackingVerbose()
{
  myClient->logTracking(false);
}

void InputHandler::resetTracking()
{
  myClient->resetTracking();
}


void InputHandler::sendInput(void)
{
  /* This method is called by the main function to send a ratioDrive
   * request with our current velocity values. If the server does 
   * not support the ratioDrive request, then we abort now: */
  if(!myClient->dataExists("ratioDrive")) return;

  /* Construct a ratioDrive request packet.  It consists
   * of three doubles: translation ratio, rotation ratio, and an overall scaling
   * factor. */
  ArNetPacket packet;
  packet.doubleToBuf(myTransRatio);
  packet.doubleToBuf(myRotRatio);
  packet.doubleToBuf(50); // use half of the robot's maximum.
  if (myPrinting)
    printf("Sending\n");
  myClient->requestOnce("ratioDrive", &packet);
  myTransRatio = 0;
  myRotRatio = 0;
}


class OutputHandler
{
public:
  OutputHandler(ArClientBase *client);
  virtual ~OutputHandler(void);
  
  void handleOutput(ArNetPacket *packet);

  void handleBatteryInfo(ArNetPacket *packet);
  void handlePhysicalInfo(ArNetPacket *packet);

protected:


  double myX;
  double myY;
  double myTh;
  double myVel;
  double myRotVel;
  double myVoltage;
  char myStatus[256];
  char myMode[32];
  ArClientBase *myClient;

  ArFunctor1C<OutputHandler, ArNetPacket *> myHandleOutputCB;
  ArFunctor1C<OutputHandler, ArNetPacket *> myHandleBatteryInfoCB;
  ArFunctor1C<OutputHandler, ArNetPacket *> myHandlePhysicalInfoCB;
  
  bool myNeedToPrintHeader;
  bool myGotBatteryInfo;
};

OutputHandler::OutputHandler(ArClientBase *client) :
  myClient(client),
  myHandleOutputCB(this, &OutputHandler::handleOutput),
  myHandleBatteryInfoCB(this, &OutputHandler::handleBatteryInfo),
  myHandlePhysicalInfoCB(this, &OutputHandler::handlePhysicalInfo),
  myNeedToPrintHeader(false),
  myGotBatteryInfo(false)
{
  /* Add a handler for battery info, and make a single request for it  */
  myClient->addHandler("physicalInfo", &myHandlePhysicalInfoCB);
  myClient->requestOnce("physicalInfo");


  /* Add a handler for battery info, and make a single request for it  */
  myClient->addHandler("batteryInfo", &myHandleBatteryInfoCB);
  myClient->requestOnce("batteryInfo");

  /* Add a handler for general info, and request it to be called every 100 ms */
  myClient->addHandler("update", &myHandleOutputCB);
  myClient->request("update", 100);
}

OutputHandler::~OutputHandler(void)
{
  /* Halt the request for data updates */
  myClient->requestStop("update");
}

void OutputHandler::handleOutput(ArNetPacket *packet)
{
  /* Extract the data from the update packet. Its format is status and
   * mode (null-terminated strings), then 6 doubles for battery voltage, 
   * x position, y position and orientation (theta) (from odometry), current
   * translational velocity, and current rotational velocity. Translation is
   * always milimeters, rotation in degrees.
   */
  memset(myStatus, 0, sizeof(myStatus));
  memset(myMode, 0, sizeof(myMode));
  packet->bufToStr(myStatus, sizeof(myStatus));
  packet->bufToStr(myMode, sizeof(myMode));
  myVoltage = ( (double) packet->bufToByte2() )/10.0;
  myX = (double) packet->bufToByte4();
  myY = (double) packet->bufToByte4();
  myTh = (double) packet->bufToByte2();
  myVel = (double) packet->bufToByte2();
  myRotVel = (double) packet->bufToByte2();

  if(myNeedToPrintHeader) 
  {
    printf("\n%6s|%6s|%6s|%6s|%6s|%6s|%15s|%20s|\n",
      "x","y","theta", "vel", "rotVel", "volts", "mode","status");
    fflush(stdout);
    myNeedToPrintHeader = false;
  }
  if (myGotBatteryInfo)
    printf("%6.0f|%6.0f|%6.1f|%6.1f|%6.1f|%6.1f|%15s|%20s|\r",
           myX, myY, myTh, myVel, myRotVel, myVoltage, myMode, myStatus);
  
  fflush(stdout);
}

void OutputHandler::handleBatteryInfo(ArNetPacket *packet)
{
  /* Get battery configuration parameters: when the robot will begin beeping and 
   * warning about low battery, and when it will automatically disconnect and
   * shutdown. */
  double lowBattery = packet->bufToDouble();
  double shutdown = packet->bufToDouble();
  printf("Low battery voltage: %6g       Shutdown battery voltage: %6g\n", lowBattery, shutdown);
  fflush(stdout);
  myNeedToPrintHeader = true;
  myGotBatteryInfo = true;
}


void OutputHandler::handlePhysicalInfo(ArNetPacket *packet)
{
  /* Get phyiscal configuration parameters: */
  char robotType[512];
  char robotSubtype[512];
  int width;
  int lengthFront;
  int lengthRear;

  packet->bufToStr(robotType, sizeof(robotType));
  packet->bufToStr(robotSubtype, sizeof(robotSubtype));
  width = packet->bufToByte2();
  lengthFront = packet->bufToByte2();
  lengthRear = packet->bufToByte2();

  printf("Type: %s Subtype: %s Width %d: LengthFront: %d LengthRear: %d\n",
         robotType, robotSubtype, width, lengthFront, lengthRear);
  fflush(stdout);
}


/* Key handler for the escape key: shutdown all of Aria. */
void escape(void)
{
  printf("esc pressed, shutting down aria\n");
  Aria::shutdown();
}

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



  /* 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");


  /* 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. */
  Aria::shutdown();
  return 0;
}

---