ArActionColorFollow.cpp

/*
ActivMedia Robotics Interface for Applications (ARIA)
Copyright (C) 2004,2005 ActivMedia Robotics, LLC


     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 
ActivMedia Robotics for information about a commercial version of ARIA at 
robots@activmedia.com or 
ActivMedia Robotics, 19 Columbia Drive, Amherst, NH 03031; 800-639-9481

*/


#include "ArExport.h"

#include "ariaOSDef.h"
#include "ArActionColorFollow.h"
#include "ArResolver.h"
#include "ArRobot.h"

// Constructor: Initialize the color follow action
ArActionColorFollow::ArActionColorFollow(const char *name, ArACTS_1_2 *acts, 
                                                  ArPTZ *camera, double speed, 
                                                  int width, int height) :
    ArAction(name, "Follows the largest blob of color.")
{
  myActs = acts;
  myAcquire = true;
  killMovement = true;
  myCamera = camera;
  myChannel = 0;
  myState = NO_TARGET;
  myMove = STOPPED;
  myLocation = CENTER;
  setChannel(1);
  myMaxTime = 2000;
  myWidth = width;
  myHeight = height;
  mySpeed = speed;
}

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


// The color follow action
ArActionDesired *ArActionColorFollow::fire(ArActionDesired currentDesired)
{
  ArACTSBlob blob;
  ArACTSBlob largestBlob;

  bool haveBlob = false;

  int numberOfBlobs;
  int blobArea = 20;

  double xRel, yRel;

  // Reset the desired action
  myDesired.reset();

  // If we are in fact connected to ACTS...
  if(myActs != NULL)
    {
      numberOfBlobs = myActs->getNumBlobs(myChannel);
      
      // If there are blobs to be seen, set the time to now
      if(numberOfBlobs != 0)
        {
          // Find the largest blob
          for(int i = 0; i < numberOfBlobs; i++)
            {
              myActs->getBlob(myChannel, i + 1, &blob);
              if(blob.getArea() > blobArea)
                {
                  haveBlob = true;
                  blobArea = blob.getArea();
                  largestBlob = blob;
                }
            }
          
          myLastSeen.setToNow();
        }
    
      // Decide the state of myState: do we have a target or not
      if (myLastSeen.mSecSince() > myMaxTime)
        {
          myState = NO_TARGET;
        }
      else
        {
          myState = TARGET;
        }
      
      // If we have a target start following
      if(myState == TARGET) myMove = FOLLOWING;
      // If not and we are allowed to acquire one, start searching
      else if(myState == NO_TARGET && myAcquire)
        {
          myMove = ACQUIRING;
        }
      // Otherwise stay still
      else myMove = STOPPED;
    }

  // We are not connected to acts, dont do anything
  else
    {
      myState = NO_TARGET;
      myMove = STOPPED;
    }

  // If we are following a blob of color
  if(myMove == FOLLOWING)
    {
      if(haveBlob)
        { 
          // Determine where the largest blob's center of gravity
          // is relative to the center of the camera
          xRel = (double)(largestBlob.getXCG() - myWidth/2.0)  / (double)myWidth;
          yRel = (double)(largestBlob.getYCG() - myHeight/2.0) / (double)myHeight;
          
          // Tilt the camera toward the blob
          if(!(ArMath::fabs(yRel) < .20) && myCamera != NULL)
            {
              if (-yRel > 0)
                myCamera->tiltRel(1);
              else
                myCamera->tiltRel(-1);
            }

          // If we can move, move after that blob
          if(!killMovement)
            {
              // Set the heading and velocity for the robot
              if (ArMath::fabs(xRel) < .10)
                {
                  myLocation = CENTER;
                  myDesired.setDeltaHeading(0);
                  myDesired.setVel(mySpeed);
                  return &myDesired;
                }
              else
                {
                  if (ArMath::fabs(-xRel * 10) <= 10)
                    {
                      if(xRel < 0) myLocation = RIGHT;
                      if(xRel > 0) myLocation = LEFT;
                      myDesired.setDeltaHeading(-xRel * 10);
                    }
                  else if (-xRel > 0)
                    {
                      myLocation = RIGHT;
                      myDesired.setDeltaHeading(10);
                    }
                  else
                    {
                      myLocation = LEFT;
                      myDesired.setDeltaHeading(-10);
                    }
                  
                  myDesired.setVel(mySpeed);
                  return &myDesired;    
                }
            }
        }
      // If we do not have a blob in sight, yet are still
      // supposed to be following one... 
      else if(!killMovement)
        {
          // head toward where the blob was last seen
          if(myLocation == LEFT)
            {
              myDesired.setDeltaHeading(-10);
              myDesired.setVel(0);
            }
          if(myLocation == RIGHT)
            {
              myDesired.setDeltaHeading(10);
              myDesired.setVel(0);
            }
          if(myLocation == CENTER)
            {
              myDesired.setDeltaHeading(0);
              myDesired.setVel(mySpeed);
            }
        
          return &myDesired; 
        }
    }
  // If we are acquiring, keep turning around in a circle
  else if(myMove == ACQUIRING)
    {
      if(myCamera != NULL) myCamera->tilt(0);

      if (!killMovement)
        {
          myDesired.setVel(0);
          myDesired.setDeltaHeading(8);
          return &myDesired;
        }
    }
  
  // If we dont meet one of the above conditions, just sit tight
  myDesired.setVel(0);
  myDesired.setDeltaHeading(0);
  return &myDesired;
}

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

// Set the camera that we are controlling
void ArActionColorFollow::setCamera(ArPTZ *camera)
{
  myCamera = camera;
}

// Toggle whether or not we should attempt to acquire a
// target or just stay still
void ArActionColorFollow::setAcquire(bool acquire)
{
  myAcquire = acquire;
}

// Kill movement
void ArActionColorFollow::stopMovement(void)
{
  killMovement = true;
}

// Start moving
void ArActionColorFollow::startMovement(void)
{
  killMovement = false;
}

// Return the channel that we are looking on
int ArActionColorFollow::getChannel(void)
{
  return myChannel;
}

// Return if we are actively trying to find a blob
// if one cannot be seen
bool ArActionColorFollow::getAcquire(void)
{
  return myAcquire;
}

// Return whether we are supposed to be moving or not
bool ArActionColorFollow::getMovement(void)
{
  return !killMovement;
}

// Return whether we can see a target or not
bool ArActionColorFollow::getBlob(void)
{
  if(myState == TARGET) return true;
  else return false;
}

---