ArActionDeceleratingLimiter.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 "ArExport.h"
#include "ariaOSDef.h"
#include "ArActionDeceleratingLimiter.h"
#include "ArRobot.h"
#include "ArCommands.h"
#include "ariaInternal.h"
#include "ArRobotConfigPacketReader.h"
#include "ArRangeDevice.h"

AREXPORT ArActionDeceleratingLimiter::ArActionDeceleratingLimiter(
    const char *name, 
    bool forwards) :
  ArAction(name,
       "Slows the robot down and cranks up deceleration so as not to hit anything in front of it.")
{
  myForwards = forwards;
  setParameters();

  myLastStopped = false;
  myUseLocationDependentDevices = true;
}

AREXPORT ArActionDeceleratingLimiter::~ArActionDeceleratingLimiter()
{

}

AREXPORT void ArActionDeceleratingLimiter::setParameters(
    double clearance,
    double sideClearanceAtSlowSpeed,
    double paddingAtSlowSpeed,
    double slowSpeed,
    double sideClearanceAtFastSpeed,
    double paddingAtFastSpeed,
    double fastSpeed,
    double preferredDecel,
    bool useEStop,
    double maxEmergencyDecel)
{
  myClearance = clearance;
  mySideClearanceAtSlowSpeed = sideClearanceAtSlowSpeed;
  myPaddingAtSlowSpeed = paddingAtSlowSpeed;
  mySlowSpeed = slowSpeed;
  mySideClearanceAtFastSpeed = sideClearanceAtFastSpeed;
  myPaddingAtFastSpeed = paddingAtFastSpeed;
  myFastSpeed = fastSpeed;
  myPreferredDecel = preferredDecel;
  myUseEStop = useEStop;
  myMaxEmergencyDecel = maxEmergencyDecel;
}

AREXPORT void ArActionDeceleratingLimiter::addToConfig(ArConfig *config, 
                               const char *section, 
                               const char *prefix)
{
  std::string strPrefix;
  std::string name;
  if (prefix == NULL || prefix[0] == '\0')
    strPrefix = "";
  else
    strPrefix = prefix;

  config->addParam(ArConfigArg(ArConfigArg::SEPARATOR), section, ArPriority::NORMAL);  

  name = strPrefix;
  name += "Clearance";
  config->addParam(
      ArConfigArg(name.c_str(), &myClearance, 
              "Don't get closer than this to something in front or back. (mm)"), 
      section, ArPriority::NORMAL);

  name = strPrefix;
  name += "SlowSpeed";
  config->addParam(
      ArConfigArg(name.c_str(),
              &mySlowSpeed,
              "Consider this speed slow (mm/sec)"),
      section, ArPriority::NORMAL);

  name = strPrefix;
  name += "SideClearanceAtSlowSpeed";
  config->addParam(
      ArConfigArg(name.c_str(),
              &mySideClearanceAtSlowSpeed, 
              "Don't get closer than this to something on the side if we're going at slow speed or below. (mm)"),
      section, ArPriority::NORMAL);

  name = strPrefix;
  name += "PaddingAtSlowSpeed";
  config->addParam(
      ArConfigArg(name.c_str(), &myPaddingAtSlowSpeed, 
              "Try to stop this far away from clearance at slow speed or below. (mm)"), 
      section, ArPriority::NORMAL);

  name = strPrefix;
  name += "FastSpeed";
  config->addParam(
      ArConfigArg(name.c_str(),
              &myFastSpeed,
              "Consider this speed fast (mm/sec)"),
      section, ArPriority::NORMAL);

  name = strPrefix;
  name += "SideClearanceAtFastSpeed";
  config->addParam(
      ArConfigArg(name.c_str(),
              &mySideClearanceAtFastSpeed, 
              "Don't get closer than this to something on the side if we're going at fast speed or above. (mm)"),
      section, ArPriority::NORMAL);

  name = strPrefix;
  name += "PaddingAtFastSpeed";
  config->addParam(
      ArConfigArg(name.c_str(), &myPaddingAtFastSpeed, 
              "Try to stop this far away from clearance at fast speed or below. (mm)"), 
      section, ArPriority::NORMAL);

  name = strPrefix;
  name += "PreferredDecel";
  config->addParam(
      ArConfigArg(name.c_str(),
              &myPreferredDecel,
              "The maximum decel we'll use until something might infringe on clearance and sideClearanceAtSlowSpeed (mm/sec/sec"),
      section, ArPriority::NORMAL);

  name = strPrefix;
  name += "MaxEmergencyDecel";
  config->addParam(
      ArConfigArg(name.c_str(),
              &myMaxEmergencyDecel,
              "The maximum decel we'll ever use, 0 means use the robot's maximum (mm/sec/sec"),
      section, ArPriority::NORMAL);

  name = strPrefix;
  name += "UseEStop";
  config->addParam(
      ArConfigArg(name.c_str(),
              &myUseEStop,
              "Whether to use an EStop to stop if something will intrude on our clearance"),
      section, ArPriority::NORMAL);

  config->addParam(ArConfigArg(ArConfigArg::SEPARATOR), section, ArPriority::NORMAL);
}

