Example #1
0
  @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);
  }
 @Override
 public void onStart(final ConnectedNode connectedNode) {
   super.onStart(connectedNode);
   this.connectedNode = connectedNode;
   saveMap();
 }