Device.java

Go to the documentation of this file.

/*
 * Copyright (C) 2011 Google Inc.
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may not
 * use this file except in compliance with the License. You may obtain a copy of
 * the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations under
 * the License.
 */

package com.github.rosjava.rosjava_extras.hokuyo.scip20;

import com.google.common.base.Preconditions;

import com.github.rosjava.rosjava_extras.hokuyo.LaserScan;
import com.github.rosjava.rosjava_extras.hokuyo.LaserScanListener;
import com.github.rosjava.rosjava_extras.hokuyo.LaserScannerConfiguration;
import com.github.rosjava.rosjava_extras.hokuyo.LaserScannerDevice;


import org.apache.commons.logging.Log;
import org.apache.commons.logging.LogFactory;
import org.ros.exception.RosRuntimeException;
import org.ros.message.Time;
import org.ros.time.RemoteUptimeClock;
import org.ros.time.TimeProvider;

import java.io.BufferedInputStream;
import java.io.BufferedOutputStream;
import java.io.BufferedReader;
import java.io.BufferedWriter;
import java.io.IOException;
import java.io.InputStream;
import java.io.InputStreamReader;
import java.io.OutputStream;
import java.io.OutputStreamWriter;
import java.nio.charset.Charset;
import java.util.concurrent.Callable;

public class Device implements LaserScannerDevice {

  private static final boolean DEBUG = false;
  private static final Log log = LogFactory.getLog(Device.class);

  private static final int STREAM_BUFFER_SIZE = 8192;
  private static final String EXPECTED_SENSOR_DIAGNOSTIC = "Sensor works well.";
  private static final double DRIFT_SENSITIVITY = 0.3;
  private static final double ERROR_REDUCTION_COEFFICIENT_SENSITIVITY = 0.3;
  private static final double LATENCY_FILTER_THRESHOLD = 1.05;
  private static final int LATENCY_FILTER_SAMPLE_SIZE = 10;
  private static final int CALIBRATION_SAMPLE_SIZE = 20;
  private static final int CALIBRATION_SAMPLING_DELAY_MILLIS = 500;

  private final BufferedInputStream bufferedInputStream;
  private final BufferedReader reader;
  private final BufferedWriter writer;
  private final LaserScannerConfiguration configuration;
  private final RemoteUptimeClock remoteUptimeClock;

  public Device(InputStream inputStream, OutputStream outputStream, TimeProvider epochTimeProvider) {
    bufferedInputStream = new BufferedInputStream(inputStream, STREAM_BUFFER_SIZE);
    reader =
        new BufferedReader(new InputStreamReader(bufferedInputStream, Charset.forName("US-ASCII")));
    writer =
        new BufferedWriter(new OutputStreamWriter(new BufferedOutputStream(outputStream,
            STREAM_BUFFER_SIZE), Charset.forName("US-ASCII")));
    remoteUptimeClock =
        RemoteUptimeClock.newDefault(epochTimeProvider, new Callable<Double>() {
          @Override
          public Double call() throws Exception {
            return (double) queryUptime();
          }
        }, DRIFT_SENSITIVITY, ERROR_REDUCTION_COEFFICIENT_SENSITIVITY, LATENCY_FILTER_SAMPLE_SIZE,
            LATENCY_FILTER_THRESHOLD);
    init();
    configuration = queryConfiguration();
  }

  private void init() {
    reset();
    String sensorDiagnostic = queryState().getSensorDiagnostic();
    Preconditions.checkState(sensorDiagnostic.equals(EXPECTED_SENSOR_DIAGNOSTIC),
        "Sensor diagnostic check failed: \"" + sensorDiagnostic + "\"");
    waitUntilReady();
    remoteUptimeClock.calibrate(CALIBRATION_SAMPLE_SIZE, CALIBRATION_SAMPLING_DELAY_MILLIS);
  }

