Exemplo n.º 1
0
  public static LinkedHashMap<String, Collection<PayloadFingerprint>> getPayloads(PlanType plan) {
    LinkedHashMap<String, Collection<PayloadFingerprint>> payloads = new LinkedHashMap<>();

    for (Maneuver m : plan.getGraph().getAllManeuvers()) {
      payloads.put(m.getId(), getPayloads(m));
    }

    return payloads;
  }
  @Override
  protected boolean processMsgLocally(MessageInfo info, IMCMessage msg) {
    // msg.dump(System.out);
    // System.out.flush();

    ImcSystem resSys = ImcSystemsHolder.lookupSystem(systemCommId);

    if (resSys != null) {
      if (resSys.getAuthorityState() == ImcSystem.IMCAuthorityState.OFF) return false;
    }

    logMessage(info, msg);

    try {
      if (bus != null) bus.post(msg);
    } catch (Exception e1) {
      e1.printStackTrace();
    } catch (Error e1) {
      e1.printStackTrace();
    }

    imcState.setMessage(msg);

    if (resSys == null) return true;

    switch (msg.getMgid()) {
      case VehicleState.ID_STATIC:
        try {
          int errorCount = msg.getInteger("error_count");
          if (errorCount > 0) resSys.setOnErrorState(true);
          else resSys.setOnErrorState(false);

          Object errEntStr = msg.getValue("error_ents");
          if (errEntStr != null) resSys.setOnErrorStateStr(errEntStr.toString());
          else resSys.setOnErrorStateStr("");
        } catch (Exception e) {
          e.printStackTrace();
        }
        break;

      case PlanControlState.ID_STATIC:
        try {
          String planId = msg.getString("plan_id");
          String maneuver = msg.getString("man_id");
          String state = msg.getString("state");
          PlanType plan = new PlanType(null);
          plan.setId(planId + "|" + I18n.textc("Man", "Maneuver (short form)") + ":" + maneuver);
          if ("EXECUTING".equalsIgnoreCase(state)) resSys.setActivePlan(plan);
          else resSys.setActivePlan(null);
        } catch (Exception e) {
          e.printStackTrace();
        }
        break;

      case EmergencyControlState.ID_STATIC:
        try {
          String planId;
          planId = msg.getString("plan_id");

          if (planId == null) planId = msg.getString("mission_id");

          String state = msg.getString("state");
          resSys.setEmergencyPlanId(planId);
          resSys.setEmergencyStatusStr(state);
        } catch (Exception e) {
          e.printStackTrace();
        }
        break;

      case EstimatedState.ID_STATIC:
        try {
          long timeMillis = msg.getTimestampMillis();

          double lat = msg.getDouble("lat");
          double lon = msg.getDouble("lon");
          double height = msg.getDouble("height");
          msg.getDouble("depth");
          msg.getDouble("altitude");
          double x = msg.getDouble("x");
          double y = msg.getDouble("y");
          double z = msg.getDouble("z");
          double phi = msg.getDouble("phi");
          double theta = msg.getDouble("theta");
          double psi = msg.getDouble("psi");

          LocationType loc = new LocationType();
          loc.setLatitudeRads(lat);
          loc.setLongitudeRads(lon);
          loc.setHeight(height);
          loc.setOffsetNorth(x);
          loc.setOffsetEast(y);
          loc.setOffsetDown(z);
          loc.convertToAbsoluteLatLonDepth();

          if (loc != null) {
            resSys.setLocation(loc, timeMillis);
          }

          resSys.setAttitudeDegrees(
              Math.toDegrees(phi), Math.toDegrees(theta), Math.toDegrees(psi), timeMillis);

          // double u = msg.getDouble("u");
          // double v = msg.getDouble("v");
          // double w = msg.getDouble("w");
          double vx = msg.getDouble("vx");
          double vy = msg.getDouble("vy");
          double vz = msg.getDouble("vz");

          double courseRad = AngleCalc.calcAngle(0, 0, vy, vx);
          double groundSpeed = Math.sqrt(vx * vx + vy * vy);
          double verticalSpeed = vz;

          resSys.storeData(
              ImcSystem.COURSE_KEY,
              (int)
                  AngleCalc.nomalizeAngleDegrees360(
                      MathMiscUtils.round(Math.toDegrees(courseRad), 0)),
              timeMillis,
              true);
          resSys.storeData(ImcSystem.GROUND_SPEED_KEY, groundSpeed, timeMillis, true);
          resSys.storeData(ImcSystem.VERTICAL_SPEED_KEY, verticalSpeed, timeMillis, true);

          double headingRad = msg.getDouble("psi");
          resSys.storeData(
              ImcSystem.HEADING_KEY,
              (int)
                  AngleCalc.nomalizeAngleDegrees360(
                      MathMiscUtils.round(Math.toDegrees(headingRad), 0)),
              timeMillis,
              true);

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

      case SimulatedState.ID_STATIC:
        try {
          long timeMillis = msg.getTimestampMillis();
          resSys.storeData(msg.getAbbrev(), msg, timeMillis, true);
        } catch (Exception e) {
          e.printStackTrace();
        }
        break;

      case OperationalLimits.ID_STATIC:
        try {
          long timeMillis = msg.getTimestampMillis();
          resSys.storeData(msg.getAbbrev(), msg, timeMillis, true);
        } catch (Exception e) {
          e.printStackTrace();
        }
        break;

      case IndicatedSpeed.ID_STATIC:
        try {
          long timeMillis = msg.getTimestampMillis();
          double value = msg.getDouble("value");
          resSys.storeData(ImcSystem.INDICATED_SPEED_KEY, value, timeMillis, true);
        } catch (Exception e) {
          e.printStackTrace();
        }
        break;

      case TrueSpeed.ID_STATIC:
        try {
          long timeMillis = msg.getTimestampMillis();
          double value = msg.getDouble("value");
          resSys.storeData(ImcSystem.TRUE_SPEED_KEY, value, timeMillis, true);
        } catch (Exception e) {
          e.printStackTrace();
        }
        break;

      case PlanDB.ID_STATIC:
        try {
          resSys.getPlanDBControl().onMessage(info, msg);
        } catch (Exception e) {
          e.printStackTrace();
        }
        break;

      case Rpm.ID_STATIC:
        try {
          long timeMillis = msg.getTimestampMillis();
          int entityId = (Integer) msg.getHeaderValue("src_ent");
          final int value = msg.getInteger("value");
          if (entityId == 0xFF) {
            resSys.storeData(ImcSystem.RPM_MAP_ENTITY_KEY, value, timeMillis, true);
          } else {
            final String entityName = EntitiesResolver.resolveName(resSys.getName(), entityId);
            if (entityName != null) {
              Object obj = resSys.retrieveData(ImcSystem.RPM_MAP_ENTITY_KEY);
              if (obj == null) {
                Map<String, Integer> map =
                    (Map<String, Integer>)
                        Collections.synchronizedMap(new HashMap<String, Integer>());
                map.put(entityName, value);
                resSys.storeData(ImcSystem.RPM_MAP_ENTITY_KEY, map, timeMillis, true);
              } else {
                @SuppressWarnings("unchecked")
                Map<String, Integer> rpms =
                    (Map<String, Integer>) resSys.retrieveData(ImcSystem.RPM_MAP_ENTITY_KEY);
                rpms.put(entityName, value);
                resSys.storeData(ImcSystem.RPM_MAP_ENTITY_KEY, rpms, timeMillis, false);
              }
            }
          }
        } catch (Exception e) {
          e.printStackTrace();
        }
        break;

      case FuelLevel.ID_STATIC:
        try {
          long timeMillis = msg.getTimestampMillis();
          FuelLevel fuelLevelMsg = (FuelLevel) msg;
          resSys.storeData(ImcSystem.FUEL_LEVEL_KEY, fuelLevelMsg, timeMillis, true);
        } catch (Exception e) {
          e.printStackTrace();
        }
        break;

      case LblConfig.ID_STATIC:
        try {
          if (((LblConfig) msg).getOp() == OP.CUR_CFG)
            resSys.storeData(
                ImcSystem.LBL_CONFIG_KEY, (LblConfig) msg, msg.getTimestampMillis(), true);
        } catch (Exception e) {
          e.printStackTrace();
        }
        break;

      case AcousticSystems.ID_STATIC:
        try {
          long timeMillis = msg.getTimestampMillis();
          AcousticSystems acousticSystemsMsg = (AcousticSystems) msg;
          resSys.storeData(ImcSystem.ACOUSTIC_SYSTEMS, acousticSystemsMsg, timeMillis, true);
        } catch (Exception e) {
          e.printStackTrace();
        }
        break;
      default:
        break;
    }

    return true;
  }
  void generatePlans(CoverageCell[][] mat, CoverageCell first) {

    Vector<String> selectedVehicles = new Vector<String>();
    Vector<SystemsList> tmp = getConsole().getSubPanelsOfClass(SystemsList.class);

    selectedVehicles.addAll(tmp.get(0).getSelectedSystems(true));
    Object planid;

    if (selectedVehicles.size() > 1)
      planid = JOptionPane.showInputDialog(getConsole(), "Enter desired plan prefix");
    else planid = JOptionPane.showInputDialog(getConsole(), "Enter desired plan name");

    MissionType mission = getConsole().getMission();

    if (mission == null) {
      GuiUtils.errorMessage(getConsole(), "Coverage Plan Solver", "No mission has been set");
      return;
    }

    if (selectedVehicles.size() <= 1) {
      CoverageCell current = first, next = current.next;
      PlanCreator creator = new PlanCreator(mission);
      creator.setLocation(first.realWorldLoc);
      // creator.addManeuver("Goto");
      while (next != null) {
        if (next.j != current.j) {
          CoverageCell pivot = current;
          while (pivot.previous != null && pivot.previous.i == current.i) pivot = pivot.previous;
          creator.setLocation(pivot.realWorldLoc);
          creator.addManeuver("Goto");
          creator.setLocation(next.realWorldLoc);
          creator.addManeuver("Goto");
        }
        current = next;
        next = current.next;
      }

      PlanType plan = creator.getPlan();
      plan.setId(planid.toString());
      plan.setVehicle(getConsole().getMainSystem());
      mission.addPlan(plan);
      mission.save(false);
      getConsole().updateMissionListeners();
    } else {
      double distance = 0;
      CoverageCell current = first, next = current.next;
      distance += current.realWorldLoc.getDistanceInMeters(next.realWorldLoc);
      while (next != null) {
        if (next.j != current.j) {
          CoverageCell pivot = current;
          while (pivot.previous != null && pivot.previous.i == current.i) pivot = pivot.previous;
        }
        distance += current.realWorldLoc.getDistanceInMeters(next.realWorldLoc);
        current = next;
        next = current.next;
      }

      double distEach = distance / selectedVehicles.size();

      current = first;
      next = current.next;
      PlanCreator creator = new PlanCreator(mission);
      creator.setLocation(current.realWorldLoc);
      distance = 0;
      int curIndex = 0;
      while (next != null) {

        if (next.j != current.j) {
          CoverageCell pivot = current;
          while (pivot.previous != null && pivot.previous.i == current.i) pivot = pivot.previous;
          creator.setLocation(pivot.realWorldLoc);
          creator.addManeuver("Goto");

          distance += current.realWorldLoc.getDistanceInMeters(next.realWorldLoc);

          if (distance < distEach) {
            creator.setLocation(next.realWorldLoc);
            creator.addManeuver("Goto");
          }
        } else distance += current.realWorldLoc.getDistanceInMeters(next.realWorldLoc);

        if (distance > distEach) {
          creator.setLocation(current.realWorldLoc);
          creator.addManeuver("Goto");
          PlanType plan = creator.getPlan();
          plan.setVehicle(selectedVehicles.get(curIndex));
          plan.setId(planid + "_" + selectedVehicles.get(curIndex++));

          mission.addPlan(plan);
          creator = new PlanCreator(mission);
          creator.setLocation(current.realWorldLoc);
          creator.addManeuver("Goto");
          distance = 0;
        }
        current = next;
        next = current.next;
      }
      PlanType plan = creator.getPlan();
      plan.setVehicle(selectedVehicles.get(curIndex));
      plan.setId(planid + "_" + selectedVehicles.get(curIndex++));

      mission.addPlan(plan);

      mission.save(false);
      getConsole().updateMissionListeners();
    }
  }