actiongroup.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 
[e-mail] or 
ActivMedia Robotics, 19 Columbia Drive, Amherst, NH 03031; 800-639-9481

*/

#include "Aria.h"

ArActionGroup *teleop;
ArActionGroup *wander;

void teleopMode(void)
{
  teleop->activateExclusive();
  printf("You can use the arrow keys to drive, and the spacebar to stop.\nFor joystick control press the trigger button and then drive.\nPress 'w' for wander mode.\nPress escape to exit.\n");
}

void wanderMode(void)
{
  wander->activateExclusive();
  printf("The robot will now just wander around avoiding things.\nPress 't' for teleop mode.\nPress escape to exit.\n");
}

/*
  This is an example of how to use action groups.
*/

int main(void)
{
  // the serial connection (robot)
  ArSerialConnection serConn;
  // tcp connection (sim)
  ArTcpConnection tcpConn;
  // robot
  ArRobot robot;

  // the group of actions for teleop
  teleop = new ArActionGroup(&robot);
  // the group of actions for wander
  wander = new ArActionGroup(&robot);

  // add the actions for teleop

  // don't hit any tables (if we have sensors)
  teleop->addAction(new ArActionLimiterTableSensor, 100);
  // limiter for close obstacles
  teleop->addAction(new ArActionLimiterForwards("speed limiter near", 
						300, 600, 250), 95);
  // limiter for far away obstacles
  teleop->addAction(new ArActionLimiterForwards("speed limiter far", 
					       300, 1100, 400), 90);
  // limiter so we don't bump things backwards
  teleop->addAction(new ArActionLimiterBackwards, 85);
  // the joydrive action (drive from joystick)
  // we need one of these our selves, so we can mess with it later
  ArActionJoydrive joydriveAct("joydrive", 400, 15);
  teleop->addAction(&joydriveAct, 50);
  // the keydrive action (drive from keyboard)
  teleop->addAction(new ArActionKeydrive, 45);
  
  // add the actions for wander
  
  // if we're stalled we want to back up and recover
  wander->addAction(new ArActionStallRecover, 100);
  // react to bumpers
  wander->addAction(new ArActionBumpers, 75);
  // avoid things closer to us
  wander->addAction(new ArActionAvoidFront("Avoid Front Near", 225, 0), 50);
  // avoid things further away
  wander->addAction(new ArActionAvoidFront, 45);
  // keep moving
  wander->addAction(new ArActionConstantVelocity("Constant Velocity", 400), 25);

  // the sonar device
  ArSonarDevice sonar;
  
  // mandatory init
  Aria::init();

  // set up the key handler for switching modes
  ArKeyHandler *keyHandler;
  // make sure there isn't already one (there should be in this example)
  if ((keyHandler = Aria::getKeyHandler()) == NULL)
  {
    keyHandler = new ArKeyHandler;
    Aria::setKeyHandler(keyHandler);
    robot.attachKeyHandler(keyHandler);
  }
  // make the callbacks
  ArGlobalFunctor teleopCB(&teleopMode);
  ArGlobalFunctor wanderCB(&wanderMode);

  // set the callbacks
  keyHandler->addKeyHandler('w', &wanderCB);
  keyHandler->addKeyHandler('W', &wanderCB);
  keyHandler->addKeyHandler('t', &teleopCB);
  keyHandler->addKeyHandler('T', &teleopCB);

  printf("This program will allow you to use a joystick or keyboard to control the robot or let the robot wander.\n");
  printf("Starting in Teleop mode, press the 'w' key to switch to wander.\n");
  teleopMode();

  // if we don't have a joystick, let 'em know
  if (!joydriveAct.joystickInited())
    printf("Do not have a joystick, only the arrow keys on the keyboard will work.\n");
  
  // set the joystick so it won't do anything if the button isn't pressed
  joydriveAct.setStopIfNoButtonPressed(false);

  // add the sonar to the robot
  robot.addRangeDevice(&sonar);
  
  // First we see if we can open the tcp connection, if we can we'll
  // assume we're connecting to the sim, and just go on...  if we
  // can't open the tcp it means the sim isn't there, so just try the
  // robot

  // modify this next line if you're not using default tcp connection
  tcpConn.setPort();

  // see if we can get to the simulator  (true is success)
  if (tcpConn.openSimple())
  {
    // we could get to the sim, so set the robots device connection to the sim
    printf("Connecting to simulator through tcp.\n");
    robot.setDeviceConnection(&tcpConn);
  }
  else
  {
    // we couldn't get to the sim, so set the port on the serial
    // connection and then set the serial connection as the robots
    // device

    // modify the next line if you're not using the first serial port
    // to talk to your robot
    serConn.setPort();
    printf(
      "Could not connect to simulator, connecting to robot through serial.\n");
    robot.setDeviceConnection(&serConn);
  }

  // try to connect, if we fail exit
  if (!robot.blockingConnect())
  {
    printf("Could not connect to robot... exiting\n");
    Aria::shutdown();
    return 1;
  }

  // set the robots maximum velocity (sonar don't work at all well if you're
  // going faster)
  robot.setAbsoluteMaxTransVel(400);

  // enable the motors, disable amigobot sounds
  robot.comInt(ArCommands::ENABLE, 1);
  robot.comInt(ArCommands::SOUNDTOG, 0);
  
  // run the robot, true here so that the run will exit if connection lost
  robot.run(true);
  
  // now exit
  Aria::shutdown();
  return 0;
}