Aria: actsColorFollowingExample.cpp

/*
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
*/


#include "Aria.h"



// Chase is an action that moves the robot toward the largest blob that
// appears in it's current field of view.
class Chase : public ArAction
{
  
public:
  
  // The state of the chase action
  enum State {
    NO_TARGET,      // There is no target in view
    TARGET,         // This is a target in view
  };

  // Constructor
  Chase(ArACTS_1_2 *acts, ArVCC4 *camera);
  
  // Destructor
  ~Chase(void);
  
  // The action
  ArActionDesired *fire(ArActionDesired currentDesired);

  // Set the ACTS channel that we want to get blob info from
  bool setChannel(int channel);

  // Return the current state of this action
  State getState(void) { return myState; }

  // Height and width of pixels from frame-grabber
  enum {
    WIDTH = 160,
    HEIGHT = 120
  };

protected:
  ArActionDesired myDesired;
  ArACTS_1_2 *myActs;
  ArVCC4 *myCamera;
  ArTime myLastSeen;
  State myState;
  int myChannel;
  int myMaxTime;
};


// Constructor: Initialize the chase action
Chase::Chase(ArACTS_1_2 *acts, ArVCC4 *camera) :
    ArAction("Chase", "Chases the largest blob.")
{
  myActs = acts;
  myCamera = camera;
  myChannel = 0;
  myState = NO_TARGET;
  setChannel(1);
  myLastSeen.setToNow();
  myMaxTime = 1000;
}

// Destructor
Chase::~Chase(void) {}


// The chase action
ArActionDesired *Chase::fire(ArActionDesired currentDesired)
{
  ArACTSBlob blob;
  ArACTSBlob largestBlob;

  bool flag = false;

  int numberOfBlobs;
  int blobArea = 10;

  double xRel, yRel;

  // Reset the desired action
  myDesired.reset();
  
  numberOfBlobs = myActs->getNumBlobs(myChannel);

  // If there are blobs to be seen, set the time to now
  if(numberOfBlobs != 0)
    {
      for(int i = 0; i < numberOfBlobs; i++)
    {
      myActs->getBlob(myChannel, i + 1, &blob);
      if(blob.getArea() > blobArea)
        {
          flag = true;
          blobArea = blob.getArea();
              largestBlob = blob;
        }
    }

      myLastSeen.setToNow();
    }

  // If we have not seen a blob in a while...
  if (myLastSeen.mSecSince() > myMaxTime)
    {
      if(myState != NO_TARGET) ArLog::log(ArLog::Normal, "Target Lost");
      myState = NO_TARGET;
    }
  else
    {
      // If we see a blob and haven't seen one before..
      if(myState != TARGET) ArLog::log(ArLog::Normal, "Target Aquired");
      myState = TARGET;
    }

  if(TARGET && flag == true)
    { 
      // Determine where the largest blob's center of gravity
      // is relative to the center of the camera
      xRel = (double)(largestBlob.getXCG() - WIDTH/2.0)  / (double)WIDTH;
      yRel = (double)(largestBlob.getYCG() - HEIGHT/2.0) / (double)HEIGHT;
      
      // Tilt the camera toward the blob
      if(!(ArMath::fabs(yRel) < .20))
    {
      if (-yRel > 0)
        myCamera->tiltRel(1);
      else
        myCamera->tiltRel(-1);
    }

      // Set the heading and velocity for the robot
      if (ArMath::fabs(xRel) < .10)
    {
      myDesired.setDeltaHeading(0);
    }
      else
    {
      if (ArMath::fabs(-xRel * 10) <= 10)
        myDesired.setDeltaHeading(-xRel * 10);
      else if (-xRel > 0)
        myDesired.setDeltaHeading(10);
      else
        myDesired.setDeltaHeading(-10);
     
    }

      myDesired.setVel(200);
      return &myDesired;    
    }
  else
    {
      myDesired.setVel(0);
      myDesired.setDeltaHeading(0);
      return &myDesired;
    }
}

// Set the channel that the blob info will be obtained from
bool Chase::setChannel(int channel)
{
  if (channel >= 1 && channel <= ArACTS_1_2::NUM_CHANNELS)
  {
    myChannel = channel;
    return true;
  }
  else
    return false;
}







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

  // The robot
  ArRobot robot;

  // A key handler to take input from keyboard
  ArKeyHandler keyHandler;

  // Sonar for basic obstacle avoidance
  ArSonarDevice sonar;

  // The camera (Cannon VC-C4)
  ArVCC4 vcc4 (&robot);

  // ACTS, for tracking blobs of color
  ArACTS_1_2 acts;

  // command line arguments
  ArArgumentParser argParser(&argc, argv);
  argParser.loadDefaultArguments();
  
  // The simple way to connect to things (takes arguments from argParser)
  ArSimpleConnector simpleConnector(&argParser);

  // Parse the arguments                
  if (!Aria::parseArgs())
  {    
    Aria::logOptions();
    keyHandler.restore();
    Aria::shutdown();
    return 1;
  }

  // Robot motion limiter actions (if obstacles are detected by sonar)
  ArActionLimiterForwards limiter("speed limiter near", 300, 600, 250);
  ArActionLimiterForwards limiterFar("speed limiter far", 300, 1100, 400);
  ArActionLimiterBackwards backwardsLimiter;
  ArActionConstantVelocity stop("stop", 0);
  ArActionConstantVelocity backup("backup", -200);

  // The color following action, defined above
  Chase chase(&acts, &vcc4);

  // Let Aria know about the key handler
  Aria::setKeyHandler(&keyHandler);

  // Add the key handler to the robot
  robot.attachKeyHandler(&keyHandler);

  // Add the sonar to the robot
  robot.addRangeDevice(&sonar);

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

  // Open a connection to ACTS
  acts.openPort(&robot);

  // Initialize the camera
  vcc4.init();

  // Wait a second.....
  ArUtil::sleep(1000);

  // Artificially keep the robot from going too fast
  robot.setAbsoluteMaxTransVel(400);

  // Enable the motors
  robot.comInt(ArCommands::ENABLE, 1);
  
  // Turn off the amigobot sounds
  robot.comInt(ArCommands::SOUNDTOG, 0);

  // Wait....
  ArUtil::sleep(200);

  // Add the actions to the robot in descending order of importance.
  robot.addAction(&limiter, 100);
  robot.addAction(&limiterFar, 99);
  robot.addAction(&backwardsLimiter, 98);
  robot.addAction(&chase, 77);
  robot.addAction(&backup, 50);
  robot.addAction(&stop, 30);

  // Run the robot processing cycle until the connection is lost
  robot.run(true);
  
  Aria::shutdown();
  return 0;
}