152 lines
5.1 KiB
C++
152 lines
5.1 KiB
C++
/*
|
|
Adept MobileRobots Robotics Interface for Applications (ARIA)
|
|
Copyright (C) 2004, 2005 ActivMedia Robotics LLC
|
|
Copyright (C) 2006, 2007, 2008, 2009, 2010 MobileRobots Inc.
|
|
Copyright (C) 2011, 2012, 2013 Adept Technology
|
|
|
|
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
|
|
Adept MobileRobots for information about a commercial version of ARIA at
|
|
robots@mobilerobots.com or
|
|
Adept MobileRobots, 10 Columbia Drive, Amherst, NH 03031; +1-603-881-7960
|
|
*/
|
|
#include "Aria.h"
|
|
|
|
/*
|
|
This program will let you joystick the robot around, and take logs for
|
|
the mapper while you drive, automatically...
|
|
|
|
Control:
|
|
|
|
Attach an analog joystick to the joyport on the robot, not on the
|
|
computer. Now calibrate the joystick: Leaving the stick centered,
|
|
press the trigger button for a second or so, then release it. Now
|
|
rotate the stick around its extremes two or three times, holding it in
|
|
each corner for a second or two. You are now ready to drive. You can
|
|
then drive the robot by holding the trigger and moving the joystick.
|
|
To make goals you can press the top button on the joystick (this
|
|
requires AROS1_5).
|
|
|
|
You could also attach USB joystick attached to robot computer (this
|
|
depends on having a robot equiped with an accessible usb port): To
|
|
drive the robot just press the trigger button and then move the
|
|
joystick how you wish the robot to move. You can use the throttle on
|
|
the side of the joystick to control the maximum (and hence scaled)
|
|
speed at which the robot drives. To make a goal you can press any of
|
|
the other buttons on the joystick itself (button 2, 3, or 4).
|
|
|
|
You could run this with the keyboard, but not very versatile. Use the
|
|
arrow keys to control the robot, and press g to make a goal.
|
|
*/
|
|
|
|
|
|
int main(int argc, char **argv)
|
|
{
|
|
Aria::init();
|
|
ArArgumentParser parser(&argc, argv);
|
|
parser.loadDefaultArguments();
|
|
|
|
ArRobot robot;
|
|
ArRobotConnector robotConnector(&parser, &robot);
|
|
ArLaserConnector laserConnector(&parser, &robot, &robotConnector);
|
|
ArAnalogGyro analogGyro(&robot);
|
|
|
|
// Always connect to the laser, and add half-degree increment and 180 degrees as default arguments for
|
|
// laser
|
|
parser.addDefaultArgument("-connectLaser -laserDegrees 180 -laserIncrement half");
|
|
|
|
if(!robotConnector.connectRobot())
|
|
{
|
|
ArLog::log(ArLog::Terse, "sickLagger: Could not connect to the robot.");
|
|
if(parser.checkHelpAndWarnUnparsed())
|
|
{
|
|
Aria::logOptions();
|
|
Aria::exit(1);
|
|
}
|
|
}
|
|
|
|
if(!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
|
|
{
|
|
Aria::logOptions();
|
|
Aria::exit(1);
|
|
}
|
|
|
|
ArKeyHandler keyHandler;
|
|
Aria::setKeyHandler(&keyHandler);
|
|
robot.attachKeyHandler(&keyHandler);
|
|
|
|
#ifdef WIN32
|
|
printf("Pausing 5 seconds so you can disconnect VNC if you are using it.\n");
|
|
ArUtil::sleep(5000);
|
|
#endif
|
|
|
|
|
|
std::string filename = "1scans.2d";
|
|
if (argc > 1)
|
|
filename = argv[1];
|
|
printf("Logging to file %s\n", filename.c_str());
|
|
|
|
|
|
|
|
|
|
ArActionGroupRatioDriveUnsafe group(&robot);
|
|
group.activateExclusive();
|
|
|
|
robot.runAsync(true);
|
|
|
|
if(!laserConnector.connectLasers(false, false, true))
|
|
{
|
|
ArLog::log(ArLog::Terse, "sickLogger: Error: Could not connect to laser(s). Use -help to list options.");
|
|
Aria::exit(3);
|
|
}
|
|
|
|
|
|
// Allow some time for first set of laser reading to arrive
|
|
ArUtil::sleep(500);
|
|
|
|
|
|
// enable the motors, disable amigobot sounds
|
|
robot.comInt(ArCommands::SONAR, 0);
|
|
robot.comInt(ArCommands::ENABLE, 1);
|
|
robot.comInt(ArCommands::SOUND, 32);
|
|
robot.comInt(ArCommands::SOUNDTOG, 0);
|
|
// enable the joystick driving from the one connected to the microcontroller
|
|
robot.comInt(ArCommands::JOYDRIVE, 1);
|
|
|
|
// Create the logging object
|
|
// This must be created after the robot and laser are connected so it can get
|
|
// correct parameters from them.
|
|
// The third argmument is how far the robot must travel before a new laser
|
|
// scan is logged.
|
|
// The fourth argument is how many degrees the robot must rotate before a new
|
|
// laser scan is logged. The sixth boolean argument is whether to place a goal
|
|
// when the g or G key is pressed (by adding a handler to the global
|
|
// ArKeyHandler) or when the robots joystick button is
|
|
// pressed.
|
|
ArLaser *laser = robot.findLaser(1);
|
|
if(!laser)
|
|
{
|
|
ArLog::log(ArLog::Terse, "sickLogger: Error, not connected to any lasers.");
|
|
Aria::exit(2);
|
|
}
|
|
ArLaserLogger logger(&robot, laser, 300, 25, filename.c_str(), true);
|
|
|
|
// just hang out and wait for the end
|
|
robot.waitForRunExit();
|
|
|
|
Aria::exit(0);
|
|
}
|