Modules/SensorBehaviorControl/ObstacleAvoiderOnGreenField.cpp

Go to the documentation of this file.

/**
* @file ObstacleAvoiderOnGreenField.cpp
* This file contains a class that demonstrates walking.
* 
* @author <A href=mailto:jhoffman@informatik.hu-berlin.de>Jan Hoffmann</A>
*/

#include "ObstacleAvoiderOnGreenField.h"
#include "Tools/Debugging/GenericDebugData.h"



ObstacleAvoiderOnGreenField::ObstacleAvoiderOnGreenField(const SensorBehaviorControlInterfaces& interfaces)
: SensorBehaviorControl(interfaces), 
maxSpeedX(140),
distanceToGround(300),
distanceOffset(50),
distanceControlInterval(50),
speedX(0, 0.6, 0.0, 0, -maxSpeedX/2, maxSpeedX, 100),
speedY(0, 1, 0, 0, -50, 50, 50),
speedPhi(0, .1, 0, 0, -pi_4, pi_4, pi/10),
gridSize(4),
timeOfLastExecute(0),
localColTableInitialized(false)
{
}

void ObstacleAvoiderOnGreenField::init() {}

void ObstacleAvoiderOnGreenField::execute()
{
  /* The module was not active for a while */
  /*
  unsigned long currentTime = SystemCall::getCurrentSystemTime();
  if((currentTime - timeOfLastExecute) > 2000) init();
  timeOfLastExecute = currentTime;
  */

  /* [JH] use the "proper" way to access the color table */
  Image cameraImage = image;
  if(!localColTableInitialized)
  {
    cameraImage.setColorTable(&colorTable);
  }
  else 
  {
    cameraImage.setColorTable(&localColTable);    
  }
  
  
  double sum,
    greenRight = 0, 
    greenLeft = 0, 
    targetY = 0, 
    targetPhi = 0, 
    targetX,
    psd = (sensorDataBuffer.lastFrame().data[SensorData::psd] / 1000) - 100.0; 
  
  int x, y, midX = image.cameraInfo.resolutionWidth / 2;

  distanceToGround = 350;

  if (psd > distanceToGround * 1.4) // don't jump off cliff!
    targetX = 0.0; // don't use negative nuber because robot might "reverse" off table
  else
    targetX = psd - (distanceToGround - distanceOffset - distanceControlInterval);
  //shouldn't this be = psp - distanceOffset - (distanceToGround) ?

  targetX *= maxSpeedX/distanceControlInterval;
  
  //OUTPUT(idText, text, "target: " << targetX << ", psd: " << psd << ", speedX: " << speedX.getVal() << ", P speed: " << speedX.getWeightP());
  
  
  /** calc how much green is in both halves */
  for(x = gridSize/2; x < midX; x += gridSize)
  {
    for(y = gridSize/2; y < image.cameraInfo.resolutionHeight; y += gridSize)
    {
      if (cameraImage.getClassifiedColor(x, y) == green)
      {
        greenLeft++;
        //cameraImage.image[y][2][x] = 255;
      }
      else
      {
        //cameraImage.image[y][0][x] = 0;
      }

      if (cameraImage.getClassifiedColor(x + midX, y) == green)
      {
        greenRight++;
        //cameraImage.image[y][1][x + midX] = 255;
      }
      else 
      {
        //cameraImage.image[y][0][x + midX] = 255;
      }
    }
  }

  //INIT_DEBUG_IMAGE(classificationY, cameraImage);
  //SEND_DEBUG_IMAGE(classificationY);
  OUTPUT(idText, text, " " << greenLeft << " : " << greenRight); 

  sum = greenLeft + greenRight;
  if (sum == 0) sum = 1;
  
  /** if robot is moving forward, turn where there's more
  *   green
  */
  if (speedX.getVal() > .7 * maxSpeedX)
  {
    targetPhi = 1.2 * (greenLeft - greenRight) / sum;
    //targetPhi *= .5;
  }
  /** if robot is too close to something, stick to the
  * direction it last turned to until obstacle is no
  * longer in front of robot
  */
  else 
  {
    if (speedPhi.getVal() < 0)
      targetPhi = -pi;
    else if (speedPhi.getVal() > 0)
      targetPhi = pi;
  }
  
  if (greenLeft > greenRight) 
  {
    ledRequest.redBottomLEDs = LEDRequest::leftOnly;
  }
  else if (greenLeft < greenRight)
  {
    ledRequest.redBottomLEDs = LEDRequest::rightOnly;
  }
  else if (greenLeft == greenRight)
  {
    if (greenLeft == 0)
      ledRequest.redBottomLEDs = LEDRequest::bothOff;
    else
      ledRequest.redBottomLEDs = LEDRequest::bothOn;
  }
  else 
    ledRequest.redBottomLEDs = LEDRequest::bothOff;
      
  ledRequest.redTopLEDs = LEDRequest::bothOff;
  
  if(sensorDataBuffer.lastFrame().data[SensorData::tailPan] > 0)
  {
    speedPhi.approximateVal(targetPhi);
    speedX.approximateVal(targetX);
    timeToCalibrate = 0;
    soundRequest.soundID = SoundRequest::okay;
    headControlMode.headControlMode = HeadControlMode::lookStraightAhead;
  }
  else //stop
  {
    headControlMode.headControlMode = HeadControlMode::lookStraightAhead;
    if (SystemCall::getTimeSince(timeToCalibrate) < 2000)
      soundRequest.soundID = SoundRequest::notokay;
    else 
      soundRequest.soundID = SoundRequest::none;
    speedPhi.approximateVal(0);
    speedX.approximateVal(0);
    if (timeToCalibrate > 0)
    {
      ledRequest.redTopLEDs = LEDRequest::bothOn;
      if (SystemCall::getTimeSince(timeToCalibrate) > 2000) // wait for one sec to allow motion to subside
      {
/*        if(robotState.getSwitches() == RobotState::headFrontDown)
        {
          soundRequest.soundID = SoundRequest::rob005;
          calibrate();
        }
*/
      }
    }
    else
    {
      timeToCalibrate = SystemCall::getCurrentSystemTime();
      ledRequest.redTopLEDs = LEDRequest::bothOff;
    }
  }
     
  motionRequest.tailRequest.tailRequestID = TailRequest::twoPositionSwitchHorizontal;
  motionRequest.motionType = MotionRequest::walk;
  motionRequest.walkRequest.walkType = WalkRequest::normal;

  /* set the motion request */
  motionRequest.motionType=MotionRequest::walk;
  if (fabs(speedX.getVal()) > 1)
    motionRequest.walkRequest.walkParams.translation.x = speedX.getVal();
  else  
    motionRequest.walkRequest.walkParams.translation.x = 0.0;
  if (fabs(speedY.getVal()) > 1)
    motionRequest.walkRequest.walkParams.translation.y = targetY;
  else 
    motionRequest.walkRequest.walkParams.translation.y = 0.0;
  if (fabs(speedPhi.getVal()) > .05)
    motionRequest.walkRequest.walkParams.rotation = speedPhi.getVal();
  else 
    motionRequest.walkRequest.walkParams.rotation = 0.0;
}