AREXPORT ArActionDesired *
ArActionDeceleratingLimiter::fire(ArActionDesired currentDesired)
{
  double dist;
  const ArRangeDevice *distRangeDevice;
  double distInner;
  const ArRangeDevice *distInnerRangeDevice;
  bool printing = false;
  double absVel;
  
  myDesired.reset();
  // see if we're going the right direction for this to work
  if (myRobot->getVel() < -100 && myForwards)
    return NULL;
  else if (myRobot->getVel() > 100 && !myForwards)
    return NULL;
  absVel = ArMath::fabs(myRobot->getVel());

  double sideClearance;
  double padding;
  
  // see if we're going slow
  if (ArMath::fabs(myRobot->getVel()) <= mySlowSpeed)
  {
    sideClearance = mySideClearanceAtSlowSpeed;
    padding = myPaddingAtSlowSpeed;
  }
  // or if we're going fast
  else if (ArMath::fabs(myRobot->getVel()) >= myFastSpeed)
  {
    sideClearance = mySideClearanceAtFastSpeed;
    padding = myPaddingAtFastSpeed;
  }
  // or if we have to interpolate
  else
  {
    sideClearance = (((mySideClearanceAtFastSpeed - 
               mySideClearanceAtSlowSpeed) * 
              ((ArMath::fabs(myRobot->getVel()) - mySlowSpeed) / 
               (myFastSpeed - mySlowSpeed))) + 
             mySideClearanceAtSlowSpeed);
    padding = (((myPaddingAtFastSpeed - 
         myPaddingAtSlowSpeed) * 
        ((ArMath::fabs(myRobot->getVel()) - mySlowSpeed) / 
         (myFastSpeed - mySlowSpeed))) + 
           myPaddingAtSlowSpeed);
  }


  if (printing)
    printf("%d side %.0f padding %.0f\n", myForwards, sideClearance, padding);
              
  if (myForwards)
    dist = myRobot->checkRangeDevicesCurrentBox(
        0,
        -(myRobot->getRobotWidth()/2.0 + sideClearance),
        myRobot->getRobotLength()/2.0 + myClearance + padding + 8000,
        (myRobot->getRobotWidth()/2.0 + sideClearance),
        NULL,
        &distRangeDevice, myUseLocationDependentDevices);
  else
    dist = myRobot->checkRangeDevicesCurrentBox(
        0,
        -(myRobot->getRobotWidth()/2.0 + sideClearance),
        -(myRobot->getRobotLength()/2.0 + myClearance + padding + 8000),
        (myRobot->getRobotWidth()/2.0 + sideClearance),
        NULL,
        &distRangeDevice, myUseLocationDependentDevices);

  if (myForwards)
    distInner = myRobot->checkRangeDevicesCurrentBox(
        0,
        -(myRobot->getRobotWidth()/2.0 + mySideClearanceAtSlowSpeed),
        myRobot->getRobotLength()/2.0 + myClearance + 8000,
        (myRobot->getRobotWidth()/2.0 + mySideClearanceAtSlowSpeed),
        NULL,
        &distInnerRangeDevice, myUseLocationDependentDevices);
  else
    distInner = myRobot->checkRangeDevicesCurrentBox(
        0,
        -(myRobot->getRobotWidth()/2.0 + mySideClearanceAtSlowSpeed),
        -(myRobot->getRobotLength()/2.0 + myClearance + 8000),
        (myRobot->getRobotWidth()/2.0 + mySideClearanceAtSlowSpeed),
        NULL,
        &distInnerRangeDevice, myUseLocationDependentDevices);
    
  // subtract off our clearance and padding to see how far we have to stop
  dist -= myRobot->getRobotLength() / 2.0;
  dist -= myClearance;
  dist -= padding;

  // this is what we estop for, so don't subtract our padding from this
  distInner -= myRobot->getRobotLength() / 2.0;
  distInner -= myClearance;

  if (printing)
    printf("%d dist %.0f\n", myForwards, dist);
  // see if we need to throw an estop
  if (distInner < 1 && ((myRobot->getVel() > 5 && myForwards) ||
           (myRobot->getVel() < -5 && !myForwards)))
  {
    if (printing && !myLastStopped)
      printf("%d Stopping\n", myForwards);
    if (ArMath::fabs(myRobot->getVel()) > 100)
    {
      if (myUseEStop)
      {
    ArLog::log(ArLog::Normal, "ArActionDeceleratingLimiter: estopping because of reading from %s", distInnerRangeDevice->getName());
    myRobot->comInt(ArCommands::ESTOP, 1);
      }
      else
      {
    ArLog::log(ArLog::Normal, "ArActionDeceleratingLimiter: maximum deceleration because of reading from %s", distInnerRangeDevice->getName());
      }
      // if we have a maximum emergency decel, use that
      if (fabs(myMaxEmergencyDecel) > 1)
      {
    if (printing)
      printf("Max emergency decel %.0f\n", 
         myMaxEmergencyDecel);
    myDesired.setTransDecel(myMaxEmergencyDecel);
      }
      //  if we don't use the robot's top decel
      else if (myRobot->getOrigRobotConfig() != NULL && 
      myRobot->getOrigRobotConfig()->hasPacketArrived())
      {
    if (printing)
      printf("Robots max decel %d\n", 
         myRobot->getOrigRobotConfig()->getTransAccelTop());
    myDesired.setTransDecel(
        myRobot->getOrigRobotConfig()->getTransAccelTop());
      }
      // if we don't have that either use our preferred decel
      else
      {
    if (printing)
      printf("Prefered decel %g\n", myPreferredDecel);
    myDesired.setTransDecel(myPreferredDecel);
      }
    }
    myLastStopped = true;
    if (myForwards)
      myDesired.setMaxVel(0);
    else
      myDesired.setMaxNegVel(0);
    myDesired.setVel(0);
    ArLog::log(ArLog::Verbose, "Stopping (inner) because of reading from %s", 
           distInnerRangeDevice->getName());
    return &myDesired;
  }
  // if our distance is greater than how far it'd take us to stop
  //printf("%.0f %.0f %.0f\n", dist, absVel, absVel * absVel / 2.0 / myRobot->getTransDecel());
  if (dist > absVel * absVel / 2.0 / myRobot->getTransDecel())
  {
    if (printing)
      printf("%d Nothing\n", myForwards);
    return NULL;
  }

  if (printing && myLastStopped)
    printf("%d moving\n", myForwards);
  myLastStopped = false;
  //printf("%f ", dist);
  //maxVel = (dist - clearance);
  double deceleration = - absVel * absVel / dist / 2.0;
  double decelerationInner = - absVel * absVel / distInner / 2.0;
  // make sure the robot or other actions aren't already decelerating
  // more than we want to

  //printf("%.0f %.0f %.0f %.0f\n", deceleration, myRobot->getTransDecel(),      currentDesired.getTransDecelStrength(), currentDesired.getTransDecel());
  if (fabs(deceleration) > fabs(myRobot->getTransDecel()) &&
      (currentDesired.getTransDecelStrength() < ArActionDesired::MIN_STRENGTH 
       || fabs(deceleration) > fabs(currentDesired.getTransDecel())))
  {
    // if our deceleration is faster than we want to decel see if we
    // actually will have to decel that fast or not
    if (fabs(myMaxEmergencyDecel) > 1 && 
    fabs(decelerationInner) > myMaxEmergencyDecel)
      myDesired.setTransDecel(myMaxEmergencyDecel);
    else if (fabs(decelerationInner) > myPreferredDecel)
      myDesired.setTransDecel(fabs(decelerationInner));
    else if (fabs(myMaxEmergencyDecel) > 1 && 
         fabs(deceleration) > myMaxEmergencyDecel)
      myDesired.setTransDecel(myMaxEmergencyDecel);
    else if (fabs(deceleration) > myPreferredDecel)
      myDesired.setTransDecel(myPreferredDecel);
    else 
      myDesired.setTransDecel(fabs(deceleration));
    if (printing)
      printf("Set deceleration to %g\n", myDesired.getTransDecel());
  }
  else
    deceleration = myRobot->getTransDecel();

  //double maxVel = absVel - deceleration  / 10.0;

  if (printing)
    printf("%d accel %.0f\n", myForwards, deceleration);
  //printf("Max vel %f (stopdist %.1f slowdist %.1f slowspeed %.1f\n", maxVel,   myStopDist, mySlowDist, mySlowSpeed);

  ArLog::log(ArLog::Verbose, "Stopping because of reading from %s", 
         distInnerRangeDevice->getName());

  if (myForwards)
    myDesired.setMaxVel(0);
  else
    myDesired.setMaxNegVel(0);
  return &myDesired;
}

---