@Override
public boolean init(
String vehicleId, Loiter man, SystemPositionAndAttitude state, Object manState) {
enteredLoiter = false;
destination = new LocationType(man.getManeuverLocation());
if (man.getManeuverLocation().getZUnits() == ManeuverLocation.Z_UNITS.DEPTH)
destination.setDepth(man.getManeuverLocation().getZ());
else if (man.getManeuverLocation().getZUnits() == ManeuverLocation.Z_UNITS.ALTITUDE)
destination.setDepth(-man.getManeuverLocation().getZ());
else destination.setDepth(0);
clockwise = man.getDirection().equalsIgnoreCase("Clockwise");
radius = man.getRadius();
loiterType = man.getLoiterType();
duration = man.getLoiterDuration();
speed = man.getSpeed();
if (man.getSpeedUnits().equals("RPM")) speed = SpeedConversion.convertRpmtoMps(speed);
else if (man.getSpeedUnits().equals("%")) // convert to RPM and then to m/s
speed = SpeedConversion.convertPercentageToMps(speed);
speed = Math.min(speed, SpeedConversion.MAX_SPEED);
if (manState != null && manState instanceof Double) {
loiterTime = (Double) manState;
}
model.setState(state);
return true;
}
@Override
public void mouseClicked(MouseEvent event, StateRenderer2D source) {
if (event.getButton() == MouseEvent.BUTTON3) {
JPopupMenu popup = new JPopupMenu();
popup
.add("Generate plans locally")
.addActionListener(
new ActionListener() {
@Override
public void actionPerformed(ActionEvent arg0) {
solve();
}
});
popup
.add("Clear polygon")
.addActionListener(
new ActionListener() {
@Override
public void actionPerformed(ActionEvent arg0) {
pe = null;
vertexCount = 0;
}
});
popup
.add("Settings")
.addActionListener(
new ActionListener() {
@Override
public void actionPerformed(ActionEvent arg0) {
PropertiesEditor.editProperties(AreaCoveragePlanner.this, getConsole(), true);
}
});
popup.show(source, event.getX(), event.getY());
} else if (pe == null) {
LocationType l = source.getRealWorldLocation(event.getPoint());
pe =
new PathElement(
MapGroup.getMapGroupInstance(getConsole().getMission()), new MapType(), l);
pe.setMyColor(Color.green.brighter());
pe.setShape(true);
pe.setFinished(true);
pe.setStroke(new BasicStroke(2.0f));
pe.addPoint(0, 0, 0, false);
vertexCount = 1;
} else {
LocationType l = source.getRealWorldLocation(event.getPoint());
double[] offsets = l.getOffsetFrom(pe.getCenterLocation());
pe.addPoint(offsets[1], offsets[0], 0, false);
vertexCount++;
}
}
@Subscribe
public void consume(SimulatedState simState) {
LocationType loc =
new LocationType(Math.toDegrees(simState.getLat()), Math.toDegrees(simState.getLon()));
loc.setHeight(simState.getHeight());
loc.translatePosition(simState.getX(), simState.getY(), simState.getZ());
simulatedState =
new SystemPositionAndAttitude(
loc, simState.getPhi(), simState.getTheta(), simState.getPsi());
lastStateMillis = System.currentTimeMillis();
}
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;
}
public ImageLayer(
String name, BufferedImage img, LocationType topLeft, LocationType bottomRight) {
this.name = name;
this.topLeft = new LocationType(topLeft);
this.image = img;
this.zoom = topLeft.getOffsetFrom(bottomRight)[0] / img.getHeight();
}
@Override
public boolean load(Document doc) {
super.load(doc);
Node node;
node = doc.selectSingleNode("//depth");
if (node != null) {
setZ(Double.parseDouble(node.getText()));
if (getZ() >= 0) setZUnits(Z_UNITS.DEPTH);
else {
setZUnits(Z_UNITS.ALTITUDE);
setZ(-getZ());
}
}
node = doc.selectSingleNode("//height");
if (node != null) {
setZUnits(Z_UNITS.HEIGHT);
setZ(Double.parseDouble(node.getText()));
}
node = doc.selectSingleNode("//altitude");
if (node != null) {
setZUnits(Z_UNITS.ALTITUDE);
setZ(Double.parseDouble(node.getText()));
}
node = doc.selectSingleNode("//z");
if (node != null) setZ(Double.parseDouble(node.getText()));
node = doc.selectSingleNode("//zunits");
if (node != null) setZUnits(Z_UNITS.valueOf(node.getText()));
return true;
}
@Override
public double getDistanceTravelled(LocationType initialPosition) {
double meters =
startFromCurrentPosition ? 0 : getStartLocation().getDistanceInMeters(initialPosition);
double depthDiff =
startFromCurrentPosition ? initialPosition.getAllZ() : getStartLocation().getAllZ();
meters += depthDiff;
return meters;
}
private void drawPath(
Graphics2D g, double scaleX, double scaleY, double minX, double minY, double timeStep) {
g.setRenderingHint(RenderingHints.KEY_ANTIALIASING, RenderingHints.VALUE_ANTIALIAS_ON);
g.setTransform(new AffineTransform());
g.setColor(new Color(0, 0, 0, 10));
// IMraLog stateParser = logSource.getLog("EstimatedState");
// IMCMessage stateEntry;
LsfIterator<EstimatedState> iterator =
logSource.getLsfIndex().getIterator(EstimatedState.class);
if (timeStep > 0)
iterator =
logSource.getLsfIndex().getIterator(EstimatedState.class, (long) (timeStep * 1000));
Point2D lastPt = null;
EstimatedState state = iterator.next();
if (state == null) {
NeptusLog.pub().error("No estimatedstate messages in the log");
return;
}
LocationType ref = IMCUtils.getLocation(state).convertToAbsoluteLatLonDepth();
LinkedHashMap<Integer, Point2D> lastStates = new LinkedHashMap<>();
while (state != null) {
LocationType loc = IMCUtils.getLocation(state).convertToAbsoluteLatLonDepth();
double[] offsets = loc.getOffsetFrom(ref);
Point2D pt = new Point2D.Double((offsets[1] - minY) * scaleY, (-minX - offsets[0]) * scaleX);
if (timeStep == 0) g.setColor(new Color(0, 0, 0, 20));
else g.setColor(Color.black);
lastPt = lastStates.get(state.getSrc());
if (lastPt != null && pt != null) g.draw(new Line2D.Double(lastPt, pt));
lastStates.put(state.getSrc(), pt);
state = iterator.next();
}
}
public void setHorizontalLocation(LocationType newLoc) {
newLoc.convertToAbsoluteLatLonDepth();
loc.setLatitude(newLoc.getLatitudeAsDoubleValue());
loc.setLongitude(newLoc.getLongitudeAsDoubleValue());
reference.setLat(newLoc.getLatitudeAsDoubleValueRads());
reference.setLon(newLoc.getLongitudeAsDoubleValueRads());
latitude = newLoc.getLatitudeAsDoubleValue();
longitude = newLoc.getLongitudeAsDoubleValue();
}
@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;
}
@Override
public SystemPositionAndAttitude step(SystemPositionAndAttitude state, double timestep) {
model.setState(state);
double distToDestination = state.getPosition().getDistanceInMeters(destination);
if (loiterType.equalsIgnoreCase("circular")) {
if (distToDestination - 2 > radius) {
model.guide(
destination, speed, destination.getDepth() >= 0 ? null : -destination.getDepth());
if (!enteredLoiter) loiterTime = 0;
} else {
enteredLoiter = true;
double perpend = state.getPosition().getXYAngle(destination) + Math.PI / 2;
LocationType loc = new LocationType(state.getPosition());
loc.setDepth(destination.getDepth());
if (clockwise) loc.translatePosition(Math.cos(perpend) * -20, Math.sin(perpend) * -20, 0);
else loc.translatePosition(Math.cos(perpend) * 20, Math.sin(perpend) * 20, 0);
model.guide(loc, speed, destination.getDepth() >= 0 ? null : -destination.getDepth());
loiterTime += timestep;
}
} else {
if (distToDestination < speed * 2) loiterTime += timestep;
else
model.guide(
destination, speed, destination.getDepth() >= 0 ? null : -destination.getDepth());
}
model.advance(timestep);
if (loiterTime > duration) finished = true;
return model.getState();
}
private void solve() {
if (pe == null) {
GuiUtils.errorMessage(getConsole(), "Coverage Plan Solver", "The polygon is not valid");
return;
}
double north, east, south, west;
double[] bounds = pe.getBounds3d();
south = bounds[PathElement.SOUTH_COORD];
west = bounds[PathElement.WEST_COORD];
north = bounds[PathElement.NORTH_COORD];
east = bounds[PathElement.EAST_COORD];
CoverageCell[][] cells =
new CoverageCell[(int) ((north - south) / grid) + 1][(int) ((east - west) / grid) + 1];
for (int i = 0; i < cells.length; i++)
for (int j = 0; j < cells[i].length; j++) {
cells[i][j] = new CoverageCell();
cells[i][j].i = i;
cells[i][j].j = j;
}
int i = 0, j = 0;
int desiredCells = 0;
for (double n = south + grid / 2; n < north; n += grid) {
j = 0;
for (double e = west + grid / 2; e < east; e += grid) {
LocationType lt = new LocationType(pe.getCenterLocation());
lt.translatePosition(n, e, 0);
CoverageCell cell = cells[i][j];
cell.realWorldLoc = lt.getNewAbsoluteLatLonDepth();
if (pe.containsPoint(lt, null)) {
cell.desired = true;
desiredCells++;
}
cells[i][j] = cell;
j++;
}
i++;
}
CoverageCell initialCell = null;
i = 0;
for (j = 0; j < cells[0].length - 1 && initialCell == null; j++)
for (i = 0; i < cells.length && initialCell == null; i++)
if (cells[i][j].desired) initialCell = cells[i][j];
if (initialCell == null) {
GuiUtils.errorMessage("Polygon coverage", "Polygon area is invalid");
return;
}
CoverageCell current = initialCell;
desiredCells--;
int dir = -1;
while (desiredCells > 0) {
current.visited = true;
current.active = false;
if (dir == 1) {
if (current.i < cells.length - 1
&& cells[current.i + 1][current.j].desired == true
&& cells[current.i + 1][current.j].visited == false) {
current.next = cells[current.i + 1][current.j];
cells[current.i + 1][current.j].previous = current;
current = current.next;
current.active = true;
} else {
dir = -1;
if (current.j == cells[0].length - 1) break;
while (!cells[current.i][current.j + 1].desired && i > 0 && current.previous != null) {
current.active = false;
current = current.previous;
}
if (i == 0) break;
current.next = cells[current.i][current.j + 1];
cells[current.i][current.j + 1].previous = current;
current = current.next;
current.active = true;
}
} else {
if (current.i > 0
&& cells[current.i - 1][current.j].desired == true
&& cells[current.i - 1][current.j].visited == false) {
current.next = cells[current.i - 1][current.j];
cells[current.i - 1][current.j].previous = current;
current = current.next;
current.active = true;
} else {
dir = 1;
if (current.j == cells[0].length - 1) break;
while (current.previous != null
&& !cells[current.i][current.j + 1].desired
&& i < cells.length) {
current.active = false;
current = current.previous;
}
if (i == cells.length) break;
current.next = cells[current.i][current.j + 1];
cells[current.i][current.j + 1].previous = current;
current = current.next;
current.active = true;
}
}
desiredCells--;
}
generatePlans(cells, initialCell);
}
public LocationType getBottomRight() {
LocationType loc = new LocationType(topLeft);
loc.translatePosition(-image.getHeight() * zoom, image.getWidth() * zoom, 0);
return loc;
}
/* (non-Javadoc)
* @see javax.servlet.http.HttpServlet#doGet(javax.servlet.http.HttpServletRequest, javax.servlet.http.HttpServletResponse)
*/
@Override
protected void doGet(HttpServletRequest req, HttpServletResponse resp)
throws ServletException, IOException {
if (req.getPathInfo().equals("/location")) {
resp.setContentType("application/neptus+xml");
PrintWriter page = resp.getWriter();
LocationType start = MyState.getLocation();
page.write(start.asXML(ImcMsgManager.getManager().getLocalId() + " Location"));
page.close();
return;
} else if (req.getPathInfo().equals("/console") || req.getPathInfo().equals("/console.png")) {
if (System.currentTimeMillis() - consoleLastRTime > 10000) {
File logImages = new File("log/images");
File[] fileList =
logImages.listFiles(
new FileFilter() {
@Override
public boolean accept(File pathname) {
if (pathname.isFile()
&& "png".equalsIgnoreCase(FileUtil.getFileExtension(pathname))) return true;
else return false;
}
});
Vector<File> fileVec = new Vector<File>();
for (File f : fileList) {
fileVec.add(f);
}
Collections.sort(fileVec);
if (fileVec.size() > 0) console = fileVec.lastElement();
else console = null;
consoleLastRTime = System.currentTimeMillis();
}
if (req.getPathInfo().equals("/console.png")) {
if (console != null && console.exists() && allowConsoleExposure) {
resp.setContentType("image/png");
FileInputStream fxStream = new FileInputStream(console);
StreamUtil.copyStreamToStream(fxStream, resp.getOutputStream());
fxStream.close();
}
resp.getOutputStream().close();
} else {
resp.setContentType("text/html;charset=UTF-8");
resp.getWriter().write("<html><head><meta http-equiv='refresh' content='30'></head><body>");
if (console != null && console.exists() && allowConsoleExposure)
resp.getWriter()
.write(
"<img src=\"/localstate/console.png\" alt=\""
+ console.getName()
+ "\" "
+ "id=\""
+ console.getName()
+ " border=\"0\">");
else resp.getWriter().write("No consoles active.");
resp.getWriter().write("</body></html>");
resp.getWriter().close();
// <img src="guides.ign.com4_files/header220001101.gif" alt="IGNguides.com" usemap="#HEADER"
// width="590" border="0" height="90">
}
} else if (req.getPathInfo().equals("/")) {
resp.setContentType("text/html;charset=UTF-8");
if (!ImcMsgManager.getManager().isRunning()) {
resp.getWriter()
.write(
"<html><head><meta http-equiv='refresh' content='30'>"
+ "<body>Comms are not running.</body></html>");
resp.getWriter().close();
return;
}
ImcId16 idImc = ImcMsgManager.getManager().getLocalId();
String ret = "<html>";
ret += "<h2>Location</h2><blockquote>";
LocationType home = MyState.getLocation();
String homeLoc = home.getLatitudeAsPrettyString() + ", " + home.getLongitudeAsPrettyString();
// ret += homeLoc+"<br/>";
ret += "<a href='/localstate/location'>" + homeLoc + "</a><br/>";
ret += "</blockquote><br>";
ret += "<h2>State</h2>";
ret += "<b>ID:</b> " + idImc + "<br>";
ret +=
"<b>Services:</b> "
+ ImcMsgManager.getManager().getAllServicesString().replaceAll(";", ";<br>")
+ "<br><br>";
ret += "<b>Comms info:</b><br>" + ImcMsgManager.getManager().getCommStatusAsHtmlFragment();
if (System.currentTimeMillis() - ImcMsgManager.getManager().getAnnounceLastArriveTime()
> DateTimeUtil.MINUTE * 5) {
ret += "<b color='red'>Announce not arriving for some time</b><br>";
}
ret += "</html>";
resp.getWriter().write(ret);
resp.getWriter().close();
}
}