  private void waitUntilReady() {
    boolean ready = false;
    while (!ready) {
      ready = true;
      write("MD0000076800001");
      try {
        checkMdmsStatus();
      } catch (MdmsException e) {
        if (DEBUG) {
          log.info("Sensor not ready.", e);
        }
        ready = false;
      }
      checkTerminator();
    }
    Preconditions.checkState(read().equals("MD0000076800000"));
    checkMdmsStatus();
    while (true) {
      String line = read(); // Data and checksum or terminating LF
      if (line.length() == 0) {
        break;
      }
      verifyChecksum(line);
    }
  }

  @Override
  public LaserScannerConfiguration getConfiguration() {
    return configuration;
  }

  private void write(String command) {
    Preconditions.checkArgument(!command.endsWith("\n"));
    try {
      writer.write(command + "\n");
      writer.flush();
      if (DEBUG) {
        log.info("Wrote: " + command);
      }
    } catch (IOException e) {
      throw new RosRuntimeException(e);
    }
    String echo = read();
    Preconditions.checkState(echo.equals(command),
        String.format("Echo does not match command: \"%s\" != \"%s\"", echo, command));
  }

  private void checkStatus() {
    String statusAndChecksum = read();
    String status = verifyChecksum(statusAndChecksum);
    Preconditions.checkState(status.equals("00"));
  }

  private void checkMdmsStatus() {
    String statusAndChecksum = read();
    String status = verifyChecksum(statusAndChecksum);
    // NOTE(damonkohler): It's not clear in the spec that both of these status
    // codes are valid.
    if (status.equals("00") || status.equals("99")) {
      return;
    }
    throw new MdmsException(status);
  }

  private void checkTmStatus() {
    String statusAndChecksum = read();
    String status = verifyChecksum(statusAndChecksum);
    if (!(status.equals("01") || status.equals("04"))) {
      return;
    }
    throw new TmException(status);
  }

  private String read() {
    String line;
    try {
      line = reader.readLine();
    } catch (IOException e) {
      throw new RosRuntimeException(e);
    }
    if (DEBUG) {
      log.info("Read: " + line);
    }
    return line;
  }

  private String verifyChecksum(String buffer) {
    Preconditions.checkArgument(buffer.length() > 0, "Empty buffer supplied to verifyChecksum().");
    String data = buffer.substring(0, buffer.length() - 1);
    char checksum = buffer.charAt(buffer.length() - 1);
    int sum = 0;
    for (int i = 0; i < data.length(); i++) {
      sum += data.charAt(i);
    }
    if ((sum & 63) + 0x30 == checksum) {
      return data;
    }
    throw new ChecksumException();
  }

  private void reset() {
    // Exit time adjust mode.
    write("TM2");
    checkTmStatus();
    checkTerminator();

    // Reset
    write("RS");
    checkStatus();
    checkTerminator();

    // Change to SCIP2.0 mode.
    write("SCIP2.0");
    try {
      checkStatus();
    } catch (IllegalStateException e) {
      // Not all devices support this command.
      if (DEBUG) {
        log.error("Switch to SCIP 2.0 failed.", e);
      }
    }
    checkTerminator();

    // Reset
    write("RS");
    checkStatus();
    checkTerminator();
  }

  private void checkTerminator() {
    Preconditions.checkState(read().length() == 0);
  }

  private long readTimestamp() {
    return Decoder.decodeValue(verifyChecksum(read()));
  }

