advanced.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

*/

/*

ActivMedia Robotics Navigation and Localization (ARNL)
Copyright (C) 2004, ActivMedia Robotics, LLC
Copyright (C) 2005, 2006 MobileRobots, Inc.

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.


*/
/*!****************************************************************************
 * 
 * File: advanced.cpp
 * 
 * Function: This is an advanced example of how to use ARNL, if you are just 
 *           trying to get things to work or not do complicated things do
 *           not use this example but instead use the other examples.
 *           
 *           The program will take as input a .map file and use it to
 *           start a localization task which will continously keep the
 *           robot localized in the map if correctly initialized..  It
 *           also starts a pathplanning task which can plan safe paths
 *           to goals after it is initialized.  The main class
 *           Advanced holds all the related classes in a single place.
 *           
 *           This program does not contain any server classes, interact
 *           with this program through the terminal via command keys,
 *           not using MobileEyes.
 *
 * User Hints: To customize this localization and path planning module for 
 *             user applications, Use all the functions called in the 
 *             main() to initialize the two tasks. Then, localize and path plan
 *             using the functions shown in interact()
 *
 ****************************************************************************/
#include "Arnl.h"
#include "Aria.h"
#include "ArNetworking.h"

/*! 
  @class Advanced.
  @brief Class holds details of the localization and path planning tasks, with callbacks, and provides an interface around them
*/
class Advanced
{
public:

  /// Base Constructor.
  Advanced(ArRobot *robot, ArSick *sick, ArSonarDevice *sonar);
  /// Base Destructor.
 ~Advanced(void);
 
  /// Start the localization thread.
  bool   initializeLocalizationTask(char* mn);
  /// Set robots current pose in the task data structure.
  bool   setTaskCurrentPose(double rx, double ry, double rt);
  /// Initialize the samples and run localize at given pose.
  bool   localizeAtSetPose(ArPose pose);
  /// Get the computed best goodness score.
  double getLocalizationScore(void);
  /// Failed tracking callback.
  void   trackingFailed(int tf);

  /// Set up the path planning.
  bool   initializePathPlanningTask(void);
  /// Plan a A* path from from to to.
  bool   pathPlanFromCurrentToDest(ArPose to, bool headflag);
  /// Path to goal success callback.
  void   goalDone(ArPose goal);
  /// Path to goal failure callback.
  void   goalFailed(ArPose goal);

  /// Shutdown the system.
  void   shutDown(void);
  /// Helper for goals.
  void   getAllGoals(ArMap* ariamap);
  /// Helper for homes
  void   getAllRobotHomes(ArMap* ariamap);

public:

  ArRobot*                  myRobot;
  ArSick*                   mySick;
  ArSonarDevice*            mySonar;
  ArMap*                    myMap;

#ifndef SONARNL
  ArLocalizationTask*       myLocaTask;
#else
  ArSonarLocalizationTask*       myLocaTask;
#endif

  ArPathPlanningTask*       myPathPlanningTask;

  std::list<ArMapObject*>   myGoalList;
  std::list<ArMapObject*>   myHomeList;
};


#define DEFAULT_NO_SAMPLES 2000
#define LOCA_PASS_THRESHOLD 0.20

#define USAGE "Usage: advanced -map mapfilename\n"

int noOfSamples = DEFAULT_NO_SAMPLES;

// Global container.
Advanced* advancedptr;
// Var for interaction.
ArKeyHandler keyHandler;
static bool roundRobinFlag = false;
/*!
 * Basic Constructor.
 *
 */
Advanced::Advanced(ArRobot *robot, ArSick *sick, ArSonarDevice *sonar)
{
  myRobot            = robot;
  mySick             = sick;
  mySonar            = sonar;

  myLocaTask         = NULL;
  myPathPlanningTask = NULL;
}
/*!
 * Basic Destructor.
 *
 */
Advanced::~Advanced(void)
{
  if(myLocaTask!=NULL)
    delete myLocaTask;
  if(myPathPlanningTask!=NULL)
    delete myPathPlanningTask;
}
/*!
 * Starts the localization thread going.
 *
 * @return true if successful.
 */
bool
Advanced::initializeLocalizationTask(char* mapname)
{
  // Start the localization task thread. This is heart of the system.
#ifndef SONARNL
  if(myRobot && mySick)
    myLocaTask = new ArLocalizationTask(myRobot, mySick, mapname);
#else
  if(myRobot && mySonar)
    myLocaTask = new ArSonarLocalizationTask(myRobot, mySonar, mapname);
#endif

  if(!myLocaTask){
    printf("Cannot allocate memory for Localization Task\n");
    return false;
  } else {
    // No of samples.
    myLocaTask->setNumSamples(noOfSamples);
    return true;
  }
}
/*!
 * Calls the localization task function around a given set pose of the robot.
 *
 * @return true if successful else false.
 */
bool
Advanced::localizeAtSetPose(ArPose pose)
{
  if(!myLocaTask){
    return false;
  }else{  
    //
    // Jump in the start of the localization thread.
    //
    int ns      = myLocaTask->getNumSamples();
    double xstd = 200.0;
    double ystd = 200.0;
    double tstd = 30.0;
    return myLocaTask->localizeRobotInMapInit(pose, ns, 
					      xstd, ystd, tstd, 
					      LOCA_PASS_THRESHOLD);
  }
}
/*!
 * Returns the goodness of the last localization result. Fraction of the
 * matched range points to the total no of range points.
 *
 * @return A measure of the goodness of the map seen by the sensor and the
 *         robots pose.
 *
 */
