simpleserverdemo.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 "Arnl.h"

class SimpleTask
{
public:
  SimpleTask(ArRobot *robot, ArPathPlanningTask *pathTask, 
	     ArServerModeGoto *modeGoto);
  virtual ~SimpleTask();
  void task(void);
  void goalDone(ArPose pose);
protected:
  ArRobot *myRobot;
  ArPathPlanningTask *myPathTask;
  ArServerModeGoto *myModeGoto;

  int myState;
  bool myGoalDone;
  ArFunctorC<SimpleTask> myTaskCB;
  ArFunctor1C<SimpleTask, ArPose> myGoalDoneCB;
};

SimpleTask::SimpleTask(ArRobot *robot, ArPathPlanningTask *pathTask, 
		       ArServerModeGoto *modeGoto) :
  myTaskCB(this, &SimpleTask::task),
  myGoalDoneCB(this, &SimpleTask::goalDone)
{
  myRobot = robot;
  myPathTask = pathTask;
  myModeGoto = modeGoto;

  // trigger the first start off
  myGoalDone = true;
  myState = 0;
  
  myRobot->addUserTask("simpleTask", 50, &myTaskCB);
  myPathTask->addGoalDoneCB(&myGoalDoneCB);
  myPathTask->addGoalFailedCB(&myGoalDoneCB);
}

SimpleTask::~SimpleTask()
{
  
}

void SimpleTask::goalDone(ArPose pose)
{
  myGoalDone = true;
}

void SimpleTask::task(void)
{
  if (!myGoalDone)
    return;
  
  myGoalDone = false;
  myState++;
  if (myState == 1)
    myModeGoto->gotoPose(ArPose(1000, 0, 0), true);
  //else if (myState == 1)
  //myModeGoto->gotoGoal("Goal1");
  else 
  {
    myState = 0;
    myModeGoto->home();
  }
}

int 
main(int argc, char *argv[])
{
  // Initialize location of Aria, Arnl and their args.
  Aria::init();
  Arnl::init();
  
  // The robot object
  ArRobot robot;
#ifndef SONARNL
  // The laser object
  ArSick sick(181, 361);
#endif

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

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

  // Load default arguments for this computer
  parser.loadDefaultArguments();

  // Parse its arguments for the simple connector.
  simpleConnector.parseArgs();

  // sonar, must be added to the robot, for teleop and wander
  ArSonarDevice sonarDev;
  // add the sonar to the robot
  robot.addRangeDevice(&sonarDev);

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

  // Stop the robot as soon as localization fails.
  ArFunctor1C<ArPathPlanningTask, int> 
  locaFailed(&pathTask, &ArPathPlanningTask::trackingFailed);
  locTask.addFailedLocalizationCB(&locaFailed);
  
  // Read in program paramaters from the files distributed with ARNL and
  // SONARNL (i.e. $ARNL/params/arnl.p or params/sonarnl.p) -- you could
  // use your own parameter file instead if you wanted to --
  // also including values from the command-line argument parser. These
  // parameters are stored in a global ArConfig object accessible via
  // Aria::getConfig().  You can store your own program parameters in there
  // as well; see ArConfig documentation.
  Aria::getConfig()->useArgumentParser(&parser);
  if (!Aria::getConfig()->parseFile(Arnl::getTypicalParamFileName()))
  {
    printf("Trouble loading configuration file, exiting\n");
    robot.disconnect();
    exit(1);
  }

  // Warn about unknown command-line parameters and exit, or print
  // command-line parameter help if "-help" given, and exit.
  if (!parser.checkHelpAndWarnUnparsed())
  {
    printf("\nUsage: %s -map mapfilename\n\n", argv[0]);
    simpleConnector.logOptions();
    robot.disconnect();
    exit(2);
  }

  // Create and open our ArNetworking server
  ArServerBase server;
  if (!server.open(7272))
  {
    printf("Could not open server port\n");
    exit(1);
  }

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


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

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

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

  // Connect the laser
  if (!sick.blockingConnect())
  {
    printf("Couldn't connect to sick, exiting\n");
    Aria::shutdown();
    return 1;
  }
#endif

  ArUtil::sleep(300);

  // If you want to set the number of localization samples change the line below
  locTask.setNumSamples(2000);
  // Localize the robot to home
  locTask.localizeRobotAtHomeBlocking();
  
  robot.lock();
  // attach services to the server base.
  ArServerInfoRobot serverInfoRobot(&server, &robot);
  ArServerInfoSensor serverInfoSensor(&server, &robot);
  ArServerInfoPath serverInfoPath(&server, &robot, &pathTask);
  ArServerInfoLocalization serverInfoLocalization(&server, &robot, &locTask);

  // Drawing in the map display:
  ArServerInfoDrawings drawings(&server);
  drawings.addRobotsRangeDevices(&robot);

#ifndef SONARNL
  // Set it up to handle maps.
  ArServerHandlerMap serverMap(&server, &arMap, ArServerHandlerMap::POINTS);
#else
  ArServerHandlerMap serverMap(&server, &arMap, ArServerHandlerMap::LINES);
#endif  


  // Set up a service that allows the client to monitor the communication
  // between the robot and the client.
  //
  ArServerHandlerCommMonitor serverCommMonitor(&server);

  // Set it up to work the 5 possible modes.
  ArServerModeGoto modeGoto(&server, &robot, &pathTask, &arMap,
			    locTask.getHomePose());

  // If we're not using sonar to localize then turn them off when we
  // stop, buf if we are using them to localize do not turn them off
  // when we stop (since then we can't localize well until we move
  // some and turn the sonar back on)
#ifndef SONARNL
  ArServerModeStop modeStop(&server, &robot, true);
#else
  ArServerModeStop modeStop(&server, &robot, false);
#endif
  ArServerModeDrive modeDrive(&server, &robot);
  ArServerModeWander modeWander(&server, &robot);
#ifndef SONARNL

  // Setup the dock if there is a docking system on board.
  ArServerModeDock *modeDock = NULL;
  const ArRobotConfigPacketReader *origConfig;
  if ((origConfig = robot.getOrigRobotConfig()) != NULL)
  {
    if (origConfig->getHasCharger() == 1)
    {
      modeDock = new ArServerModeDockPioneer(&server, &robot, &locTask, 
					     &pathTask);
      modeDock->setDockingVoltage(11.5);
      modeDock->setDoneChargingMinutes(30);
      modeDock->checkDock();
      modeDock->addAsDefaultMode();
    }
  }
#endif

  modeStop.addAsDefaultMode();
  // Arrange to have the configuration changes made by client handled.
  ArServerHandlerConfig handlerConfig(&server, Aria::getConfig());

  SimpleTask simpleTask(&robot, &pathTask, &modeGoto);


  // Now turn on motors if they are off, unlock the robot so other 
  // threads can access it, and let the server run here until shut down.
  robot.enableMotors();
  robot.unlock();
  server.run();
  exit(0);

}