@Override
  public void setup(Simulator simulator) {
    super.setup(simulator);

    AquaticDrone drone = (AquaticDrone) simulator.getRobots().get(0);
    double x = 0, y = 0;
    drone.setPosition(x, y);
    drone.setOrientation(simulator.getRandom().nextDouble() * Math.PI * 2);
    Waypoint wp = new Waypoint("wp." + drone.getName(), CoordinateUtilities.cartesianToGPS(x, y));
    drone.getEntities().add(wp);
    drone.setActiveWaypoint(wp);
    LightPole lp = new LightPole(simulator, "wp." + drone.getName(), x, y, 5);
    addObject(lp);
  }
Beispiel #2
0
public class RealAquaticDroneCI extends Thread implements AquaticDroneCI {

  private static long CYCLE_TIME = 100; // in miliseconds

  private String status = "";
  private String initMessages = "\n";
  private IOManager ioManager;
  private ControllerMessageHandler messageHandler;

  private ConnectionListener connectionListener;
  private MotorConnectionListener motorConnectionListener;
  private CommandConnectionListener commandConnectionListener;
  private BroadcastHandler broadcastHandler;

  private List<MessageProvider> messageProviders = new ArrayList<MessageProvider>();
  private ArrayList<CISensor> cisensors = new ArrayList<CISensor>();

  private boolean run = true;
  private long startTimeInMillis;
  private double timestep = 0;
  private double leftSpeed = 0;
  private double rightSpeed = 0;
  private double behaviorTimestep = 0;

  private CIBehavior activeBehavior = null;
  private ArrayList<Entity> entities = new ArrayList<Entity>();

  private Waypoint activeWaypoint;

  private DroneType droneType = DroneType.DRONE;

  private ArrayList<CIBehavior> alwaysActiveBehaviors = new ArrayList<CIBehavior>();

  private RobotLogger logger;

  private double currentOrientation = 0;
  private double currentGPSOrientation = 0;
  private LatLon currentLatLon = null;
  private LatLon prevMeasuredLatLon = null;

  private RobotKalman kalmanFilterGPS;
  private RobotKalman kalmanFilterCompass;
  private LatLon origin = CoordinateUtilities.cartesianToGPS(0, 0);

  private double rudderHeading = 0;
  private double rudderSpeed = 0;

  @Override
  public void begin(HashMap<String, CIArguments> args) {
    this.startTimeInMillis = System.currentTimeMillis();

    addShutdownHooks();

    initIO(args.get("--io"));
    initMessageProviders();
    initConnections();

    configureArguments(args.get("--general"));

    messageHandler = new ControllerMessageHandler(this);
    messageHandler.start();

    setStatus("Running!\n");

    log(LogCodex.encodeLog(LogType.MESSAGE, initMessages));
  }

  @Override
  public void run() {
    while (run) {

      updateSensors();

      long lastCycleTime = System.currentTimeMillis();
      CIBehavior current = activeBehavior;
      try {

        if (current != null) {

          current.step(behaviorTimestep++);

          for (CIBehavior b : alwaysActiveBehaviors) b.step(timestep);

          if (current.getTerminateBehavior()) {
            stopActiveBehavior();
          }
        }

      } catch (Exception e) {
        e.printStackTrace();
      }

      ioManager.setMotorSpeeds(leftSpeed, rightSpeed);

      if (broadcastHandler != null) broadcastHandler.update(timestep);

      long timeToSleep = CYCLE_TIME - (System.currentTimeMillis() - lastCycleTime);

      if (timeToSleep > 0) {
        try {
          Thread.sleep(timeToSleep);
        } catch (InterruptedException e) {
        }
      }

      timestep++;
    }
  }

  private void addShutdownHooks() {
    Runtime.getRuntime()
        .addShutdownHook(
            new Thread() {
              public void run() {
                shutdown();
              }
            });
  }

  @Override
  public void shutdown() {
    log(LogCodex.encodeLog(LogType.MESSAGE, "Shutting down Controller..."));

    run = false;

    if (logger != null) logger.stopLogging();

    ioManager.shutdown();

    System.out.println("# Finished Controller cleanup!");
  }

  @Override
  public void reset() {
    ioManager.shutdownMotors();

    if (activeBehavior != null) {
      activeBehavior.cleanUp();
      activeBehavior = null;
      ioManager.setMotorSpeeds(leftSpeed, rightSpeed);
    }
    try {
      // make sure that the current control step is processed
      Thread.sleep(100);
    } catch (InterruptedException e) {
    }
  }

  @Override
  public void processInformationRequest(Message request, ConnectionHandler conn) {
    messageHandler.addMessage(request, conn);
  }

  // Init's
  private void initIO(CIArguments args) {
    ioManager = new IOManager(this, args);
    initMessages += ioManager.getInitMessages();
  }

