Connects to both robot and GPS, allows teleoperation, and prints robot position and GPS data.
#include "Aria.h"
#include "ArGPS.h"
#include "ArGPSConnector.h"
#include <assert.h>
class GPSLogTask {
public:
myRobot(robot),
myGPS(gps),
myTaskFunctor(this, &GPSLogTask::doTask),
myJoyHandler(joy),
myButtonDown(false)
{
puts("RobotX\tRobotY\tRobotTh\tRobotVel\tRobotRotVel\tRobotLatVel\tLatitude\tLongitude\tAltitude\tSpeed\tGPSTimeSec\tGPSTimeMSec\tFixType\tNumSats\tPDOP\tHDOP\tVDOP\tGPSDataReceived");
}
void lock() { myMutex.lock(); }
void unlock() { myMutex.unlock(); }
protected:
void doTask()
{
if(myJoyHandler)
{
for(unsigned int b = 2; b <= myJoyHandler->getNumButtons(); ++b)
if(myJoyHandler->getButton(b)) {
if(!myButtonDown)
printf("--------------- Joystick button %d pressed.\n", b);
myButtonDown = true;
}
else
myButtonDown = false;
}
lock();
int f = myGPS->read(50);
printf("%.4f\t%.4f\t%.4f\t%.4f\t%.4f\t%.4f"
"\t%2.8f\t%2.8f\t%4.4f\t%4.4f"
"\t%lu\t%lu\t%s"
"\t%u\t%2.4f\t%2.4f\t%2.4f"
"\t%s\n",
myRobot->getX(), myRobot->getY(), myRobot->getTh(), myRobot->getVel(), myRobot->getRotVel(), (myRobot->hasLatVel())?(myRobot->getLatVel()):0,
myGPS->getLatitude(), myGPS->getLongitude(), myGPS->getAltitude(), myGPS->getSpeed(),
myGPS->getGPSPositionTimestamp().getSec(), myGPS->getGPSPositionTimestamp().getMSec(), myGPS->getFixTypeName(),
myGPS->getNumSatellitesTracked(), myGPS->getPDOP(), myGPS->getHDOP(), myGPS->getVDOP(),
((f&ArGPS::ReadUpdated)?"yes":"no")
);
unlock();
}
private:
bool myButtonDown;
};
int main(int argc, char** argv)
{
argParser.loadDefaultArguments();
{
return -1;
}
if(!connector.connectRobot(&robot))
{
return -2;
}
ArGPS *gps = gpsConnector.createGPS(&robot);
assert(gps);
{
ArLog::log(
ArLog::Terse,
"gpsRobotTaskExample: Error connecting to GPS device. Try -gpsType, -gpsPort, and/or -gpsBaud command-line arguments. Use -help for help. Exiting.");
return -3;
}
connector.setupLaser(&laser);
return 0;
}
bool haveJoystick(void)
Returns if the joystick was successfully initialized or not.
Definition: ArJoyHandler.h:87
virtual bool connect(unsigned long connectTimeout=20000)
Check that the device connection (e.g.
Definition: ArGPS.cpp:181
Compatability class used to access SICK LMS-200 laser rangefinder device in versions of ARIA prior to...
Definition: ArSick.h:48
place item last in the list
Definition: ariaTypedefs.h:71
Factory for creating GPS interface object (for any kind of GPS supported by ARIA) based on robot para...
Definition: ArGPSConnector.h:67
Interfaces to a computer joystick.
Definition: ArJoyHandler.h:77
Use terse logging.
Definition: ArLog.h:61
Legacy connector for robot and laser.
Definition: ArSimpleConnector.h:51
bool comInt(unsigned char command, short int argument)
Sends a command to the robot with an int for argument.
Definition: ArRobot.cpp:5634
virtual bool blockingConnect(void)
Connect to the laser while blocking.
Definition: ArLMS2xx.cpp:1206
void enableMotors()
Enables the motors on the robot.
Definition: ArRobot.cpp:6521
void addRangeDevice(ArRangeDevice *device)
Adds a rangeDevice to the robot's list of them, and set the ArRangeDevice object's robot pointer to t...
Definition: ArRobot.cpp:5757
static void log(LogLevel level, const char *str,...)
Log a message, with formatting and variable number of arguments.
Definition: ArLog.cpp:93
static void addExitCallback(ArFunctor *functor, int position=50)
Adds a functor to by called before program exit by Aria::exit()
Definition: Aria.cpp:419
ArJoyHandler * getJoyHandler(void)
Gets the joyHandler.
Definition: ArActionJoydrive.h:78
static void init(SigHandleMethod method=SIGHANDLE_THREAD, bool initSockets=true, bool sigHandleExitNotShutdown=true)
Initialize Aria global data struture and perform OS-specific initialization, including adding OS sign...
Definition: Aria.cpp:128
Central class for communicating with and operating the robot.
Definition: ArRobot.h:82
Parse and store program command-line arguments for use by other ARIA classes.
Definition: ArArgumentParser.h:64
static void logOptions(void)
Logs all the options for the program (Calls all the callbacks added with addLogOptionsCB())
Definition: Aria.cpp:794
Keep track of recent sonar readings from a robot as an ArRangeDevice.
Definition: ArSonarDevice.h:51
static bool init(LogType type, LogLevel level, const char *fileName="", bool logTime=false, bool alsoPrint=false, bool printThisCall=true)
Initialize the logging utility with options.
Definition: ArLog.cpp:437
Use stderr for logging.
Definition: ArLog.h:55
This action will use the joystick for input to drive the robot.
Definition: ArActionJoydrive.h:51
virtual void runAsync(void)
Run in its own thread.
Definition: ArRangeDeviceThreaded.h:58
void runAsync(bool stopRunIfNotConnected, bool runNonThreadedPacketReader=false)
Starts the instance to do processing in its own new thread.
Definition: ArRobot.cpp:301
int unlock()
Unlock the robot instance.
Definition: ArRobot.h:1272
bool addAction(ArAction *action, int priority)
Adds an action to the list with the given priority.
Definition: ArRobot.cpp:3278
static bool parseArgs(void)
Parses the arguments for the program (calls all the callbacks added with addParseArgsCB())
Definition: Aria.cpp:759
This action will use the keyboard arrow keys for input to drive the robot.
Definition: ArActionKeydrive.h:37
Functor for a member function with return value and 1 parameter.
Definition: ArFunctor.h:2776
Use normal logging.
Definition: ArLog.h:62
Action to limit the backwards motion of the robot based on range sensor readings.
Definition: ArActionLimiterBackwards.h:40
int lock()
Lock the robot instance.
Definition: ArRobot.h:1268
void setStopIfNoButtonPressed(bool stopIfNoButtonPressed)
Set if we'll stop if no button is pressed, otherwise just do nothing.
Definition: ArActionJoydrive.cpp:97
GPS Device Interface.
Definition: ArGPS.h:104
bool com(unsigned char command)
Sends a command to the robot with no arguments.
Definition: ArRobot.cpp:5621
Cross-platform mutex wrapper class.
Definition: ArMutex.h:60
Action to limit the forwards motion of the robot based on range sensor readings.
Definition: ArActionLimiterForwards.h:39
WaitState waitForRunExit(unsigned int msecs=0)
Suspend calling thread until the ArRobot run loop has exited.
Definition: ArRobot.cpp:2923