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();
}
}