Example #1
0
  @Override
  public void onMessage(IMCMessage message) {
    String id = message.getSourceName();
    if (!sidePanels.containsKey(id)) {
      SideVehiclePanel sp = new SideVehiclePanel();
      sp.setVehicle(id);
      sp.setSize((int) (getWidth() * 0.666), getHeight() / (1 + sidePanels.size()));
      sp.setBorder(BorderFactory.createLineBorder(Color.red));
      sidePanels.put(id, sp);
      BackVehiclePanel bp = new BackVehiclePanel();
      bp.setVehicle(id);
      bp.setSize((int) (getWidth() * 0.333), getHeight() / (1 + sidePanels.size()));
      bp.setBorder(BorderFactory.createLineBorder(Color.red));
      backPanels.put(id, bp);
      JPanel p =
          new JPanel(new TableLayout(new double[] {0.666, 0.333}, new double[] {TableLayout.FILL}));
      p.add(sp, "0,0");
      p.add(bp, "1,0");

      add(p);
      invalidate();
      revalidate();
    }
    sidePanels.get(id).setPitch(message.getFloat("theta"));
    sidePanels.get(id).setDepth(message.getFloat("depth"));
    backPanels.get(id).setRoll(message.getFloat("phi"));
    repaint();
  }
Example #2
0
  /**
   * Testing purposes
   *
   * @param args <b>ignored</b>
   */
  public static void main(String[] args) {
    ConfigFetch.initialize();

    IMCMessage msg = new PlanControl();
    msg.dump(System.out);
    showProperties(msg, null, true);
    msg.dump(System.out);
  }
  private DataDiscretizer parseLog(String entity) {
    DataDiscretizer dd = new DataDiscretizer(10);

    IMraLog parser = logSource.getLog(messageName);
    IMraLog stateParser = logSource.getLog("EstimatedState");

    if (parser == null || stateParser == null) {
      return dd;
    }

    IMCMessage entry = parser.nextLogEntry();
    IMCMessage stateEntry = stateParser.nextLogEntry();
    LocationType ref = IMCUtils.getLocation(stateEntry).convertToAbsoluteLatLonDepth();

    entityList.clear();
    entityList.add("ALL");
    while (entry != null) {
      parser.advance(1);
      entry = parser.getCurrentEntry();
      if (entry != null) {
        String entName = logSource.getEntityName(entry.getSrc(), entry.getSrcEnt());

        if (!entityList.contains(entName)) entityList.add(entName);

        if (!entity.equalsIgnoreCase("ALL") && !entName.equalsIgnoreCase(entity)) continue;

        stateEntry = stateParser.getEntryAtOrAfter(parser.currentTimeMillis());

        LocationType loc = new LocationType();
        loc.setLatitudeRads(stateEntry.getDouble("lat"));
        loc.setLongitudeRads(stateEntry.getDouble("lon"));
        loc.translatePosition(
            stateEntry.getDouble("x"), stateEntry.getDouble("y"), stateEntry.getDouble("z"));
        double[] offsets = loc.getOffsetFrom(ref);

        double[] vals = new double[4];

        vals[0] = Double.NaN;

        if (stateEntry != null) {
          vals[0] = entry.getDouble(varName);
          dd.addPoint(offsets[1], -offsets[0], vals);
        }
      }
    }
    entCombo.setModel(new DefaultComboBoxModel<>(entityList));

    return dd;
  }
 /*
  * (non-Javadoc)
  *
  * @see pt.lsts.neptus.util.comm.manager.CommonCommBaseImplementation#triggerExtraActionOnSetActive(boolean,
  * pt.lsts.neptus.messages.listener.MessageInfo, pt.lsts.neptus.messages.IMessage)
  */
 @Override
 protected void triggerExtraActionOnSetActive(
     boolean isActive, MessageInfo info, IMCMessage message) {
   ImcSystem sys = ImcSystemsHolder.lookupSystem(getSystemCommId());
   if (sys != null) {
     if (message == null
         || !StringUtils.isTokenInList("Announce,EntityList", message.getAbbrev())) {
       if (!useActivityCounter) {
         sys.setActive(isActive);
         // NeptusLog.pub().info("<###> "+sys.getName()+": "+isActive()+"  "+(message !=
         // null?message.getAbbrevName():""));
       } else {
         // If IMCAuthorityState.OFF then we consider not active
         if (sys.getAuthorityState() == ImcSystem.IMCAuthorityState.OFF) {
           sys.setActive(false);
         } else if (!isActive) {
           sys.setActive(false);
         } else {
           activityCounter.add(System.currentTimeMillis());
           int vecSize = activityCounter.size();
           if (vecSize > 3) {
             activityCounter.remove(0);
           }
           vecSize = activityCounter.size();
           if (vecSize == 3) {
             if (activityCounter.get(2) - activityCounter.get(0) <= 3000) sys.setActive(true);
             else sys.setActive(false);
           } else {
             sys.setActive(false);
           }
         }
       }
     }
   }
 }
