[Aria-users] new example of ArActionGoto

[Matt LaFary <XXXXXXXX>]

I've taken the old gotoTest.cpp and modified it to show how I'd intended 
people to go to multiple goals off of the same action.  I tested this 
and it works fine, though I only tested it in Linux but it should work 
in Windows.  I wrote this now instead of looking at yours because I've 
meant to write this example for a long long time.  It'll also be in the 
next Aria release in examples.

Let me know if you have problems with this one.

Matt LaFary
MobileRobots Inc
ActivMedia Robotics

Villela, Mark (AZ76) wrote:
> I’m having problems with a simple program that was intended to move a 
> robot through 4 waypoints, avoiding any obstacles that may be in the way.
> 
> Everything works well, except that the robot does not go to the last 
> waypoint (last side of the square path).
> 
>  
> 
> I’ve tried changing the last waypoint from (0, 0) to (6000, 6000) or 
> (6000, 0) with no success.  However, if the last waypoint is changed to 
> (0, 9000), the robot moves to that position as expected.
> 
>  
> 
> Any suggestions to what the problem might be?   I’ve tried this with 
> MobileSim and a P3AT with identical results.
> 
>  
> 
> Here is the code:
> 
>  
> 
>  
> 
> #include "Aria.h"
> 
>  
> 
> /*
> 
>   This program will have the robot move in a square, with a 24 meters 
> perimeter, using object avoidance routines.  When no objects are 
> present, the robot will move at a constant velocity.
> 
>  
> 
>   You can press escape while it is running to cause the program to
> 
>   stop and exit.
> 
>  
> 
> WARNING: as it is, the robot is not completing the last side of the square.
> 
> */
> 
>  
> 
> int main(int argc, char **argv)
> 
> {
> 
>   // robot
> 
>   ArRobot robot;
> 
>   // sonar, must be added to the robot
> 
>   ArSonarDevice sonar;
> 
>  
> 
>   // the actions we'll use
> 
>   ArActionStallRecover recover;
> 
>   ArActionBumpers bumpers;
> 
>   ArActionAvoidFront avoidFrontNear("Avoid Front Near", 1000, 100, 15);
> 
>   // goto the goal
> 
>   ArActionGoto gotoPose1("goto", ArPose(6000, 0));
> 
>   ArActionGoto gotoPose2("goto", ArPose(6000, 6000));
> 
>   ArActionGoto gotoPose3("goto", ArPose(0, 6000));
> 
>   ArActionGoto gotoPose4("goto", ArPose(0, 0));
> 
>  
> 
>   // Make a key handler, so that escape will shut down the program
> 
>   // cleanly
> 
>   ArKeyHandler keyHandler;
> 
>  
> 
>   // mandatory init
> 
>   Aria::init();
> 
>  
> 
>   ArSimpleConnector connector(&argc, argv);
> 
>  
> 
>   if (!connector.parseArgs() || argc > 1)
> 
>   {
> 
>     connector.logOptions();
> 
>     exit(1);
> 
>   }
> 
>  
> 
>   printf("This program will make the robot wander around, avoiding 
> obstacles.\nPress escape to exit.\n");
> 
>  
> 
>   // Add the key handler to Aria so other things can find it
> 
>   Aria::setKeyHandler(&keyHandler);
> 
>  
> 
>   // Attach the key handler to a robot now, so that it actually gets
> 
>   // some processing time so it can work, this will also make escape
> 
>   // exit
> 
>   robot.attachKeyHandler(&keyHandler);
> 
>  
> 
>   // add the sonar to the robot
> 
>   robot.addRangeDevice(&sonar);
> 
>  
> 
>   // try to connect, if we fail exit
> 
>   if (!connector.connectRobot(&robot))
> 
>   {
> 
>     printf("Could not connect to robot... exiting\n");
> 
>     Aria::shutdown();
> 
>     return 1;
> 
>   }
> 
>  
> 
>   // turn on the motors, turn off amigobot sounds
> 
>   robot.comInt(ArCommands::ENABLE, 1);
> 
>   robot.comInt(ArCommands::SOUNDTOG, 0);
> 
>  
> 
>   // add the actions
> 
>   robot.addAction(&recover, 100);
> 
>   robot.addAction(&bumpers, 75);
> 
>   robot.addAction(&avoidFrontNear, 51);
> 
>   robot.addAction(&gotoPose1, 50);
> 
>   robot.addAction(&gotoPose2, 49);
> 
>   robot.addAction(&gotoPose3, 48);
> 
>   robot.addAction(&gotoPose4, 47);
> 
>  
> 
>   // start the robot running, true so that if we lose connection the run 
> stops
> 
>   robot.run(true);
> 
>  
> 
>  
> 
>   // now exit
> 
>   Aria::shutdown();
> 
>   return 0;
> 
> }
> 
>  
> 
>  
> 
>  
> 
> Thanks,
> 
>  
> 
> Mark
> 
>  
> 
/*
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 
MobileRobots for information about a commercial version of ARIA at 
XXXXXXXX or 
MobileRobots Inc, 19 Columbia Drive, Amherst, NH 03031; 800-639-9481

*/

#include "Aria.h"

/*
  This program will just have the robot drive in a square
  demonstrating the ways you can use ArActionGoto to do more than one
  pose from the same behavior.

  You can press escape while it was running to cause the program to
  close up and exit.  
**/

int main(int argc, char **argv)
{
  // mandatory init
  Aria::init();

  // set up our parser
  ArArgumentParser parser(&argc, argv);
  // set up our simple connector
  ArSimpleConnector simpleConnector(&parser);
  
  // robot
  ArRobot robot;
  // sonar, must be added to the robot
  ArSonarDevice sonar;

  // load the default arguments 
  parser.loadDefaultArguments();

  // parse the command line... fail and print the help if the parsing fails
  // or if the help was requested
  if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
  {    
    Aria::logOptions();
    exit(1);
  }
  
  //ArLog::init(ArLog::StdOut, ArLog::Verbose);

  // Make a key handler, so that escape will shut down the program
  // cleanly
  ArKeyHandler keyHandler;

  // Add the key handler to Aria so other things can find it
  Aria::setKeyHandler(&keyHandler);

  // Attach the key handler to a robot now, so that it actually gets
  // some processing time so it can work, this will also make escape
  // exit
  robot.attachKeyHandler(&keyHandler);
  printf("You may press escape to exit\n");

  // add the sonar to the robot
  robot.addRangeDevice(&sonar);
  
  // set up the robot for connecting
  if (!simpleConnector.connectRobot(&robot))
  {
    printf("Could not connect to robot... exiting\n");
    Aria::exit(1);
  }

  // turn on the motors, turn off amigobot sounds
  robot.comInt(ArCommands::ENABLE, 1);
  robot.comInt(ArCommands::SOUNDTOG, 0);

  // 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;
  // goto the goal
  ArActionGoto gotoPose("goto");

  // add the actions
  robot.addAction(&tableLimiter, 100);
  robot.addAction(&limiter, 95);
  robot.addAction(&limiterFar, 90);
  robot.addAction(&gotoPose, 50);
  
  // start the robot running, true so that if we lose connection the run stops
  robot.runAsync(true);

  // keep track of if this is our first time through or not so that we
  // don't have to start with a goal
  bool first = true;
  // keep track of what goal we're on, so that we can go to a sequence
  // of goals in an order
  int goalNum = 0;
  while (Aria::getRunning()) 
  {
    robot.lock();
    // see if this is our first goal, which means we need to set one,
    // or see if we've achieved our goal.  It should be obvious but
    // you could extend this mechanism here to checking if the robot
    // hasn't moved or if its taken too long to get to a goal
    if (first || gotoPose.haveAchievedGoal())
    {
      first = false;
      goalNum++;
      // start again if we get past how many goals we have
      if (goalNum > 4)
	goalNum = 1;
      // set our positions for the different goals
      if (goalNum == 1)
	gotoPose.setGoal(ArPose(6000, 0));
      else if (goalNum == 2)
	gotoPose.setGoal(ArPose(6000, 6000));
      else if (goalNum == 3)
	gotoPose.setGoal(ArPose(0, 6000));
      else if (goalNum == 4)
	gotoPose.setGoal(ArPose(0, 0));

      ArLog::log(ArLog::Normal, "Going to next goal at %.0f %.0f", 
		 gotoPose.getGoal().getX(), gotoPose.getGoal().getY());


    }
    robot.unlock();
  }
  
  // now exit
  Aria::shutdown();
  return 0;
}