guiserver.cpp

sonarnl localization and path planning by mobile robots

/*

MobileRobots Advanced Robotics Navigation and Localization (ARNL)
Copyright (C) 2004, 2005, ActivMedia Robotics LLC.
Copyright (C) 2006, 2007, MobileRobots Inc.
All Rights Reserved

MobileRobots Inc does not make any representations about the
suitability of this software for any purpose.  It is provided "as is"
without express or implied warranty.

The license for this software is distributed as LICENSE.txt in the top
level directory.

[e-mail]
MobileRobots
19 Columbia Drive
Amherst, NH 03031
800-639-9481

*/

#include "Aria.h"
#include "ArNetworking.h"
#include "ArSystemStatus.h"
#include "Arnl.h"

/** @example guiServer.cpp Deluxe example of almost all ARNL features

  This is an example of using ARNL or SONARNL and ArNetworking to provide a
  server program that remote clients, such as MobileEyes, can connect
  to. MobileEyes sends requests to this program to go to goals,
  get the current map, set ARNL and other configuration parameters,
  and get the current state of the robot and sensors. This example server
  includes almost all features provided by ARNL (or SONARNL), including
  localization, path planning and navigation to a map goal or abritrary point,
  sharing information with other servers peer-to-peer or through 
  a "central server", using IR and bumper sensors, stopping the robot
  if localization fails, global replanning if a path is not followable,
  use of special SICK laser reflector beacons, as well as various networking
  services for MobileEyes and other clients, such as diagnostic visualizations,
  access to configuration parameters, access to camera control and 
  video images (if SAVServer is running), special debugging commands 
  ("custom commands"), file uploading/downloading and gathering raw laser
  scans using Mapper3, and safe and unsafe teleoperation.

  Note, guiServer will allow remote clients to make map scans and to put 
  and get files from the ARNL examples directory, if you do not wish this 
  to be possible modify the guiServer code (search for ArServerHandlerMapping
  for map making ArServerFileLister for file getting and putting).

  You can also start up guiServer with an option for using user and
  password information to allow connection.  To do this you should edit
  examples/guiServer.userInfo and change the '.' for passwords to the
  password you wish and then pass guiServer an additional '-userInfo
  guiServer.userInfo' argument.  
*/


/** This example class contains methods invoked after ARNL reaches or fails to reach a goal. */
class ExampleGoalTasks {

protected:
  ArPathPlanningTask *myPathPlanner;
  ArFunctor1C<ExampleGoalTasks, ArPose> myGoalDoneCB;
  ArFunctor1C<ExampleGoalTasks, ArPose> myGoalFailedCB;

public:

  ExampleGoalTasks(ArPathPlanningTask* pathPlanner) : 
    myPathPlanner(pathPlanner),
    myGoalDoneCB(this, &ExampleGoalTasks::reachedGoal),
    myGoalFailedCB(this, &ExampleGoalTasks::failedGoal)
  {
    pathPlanner->addGoalDoneCB(&myGoalDoneCB);
    pathPlanner->addGoalFailedCB(&myGoalFailedCB);
  }

  ~ExampleGoalTasks()
  {
    myPathPlanner->remGoalDoneCB(&myGoalDoneCB);
    myPathPlanner->remGoalFailedCB(&myGoalFailedCB);
  }

  void reachedGoal(ArPose pose)
  {
    ArLog::log(ArLog::Normal, "guiServer: reached  the goal.");
  }

  void failedGoal(ArPose pose)
  {
    ArLog::log(ArLog::Normal, "guiServer: failed to reach the goal!");
  }

};



/** Main function */
int 
main(int argc, char *argv[])
{
  // Initialize location of Aria, Arnl and their args.
  Aria::init();
  Arnl::init();
  
  // log to a file
  //ArLog::init(ArLog::File, ArLog::Normal, "log.txt", true, true);
  //ArLog::init(ArLog::File, ArLog::Verbose);
 
  // To get CPU usage and wireless information from Linux
  ArSystemStatus::runRefreshThread();

  // The robot object
  ArRobot robot;

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

  // Set up our simpleConnector
  ArSimpleConnector simpleConnector(&parser);

  // Set up our simpleOpener
  ArServerSimpleOpener simpleOpener(&parser);

  // Set up our client for the central server
  ArClientSwitchManager clientSwitch(&server, &parser);
    
  // Load default arguments for this computer (from /etc/Aria.args, environment
  // variables, and other places)
  parser.loadDefaultArguments();

  // set up a gyro
  ArAnalogGyro gyro(&robot);
  
  // 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);
  }


