Example program that activates an onboard gyro if it exists and uses its data to correct the robot pose.This program uses ArActionKeydrive and ArActionJoydrive to allow teleoperation with the keyboard or joystick, and displays gyro data. Additional keys (numbers 0-9 and letters q, w, e, r, t, y, u, i, o, p) activate preset rotation velocities.
#include "Aria.h"
#include "ArAnalogGyro.h"
class GyroTask
{
public:
~GyroTask(void) {}
void doTask(void);
protected:
bool gotGyroPacket;
};
GyroTask::GyroTask(
ArRobot *robot) :
myTaskCB(this, &GyroTask::doTask),
myPacketHandlerCB(this, &GyroTask::handlePacket),
gotGyroPacket(false)
{
myRobot = robot;
{
if (myRobot != NULL)
myRobot->attachKeyHandler(keyHandler);
else
ArLog::log(
ArLog::Terse,
"GyroTask: No robot to attach a keyHandler to, keyHandling won't work... either make your own keyHandler and drive it yourself, make a keyhandler and attach it to a robot, or give this a robot to attach to.");
}
}
void GyroTask::doTask(void)
{
printf("gyro th (mode 1 only):%8.4f encoder th:%8.4f ArRobot mixed th:%8.4f temp:%d ave:%g gyro packets:%s\n", myGyro->getHeading(), myRobot->getRawEncoderPose().getTh(), myRobot->getTh(), myGyro->getTemperature(), myGyro->getAverage(), gotGyroPacket?"received":"not received");
}
{
gotGyroPacket = true;
return true;
}
int main(int argc, char **argv)
{
GyroTask gyro(&robot);
parser.loadDefaultArguments();
{
return 1;
}
printf("This program will allow you to use a joystick or keyboard to control the robot.\nYou can use the arrow keys to drive, and the spacebar to stop.\nFor joystick control press the trigger button and then drive.\nPress escape to exit.\n");
printf("Do not have a joystick, only the arrow keys on the keyboard will work.\n");
if (!connector.connectRobot())
{
printf("Could not connect to robot... exiting\n");
return 1;
}
return 0;
}
place item last in the list
Definition: ariaTypedefs.h:71
Perform actions when keyboard keys are pressed.
Definition: ArKeyHandler.h:65
static ArKeyHandler * getKeyHandler(void)
Gets a pointer to the global key handler, if one has been set with setKeyHandler()
Definition: Aria.cpp:630
Use onboard gyro to improve the heading in an ArRobot object's pose value.
Definition: ArAnalogGyro.h:95
void setRotVel(double velocity)
Sets the rotational velocity.
Definition: ArRobot.cpp:2099
Use terse logging.
Definition: ArLog.h:61
void setHeading(double heading)
Sets the heading.
Definition: ArRobot.cpp:2080
ArTypes::UByte getID(void)
returns the ID of the packet
Definition: ArRobotPacket.cpp:81
bool comInt(unsigned char command, short int argument)
Sends a command to the robot with an int for argument.
Definition: ArRobot.cpp:5634
Connect to robot or simulator based on program command line parameters.
Definition: ArRobotConnector.h:80
static void setKeyHandler(ArKeyHandler *keyHandler)
Sets the key handler, so that other classes can find it using getKeyHandler()
Definition: Aria.cpp:624
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 exit(int exitCode=0)
Shutdown all Aria processes/threads, call exit callbacks, and exit the program.
Definition: Aria.cpp:367
static void log(LogLevel level, const char *str,...)
Log a message, with formatting and variable number of arguments.
Definition: ArLog.cpp:93
Functor for a member function with 1 parameter.
Definition: ArFunctor.h:1881
bool addKeyHandler(int keyToHandle, ArFunctor *functor)
This adds a keyhandler, when the keyToHandle is hit, functor will fire.
Definition: ArKeyHandler.cpp:144
bool joystickInited(void)
Whether the joystick is initalized or not.
Definition: ArActionJoydrive.cpp:108
bool addUserTask(const char *name, int position, ArFunctor *functor, ArTaskState::State *state=NULL)
Adds a user task to the list of synchronous taskes.
Definition: ArRobot.cpp:3021
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
int, set to 1 to enable gyro packets, 0 to disable
Definition: ArCommands.h:89
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
This action will use the joystick for input to drive the robot.
Definition: ArActionJoydrive.h:51
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
void setStopIfNoButtonPressed(bool stopIfNoButtonPressed)
Set if we'll stop if no button is pressed, otherwise just do nothing.
Definition: ArActionJoydrive.cpp:97
void run(bool stopRunIfNotConnected, bool runNonThreaded=false)
Starts the instance to do processing in this thread.
Definition: ArRobot.cpp:255
Represents the packets sent to the robot as well as those received from it.
Definition: ArRobotPacket.h:41
int, enable (1) or disable (0) motors
Definition: ArCommands.h:43