@Override
public void loadFromXML(String XML) {
try {
Document doc = DocumentHelper.parseText(XML);
// basePoint
Node node = doc.selectSingleNode("StationKeeping/basePoint/point");
ManeuverLocation loc = new ManeuverLocation();
loc.load(node.asXML());
setManeuverLocation(loc);
// Speed
Node speedNode = doc.selectSingleNode("StationKeeping/speed");
setSpeed(Double.parseDouble(speedNode.getText()));
setSpeedUnits(speedNode.valueOf("@unit"));
// Duration
setDuration(Integer.parseInt(doc.selectSingleNode("StationKeeping/duration").getText()));
// Trajectory
setRadius(
Double.parseDouble(doc.selectSingleNode("StationKeeping/trajectory/radius").getText()));
} catch (Exception e) {
NeptusLog.pub().error(this, e);
return;
}
}
@Override
public void parseIMCMessage(IMCMessage message) {
setMaxTime((int) message.getDouble("timeout"));
setSpeed(message.getDouble("speed"));
ManeuverLocation pos = new ManeuverLocation();
pos.setLatitude(Math.toDegrees(message.getDouble("lat")));
pos.setLongitude(Math.toDegrees(message.getDouble("lon")));
pos.setZ(message.getDouble("z"));
String zunits = message.getString("z_units");
if (zunits != null)
pos.setZUnits(pt.up.fe.dceg.neptus.mp.ManeuverLocation.Z_UNITS.valueOf(zunits));
setManeuverLocation(pos);
String speed_units = message.getString("speed_units");
if (speed_units.equals("METERS_PS")) setSpeedUnits("m/s");
else if (speed_units.equals("RPM")) setSpeedUnits("RPM");
else setSpeedUnits("%");
setDuration((int) message.getDouble("duration"));
setRadius(message.getDouble("radius"));
setCustomSettings(message.getTupleList("custom"));
}
public void translate(double offsetNorth, double offsetEast, double offsetDown) {
location.translatePosition(offsetNorth, offsetEast, offsetDown);
}
public void setManeuverLocation(ManeuverLocation location) {
this.location = location.clone();
}
@Override
public ManeuverLocation getEndLocation() {
return location.clone();
}