#ifndef SONARNL
  // The laser object
  ArSick sick(361, 181);

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


#endif // ifndef SONARNL

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

  // Add the sonar to the robot
  robot.addRangeDevice(&sonarDev);
  
  // Set up where we'll look for files
  char fileDir[1024];
  ArUtil::addDirectories(fileDir, sizeof(fileDir), Aria::getDirectory(), 
			 "examples");
  
  // Set up the map, this will look for files in the examples
  // directory (unless the file name starts with a /, \, or .
  // You can take out the 'fileDir' argument to look in the current directory
  // instead
  ArMap arMap(fileDir);
  // set it up to ignore empty file names (otherwise the parseFile
  // on the config will fail)
  arMap.setIgnoreEmptyFileName(true);

#ifndef SONARNL
  // Initialize the localization (for the laser)
  ArLocalizationTask locTask(&robot, &sick, &arMap);
  // Make the path task (for the laser)
  ArPathPlanningTask pathTask(&robot, &sick, &sonarDev, &arMap);
  // Set up things so data can be logged (only do it with the laser
  // since it can overrun a 9600 serial connection which the sonar is
  // more likely to have)
  ArDataLogger dataLogger(&robot);
  dataLogger.addToConfig(Aria::getConfig());

#else  // ifndef SONARNL
  // Initialize the localization (for SONARNL)
  ArSonarLocalizationTask locTask(&robot, &sonarDev, &arMap);
  // Make the path task (for SONARNL)
  ArPathPlanningTask pathTask(&robot, &sonarDev, &arMap);
#endif  // ifndef SONARNL

  // add our logging to the config
  ArLog::addToConfig(Aria::getConfig());

  // Object with things to do if we reach or fail to reach a goal:
  ExampleGoalTasks goalTasks(&pathTask);
  
  // First open the server up
  if (!simpleOpener.open(&server, fileDir, 240))
  {
    if (simpleOpener.wasUserFileBad())
      ArLog::log(ArLog::Normal, "Bad user file");
    else
      ArLog::log(ArLog::Normal, "Could not open server port");
    exit(2);
  }

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

  // Set up a class that'll put the movement parameters into the config stuff
  ArRobotConfig robotConfig(&robot);
  robotConfig.addAnalogGyro(&gyro);

  robot.enableMotors();
  robot.clearDirectMotion();

  // if we are connected to a simulator, reset it to its start position
  robot.comInt(ArCommands::RESETSIMTOORIGIN, 1);

#ifndef SONARNL
  // Set up the laser before handing it to the robot.
  simpleConnector.setupLaser(&sick);
#endif // ifndef SONARNL


  // Start the robot thread.
  robot.runAsync(true);
  
#ifndef SONARNL
  // Start the laser thread.
  sick.runAsync();

  robot.com2Bytes(31, 11, 0); 
  ArUtil::sleep(1000);
  robot.com2Bytes(31, 11, 1);

  // Connect the laser
  if (!sick.blockingConnect())
  {
    ArLog::log(ArLog::Normal, "Couldn't connect to sick, exiting");
    Aria::exit(4);
  }

  ArServerHandlerMultiRobot *handlerMultiRobot = NULL;
  ArMultiRobotRangeDevice *multiRobotRangeDevice = NULL;

  ArServerHandlerMultiRobotPeer *multiRobotPeer = NULL;
  ArMultiRobotPeerRangeDevice *multiRobotPeerRangeDevice = NULL;

  // if we're using the central server then we want to create the
  // multiRobot central classes
  if (clientSwitch.getCentralServerHostName() != NULL)
  {
    // Make the handler for multi robot information (this sends the
    // information to the central server)
    
    handlerMultiRobot = new ArServerHandlerMultiRobot(&server, &robot, 
						      &pathTask,
						      &locTask, &arMap);
    
    // the range device that handles the multi robot information from
    // the central server
    multiRobotRangeDevice = new ArMultiRobotRangeDevice(&server);
    
    robot.addRangeDevice(multiRobotRangeDevice);
    pathTask.addRangeDevice(multiRobotRangeDevice, 
			    ArPathPlanningTask::BOTH);
    
    // Set the range device so that we can see the information its using
    // to avoid, you can comment these out in order to not see them
    multiRobotRangeDevice->setCurrentDrawingData(
	    new ArDrawingData("polyDots", ArColor(125, 125, 0),