  private void initConnections() {
    try {

      System.out.println("Starting connections...");

      connectionListener = new ConnectionListener(this);
      connectionListener.start();

      initMessages += "[INIT] ConnectionListener: ok\n";

      motorConnectionListener = new MotorConnectionListener(this);
      motorConnectionListener.start();

      commandConnectionListener = new CommandConnectionListener(this);
      commandConnectionListener.start();

      ArrayList<BroadcastMessage> broadcastMessages = new ArrayList<BroadcastMessage>();

      broadcastMessages.add(new HeartbeatBroadcastMessage(this));
      broadcastMessages.add(new PositionBroadcastMessage(this));
      broadcastMessages.add(new SharedDroneBroadcastMessage(this));

      broadcastHandler = new RealBroadcastHandler(this, broadcastMessages);

      initMessages += "[INIT] MotorConnectionListener: ok\n";

      log(LogCodex.encodeLog(LogType.MESSAGE, "IP " + getNetworkAddress()));
    } catch (IOException e) {
      initMessages +=
          "[INIT] Unable to start Network Connection Listeners! (" + e.getMessage() + ")\n";
    }
  }

  /**
   * Create a message provider for all the possible message provider classes like the inputs,
   * outputs, system information queries
   */
  private void initMessageProviders() {
    System.out.println("Creating Message Providers:");
    // @miguelduarte42 SystemInfoMessageProvider takes ~20 seconds and it
    // is not currently used, so I just removed it
    //		messageProviders.add(new SystemInfoMessageProvider());
    //		System.out.println("\tSystemInfoMessageProvider");
    messageProviders.add(new SystemStatusMessageProvider(this));
    System.out.println("\tSystemStatusMessageProvider");

    for (ControllerInput i : ioManager.getInputs()) {
      if (i instanceof MessageProvider) {
        messageProviders.add((MessageProvider) i);
        System.out.println("\t" + i.getClass().getSimpleName());
      }
    }

    for (ControllerOutput o : ioManager.getOutputs()) {
      if (o instanceof MessageProvider) {
        messageProviders.add((MessageProvider) o);
        System.out.println("\t" + o.getClass().getSimpleName());
      }
    }

    messageProviders.add(new EntityMessageProvider(this));
    System.out.println("\tEntityMessageProvider");
    messageProviders.add(new EntitiesMessageProvider(this));
    System.out.println("\tEntitiesMessageProvider");
    messageProviders.add(new BehaviorMessageProvider(this));
    System.out.println("\tBehaviorMessageProvider");
    messageProviders.add(new NeuralActivationsMessageProvider(this));
    System.out.println("\tNeuralActivationsMessageProvider");
    messageProviders.add(new LogMessageProvider(this));
    System.out.println("\tLogMessageProvider");
  }

  // Behaviors
  @Override
  public void startBehavior(CIBehavior b) {
    stopActiveBehavior();
    activeBehavior = b;
    behaviorTimestep = 0;
    activeBehavior.start();

    String str = "Starting CIBehavior " + activeBehavior.toString();
    log(LogCodex.encodeLog(LogType.MESSAGE, str));
  }

  @Override
  public void stopActiveBehavior() {
    if (activeBehavior != null) {
      activeBehavior.cleanUp();

      String str = "Stopping CIBehavior " + activeBehavior.toString();
      log(LogCodex.encodeLog(LogType.MESSAGE, str));

      activeBehavior = null;
      ioManager.setMotorSpeeds(leftSpeed, rightSpeed);
    }
  }

  // Setters
  @Override
  public void setLed(int index, LedState state) {
    if (ioManager.getDebugLeds() != null) {
      if (index >= 0 && index < ioManager.getDebugLeds().getNumberOfOutputs()) {
        switch (state) {
          case ON:
            ioManager.getDebugLeds().removeBlinkLed(index);
            ioManager.getDebugLeds().setValue(index, 1);
            break;
          case OFF:
            ioManager.getDebugLeds().removeBlinkLed(index);
            ioManager.getDebugLeds().setValue(index, 0);
            break;
          case BLINKING:
            ioManager.getDebugLeds().addBlinkLed(index);
            break;
        }
      } else {
        if (logger != null)
          logger.logError(
              "Invalid led index: "
                  + index
                  + ", must be >= 0 and < "
                  + ioManager.getDebugLeds().getNumberOfOutputs());
      }
    }
  }

  @Override
  public void setMotorSpeeds(double left, double right) {
    leftSpeed = left;
    rightSpeed = right;
  }

  public void setStatus(String status) {
    this.status = status;
  }

  // Getters
  @Override
  public String getNetworkAddress() {
    return NetworkUtils.getAddress();
  }

  @Override
  public BroadcastHandler getBroadcastHandler() {
    return broadcastHandler;
  }

