ArActionDesired.h

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

#ifndef ARACTIONDESIRED_H
#define ARACTIONDESIRED_H

#include "ariaUtil.h"

class ArActionDesiredChannel
{
public:
  AREXPORT static const double NO_STRENGTH;
  AREXPORT static const double MIN_STRENGTH;
  AREXPORT static const double MAX_STRENGTH;
  
  ArActionDesiredChannel() { reset(); myOverrideDoesLessThan = true; }
  ~ArActionDesiredChannel() {}
  void setOverrideDoesLessThan(bool overrideDoesLessThan) 
    { myOverrideDoesLessThan = overrideDoesLessThan; }
  void setDesired(double desired, double desiredStrength, 
          bool allowOverride = false) 
    {
      myDesired = desired;
      myStrength = desiredStrength; 
      myAllowOverride = allowOverride;
      if (myStrength > MAX_STRENGTH)
    myStrength = MAX_STRENGTH;
      if (myStrength < MIN_STRENGTH)
    myStrength = NO_STRENGTH;
    }
  double getDesired(void) const { return myDesired; }
  double getStrength(void) const { return myStrength; }
  double getAllowOverride(void) const { return myAllowOverride; }
  void reset(void) 
    { myDesired = 0; myStrength = NO_STRENGTH; myAllowOverride = true; }
  void merge(ArActionDesiredChannel *desiredChannel)
    {
      double otherStrength = desiredChannel->getStrength();
      double oldStrength = myStrength;
      if (myStrength + otherStrength > MAX_STRENGTH)
    otherStrength = MAX_STRENGTH - myStrength;
      myStrength = myStrength + otherStrength;
      myAllowOverride = myAllowOverride && desiredChannel->getAllowOverride();
      // if we're allowing override just set myDesired to the least
      // (or greatest) value
      if (myAllowOverride && myStrength >= MIN_STRENGTH)
      {
    // if both have strength get the min/max
    if (oldStrength >= MIN_STRENGTH && 
        desiredChannel->getStrength() >= MIN_STRENGTH)
    {
      if (myOverrideDoesLessThan)
        myDesired = ArUtil::findMin(myDesired, 
                    desiredChannel->getDesired());
      else if (!myOverrideDoesLessThan)
        myDesired = ArUtil::findMax(myDesired, 
                    desiredChannel->getDesired());
    }
    // if only it has strength use it
    else if (desiredChannel->getStrength() >= MIN_STRENGTH)
    {
      myDesired = desiredChannel->getDesired();
    }
    // if only this has strength then we don't need to do anything
      }
      else if (myStrength >= MIN_STRENGTH)
    myDesired = (((oldStrength * myDesired) + 
              (desiredChannel->getDesired() * otherStrength)) 
             / (myStrength));
    }
  void startAverage(void)
    {
      myDesiredTotal = myDesired * myStrength;
      myStrengthTotal = myStrength;
    }
  void addAverage(ArActionDesiredChannel *desiredChannel)
    {
      myAllowOverride = myAllowOverride && desiredChannel->getAllowOverride();
      // if we're allowing override then myDesired is just the least
      // of the values thats going to come through... still compute
      // the old way in case something doesn't want to override it
      if (myAllowOverride)
      {
    // if both have strength get the min/max
    if (myStrength >= MIN_STRENGTH && 
        desiredChannel->getStrength() >= MIN_STRENGTH)
    {
      if (myOverrideDoesLessThan)
        myDesired = ArUtil::findMin(myDesired, 
                    desiredChannel->getDesired());
      else if (!myOverrideDoesLessThan)
        myDesired = ArUtil::findMax(myDesired, 
                    desiredChannel->getDesired());
    }
    // if only it has strength use it
    else if (desiredChannel->getStrength() >= MIN_STRENGTH)
    {
      myDesired = desiredChannel->getDesired();
    }
    // if only this has strength then we don't need to do anything
      }
      myDesiredTotal += (desiredChannel->getDesired() * 
             desiredChannel->getStrength());
      myStrengthTotal += desiredChannel->getStrength();
    }
  void endAverage(void)
    {
      if (myStrengthTotal < MIN_STRENGTH)
      {
    myStrength = NO_STRENGTH;
    return;
      }
      // if we're overriding we just use what myDesired already is
      if (!myAllowOverride)
    myDesired = (myDesiredTotal / myStrengthTotal);
      myStrength = myStrengthTotal;
      if (myStrength > MAX_STRENGTH)
    myStrength = MAX_STRENGTH;
    }
protected:
  double myDesired;
  double myStrength;
  bool myAllowOverride;
  double myDesiredTotal;
  double myStrengthTotal;
  bool myOverrideDoesLessThan;
};


class ArActionDesired
{
public:
  AREXPORT static const double NO_STRENGTH;
  AREXPORT static const double MIN_STRENGTH;
  AREXPORT static const double MAX_STRENGTH;
  ArActionDesired() 
    { myHeadingSet = false; 
    myTransDecelDes.setOverrideDoesLessThan(false); 
    myRotDecelDes.setOverrideDoesLessThan(false); }
  virtual ~ArActionDesired() {}
  