Example #5
0
  @Override
  public void parseIMCMessage(IMCMessage message) {
    if (!DEFAULT_ROOT_ELEMENT.equalsIgnoreCase(message.getAbbrev())) return;
    pt.lsts.imc.Elevator elev = null;
    try {
      elev = pt.lsts.imc.Elevator.clone(message);
    } catch (Exception e) {
      e.printStackTrace();
      return;
    }

    setMaxTime(elev.getTimeout());
    ManeuverLocation loc = new ManeuverLocation();
    loc.setLatitudeRads(elev.getLat());
    loc.setLongitudeRads(elev.getLon());
    loc.setZ(elev.getEndZ());
    NeptusLog.pub().info("<###> " + elev.getEndZUnits());
    //
    // loc.setZUnits(pt.lsts.neptus.mp.ManeuverLocation.Z_UNITS.valueOf(elev.getEndZUnits().toString()));
    loc.setZUnits(ManeuverLocation.Z_UNITS.valueOf(message.getString("end_z_units").toString()));
    setManeuverLocation(loc);
    startZ = (float) elev.getStartZ();
    startZUnits = ManeuverLocation.Z_UNITS.valueOf(message.getString("start_z_units").toString());
    setRadius((float) elev.getRadius());
    setSpeed(elev.getSpeed());
    setStartFromCurrentPosition((elev.getFlags() & pt.lsts.imc.Elevator.FLG_CURR_POS) != 0);
    setCustomSettings(elev.getCustom());

    switch (elev.getSpeedUnits()) {
      case RPM:
        speedUnits = "RPM";
        break;
      case METERS_PS:
        speedUnits = "m/s";
        break;
      case PERCENTAGE:
        speedUnits = "%";
        break;
      default:
        break;
    }
  }
Example #6
0
  private synchronized void writeMessageText(String message) {
    SimpleAttributeSet attrTS = new SimpleAttributeSet();
    attrTS.addAttribute(ColorConstants.Foreground, Color.DARK_GRAY);
    attrTS.addAttribute(StyleConstants.Bold, true);
    logMsg.setValue("source", "ccu");
    logMsg.setValue("destination", "ccu");

    Document doc = msgTextArea.getDocument();
    try {
      SimpleAttributeSet attr = new SimpleAttributeSet();
      attr.addAttribute(ColorConstants.Foreground, Color.blue);
      doc.insertString(doc.getLength(), "[" + getTimeStamp() + "]: ", attrTS);

      attr = new SimpleAttributeSet();
      attr.addAttribute(ColorConstants.Foreground, Color.black);
      doc.insertString(doc.getLength(), message + "\n", attr);

      msgTextArea.setCaretPosition(doc.getLength());
    } catch (Exception e) {
    }
  }
  /**
   * @param info
   * @param msg
   */
  private void logMessage(MessageInfo info, IMCMessage msg) {
    if (logReceivedMsg) {
      try {
        String noLogStr = info.getProperty(MessageInfo.NOT_TO_LOG_MSG_KEY);
        if ("true".equalsIgnoreCase(noLogStr)) {
          NeptusLog.pub().info("Skip logging message " + msg.getMessageType().getShortName() + "!");
        } else {
          String strUDP = "<UDP peer identified by IP>";

          // Pass as pub/sub directly the int ID's present on the message
          String pub = msg.getHeaderValue("src").toString();
          msg.getHeaderValue("dst").toString();
          if (strUDP.equals(pub)) pub = getSystemIdName().toUpperCase();
          //                    NeptusMessageLogger.getLogger().logMessage(pub, sub,
          // info.getTimeReceivedNanos() / 1000000, msg);
          NeptusMessageLogger.logMessage(msg);
        }
      } catch (Exception e) {
        NeptusLog.pub()
            .error("Error logging message " + msg.getMessageType().getShortName() + "!", e);
      }
    }
  }
  @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;
  }
Example #9
0
 /** @see PropertiesProvider#getPropertiesDialogTitle() */
 public String getPropertiesDialogTitle() {
   return "Properties for "
       + message.getMessageType().getShortName()
       + " message, with ID "
       + message.getMessageType().getId();
 }