@Override
public void onStart(ConnectedNode connectedNode) {
this.connectedNode = connectedNode;
super.onStart(connectedNode);
odoComputer = new OdoComputer(1.7, 14.3, connectedNode.getCurrentTime());
ultasonicPublisher = connectedNode.newPublisher(namespace.join("distance"), Float32._TYPE);
voltagePublisher = connectedNode.newPublisher(namespace.join("voltage"), Float32._TYPE);
odometryPublisher = connectedNode.newPublisher(namespace.join("odom"), Odometry._TYPE);
tfPublisher = connectedNode.newPublisher(namespace.join("tf"), TransformStamped._TYPE);
Subscriber<Twist> motorSpeedSubscriber =
connectedNode.newSubscriber(namespace.join("cmd_vel"), Twist._TYPE);
motorSpeedSubscriber.addMessageListener(
new MessageListener<Twist>() {
@Override
public void onNewMessage(Twist msg) {
double x = msg.getLinear().getX();
double y = msg.getAngular().getZ();
if (Math.abs(x) < 0.02) x = 0;
if (Math.abs(y) < 0.02) y = 0;
double s = x < 0 ? -1 : 1;
double speed = Math.sqrt(x * x + y * y) / Math.sqrt(2);
double twist = -200 * y / speed;
speed *= 100 * s;
try {
brick.MOTOR.steerMotors(NXT.Motor.A, NXT.Motor.B, (int) speed, (int) twist, 10000);
} catch (IOException e) {
NXTNode.this.connectedNode.getLog().error("Motor error", e);
}
}
});
Subscriber<Int16> toneSubscriber =
connectedNode.newSubscriber(namespace.join("tone"), Int16._TYPE);
toneSubscriber.addMessageListener(
new MessageListener<Int16>() {
@Override
public void onNewMessage(Int16 freq) {
try {
brick.SYSTEM.playTone(freq.getData(), 500);
} catch (IOException e) {
NXTNode.this.connectedNode.getLog().error("Tone error", e);
}
}
});
connectedNode
.getScheduledExecutorService()
.scheduleAtFixedRate(new SensorSample(), loopMs, loopMs, TimeUnit.MILLISECONDS);
}
