1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
|
/* A simple example of connecting to and driving the robot with direct
* motion commands.
*/
import com.mobilerobots.Aria.*;
public class simple {
static {
try {
System.loadLibrary("AriaJava");
} catch (UnsatisfiedLinkError e) {
System.err.println("Native code library libAriaJava failed to load. Make sure that its directory is in your library path; See javaExamples/README.txt and the chapter on Dynamic Linking Problems in the SWIG Java documentation (http://www.swig.org) for help.\n" + e);
System.exit(1);
}
}
public static void main(String argv[]) {
System.out.println("Starting Java Test");
Aria.init();
ArRobot robot = new ArRobot();
ArSimpleConnector conn = new ArSimpleConnector(argv);
if(!Aria.parseArgs())
{
Aria.logOptions();
Aria.exit(1);
}
if (!conn.connectRobot(robot))
{
System.err.println("Could not connect to robot, exiting.\n");
System.exit(1);
}
robot.runAsync(true);
robot.lock();
System.out.println("Sending command to move forward 1 meter...");
robot.enableMotors();
robot.move(1000);
robot.unlock();
System.out.println("Sleeping for 5 seconds...");
ArUtil.sleep(5000);
robot.lock();
System.out.println("Sending command to rotate 90 degrees...");
robot.setHeading(90);
robot.unlock();
System.out.println("Sleeping for 5 seconds...");
ArUtil.sleep(5000);
robot.lock();
System.out.println("Robot coords: robot.getX()=" + robot.getX() + ", robot.getY()=" + robot.getY() + ", robot.getTh()=" + robot.getTh());
ArPose p = robot.getPose();
System.out.println(" pose.getX()=" + p.getX() + ", pose.getY()=" + p.getY() + ", pose.getTh()=" + p.getTh());
double[] xout = {0};
double[] yout = {0};
double[] thout = {0};
p.getPose(xout, yout, thout);
System.out.println(" pose.getPose(): x=" + xout[0] + ", y=" + yout[0] + ", th=" + thout[0]);
robot.unlock();
robot.lock();
System.out.println("exiting.");
robot.stopRunning(true);
robot.unlock();
robot.lock();
robot.disconnect();
robot.unlock();
Aria.exit(0);
}
}
|