void ObstacleAvoiderOnGreenField::calibrate()
{
  //distanceToGround = (sensorDataBuffer.lastFrame().data[SensorData::psd] / 1000) - 100;

  localColTable.clear();

  int x, y;

  for(x = gridSize/2; x < image.cameraInfo.resolutionWidth; x += gridSize)
  {
    for(y = gridSize/2; y < image.cameraInfo.resolutionHeight; y += gridSize)
    {
      localColTable.addColorClass(green, image.image[y][0][x], image.image[y][1][x], image.image[y][2][x], 2);
    }
  }

  localColTableInitialized = true;
}



bool ObstacleAvoiderOnGreenField::handleMessage(InMessage& message)
{
  bool handled = false;
  
  switch(message.getMessageID())
  {
  case idGenericDebugData:
    GenericDebugData d;
    message.bin >> d;
    if(d.id == GenericDebugData::braitenbergPIDs)
    {
      OUTPUT(idText,text,"generic debug message (Braitenberg) handled by module ObstacleAvoiderOnGreenField");
      
      speedX.setWeightP(d.data[0]);
      speedX.setWeightI(d.data[1]);
      speedX.setWeightD(d.data[2]);
      
      speedPhi.setWeightP(d.data[3]);
      speedPhi.setWeightI(d.data[4]);
      speedPhi.setWeightD(d.data[5]);
      
      distanceOffset = d.data[6];
      distanceControlInterval = d.data[7];
      distanceToGround = d.data[8];
    }
    handled = true;
    break;

  }
  return handled;
}


