@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();
}
/**
* 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);
}
}
}
}
}
}
@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;
}
}
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;
}
/** @see PropertiesProvider#getPropertiesDialogTitle() */
public String getPropertiesDialogTitle() {
return "Properties for "
+ message.getMessageType().getShortName()
+ " message, with ID "
+ message.getMessageType().getId();
}
