115 lines
3.6 KiB
Java
115 lines
3.6 KiB
Java
/*
|
|
Shows how to add a task callback to ArRobot's synchronization/processing cycle
|
|
|
|
This program will just have the robot wander around, it uses some avoidance
|
|
routines, then just has a constant velocity. A sensor interpretation task callback is invoked
|
|
by the ArRobot object every cycle as it runs, which records the robot's current
|
|
pose and velocity.
|
|
|
|
In Java, to pass an ArFunctor object to Aria, you must define a subclass
|
|
of ArFunctor which overrides the invoke() method.
|
|
|
|
Note that tasks must take a small amount of time to execute, to avoid delaying the
|
|
robot cycle.
|
|
*/
|
|
|
|
import com.mobilerobots.Aria.*;
|
|
|
|
class PrintingTask extends ArFunctor
|
|
{
|
|
private ArRobot myRobot;
|
|
|
|
|
|
// Constructor. Adds our 'user task' to the given robot object.
|
|
public PrintingTask(ArRobot robot)
|
|
{
|
|
System.out.println("Java PrintingTask: adding sensor interpretation task to ArRobot...");
|
|
myRobot = robot;
|
|
myRobot.addSensorInterpTask("PrintingTask", 50, this);
|
|
}
|
|
|
|
// This method will be called when Aria invokes the task functor
|
|
public void invoke()
|
|
{
|
|
// print out some info about the robot
|
|
System.out.println("Java PrintingTask: x " + (int) myRobot.getX() +
|
|
" y " + (int) myRobot.getY() +
|
|
" th " + (int) myRobot.getTh() +
|
|
" vel " + (int) myRobot.getVel() +
|
|
" mpacs " + (int) myRobot.getMotorPacCount());
|
|
|
|
// Need sensor readings? Try myRobot.getRangeDevices() to get all
|
|
// range devices, then for each device in the list, call lockDevice(),
|
|
// getCurrentBuffer() to get a list of recent sensor reading positions, then
|
|
// unlockDevice().
|
|
}
|
|
|
|
}
|
|
|
|
|
|
public class robotSyncTaskExample
|
|
{
|
|
static {
|
|
try {
|
|
System.loadLibrary("AriaJava");
|
|
} catch(UnsatisfiedLinkError e) {
|
|
System.err.println("Native code library (libAriaJava.so or AriaJava.dll) failed to load. Make sure that it is in your library path.");
|
|
System.exit(1);
|
|
}
|
|
}
|
|
|
|
public static void main(String argv[])
|
|
{
|
|
Aria.init();
|
|
ArArgumentParser parser = new ArArgumentParser(argv);
|
|
parser.loadDefaultArguments();
|
|
ArSimpleConnector con = new ArSimpleConnector(parser);
|
|
if(!Aria.parseArgs())
|
|
{
|
|
Aria.logOptions();
|
|
System.exit(1);
|
|
}
|
|
|
|
ArRobot robot = new ArRobot();
|
|
|
|
ArSonarDevice sonar = new ArSonarDevice();
|
|
|
|
PrintingTask printingTask = new PrintingTask(robot);
|
|
|
|
// the actions we will use to wander
|
|
ArActionStallRecover recover = new ArActionStallRecover();
|
|
ArActionAvoidFront avoidFront = new ArActionAvoidFront();
|
|
ArActionConstantVelocity constantVelocity = new ArActionConstantVelocity("Constant Velocity", 400);
|
|
|
|
|
|
// add the sonar object to the robot
|
|
robot.addRangeDevice(sonar);
|
|
|
|
// open the connection to the robot; if this fails exit
|
|
if(!con.connectRobot(robot))
|
|
{
|
|
System.err.println("Could not connect to the robot.");
|
|
System.exit(1);
|
|
}
|
|
System.out.println("Java robotSyncTaskExample: Connected to the robot. (Press Ctrl-C to exit)");
|
|
|
|
|
|
// turn on the motors
|
|
robot.enableMotors();
|
|
|
|
// add the wander actions
|
|
robot.addAction(recover, 100);
|
|
robot.addAction(avoidFront, 50);
|
|
robot.addAction(constantVelocity, 25);
|
|
|
|
// Start the robot process cycle running. Each cycle, it calls the robot's
|
|
// tasks. When the PrintingTask was created above, it added a new
|
|
// task to the robot. 'true' means that if the robot connection
|
|
// is lost, then ArRobot's processing cycle ends and this call returns.
|
|
robot.run(true);
|
|
|
|
System.out.println("Java robotSyncTaskExample: Disconnected. Goodbye.");
|
|
|
|
}
|
|
}
|