ArServerModeRatioDrive.cpp

Go to the documentation of this file.

/*
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"
#include "ArExport.h"
#include "ArServerModeRatioDrive.h"
#include "ArServerHandlerCommands.h"

AREXPORT ArServerModeRatioDrive::ArServerModeRatioDrive(
        ArServerBase *server, ArRobot *robot, bool takeControlOnJoystick,
        bool useComputerJoystick, bool useRobotJoystick, 
        bool useServerCommands, const char *name) : 
  ArServerMode(robot, server, name),
  myRatioDriveGroup(robot),
  myJoyUserTaskCB(this, &ArServerModeRatioDrive::joyUserTask),
  myServerSetSafeDriveCB(this, 
                         &ArServerModeRatioDrive::serverSetSafeDrive),
  myServerGetSafeDriveCB(this, 
                         &ArServerModeRatioDrive::serverGetSafeDrive),
  myServerRatioDriveCB(this, &ArServerModeRatioDrive::serverRatioDrive),
  myRatioFireCB(this, &ArServerModeRatioDrive::ratioFireCallback),
  myServerSafeDrivingEnableCB(this, 
                             &ArServerModeRatioDrive::serverSafeDrivingEnable),
  myServerSafeDrivingDisableCB(this, 
                             &ArServerModeRatioDrive::serverSafeDrivingDisable)
{
  myHandlerCommands = NULL;
  myDriveSafely = true;
  myTakeControlOnJoystick = takeControlOnJoystick;
  myUseComputerJoystick = useComputerJoystick;
  myUseRobotJoystick = useRobotJoystick;
  myUseServerCommands = useServerCommands;
  myUseLocationDependentDevices = true;

  // add the actions, put the ratio input on top, then have the
  // limiters since the ratio doesn't touch decel except lightly
  // whereas the limiter will touch it strongly

  myRatioAction = new ArActionRatioInput;
  myRatioDriveGroup.addAction(myRatioAction, 50);

  myLimiterForward = new ArActionDeceleratingLimiter(
          "DeceleratingLimiterForward", true);
  myRatioDriveGroup.addAction(myLimiterForward, 40);

  myLimiterBackward = new ArActionDeceleratingLimiter(
          "DeceleratingLimiterBackward", false);
  myRatioDriveGroup.addAction(myLimiterBackward, 39);

  myMovementParameters = new ArActionMovementParameters("TeleopMovementParameters", false);
  myRatioDriveGroup.addAction(myMovementParameters, 1);

  myRatioFireCB.setName("ArServerModeRatioDrive");
  myRatioAction->addFireCallback(30, &myRatioFireCB);
  

  
  if (myServer != NULL && myUseServerCommands)
  {
    addModeData("ratioDrive", "drives the robot as with a joystick",
                &myServerRatioDriveCB,
                "double: transRatio; double: rotRatio; double: throttleRatio ",
                "none", "Movement", "RETURN_NONE");
    myServer->addData("setSafeDrive", 
                      "sets whether we drive the robot safely or not",
                      &myServerSetSafeDriveCB,
                      "byte: 1 == drive safely, 0 == drive unsafely",
                      "none", "UnsafeMovement", "RETURN_NONE");
    myServer->addData("getSafeDrive", 
                      "gets whether we drive the robot safely or not",
                      &myServerGetSafeDriveCB,
                      "none", 
                      "byte: 1 == driving safely, 0 == driving unsafely", 
                      "Movement", "RETURN_SINGLE");
  }

  if (myUseComputerJoystick)
  {
    myJoydrive = new ArRatioInputJoydrive(robot, myRatioAction);
    if ((myJoyHandler = Aria::getJoyHandler()) == NULL)
    {
      myJoyHandler = new ArJoyHandler;
      myJoyHandler->init();
      Aria::setJoyHandler(myJoyHandler);
    }
  }
  if (myUseRobotJoystick)
  {
    myRobotJoydrive = new ArRatioInputRobotJoydrive(robot, myRatioAction);
    if ((myRobotJoyHandler = Aria::getRobotJoyHandler()) == NULL)
    {
      myRobotJoyHandler = new ArRobotJoyHandler(robot);
      Aria::setRobotJoyHandler(myRobotJoyHandler);
    }
  }
  if (myUseRobotJoystick || myUseComputerJoystick)
  {
    std::string taskName = name;
    taskName += "::joyUserTask";
    myRobot->addUserTask(taskName.c_str(), 75, &myJoyUserTaskCB);
  }

  myPrinting = false;
}

AREXPORT ArServerModeRatioDrive::~ArServerModeRatioDrive()
{

}

AREXPORT void ArServerModeRatioDrive::addToConfig(ArConfig *config, 
                                             const char *section)
{
  myRatioAction->addToConfig(config, section);
  myLimiterForward->addToConfig(config, section, "Forward");
  myLimiterBackward->addToConfig(config, section, "Backward");
  myMovementParameters->addToConfig(config, section, "Teleop");
  
}

AREXPORT void ArServerModeRatioDrive::activate(void)
{
  ratioDrive(0, 0, 100);
}

AREXPORT void ArServerModeRatioDrive::deactivate(void)
{
  myRatioDriveGroup.deactivate();
  baseDeactivate();
}

AREXPORT void ArServerModeRatioDrive::setSafeDriving(bool safe, bool internal)
{
  if (!internal)
    myRobot->lock();
  // if this is a change then print it
  if (safe != myDriveSafely)
  {
    if (safe)
    {
      ArLog::log(ArLog::Normal, "%s: Driving safely again", myName.c_str());
    }
    else
    {
      ArLog::log(ArLog::Normal, "%s: Driving UNSAFELY", myName.c_str());
    }
    myNewDriveSafely = true;
  }
  myDriveSafely = safe;
  if (isActive())
    ratioDrive(myRatioAction->getTransRatio(), myRatioAction->getRotRatio(),
               myRatioAction->getThrottleRatio());
  if (!internal)
    myRobot->unlock();
}

AREXPORT bool ArServerModeRatioDrive::getSafeDriving(void)
{
  return myDriveSafely;
}

AREXPORT void ArServerModeRatioDrive::serverSafeDrivingEnable(void)
{
  setSafeDriving(true);
}

AREXPORT void ArServerModeRatioDrive::serverSafeDrivingDisable(void)
{
  setSafeDriving(false);
}

AREXPORT void ArServerModeRatioDrive::addControlCommands(ArServerHandlerCommands *handlerCommands)
{
  if (!myUseServerCommands)
  {
    ArLog::log(ArLog::Normal, 
               "ArServerModeRatioDrive::addControlCommands: Tried to add control commands to a ratio drive not using the server");
    return;
  }
  myHandlerCommands = handlerCommands;
  myHandlerCommands->addCommand(
          "safeRatioDrivingEnable",
          "Enables safe driving with ratioDrive, which will attempt to prevent collisions (default)",
          &myServerSafeDrivingEnableCB, "UnsafeMovement");
  myHandlerCommands->addCommand(
          "safeRatioDrivingDisable",
          "Disables safe driving with ratioDrive, this is UNSAFE and will let you drive your robot into things or down stairs, use at your own risk",
          &myServerSafeDrivingDisableCB, "UnsafeMovement");
}

AREXPORT void ArServerModeRatioDrive::ratioDrive(double transRatio, 
                                                 double rotRatio,
                                                 double throttleRatio)
{
  bool wasActive;
  wasActive = isActive();
  if (!wasActive && !baseActivate())
    return;
  if (!wasActive || myNewDriveSafely)
  {
    myRobot->clearDirectMotion();
    if (myDriveSafely)
    {
      myRatioDriveGroup.activateExclusive();
      myMode = "Drive";
      ArLog::log(ArLog::Normal, "%s: Driving safely", myName.c_str());
    }
    else
    {
      myRobot->deactivateActions();
      myRatioAction->activate();
      myMode = "UNSAFE Drive";
      ArLog::log(ArLog::Normal, "%s: Driving unsafely", myName.c_str());
    }
  }
  myNewDriveSafely = false;
  myTransRatio = transRatio;
  myRotRatio = rotRatio;
  myThrottleRatio = throttleRatio;
  setActivityTimeToNow();
  if (myPrinting)
    printf("cmd %.0f %.0f %.0f\n", transRatio, rotRatio, throttleRatio);
  //myRatioAction.setRatios(transRatio, rotRatio, throttleRatio);
}

AREXPORT void ArServerModeRatioDrive::serverRatioDrive(ArServerClient *client, 
                                                       ArNetPacket *packet)
{
  double transRatio;
  double rotRatio;
  double throttleRatio;
  transRatio = packet->bufToDouble();
  rotRatio = packet->bufToDouble();
  throttleRatio = packet->bufToDouble();
  if (!myDriveSafely && !client->hasGroupAccess("UnsafeMovement"))    
    serverSafeDrivingEnable();
  myRobot->lock();
  ratioDrive(transRatio, rotRatio, throttleRatio);
  myRobot->unlock();
}

AREXPORT void ArServerModeRatioDrive::serverSetSafeDrive(
        ArServerClient *client, ArNetPacket *packet)
{
  if (packet->bufToUByte() == 0)
    setSafeDriving(false);
  else
    setSafeDriving(true);
}

AREXPORT void ArServerModeRatioDrive::serverGetSafeDrive(
        ArServerClient *client, ArNetPacket *packet)
{
  ArNetPacket sendPacket;
  if (getSafeDriving())
    sendPacket.uByteToBuf(1);
  else
    sendPacket.uByteToBuf(0);
  
  client->sendPacketTcp(&sendPacket);
}


AREXPORT void ArServerModeRatioDrive::joyUserTask(void)
{
  
  // if we're not active but we should be
  if (myTakeControlOnJoystick && !isActive() && 
      ((myUseComputerJoystick && myJoyHandler->haveJoystick() && 
        myJoyHandler->getButton(1)) || 
       (myUseRobotJoystick && myRobotJoyHandler->gotData() && 
        myRobotJoyHandler->getButton1())))
  {
    if (ArServerMode::getActiveMode() != NULL)
      ArLog::log(ArLog::Normal, 
                 "%s: Activating instead of %s because of local joystick", 
                 ArServerMode::getActiveMode()->getName(),
                 myName.c_str());
    else
      ArLog::log(ArLog::Normal, 
                 "%s: Activating because of local joystick",
                 myName.c_str());
    activate();
  }
}

AREXPORT void ArServerModeRatioDrive::userTask(void)
{
  // Sets the robot so that we always thing we're trying to move in
  // this mode
  myRobot->forceTryingToMove();
  
  // if the joystick is pushed then set that we're active, server
  // commands'll go into ratioDrive and set it there too
  if ((myUseComputerJoystick && myJoyHandler->haveJoystick() && 
       myJoyHandler->getButton(1)) ||
      (myUseRobotJoystick && myRobotJoyHandler->gotData() && 
       myRobotJoyHandler->getButton1()))
  {
    setActivityTimeToNow();
  }


  if (!myStatusSetThisCycle)
  {
    if (myRobot->isLeftMotorStalled() || myRobot->isRightMotorStalled())
      myStatus = "Stalled";
    // this works because if the motors stalled above caught it, if
    // not and more values it means a stall
    else if (myRobot->getStallValue())
      myStatus = "Bumped";
    else if (ArMath::fabs(myRobot->getVel()) < 2 && 
             ArMath::fabs(myRobot->getRotVel()) < 2)
      myStatus = "Stopped";
    else
      myStatus = "Driving";
  }

  myStatusSetThisCycle = false;
}

AREXPORT void ArServerModeRatioDrive::ratioFireCallback(void)
{
  if (myPrinting)
    printf("Doing %.0f %.0f %.0f\n", myTransRatio, myRotRatio, 
           myThrottleRatio);
  myRatioAction->setRatios(myTransRatio, myRotRatio, myThrottleRatio);
}

AREXPORT void ArServerModeRatioDrive::setUseLocationDependentDevices(
        bool useLocationDependentDevices, bool internal)
{
  if (!internal)
    myRobot->lock();
  // if this is a change then print it
  if (useLocationDependentDevices != myUseLocationDependentDevices)
  {
    if (useLocationDependentDevices)
    {
      ArLog::log(ArLog::Normal, "%s: Using location dependent range devices",
                 myName.c_str());
    }
    else
    {
      ArLog::log(ArLog::Normal, 
                 "%s: Not using location dependent range devices",
                 myName.c_str());
    }
    myUseLocationDependentDevices = useLocationDependentDevices;
    myLimiterForward->setUseLocationDependentDevices(
            myUseLocationDependentDevices);
    myLimiterBackward->setUseLocationDependentDevices(
            myUseLocationDependentDevices);
  }
  if (!internal)
    myRobot->unlock();
}

AREXPORT bool ArServerModeRatioDrive::getUseLocationDependentDevices(void)
{
  return myUseLocationDependentDevices;
}

---