  @Override
  public CIBehavior getActiveBehavior() {
    return activeBehavior;
  }

  @Override
  public ArrayList<CISensor> getCISensors() {
    return cisensors;
  }

  @Override
  public ArrayList<Entity> getEntities() {
    return entities;
  }

  @Override
  public String getInitMessages() {
    return initMessages;
  }

  @Override
  public List<MessageProvider> getMessageProviders() {
    return messageProviders;
  }

  @Override
  public double getCompassOrientationInDegrees() {
    return currentOrientation;
  }

  @Override
  public LatLon getGPSLatLon() {
    return currentLatLon;
  }

  @Override
  public double getGPSOrientationInDegrees() {
    return currentGPSOrientation;
  }

  private void updateSensors() {

    LatLon measuredLatLon = updateGPS();
    double measuredCompass = updateCompass();

    if (kalmanFilterGPS != null) {
      if (measuredLatLon != null) {
        // timestep < 100 so that the filtered position converges to the real position
        if (timestep < 100
            || prevMeasuredLatLon == null
            || prevMeasuredLatLon.getLat() != measuredLatLon.getLat()
            || prevMeasuredLatLon.getLon() != measuredLatLon.getLon()) {
          RobotLocation rl = kalmanFilterGPS.getEstimation(measuredLatLon, currentOrientation);
          prevMeasuredLatLon = measuredLatLon;
          currentLatLon = rl.getLatLon();
        }
      }
    } else {
      currentLatLon = measuredLatLon;
    }

    if (kalmanFilterCompass != null) {

      if (measuredCompass != -1) {
        RobotLocation rl = kalmanFilterCompass.getEstimation(origin, measuredCompass);
        currentOrientation = rl.getOrientation();
      }
    } else {
      currentOrientation = measuredCompass;
    }
  }

  private LatLon updateGPS() {
    try {

      GPSData gpsData = ioManager.getGpsModule().getReadings();

      currentGPSOrientation = gpsData.getOrientation();

      if (gpsData.getLatitudeDecimal() == 0 || gpsData.getLongitudeDecimal() == 0) return null;
      else return new LatLon(gpsData.getLatitudeDecimal(), gpsData.getLongitudeDecimal());
    } catch (Exception e) {
      currentOrientation = -1;
      return null;
    }
  }

  private double updateCompass() {
    try {
      double orientation = ioManager.getCompassModule().getHeadingInDegrees();
      return (orientation % 360.0);
    } catch (Exception e) {
      return -1;
    }
  }

  @Override
  public double getTimeSinceStart() {
    long elapsedMillis = System.currentTimeMillis() - this.startTimeInMillis;
    return ((double) elapsedMillis) / 1000.0;
  }

  @Override
  public String getStatus() {
    return status;
  }

  public IOManager getIOManager() {
    return ioManager;
  }

  public void startLogger() {
    FileLogger fileLogger = new FileLogger(this);
    fileLogger.start();
    this.logger = fileLogger;
  }

  @Override
  public Waypoint getActiveWaypoint() {
    return activeWaypoint;
  }

  @Override
  public void setActiveWaypoint(Waypoint wp) {
    this.activeWaypoint = wp;
  }

  public void log(String msg) {
    if (logger != null) logger.logMessage(msg);
  }

  @Override
  public RobotLogger getLogger() {
    return logger;
  }

  @Override
  public DroneType getDroneType() {
    return droneType;
  }

  @Override
  public void setDroneType(DroneType droneType) {
    this.droneType = droneType;
  }

  private void configureArguments(CIArguments args) {

    if (args.getArgumentIsDefined("dronetype"))
      setProperty("dronetype", args.getArgumentAsString("dronetype"));

    if (args.getFlagIsTrue("filelogger")) this.startLogger();

    if (args.getArgumentIsDefined("compassoffset") && ioManager.getCompassModule() != null)
      ioManager.getCompassModule().setOffset(args.getArgumentAsInt("compassoffset"));

    if (args.getArgumentIsDefined("changewaypoint"))
      setProperty("changewaypoint", args.getArgumentAsString("changewaypoint"));

    if (args.getArgumentIsDefined("avoiddrones"))
      setProperty("avoiddrones", args.getArgumentAsString("avoiddrones"));

    if (args.getArgumentIsDefined("avoidobstacles"))
      setProperty("avoidobstacles", args.getArgumentAsString("avoidobstacles"));

    if (args.getArgumentIsDefined("kalmanfilter"))
      setProperty("kalmanfilter", args.getArgumentAsString("kalmanfilter"));
  }

  @Override
  public double getLeftMotorSpeed() {
    return leftSpeed;
  }

  @Override
  public double getRightMotorSpeed() {
    return rightSpeed;
  }

  @Override
  public void replaceEntity(Entity e) {
    synchronized (entities) {
      entities.remove(e);
      entities.add(e);
    }
  }

