Java tutorial
/* Copyright 2009 Hochschule Offenburg * Klaus Dorer, Mathias Ehret, Stefan Glaser, Thomas Huber, * Simon Raffeiner, Srinivasa Ragavan, Thomas Rinklin, * Joachim Schilling, Rajit Shahi * * This file is part of magmaOffenburg. * * magmaOffenburg is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * magmaOffenburg is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with magmaOffenburg. If not, see <http://www.gnu.org/licenses/>. */ package magma.tools.benchmark.model.proxy; import java.net.Socket; import org.apache.commons.math3.geometry.euclidean.threed.Vector3D; import magma.tools.SAProxy.impl.AgentProxy; import magma.tools.benchmark.model.bench.RunInformation; import magma.util.symboltreeparser.SymbolNode; import magma.util.symboltreeparser.SymbolTreeParser; /** * Special proxy for benchmarking agents * @author kdorer */ public class BenchmarkAgentProxy extends AgentProxy { /** counts the number of cycles both legs do not have force */ private int bothLegsOffGround; /** counts the number of cycles one leg do not have force */ private int oneLegOffGround; /** counts the number of cycles both legs have force */ private int noLegOffGround; /** counts the number of cycles in which at least one leg has force */ private int legOnGround; private boolean startCount; private boolean stopCount; private boolean beaming; private final boolean allowPlayerBeaming; private final boolean isGazebo; private RunInformation runInfo; private SymbolTreeParser parser = new SymbolTreeParser(); private Vector3D groundTruthPosition = Vector3D.ZERO; private float groundTruthOrientation; private float time; public BenchmarkAgentProxy(Socket clientSocket, String ssHost, int ssPort, boolean showMessages, boolean allowPlayerBeaming, boolean isGazebo) { super(clientSocket, ssHost, ssPort, showMessages); this.allowPlayerBeaming = allowPlayerBeaming; this.isGazebo = isGazebo; beaming = true; runInfo = new RunInformation(); } public void startCount() { if (startCount) { return; } bothLegsOffGround = 0; oneLegOffGround = 0; noLegOffGround = 0; legOnGround = 0; startCount = true; stopCount = false; } public void stopCount() { stopCount = true; } public void noBeaming() { beaming = false; } /** * Called before a message from the server was forwarded to the client * @param msg the message received from the server */ @Override protected byte[] onNewServerMessage(byte[] msg) { if (isGazebo) { msg = extractGazeboGroundTruth(new String(msg)).getBytes(); } double leftPressure = 0; double rightPressure = 0; String message = new String(msg); if (isGazebo) { extractTime(message); } while (message.contains("(FRP (n lf") || message.contains("(FRP (n rf")) { Vector3D leftForce = getForce(message, "(FRP (n lf"); if (leftForce == null) { return msg; } Vector3D rightForce = getForce(message, "(FRP (n rf"); if (rightForce == null) { return msg; } leftPressure += leftForce.getNorm(); rightPressure += rightForce.getNorm(); message = message.replaceFirst("\\(FRP \\(n lf", ""); message = message.replaceFirst("\\(FRP \\(n rf", ""); } if (startCount && !stopCount) { if (leftPressure < 0.01 && rightPressure < 0.01) { bothLegsOffGround++; if (showMessages) { System.out.println("off ground"); } } else { legOnGround++; if (leftPressure >= 0.01 && rightPressure >= 0.01) { noLegOffGround++; } else { oneLegOffGround++; } } } return msg; } private String extractGazeboGroundTruth(String message) { int groundTruthIndex = message.lastIndexOf(" (mypos"); if (groundTruthIndex < 0) { return message; } String groundTruth = message.substring(groundTruthIndex).trim(); SymbolNode result = parser.parse(groundTruth); SymbolNode myPos = (SymbolNode) result.children.get(0); groundTruthPosition = new Vector3D(parseFloat(myPos, 1), parseFloat(myPos, 2), parseFloat(myPos, 3)); SymbolNode myOrien = (SymbolNode) result.children.get(1); groundTruthOrientation = parseFloat(myOrien, 1); // ground truth in gazebo is always at end return message.substring(0, groundTruthIndex); } private void extractTime(String message) { SymbolNode root = parser.parse(message); for (Object n : root.children) { SymbolNode node = (SymbolNode) n; if (node.children.get(0).equals("time")) { SymbolNode now = (SymbolNode) node.children.get(1); time = parseFloat(now, 1); break; } } } private float parseFloat(SymbolNode node, int index) { return Float.parseFloat((String) node.children.get(index)); } private Vector3D getForce(String message, String which) { int forceIndex = message.indexOf(which); if (forceIndex >= 0) { int fIndex = message.indexOf("(f", forceIndex); if (fIndex >= 0) { int eIndex = message.indexOf(")", fIndex); String forceString = message.substring(fIndex + 3, eIndex); String[] values = forceString.split(" "); if (values.length == 3) { float x = Float.valueOf(values[0]); float y = Float.valueOf(values[1]); float z = Float.valueOf(values[2]); return new Vector3D(x, y, z); } } System.out.println("Strange force message: " + message.substring(forceIndex, forceIndex + 30)); return null; } return Vector3D.ZERO; } /** * Called before a client message has been forwarded to the server * @param message the message received from the client * @return true if the message should be forwarded to the server */ @Override public byte[] onNewClientMessage(byte[] message) { if (allowPlayerBeaming) { return message; } String msgString = new String(message); if (msgString.contains("(beam")) { System.out.println("Agent may not beam! Message ignored."); if (beaming) { msgString = ""; } else { return null; } } if (beaming) { msgString += "(beam " + runInfo.getBeamX() + " " + runInfo.getBeamY() + " 0)"; message = msgString.getBytes(); } return message; } public int getBothLegsOffGround() { return bothLegsOffGround; } public int getOneLegOffGround() { return oneLegOffGround; } public int getNoLegOffGround() { return noLegOffGround; } public int getLegOnGround() { if (legOnGround < 100) { return 100; } return legOnGround; } public void updateProxy(RunInformation runInfo) { this.runInfo = runInfo; } public Vector3D getGroundTruthPosition() { return groundTruthPosition; } public float getGroundTruthOrientation() { return groundTruthOrientation; } public float getTime() { return time; } }