/*
* Change log :
* 
* $Log: ObstacleAvoiderOnGreenField.cpp,v $
* Revision 1.4  2004/06/02 17:18:22  spranger
* MotionRequest cleanup
*
* Revision 1.3  2004/05/24 14:15:49  juengel
* New button evaluation.
*
* Revision 1.2  2004/05/23 12:08:26  loetzsch
* clean up in class HeadControlMode
*
* Revision 1.1.1.1  2004/05/22 17:20:52  cvsadm
* created new repository GT2004_WM
*
* Revision 1.9  2004/03/19 19:03:49  roefer
* Warnings removed
*
* Revision 1.8  2004/03/17 10:22:55  jhoffman
* added ERS7 support, removed some image size related bugs
*
* Revision 1.7  2004/03/08 02:11:49  roefer
* Interfaces should be const
*
* Revision 1.6  2004/01/12 16:00:47  jhoffman
* *** empty log message ***
*
* Revision 1.5  2003/12/31 23:50:39  roefer
* Removed inclusion of RobotDimensions.h because it is not used
*
* Revision 1.4  2003/12/15 11:49:06  juengel
* Introduced CameraInfo
*
* Revision 1.3  2003/12/09 16:28:15  jhoffman
* - added sounds to obstacle avoider on green field
* - added calibration mode for obstacle avoider on green field
*
* Revision 1.2  2003/11/22 09:33:01  jhoffman
* added "Yet Another Inv Kin Walking Engine"
* experimental stage, doesn't do much yet
* no Fourier inside!
*
* Revision 1.1  2003/10/06 14:10:15  cvsadm
* Created GT2004 (M.J.)
*
* Revision 1.2  2003/09/26 11:38:52  juengel
* - sorted tools
* - clean-up in DataTypes
*
* Revision 1.1.1.1  2003/07/02 09:40:24  cvsadm
* created new repository for the competitions in Padova from the 
* tamara CVS (Tuesday 2:00 pm)
*
* removed unused solutions
*
* Revision 1.9  2003/06/27 13:22:52  jhoffman
* minor note
*
* Revision 1.8  2003/06/11 16:10:31  dueffert
* some cleanup
*
* Revision 1.7  2003/05/02 18:15:18  risler
* SensorDataBuffer added
* replaced SensorData with SensorDataBuffer
* full SensorData resolution now accessible
*
* Revision 1.6  2003/03/05 15:27:28  jhoffman
* no message
*
* Revision 1.5  2003/02/21 18:32:04  roefer
* pColorTable -> colorTable finished
*
* Revision 1.4  2003/02/18 21:29:17  osterhues
* Changed all instances of ColorTable64 to new base class ColorTable
*
* Revision 1.3  2003/02/17 10:55:08  dueffert
* greenhills backport
*
* Revision 1.2  2003/02/02 23:29:21  jhoffman
* adjustments for it to work reliably and with any walking engine
*
* Revision 1.1  2003/01/30 18:26:40  jhoffman
* added ObstacleAvoider
*

*/

---