sickTeleop.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 "Aria.h"

/*
  This is an example of how to use the limiting behaviors.
  
  The way it works is that it has a limiting behavior higher priority
  than the joydrive action behavior.  So the joydrive action can try
  to do whatever it wants, but it won't work.
*/


int main(int argc, char **argv)
{
  int ret;
  std::string str;
  // robot
  ArRobot robot;

  // the laser
  ArSick sick;
  // the joydrive action
  ArActionJoydrive jdAct;
  
  // limiter for close obstacles
  ArActionLimiterForwards limiter("speed limiter near", 300, 600, 250);
  // limiter for far away obstacles
  ArActionLimiterForwards limiterFar("speed limiter far", 300, 1100, 400);
  // limiter so we don't bump things backwards
  // limiter for far away obstacles
  ArActionLimiterTableSensor tableLimiter;
  // limiter so we don't bump things backwards
  ArActionLimiterBackwards backwardsLimiter;
  // sonar device, so the limiter will work, this must be added to the robot
  //ArSonarDevice sonar;
  
  // mandatory init
  Aria::init();

  // add the sonar to the robot
  //robot.addRangeDevice(&sonar);
  // Set the laser onto the robot
  robot.addRangeDevice(&sick);

  ArSimpleConnector connector(&argc, argv);
  
  if (!connector.parseArgs() || argc > 1)
  {
    connector.logOptions();
    exit(1);
  }

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

  connector.setupLaser(&sick);

  // now that we're connected to the robot, connect to the laser
  sick.runAsync();

  if (!sick.blockingConnect())
  {
    printf("Could not connect to SICK laser... exiting\n");
    Aria::shutdown();
    return 1;
  }


  // enable the motors, disable amigobot sounds
  robot.comInt(ArCommands::ENABLE, 1);
  robot.comInt(ArCommands::SOUNDTOG, 0);

  // add the cations, put the limiters on top, then have the action,
  // this will keep the action from being able to drive too fast and hit
  // something
  robot.addAction(&tableLimiter, 100);
  robot.addAction(&limiter, 95);
  robot.addAction(&limiterFar, 90);
  robot.addAction(&backwardsLimiter, 85);
  robot.addAction(&jdAct, 50);
  
  // run the robot, true here so that the run will exit if connection lost
  robot.run(true);
  
  // now exit
  Aria::shutdown();
  return 0;
}

---