int AvatarSimulation::recoveryFinish() {
DataStream lds;
if ( STATE(AgentBase)->started ) {
// redo relevant start items
if ( STATE(AvatarSimulation)->execSimulationId == nilUUID ) { // don't know the simulation executive yet
UUID simExecType;
UuidFromString( (RPC_WSTR)_T(ExecutiveSimulation_UUID), &simExecType );
UUID thread = this->conversationInitiate( AvatarSimulation_CBR_convRequestExecutiveSimulationId, DDB_REQUEST_TIMEOUT );
if ( thread == nilUUID ) {
return 1;
}
lds.reset();
lds.packUUID( &simExecType );
lds.packChar( -1 );
lds.packUUID( &thread );
this->sendMessage( this->hostCon, MSG_RUNIQUEID, lds.stream(), lds.length() );
lds.unlock();
}
if ( STATE(AvatarSimulation)->simConfigured ) {
// add SimWake timeout
STATE(AvatarSimulation)->simWakeTimeout = this->addTimeout( AvatarSimulation_SIMWAKE_PERIOD, AvatarSimulation_CBR_cbSimWake );
}
}
if ( AvatarBase::recoveryFinish() )
return 1;
return 0;
}
int AvatarSimulation::conProcessMessage( spConnection con, unsigned char message, char *data, unsigned int len ) {
DataStream lds;
if ( !AvatarBase::conProcessMessage( con, message, data, len ) ) // message handled
return 0;
switch ( message ) {
case AvatarSimulation_MSGS::MSG_COLLECT_LANDMARK:
{
unsigned char code;
float x, y;
UUID initiator;
UUID thread;
lds.setData( data, len );
code = lds.unpackUChar();
x = lds.unpackFloat32();
y = lds.unpackFloat32();
lds.unpackUUID( &initiator );
lds.unpackUUID( &thread );
lds.unlock();
// ask the simulation to collect
lds.reset();
lds.packUUID( &STATE(AvatarBase)->avatarUUID );
lds.packUChar( code );
lds.packFloat32( x );
lds.packFloat32( y );
lds.packUUID( &thread );
this->sendMessageEx( this->hostCon, MSGEX(ExecutiveSimulation_MSGS,MSG_AVATAR_COLLECT_LANDMARK), lds.stream(), lds.length(), &STATE(AvatarSimulation)->execSimulationId );
lds.unlock();
// record collection
COLLECTION_TASK_INFO ct;
ct.initiator = initiator;
ct.thread = thread;
this->collectionTask.push_back( ct );
}
break;
case AvatarSimulation_MSGS::MSG_DEPOSIT_LANDMARK:
{
unsigned char code;
lds.setData( data, len );
code = lds.unpackUChar();
lds.unlock();
// update avatar capacity
lds.reset();
lds.packUUID( &STATE(AvatarBase)->avatarUUID );
lds.packInt32( DDBAVATARINFO_CARGO );
lds.packInt32( 1 );
lds.packBool( 0 ); // unload
lds.packUChar( code );
this->sendMessage( this->hostCon, MSG_DDB_AVATARSETINFO, lds.stream(), lds.length() );
lds.unlock();
}
break;
default:
return 1; // unhandled message
}
return 0;
}
int AvatarSimulation::parseAvatarOutput( DataStream *ds ) {
DataStream lds;
char evt;
while ( (evt = ds->unpackChar()) != ExecutiveSimulation_Defs::SAE_STREAM_END ) {
switch (evt) {
case ExecutiveSimulation_Defs::SAE_MOVE_FINISHED:
{
char moveId;
moveId = ds->unpackChar();
if ( moveId == STATE(AvatarSimulation)->moveId ) // make sure this is the current move
STATE(AvatarSimulation)->moveDone = true;
}
break;
case ExecutiveSimulation_Defs::SAE_POSE_UPDATE:
{
_timeb *t;
float x, y, r;
t = (_timeb *)ds->unpackData(sizeof(_timeb));
x = ds->unpackFloat32();
y = ds->unpackFloat32();
r = ds->unpackFloat32();
STATE(AvatarSimulation)->lastUpdateVelLin = ds->unpackFloat32();
STATE(AvatarSimulation)->lastUpdateVelAng = ds->unpackFloat32();
this->updateSimPos( x, y, r, t );
}
break;
case ExecutiveSimulation_Defs::SAE_SENSOR_SONAR:
{
char index;
_timeb *t;
SonarReading sr;
index = ds->unpackChar();
t = (_timeb *)ds->unpackData(sizeof(_timeb));
sr.value = ds->unpackFloat32();
if ( STATE(AvatarBase)->haveTarget ) { // only submit if we have a target
this->submitSensorSonar( &this->sonarId[index], t, &sr );
}
}
break;
case ExecutiveSimulation_Defs::SAE_SENSOR_CAMERA:
{
char index;
_timeb *t;
int dataSize;
void *data;
CameraReading cr;
cr.w = cr.h = 0;
sprintf_s( cr.format, sizeof(cr.format), "stream" );
index = ds->unpackChar();
t = (_timeb *)ds->unpackData(sizeof(_timeb));
dataSize = ds->unpackInt32();
data = ds->unpackData( dataSize );
if ( STATE(AvatarBase)->haveTarget ) { // only submit if we have a target
this->submitSensorCamera( &this->cameraId[index], t, &cr, data, dataSize );
}
if ( STATE(AvatarSimulation)->requestedImages > 0 ) { // make sure we were expecting something
STATE(AvatarSimulation)->requestedImages--;
if ( STATE(AvatarSimulation)->requestedImages == 0 ) {
this->cameraCommandFinished();
}
}
}
break;
case ExecutiveSimulation_Defs::SAE_COLLECT:
{
unsigned char code, success;
UUID thread;
code = ds->unpackUChar();
success = ds->unpackChar();
ds->unpackUUID( &thread );
if ( success ) {
// update avatar capacity
lds.reset();
lds.packUUID( &STATE(AvatarBase)->avatarUUID );
lds.packInt32( DDBAVATARINFO_CARGO );
lds.packInt32( 1 );
lds.packBool( 1 ); // load
lds.packUChar( code );
this->sendMessage( this->hostCon, MSG_DDB_AVATARSETINFO, lds.stream(), lds.length() );
lds.unlock();
// update landmark status
lds.reset();
lds.packUChar( code );
lds.packInt32( DDBLANDMARKINFO_COLLECTED );
this->sendMessage( this->hostCon, MSG_DDB_LANDMARKSETINFO, lds.stream(), lds.length() );
lds.unlock();
}
// notify initiator
if ( this->collectionTask.front().thread == thread ) {
Log.log( 0, "AvatarSimulation::parseAvatarOutput: collection attempt finished, %d", success );
lds.reset();
lds.packUUID( &thread );
lds.packChar( success );
this->sendMessage( this->hostCon, MSG_RESPONSE, lds.stream(), lds.length(), &this->collectionTask.front().initiator );
lds.unlock();
this->collectionTask.pop_front();
} else {
Log.log( 0, "AvatarSimulation::parseAvatarOutput: out of order collection!" );
// what happened?
}
}
break;
default:
Log.log( 0, "AvatarSimulation::parseAvatarOutput: unknown event %d!", evt );
return 1;
}
}
return 0;
}