  virtual void setVel(double vel, double strength = MAX_STRENGTH)
    { myVelDes.setDesired(vel, strength); }

  virtual void setDeltaHeading(double deltaHeading, 
                   double strength = MAX_STRENGTH)
    { myDeltaHeadingDes.setDesired(deltaHeading, strength); }

  virtual void setHeading(double heading, double strength = MAX_STRENGTH)
    { myHeading = heading; myHeadingStrength = strength; myHeadingSet = true; }


  virtual void setRotVel(double rotVel, double strength = MAX_STRENGTH)
    { myRotVelDes.setDesired(rotVel, strength); }



  virtual void setMaxVel(double maxVel, double strength = MAX_STRENGTH,
             bool useSlowest = true)
    { myMaxVelDes.setDesired(maxVel, strength, useSlowest); }

  virtual void setMaxNegVel(double maxVel, double strength = MAX_STRENGTH,
                bool useSlowest = true)
    { myMaxNegVelDes.setDesired(maxVel, strength, useSlowest); }


  virtual void setTransAccel(double transAccel, 
                 double strength = MAX_STRENGTH,
                 bool useSlowest = true)
    { myTransAccelDes.setDesired(transAccel, strength, useSlowest);  }


  virtual void setTransDecel(double transDecel, double strength = MAX_STRENGTH,
                 bool useFastestDecel = true)
    { myTransDecelDes.setDesired(transDecel, strength, useFastestDecel);  }


  virtual void setMaxRotVel(double maxVel, double strength = MAX_STRENGTH,
                bool useSlowest = true)
    { myMaxRotVelDes.setDesired(maxVel, strength, useSlowest); }


  virtual void setRotAccel(double rotAccel, double strength = MAX_STRENGTH,
               bool useSlowest = true)
    { myRotAccelDes.setDesired(rotAccel, strength, useSlowest);  }


  virtual void setRotDecel(double rotDecel, double strength = MAX_STRENGTH,
               bool useFastest = true)
    { myRotDecelDes.setDesired(rotDecel, strength, useFastest);  }

  virtual void reset(void) 
    {
      myVelDes.reset(); myDeltaHeadingDes.reset(); 
      myMaxVelDes.reset(); myMaxNegVelDes.reset(); myMaxRotVelDes.reset(); 
      myRotVelDes.reset();
      myTransAccelDes.reset(); myTransDecelDes.reset();
      myRotAccelDes.reset(); myRotDecelDes.reset();
      myHeadingSet = false;
    }

  virtual double getVel(void) const
    { return myVelDes.getDesired(); }
  virtual double getVelStrength(void) const
    { return myVelDes.getStrength(); }
  virtual double getHeading(void) const
    { return myHeading; }
  virtual double getHeadingStrength(void) const
    { return myHeadingStrength; }
  virtual double getDeltaHeading(void) const
    { return myDeltaHeadingDes.getDesired(); }
  virtual double getDeltaHeadingStrength(void) const
    { return myDeltaHeadingDes.getStrength(); }
  virtual double getRotVel(void) const { return myRotVelDes.getDesired(); }
  virtual double getRotVelStrength(void) const 
    { return myRotVelDes.getStrength(); }

  virtual double getMaxVel(void) const
    { return myMaxVelDes.getDesired(); }
  virtual double getMaxVelStrength(void) const
    { return myMaxVelDes.getStrength(); }
  virtual double getMaxVelSlowestUsed(void) const
    { return myMaxVelDes.getAllowOverride(); }
  virtual double getMaxNegVel(void) const
    { return myMaxNegVelDes.getDesired(); }
  virtual double getMaxNegVelStrength(void) const
    { return myMaxNegVelDes.getStrength(); }
  virtual double getMaxNegVelSlowestUsed(void) const
    { return myMaxNegVelDes.getAllowOverride(); }
  virtual double getTransAccel(void) const
    { return myTransAccelDes.getDesired(); }
  virtual double getTransAccelStrength(void) const
    { return myTransAccelDes.getStrength(); }
  virtual double getTransAccelSlowestUsed(void) const
    { return myTransAccelDes.getAllowOverride(); }
  virtual double getTransDecel(void) const
    { return myTransDecelDes.getDesired(); }
  virtual double getTransDecelStrength(void) const
    { return myTransDecelDes.getStrength(); }
  virtual double getTransDecelFastestUsed(void) const
    { return myTransDecelDes.getAllowOverride(); }

  virtual double getMaxRotVel(void) const
    { return myMaxRotVelDes.getDesired(); }
  virtual double getMaxRotVelStrength(void) const
    { return myMaxRotVelDes.getStrength(); }
  virtual double getMaxRotVelSlowestUsed(void) const
    { return myMaxRotVelDes.getAllowOverride(); }
  virtual double getRotAccel(void) const
    { return myRotAccelDes.getDesired(); }
  virtual double getRotAccelStrength(void) const
    { return myRotAccelDes.getStrength(); }
  virtual double getMaxAccelSlowestUsed(void) const
    { return myRotAccelDes.getAllowOverride(); }
  virtual double getRotDecel(void) const
    { return myRotDecelDes.getDesired(); }
  virtual double getRotDecelStrength(void) const
    { return myRotDecelDes.getStrength(); }
  virtual double getRotDecelFastestUsed(void) const
    { return myRotDecelDes.getAllowOverride(); }