  @Override
  public void setRudder(double heading, double speed) {

    this.rudderHeading = heading;
    this.rudderSpeed = speed;

    double angleInDegrees = getTurningAngleFromHeading(heading) * -1;
    double motorDifference = 0;
    double forwardComponent = 0;
    double turningComponent = 0;
    double turningSpeed = 0;

    double lw = 0;
    double rw = 0;

    if (Math.abs(heading) >= 0.9 /* || Math.abs(heading) < 0.1*/) {

      if (Math.abs(heading) >= 0.9) heading = 1.0 * Math.signum(heading);

      if (Math.abs(heading) <= 0.1) heading = 0;

      angleInDegrees = getTurningAngleFromHeading(heading) * -1;
      motorDifference = getMotorDifferenceFromTurningAngle(Math.abs(angleInDegrees));

      forwardComponent = 1.0 - motorDifference;
      turningComponent = 1.0 - forwardComponent;

      turningComponent *= speed;
      forwardComponent *= speed;

      if (heading > 0) {
        lw = turningComponent;
      } else if (heading < 0) {
        rw = turningComponent;
      } else {
        lw = forwardComponent;
        rw = forwardComponent;
      }

      angleInDegrees = getTurningAngleFromTurningSpeed(turningSpeed) * Math.signum(angleInDegrees);

    } else {

      motorDifference = getMotorDifferenceFromAngleOneFullMotor(Math.abs(angleInDegrees));

      if (heading > 0) {
        lw = 1;
        rw = 1 - motorDifference;
      } else if (heading < 0) {
        lw = 1 - motorDifference;
        rw = 1;
      }

      lw *= speed;
      rw *= speed;
    }

    if (speed < 0.01) {
      lw = 0;
      rw = 0;
    }

    setMotorSpeeds(lw, rw);
  }

  private double getMotorDifferenceFromAngleOneFullMotor(double angle) {
    return -0.0068 * Math.pow(angle, 2) + 0.1614 * angle + 0.0903;
  }

  public double getTurningAngleFromDifferenceOneMotorFull(double speedDifference) {
    return 10.564 * speedDifference - 2.0412;
  }

  public double getTurningSpeedFromMotorDifferenceOneMotorFull(double difference) {
    return (-28.958 * difference + 139.88) / 100.0;
  }

  private double getMotorDifferenceFromTurningAngle(double angle) {
    return Math.min(0.0098 * Math.pow(angle, 2) + 0.0244 * angle, 1);
  }

  private double getTurningAngleFromTurningSpeed(double turningSpeed) {
    return 7.6401 * turningSpeed;
  }

  private double getTurningAngleFromHeading(double heading) {
    return 9 * heading;
  }

  public double getRudderHeading() {
    return rudderHeading;
  }

  public double getRudderSpeed() {
    return rudderSpeed;
  }

  @Override
  public double getMotorSpeedsInPercentage() {
    return (getLeftMotorSpeed() + getRightMotorSpeed()) / 2.0;
  }

  @Override
  public void setProperty(String name, String value) {
    if (name.equals("changewaypoint")) {
      boolean found =
          findBehavior(
              ChangeWaypointCIBehavior.class, alwaysActiveBehaviors.iterator(), value.equals("0"));
      if (value.equals("1") && !found)
        alwaysActiveBehaviors.add(new ChangeWaypointCIBehavior(new CIArguments(""), this));
    }

    if (name.equals("avoiddrones")) {
      boolean found =
          findBehavior(
              AvoidDronesInstinct.class, alwaysActiveBehaviors.iterator(), value.equals("0"));
      if (value.equals("1") && !found)
        alwaysActiveBehaviors.add(new AvoidDronesInstinct(new CIArguments(""), this));
    }

    if (name.equals("avoidobstacles")) {
      boolean found =
          findBehavior(
              AvoidObstaclesInstinct.class, alwaysActiveBehaviors.iterator(), value.equals("0"));
      if (value.equals("1") && !found)
        alwaysActiveBehaviors.add(new AvoidObstaclesInstinct(new CIArguments(""), this));
    }

    if (name.equals("kalmanfilter")) {
      if (value.equals("1")) {
        kalmanFilterGPS = new RobotKalman();
        kalmanFilterCompass = new RobotKalman();
      } else {
        kalmanFilterGPS = null;
        kalmanFilterCompass = null;
      }
    }

    if (name.equals("dronetype")) droneType = DroneType.valueOf(value);
  }

  private boolean findBehavior(Class<?> c, Iterator<?> i, boolean remove) {
    boolean found = false;

    while (i.hasNext()) {
      Object current = i.next();
      if (c.isInstance(current)) {
        found = true;
        if (remove) i.remove();
        break;
      }
    }
    return found;
  }
}