/*
* Player Java Client 3 - SimulationInterface.java
* Copyright (C) 2002-2006 Radu Bogdan Rusu, Maxim Batalin
*
* This program 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 2 of the License, or
* (at your option) any later version.
*
* This program 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 this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
* $Id$
*
*/
package javaclient3;
import java.io.IOException;
import java.util.logging.Level;
import java.util.logging.Logger;
import javaclient3.structures.PlayerMsgHdr;
import javaclient3.structures.PlayerPose;
import javaclient3.structures.simulation.PlayerSimulationData;
import javaclient3.structures.simulation.PlayerSimulationPose2dReq;
import javaclient3.xdr.OncRpcException;
import javaclient3.xdr.XdrBufferDecodingStream;
import javaclient3.xdr.XdrBufferEncodingStream;
/**
* Player devices may either be real hardware or virtual devices generated by
* a simulator such as Stage or Gazebo. This interface provides direct access
* to a simulator.
* <br><br>
* This interface doesn't do much yet. It is in place to later support things
* like pausing and restarting the simulation clock, saving and loading, etc.
* It is documented because it is used by the stg_simulation driver; required
* by all stageclient drivers (stg_*).
* @author Radu Bogdan Rusu
* @version
* <ul>
* <li>v2.0 - Player 2.0 supported
* </ul>
*/
public class SimulationInterface extends PlayerDevice {
private static final boolean isDebugging = PlayerClient.isDebugging;
// Logging support
private Logger logger = Logger.getLogger (SimulationInterface.class.getName ());
private PlayerSimulationData psp2ddata;
private boolean readyPsp2ddata = false;
private PlayerSimulationPose2dReq psp2dreq;
private boolean readyPsp2dreq = false;
/**
* Constructor for SimulationInterface.
* @param pc a reference to the PlayerClient object
*/
public SimulationInterface (PlayerClient pc) { super(pc); }
/**
* Read the simulation data
*/
public synchronized void readData (PlayerMsgHdr header) {
try {
this.timestamp = header.getTimestamp ();
psp2ddata = new PlayerSimulationData ();
// Buffer for reading data
byte[] buffer = new byte[4];
// Read data
is.readFully (buffer, 0, 4);
// Begin decoding the XDR buffer
XdrBufferDecodingStream xdr = new XdrBufferDecodingStream (buffer);
xdr.beginDecoding ();
psp2ddata.setData (xdr.xdrDecodeByte ());
xdr.endDecoding ();
xdr.close ();
readyPsp2ddata = true;
} catch (IOException e) {
throw new PlayerException
("[Simulation] : Error reading payload: " +
e.toString(), e);
} catch (OncRpcException e) {
throw new PlayerException
("[Simulation] : Error while XDR-decoding payload: " +
e.toString(), e);
}
}
/**
* Get the 2D pose of a named simulation object.
* @return an object of type PlayerSimulationPose2dReq containing the required pose data
*/
public PlayerSimulationPose2dReq getSimulationPose2D () { return psp2dreq; }
/**
* Check if pose data is available.
* @return true if ready, false if not ready
*/
public boolean isPose2DReady () {
if (readyPsp2dreq) {
readyPsp2dreq = false;
return true;
}
return false;
}
/**
* Get the data.
* @return an object of type PlayerSimulationData containing the required data
*/
public PlayerSimulationData getData () { return this.psp2ddata; }
/**
* Check if data is available.
* @return true if ready, false if not ready
*/
public boolean isDataReady () {
if (readyPsp2ddata) {
readyPsp2ddata = false;
return true;
}
return false;
}
/**
* Placeholder for command.
* <br><br>
* See the player_simulation_cmd structure from player.h
* @param cmd a single byte of as-yet-unspecified command.
*/
public void setPosition (byte cmd) {
try {
sendHeader (PLAYER_MSGTYPE_CMD, 0, 4);
XdrBufferEncodingStream xdr = new XdrBufferEncodingStream (4);
xdr.beginEncoding (null, 0);
xdr.xdrEncodeByte ((byte)cmd);
xdr.endEncoding ();
os.write (xdr.getXdrData (), 0, xdr.getXdrLength ());
xdr.close ();
os.flush ();
} catch (IOException e) {
throw new PlayerException
("[Simulation] : Couldn't send command: " +
e.toString(), e);
} catch (OncRpcException e) {
throw new PlayerException
("[Simulation] : Error while XDR-encoding command: " +
e.toString(), e);
}
}
/**
* Configuration request: set 2D pose of a named simulation object.
*<br><br>
* To set the pose of an object in a simulator, send a
* PLAYER_SIMULATION_REQ_SET_POSE2D request. Response will be null.
* <br><br>
* See the player_simulation_pose2d_req structure from player.h
*/
public void set2DPose (String identifier, PlayerPose pp) {
String temp = identifier;
if (identifier.length () > PLAYER_SIMULATION_IDENTIFIER_MAXLEN)
temp = identifier.substring (0, PLAYER_SIMULATION_IDENTIFIER_MAXLEN);
try {
int leftOvers = 0;
// Take care of the residual zero bytes
if ((temp.length () % 4) != 0)
leftOvers = 4 - (temp.length () % 4);
int size = 8 + 24 + temp.length () + leftOvers;
sendHeader (PLAYER_MSGTYPE_REQ, PLAYER_SIMULATION_REQ_SET_POSE2D, size);
XdrBufferEncodingStream xdr = new XdrBufferEncodingStream (8);
xdr.beginEncoding (null, 0);
xdr.xdrEncodeInt (temp.length () + 1);
xdr.xdrEncodeInt (temp.length () + 1);
xdr.endEncoding ();
os.write (xdr.getXdrData (), 0, xdr.getXdrLength ());
xdr.close ();
os.writeBytes (temp);
byte[] buf = new byte[leftOvers];
os.write (buf, 0, leftOvers);
xdr = new XdrBufferEncodingStream (24);
xdr.beginEncoding (null, 0);
xdr.xdrEncodeDouble (pp.getPx ());
xdr.xdrEncodeDouble (pp.getPy ());
xdr.xdrEncodeDouble (pp.getPa ());
xdr.endEncoding ();
os.write (xdr.getXdrData (), 0, xdr.getXdrLength ());
xdr.close ();
os.flush ();
} catch (IOException e) {
throw new PlayerException
("[Simulation] : Couldn't request " +
"PLAYER_SIMULATION_REQ_SET_POSE2D: " +e.toString(), e);
} catch (OncRpcException e) {
throw new PlayerException
("[Simulation] : Error while XDR-encoding SET_POSE2D request: "
+ e.toString(), e);
}
}
/**
* Configuration request: get 2D pose of a named simulation object.
*<br><br>
* To set or get the pose of an object in a simulator, use this message type. If the
* subtype is PLAYER_SIMULATION_GET_POSE2D, the server will attempt to locate the
* named object and reply with the same packet with (x,y,a) filled in. For all message
* subtypes, if the named object does not exist, or some other error occurs, the request
* should reply NACK.<br><br>
* See the player_simulation_pose2d_req structure from player.h
* @param identifier the identifier of the object we want to locate
*/
public void get2DPose (String identifier) {
String temp = identifier;
if (identifier.length () > PLAYER_SIMULATION_IDENTIFIER_MAXLEN)
temp = identifier.substring (0, PLAYER_SIMULATION_IDENTIFIER_MAXLEN);
try {
int leftOvers = 0;
// Take care of the residual zero bytes
if ((temp.length () % 4) != 0)
leftOvers = 4 - (temp.length () % 4);
int size = 8 + 12 + temp.length () + leftOvers;
sendHeader (PLAYER_MSGTYPE_REQ, PLAYER_SIMULATION_REQ_GET_POSE2D, size);
XdrBufferEncodingStream xdr = new XdrBufferEncodingStream (8);
xdr.beginEncoding (null, 0);
xdr.xdrEncodeInt (temp.length () + 1); // name_count
xdr.xdrEncodeInt (temp.length () + 1); // array_count
xdr.endEncoding ();
os.write (xdr.getXdrData (), 0, xdr.getXdrLength ());
xdr.close ();
os.writeBytes (temp);
byte[] buf = new byte[leftOvers];
os.write (buf, 0, leftOvers);
buf = new byte[12];
os.write (buf, 0, 12);
os.flush ();
} catch (IOException e) {
throw new PlayerException
("[Simulation] : Couldn't request " +
"PLAYER_SIMULATION_REQ_GET_POSE2D: " +e.toString(), e);
} catch (OncRpcException e) {
throw new PlayerException
("[Simulation] : Error while XDR-encoding GET_POSE2D request: "
+ e.toString(), e);
}
}
/**
* Handle acknowledgement response messages.
* @param header Player header
*/
protected void handleResponse (PlayerMsgHdr header) {
try {
switch (header.getSubtype ()) {
case PLAYER_SIMULATION_REQ_GET_POSE2D: {
psp2dreq = new PlayerSimulationPose2dReq ();
// Buffer for reading name_count
byte[] buffer = new byte[8];
// Read name_count
is.readFully (buffer, 0, 8);
// Begin decoding the XDR buffer
XdrBufferDecodingStream xdr = new XdrBufferDecodingStream (buffer);
xdr.beginDecoding ();
int nameCount = xdr.xdrDecodeInt ();
xdr.endDecoding ();
xdr.close ();
psp2dreq.setName_count (nameCount);
buffer = new byte[PLAYER_SIMULATION_IDENTIFIER_MAXLEN];
is.readFully (buffer, 0, nameCount);
String name = new String (buffer).substring (0, nameCount-1);
psp2dreq.setName (name.toCharArray());
// Take care of the residual zero bytes
if ((nameCount % 4) != 0)
is.readFully (buffer, 0, 4 - (nameCount % 4));
// Buffer for reading pose
buffer = new byte[24];
// Read pose
is.readFully (buffer, 0, 24);
PlayerPose pp = new PlayerPose ();
// Begin decoding the XDR buffer
xdr = new XdrBufferDecodingStream (buffer);
xdr.beginDecoding ();
pp.setPx (xdr.xdrDecodeDouble ());
pp.setPy (xdr.xdrDecodeDouble ());
pp.setPa (xdr.xdrDecodeDouble ());
xdr.endDecoding ();
xdr.close ();
psp2dreq.setPose (pp);
readyPsp2dreq = true;
break;
}
case PLAYER_SIMULATION_REQ_SET_POSE2D: {
break;
}
default: {
if (isDebugging)
logger.log (Level.FINEST, "[Simulation][Debug] : " +
"Unexpected response " + header.getSubtype () +
" of size = " + header.getSize ());
break;
}
}
} catch (IOException e) {
throw new PlayerException
("[Simulation] : Error reading payload: " +
e.toString(), e);
} catch (OncRpcException e) {
throw new PlayerException
("[Simulation] : Error while XDR-decoding payload: " +
e.toString(), e);
}
}
}