package adaptive.agents.drivers;
import adaptive.ports.PioneerCommand;
import comms.core.*;
//import adaptive.core.net.*;
import adaptive.core.*;
import java.net.*;
import java.lang.*;
import java.io.*;
/*****************************************************************************
*
*
* I took the original PBATankActuatorDriver and made it output an object
* containing the v and w commands. The input and output of this agent is an
* object of type TankCommand as defined in adaptive.ports.TankCommand
*
* This should save some wiring in the GUI. though it is probably more
* overhead, I still think it's better.
*
*
* @author Rich Malak rmalak@cs.cmu.edu
*
*
*
* REVISION HISTORY:
*
* Revision 1.1 14 feb 00 rmalak
* renamed as PioneerActuatorDriver
*
* Revision 1.0 99/6/29 rmalak
* Guts mostly taken from PBATankActuatorDrivere.PBSAgent.(PBSAgent.java:77)
* $Log: PBAPioneerActuatorDriver.java,v $
* Revision 1.1.2.5 2000/03/01 03:43:14 jrj
* fixed a little bug in how I did the state thingy
*
* Revision 1.1.2.4 2000/02/28 06:41:15 jrj
* added state field to change the RAVEname in ModuleManager. Note: if
* the state is null or that field is equal to "" it defaults to the old
* way of setting the RAVEname. I also intentionally dump a state of
* null, so moving won't be interfered with by this change.
*
*
*
****************************************************************************/
public class PBAPioneerActuatorDriver extends PBSAgent implements Driver
{
static final boolean VERBOSE = false; //set to true for velocity change messages
public final static int INPORT = 0;
public final static int OUTPORT = 0;
public final static String ACTUATOR_HEADER = new String( "JoyStick SpankMe " );
private final static String VELOCITY_TOKEN = "Velocities";
private PioneerCommand prev;
protected String raveDummy;
/**********************************************************************
Description: Required default constructor required by Agent interface.
@param none
@return nada
@exception none
**********************************************************************/
public void initialize()
{
setNumInputsOutputs( 1, 1 );
setInputPortType( PioneerCommand.class, INPORT );
setOutputPortType( PioneerCommand.class, OUTPORT );
this.setSleepTime(0);//sleep taken care of in runLoop explicitly
} /* initialize() */
/**********************************************************************
initializes output values.
@param none
@return nada
@exception none
**********************************************************************/
protected boolean initOutputVals()
{
setOutput( new PioneerCommand(0.0f, 0.0f) , OUTPORT );
return( true );
} /* initOutputVals() */
/**********************************************************************
Description: allocates data internals.
@param none
@return nada
@exception none
**********************************************************************/
protected void allocateInternals()
{
prev = new PioneerCommand();
} /* allocateInternals() */
/**********************************************************************
Description: default data internals.
@param none
@return nada
@exception none
**********************************************************************/
public void defaultInternals(){
/* get the DNS name of the localhost */
Object[] arr=(Object[])getDefaultState();
this.raveDummy=(String)arr[0];
}
public Object getDefaultState(){
Object[] state=new Object[1];
String raveDummy;
try{
String fullLocalHost = adaptive.core.net.LocalHost.getFullLocalHostName();
raveDummy = fullLocalHost.substring(0,fullLocalHost.indexOf((int) '.'));
}catch( UnknownHostException e ){
System.out.println( "Unknown localhost!!" );
raveDummy="unknown";
}
state[0]=raveDummy;
return state;
}
public Object getStateDescription(){
String[] descriptors=new String[1];
descriptors[0]=new String("Robot's RAVE name");
return descriptors;
// return new String("Robot's RAVE name");
}
/**********************************************************************
Save internal state. No real state to save so don't do anything.
@param
@return
@exception
**********************************************************************/
protected Object dumpState()
{
//I'm intentionally not dumping the state so that having the raveName in the state won't prevent moving.
return null;
}
/**********************************************************************
Description: initMessages
@param none
@return nada
@exception none
**********************************************************************/
public void initMessages() {
try {
registerForMessage( raveDummy, VELOCITY_TOKEN );
/* tell the dummy robot that I'm alive */
sendMessage( raveDummy, new Message( "COMMANDER" ) );
}
catch( UnknownHostException e )
{
System.out.println( "Unknown localhost!!" );
}
catch( IOException ioe) {
System.out.println( "Communications send error.");
System.out.println( ioe.getMessage());
}
}
/******************************************************************
*
*
* This method is called inside stop().
* We need to stop the tanks motors just in case.
*
*
* @param
*
* @return
*
* @exception
******************************************************************/
protected void endMessages(){
try {
System.out.println("Before send endMessage");
sendMessage( raveDummy,
new Message( ACTUATOR_HEADER + 0.0f + " " + 0.0f ) );
System.out.println("After send endMessage");
}
catch(IOException ioe) {
}
catch(NoPhoneNumberException npne) {
}
}
/**********************************************************************
takes a dumped object and loads it into my state.
@param none
@return true == OK, false == bad.
@exception none
**********************************************************************/
protected boolean processInternalState(Object state){
boolean retval=true;
Object[] statearr;
if (state!=null){
try{
statearr=(Object[])state;
this.raveDummy=(String)statearr[0];
if (this.raveDummy.equals("")){
defaultInternals();
}
}catch(Exception e){
e.printStackTrace();
retval=false;
defaultInternals();
}
}
else{
defaultInternals();
}
return (retval);
} /* processInternalState() */
/**********************************************************************
Description: Required main loop of the agent.
@param none
@return nada
@exception none
**********************************************************************/
public void runLoop()
{
PioneerCommand tc = new PioneerCommand();
try
{
/* determines where the robot is in the world */
tc.setCommand((PioneerCommand)getInput(INPORT));
/* let's see if these are different */
if( (tc.getV() != prev.getV()) || (tc.getW() != prev.getW()) )
{
if(VERBOSE) System.out.println("Setting velocity: "+tc.getV()+" "+tc.getW());
sendMessage( raveDummy, new Message( ACTUATOR_HEADER + tc.getV() + " " + tc.getW() ) );
prev.setCommand(tc);
}
sleep( 50 );
}
catch( Exception e )
{
}
} /* runLoop() */
/**********************************************************************
receives a message from the dummy robot.
@param
@return
@exception
**********************************************************************/
public synchronized void receiveMessage( Message m ) {
String mess = m.getMessage();
String strFloat;
int mStart, spPos;
Float vReading, wReading;
try {
if(mess.startsWith(VELOCITY_TOKEN) ) {
/* set the v and w output velocities, these are sent from dummy
right after it executes the set_velocity() command.
So these are the commanded velocities. Especially useful
when the joysticking the robot */
/* note for corrupt messages, exceptions will be thrown and
no output will be set */
/* index of char after token and space*/
mStart = VELOCITY_TOKEN.length()+1;
spPos = mess.indexOf((int)' ',mStart);
/* the v velocity as a string */
strFloat = mess.substring(mStart,spPos);
vReading = new Float(strFloat);
/* index of char after v velocity and space*/
mStart = spPos+1;
spPos = mess.indexOf((int)' ',mStart);
/* the w velocity as a string */
strFloat = mess.substring(mStart,spPos);
wReading = new Float(strFloat);
/* set outputs */
//System.out.println("Pioneer Actuator Velocities: "+vReading+" "+wReading);
setOutput( new PioneerCommand(vReading.floatValue(), wReading.floatValue()), OUTPORT);
}
}
catch(RuntimeException rte) {
/* just don't set any output values when the message is not right*/
}
}
} /* class PBAPioneerActuatorDriver */