  @Override
  public void startScanning(final LaserScanListener listener) {
    // TODO(damonkohler): Use NodeMainExecutor ExecutorService.
    new Thread() {
      @Override
      public void run() {
        while (true) {
          String command = "MD00000768000%02d";
          write(String.format(command, 99));
          checkMdmsStatus();
          checkTerminator();
          String scansRemaining = "99";
          while (!scansRemaining.equals("00")) {
            String commandEcho = read();
            scansRemaining = commandEcho.substring(commandEcho.length() - 2);
            checkMdmsStatus();
            long timestamp = readTimestamp();
            StringBuilder data = new StringBuilder();
            boolean checksumOk = true;
            while (true) {
              String line = read(); // Data and checksum or terminating LF
              if (line.length() == 0) {
                if (checksumOk) {
                  try {
                    Time time = new Time(remoteUptimeClock.toLocalUptime(timestamp));
                    int[] ranges = Decoder.decodeValues(data.toString(), 3);
                    listener.onNewLaserScan(new LaserScan(time, ranges));
                  } catch (IllegalArgumentException e) {
                    log.error("Failed to decode scan data.", e);
                    break;
                  }
                }
                break;
              }
              try {
                data.append(verifyChecksum(line));
              } catch (ChecksumException e) {
                // NOTE(damonkohler): Even though this checksum is incorrect, we
                // continue processing the scan data so that we don't lose
                // synchronization. Once the complete laser scan has arrived, we
                // will drop it and continue with the next incoming scan.
                checksumOk = false;
                log.error("Invalid checksum.", e);
              }
            }
          }
          remoteUptimeClock.update();
        }
      }
    }.start();
  }

  private String readAndStripSemicolon() {
    String buffer = read();
    Preconditions.checkState(buffer.charAt(buffer.length() - 2) == ';');
    return buffer.substring(0, buffer.length() - 2) + buffer.charAt(buffer.length() - 1);
  }

  private LaserScannerConfiguration queryConfiguration() {
    Configuration.Builder builder = new Configuration.Builder();
    write("PP");
    checkStatus();
    builder.parseModel(verifyChecksum(readAndStripSemicolon()));
    builder.parseMinimumMeasurement(verifyChecksum(readAndStripSemicolon()));
    builder.parseMaximumMeasurement(verifyChecksum(readAndStripSemicolon()));
    builder.parseTotalSteps(verifyChecksum(readAndStripSemicolon()));
    builder.parseFirstStep(verifyChecksum(readAndStripSemicolon()));
    builder.parseLastStep(verifyChecksum(readAndStripSemicolon()));
    builder.parseFrontStep(verifyChecksum(readAndStripSemicolon()));
    builder.parseStandardMotorSpeed(verifyChecksum(readAndStripSemicolon()));
    checkTerminator();
    return builder.build();
  }

  private State queryState() {
    State.Builder builder = new State.Builder();
    write("II");
    checkStatus();
    builder.parseModel(verifyChecksum(readAndStripSemicolon()));
    builder.parseLaserIlluminationState(verifyChecksum(readAndStripSemicolon()));
    builder.parseMotorSpeed(verifyChecksum(readAndStripSemicolon()));
    builder.parseMeasurementMode(verifyChecksum(readAndStripSemicolon()));
    builder.parseBitRate(verifyChecksum(readAndStripSemicolon()));
    builder.parseTimeStamp(verifyChecksum(readAndStripSemicolon()));
    builder.parseSensorDiagnostic(verifyChecksum(readAndStripSemicolon()));
    checkTerminator();
    return builder.build();
  }

  private long queryUptime() {
    // Enter time adjust mode
    write("TM0");
    checkTmStatus();
    checkTerminator();
    // Read the current time stamp
    write("TM1");
    checkTmStatus();
    // We assume that the communication lag is symmetrical meaning that the
    // sensor's time is exactly in between the start time and the current time.
    long timestamp = readTimestamp();
    checkTerminator();
    // Leave adjust mode
    write("TM2");
    checkTmStatus();
    checkTerminator();
    return timestamp;
  }

  @Override
  public void shutdown() {
    try {
      reader.close();
    } catch (IOException e) {
      // Ignore spurious shutdown errors.
      e.printStackTrace();
    }
    try {
      writer.close();
    } catch (IOException e) {
      // Ignore spurious shutdown errors.
      e.printStackTrace();
    }
  }
}