double
Advanced::getLocalizationScore(void)
{
  return myLocaTask->getLocalizationScore();
}
/*!
 * This is the called when tracking fails.
 *
 * @param timesfailed: No of times the localization failed.
 */
void
Advanced::trackingFailed(int timesfailed)
{
  //
  // Probably should be doing something more than this in real use.
  //
  printf("\07Tracking Failed:Main\n");
  myPathPlanningTask->trackingFailed(timesfailed);
}

/*!
 * Sets up for path planning.
 *
 * @return true if successful.
 */
bool
Advanced::initializePathPlanningTask(void)
{
#ifdef SONARNL
  if(myRobot && mySonar && myMap)
    myPathPlanningTask = new ArPathPlanningTask(myRobot, mySonar, 
						myMap);
#else
  if(myRobot && mySick && mySonar && myMap)
    myPathPlanningTask = new ArPathPlanningTask(myRobot, mySick, mySonar, 
						myMap);
#endif
					      
  if(!myPathPlanningTask){
    return false;
  }else{
    myRobot->lock();
    myRobot->enableMotors();
    myRobot->unlock();
    return true;
  }
}
/*!
 * Plans a collision free path  from current pose to the destination.
 *
 * @param to: The destination pose.
 * @param headflag: Flag to indicate if robot needs to orient after 
 *                  reaching goal.
 *
 * @return true if successful.
 */
bool
Advanced::pathPlanFromCurrentToDest(ArPose to, bool headflag)
{
  //
  // Set the Goal and the flags to set off the path planning action.
  //
  if(myPathPlanningTask){
    return myPathPlanningTask->pathPlanToPose(to, headflag);
  }else{
    return false;
  }
}
/*!
 * This is the called when path to goal reached.
 *
 * @param goal: Goal it was assigned.
 */
void
Advanced::goalDone(ArPose goal)
{
  static int goalCount = 0;

  printf("goalDone:Main: %5.2f %5.2f %5.2f\07\n", 
	 goal.getX(), goal.getY(), goal.getTh());

  if(roundRobinFlag){
    if(myLocaTask->getInitializedFlag()){
      if(myGoalList.size()>0){
	ArMapObject* front = myGoalList.front();
	ArPose top = front->getPose();
	bool headingFlag = false;
	if(strcasecmp(front->getType(), "GoalWithHeading") == 0)
	  headingFlag = true;
	else
	  headingFlag = false;
	printf("Moving to next goal:%s %5.2f %5.2f %5.2f: %d poses %d Done\n", 
	       front->getName(),
	       top.getX(), top.getY(), top.getTh(), 
	       myGoalList.size(), goalCount++);
	myGoalList.pop_front();
	myGoalList.push_back(front);

	myPathPlanningTask->pathPlanToPose(top, headingFlag);
      }
    }
  }else{
    printf("Localize the robot first\n");
  }
}
/*!
 * This is the called when path to goal fails
 *
 * @param goal: Goal it was assigned.
 */
void
Advanced::goalFailed(ArPose goal)
{
  printf("goalFailed:Main: %5.2f %5.2f %5.2f\07\n", 
	 goal.getX(), goal.getY(), goal.getTh());
}
/*!
 * Shuts down the system.
 *
 */
void
Advanced::shutDown(void)
{
  Aria::exit(0);
  //
  // Stop the path planning thread.
  //
  if(myPathPlanningTask){
    myPathPlanningTask->stopRunning();
//    delete myPathPlanningTask;
    printf("Stopped Path Planning Thread\n");
  }
  //
  // Stop the localization thread.
  //
  if(myLocaTask){
    myLocaTask->stopRunning();
    delete myLocaTask;
    printf("Stopped Localization Thread\n");
  }
  //
  // Stop the laser thread.
  //
  if(mySick)
  {
    mySick->lockDevice();
    mySick->disconnect();
    mySick->unlockDevice();
    printf("Stopped Laser Thread\n");
  }
  //
  // Stop the robot thread.
  //
  myRobot->lock();
  myRobot->stopRunning();
  myRobot->unlock();
  printf("Stopped Robot Thread\n");
  //
  // Exit Aria
  //
  Aria::shutdown(); 
  printf("Aria Shutdown\n");

}
/*!
 * Gets the list of goals.
 *
 * @param mapdata: Pointer to the map class.
 *
 * @return list of goal objs
 *
 */
void
Advanced::getAllGoals(ArMap* ariamap)
{
  myGoalList.clear();
  std::list<ArMapObject *>::iterator objIt;
  int i=0;
  ArMapObject* obj;
  for (objIt = ariamap->getMapObjects()->begin(); 
       objIt != ariamap->getMapObjects()->end(); 
       objIt++)
  {
    //
    // Get the forbidden lines and fill the occupancy grid there.
    //
    obj = (*objIt);
    if (strcasecmp(obj->getType(), "GoalWithHeading") == 0)