  virtual void merge(ArActionDesired *actDesired)
    {
      if (actDesired == NULL)
    return;
      myVelDes.merge(&actDesired->myVelDes);
      // if we're already using rot or delt use that, otherwise use what it wants
      if (myDeltaHeadingDes.getStrength() > NO_STRENGTH)
      {
    myDeltaHeadingDes.merge(&actDesired->myDeltaHeadingDes);
      }
      else if (myRotVelDes.getStrength() > NO_STRENGTH)
      {
    myRotVelDes.merge(&actDesired->myRotVelDes);
      }
      else
      {
    myDeltaHeadingDes.merge(&actDesired->myDeltaHeadingDes);
    myRotVelDes.merge(&actDesired->myRotVelDes);
      }
      myMaxVelDes.merge(&actDesired->myMaxVelDes);
      myMaxNegVelDes.merge(&actDesired->myMaxNegVelDes);
      myMaxRotVelDes.merge(&actDesired->myMaxRotVelDes);
      myTransAccelDes.merge(&actDesired->myTransAccelDes);
      myTransDecelDes.merge(&actDesired->myTransDecelDes);
      myRotAccelDes.merge(&actDesired->myRotAccelDes);
      myRotDecelDes.merge(&actDesired->myRotDecelDes);
    }

  virtual void startAverage(void)
    {
      myVelDes.startAverage();
      myRotVelDes.startAverage();
      myDeltaHeadingDes.startAverage();
      myMaxVelDes.startAverage();
      myMaxNegVelDes.startAverage();
      myMaxRotVelDes.startAverage();
      myTransAccelDes.startAverage();
      myTransDecelDes.startAverage();
      myRotAccelDes.startAverage();
      myRotDecelDes.startAverage();
    }

  virtual void addAverage(ArActionDesired *actDesired)
    {
      if (actDesired == NULL)
    return;
      myVelDes.addAverage(&actDesired->myVelDes);
      // if we're using one of rot or delta heading use that,
      // otherwise use whatever they're using
      if (myRotVelDes.getStrength() > NO_STRENGTH)
      {
    myRotVelDes.addAverage(
        &actDesired->myRotVelDes);
      }
      else if (myDeltaHeadingDes.getStrength() > NO_STRENGTH)
      {
    myDeltaHeadingDes.addAverage(
        &actDesired->myDeltaHeadingDes);
      }
      else
      {
    myRotVelDes.addAverage(
        &actDesired->myRotVelDes);
    myDeltaHeadingDes.addAverage(
        &actDesired->myDeltaHeadingDes);
      }

      myMaxVelDes.addAverage(&actDesired->myMaxVelDes);
      myMaxNegVelDes.addAverage(&actDesired->myMaxNegVelDes);
      myMaxRotVelDes.addAverage(&actDesired->myMaxRotVelDes);
      myTransAccelDes.addAverage(&actDesired->myTransAccelDes);
      myTransDecelDes.addAverage(&actDesired->myTransDecelDes);
      myRotAccelDes.addAverage(&actDesired->myRotAccelDes);
      myRotDecelDes.addAverage(&actDesired->myRotDecelDes);
    }

  virtual void endAverage(void)
    {
      myVelDes.endAverage();
      myRotVelDes.endAverage();
      myDeltaHeadingDes.endAverage();
      myMaxVelDes.endAverage();
      myMaxNegVelDes.endAverage();
      myMaxRotVelDes.endAverage();
      myTransAccelDes.endAverage();
      myTransDecelDes.endAverage();
      myRotAccelDes.endAverage();
      myRotDecelDes.endAverage();
    }

  virtual void accountForRobotHeading(double robotHeading)
    {
      if (myHeadingSet)
    setDeltaHeading(ArMath::subAngle(myHeading, robotHeading), 
            myHeadingStrength);
      myHeadingSet = false;
    }
  AREXPORT virtual void log(void) const;
  AREXPORT virtual bool isAnythingDesired(void) const;

protected:
  double myHeading;
  double myHeadingStrength;
  bool myHeadingSet;
  ArActionDesiredChannel myRotVelDes;
  ArActionDesiredChannel myVelDes;
  ArActionDesiredChannel myDeltaHeadingDes;
  ArActionDesiredChannel myMaxVelDes;
  ArActionDesiredChannel myMaxNegVelDes;
  ArActionDesiredChannel myMaxRotVelDes;
  ArActionDesiredChannel myTransAccelDes;
  ArActionDesiredChannel myTransDecelDes;
  ArActionDesiredChannel myRotAccelDes;
  ArActionDesiredChannel myRotDecelDes;
};


#endif

---