// Function: cmptShutdown
// Access: Public
// Description: This function allows the abstracted component functionality contained in this file to be stoped from an external source.
// If the component is in the running state, this function will terminate all threads running in this file
// This function will also close the Jms connection to the Node Manager and check out the component from the Node Manager
int cmptShutdown(void)
{
double timeOutSec;
if(cmpt->state != JAUS_SHUTDOWN_STATE) // Execute the shutdown routines only if the component is running
{
cmptRun = FALSE;
timeOutSec = getTimeSeconds() + CMPT_THREAD_TIMEOUT_SEC;
while(cmptThreadRunning)
{
usleep(100000);
if(getTimeSeconds() >= timeOutSec)
{
pthread_cancel(cmptThreadId);
cmptThreadRunning = FALSE;
cError("cmpt: cmptThread Shutdown Improperly\n");
break;
}
}
nodeManagerClose(cmptNmi); // Close Node Manager Connection
jausSubsystemDestroy(cmpt->node->subsystem);
jausNodeDestroy(cmpt->node);
jausServicesDestroy(cmpt->services);
cmpt->state = JAUS_SHUTDOWN_STATE;
propertiesDestroy(cmptProperties);
}
return 0;
}
// Function: wdShutdown
// Access: Public
// Description: This function allows the abstracted component functionality contained in this file to be stoped from an external source.
// If the component is in the running state, this function will terminate all threads running in this file
// This function will also close the Jms connection to the Node Manager and check out the component from the Node Manager
int wdShutdown(void)
{
double timeOutSec;
if(wd && wd->state != JAUS_SHUTDOWN_STATE) // Execute the shutdown routines only if the component is running
{
wdRun = FALSE;
timeOutSec = getTimeSeconds() + WD_THREAD_TIMEOUT_SEC;
while(wdThreadRunning)
{
SLEEP_MS(1000);
if(getTimeSeconds() >= timeOutSec)
{
pthread_cancel(wdThreadId);
wdThreadRunning = FALSE;
//cError("wd: wdThread Shutdown Improperly\n");
break;
}
}
nodeManagerClose(wdNmi); // Close Node Manager Connection
jausSubsystemDestroy(wd->node->subsystem);
jausNodeDestroy(wd->node);
jausServicesDestroy(wd->services);
wd->state = JAUS_SHUTDOWN_STATE;
propertiesDestroy(wdProperties);
}
return 0;
}
int scManagerUpdateServiceConnection(ServiceConnection sc, unsigned short sequenceNumber)
{
int returnValue = JAUS_FALSE;
sc->lastSentTime = getTimeSeconds();
if(sequenceNumber == sc->sequenceNumber)
{
returnValue = SC_ERROR_SEQUENCE_NUMBERS_EQUAL;
}
else if(sequenceNumber == 0 && sc->sequenceNumber != 65535)
{
returnValue = SC_ERROR_SEQUENCE_NUMBER_OUT_OF_SYNC;
}
else if(sequenceNumber - sc->sequenceNumber != 1 && sequenceNumber !=0)
{
returnValue = SC_ERROR_SEQUENCE_NUMBER_OUT_OF_SYNC;
}
else
{
returnValue = JAUS_TRUE;
}
sc->sequenceNumber = sequenceNumber;
return returnValue;
}
void scManagerProcessConfirmScMessage(NodeManagerInterface nmi, ConfirmServiceConnectionMessage message)
{
ServiceConnection prevSc = NULL;
ServiceConnection sc = nmi->scm->incommingSc;
TerminateServiceConnectionMessage terminateSc;
JausMessage txMessage;
while(sc)
{
if( sc->commandCode == message->serviceConnectionCommandCode && sc->address->id == message->source->id )
{
if( message->responseCode == JAUS_SC_SUCCESSFUL )
{
sc->confirmedUpdateRateHz = message->confirmedPeriodicUpdateRateHertz;
sc->instanceId = message->instanceId;
sc->isActive = JAUS_TRUE;
sc->sequenceNumber = 65535;
sc->lastSentTime = getTimeSeconds();
}
else
{
// Set SC Inactive
sc->isActive = JAUS_FALSE;
// Remove Service Connection
if(prevSc)
{
prevSc->nextSc = sc->nextSc;
sc->nextSc = NULL;
}
else
{
nmi->scm->incommingSc = sc->nextSc;
sc->nextSc = NULL;
}
nmi->scm->incommingScCount--;
}
return;
}
prevSc = sc;
sc = sc->nextSc;
}
// The SC was not found, so send a terminate to prevent streaming
if( message->responseCode == JAUS_SC_SUCCESSFUL )
{
terminateSc = terminateServiceConnectionMessageCreate();
terminateSc->source->id = nmi->cmpt->address->id;
terminateSc->destination->id = message->source->id;
terminateSc->serviceConnectionCommandCode = message->serviceConnectionCommandCode;
terminateSc->instanceId = message->instanceId;
txMessage = terminateServiceConnectionMessageToJausMessage(terminateSc);
nodeManagerSend(nmi, txMessage);
jausMessageDestroy(txMessage);
terminateServiceConnectionMessageDestroy(terminateSc);
}
}
bool InfluenceWander::update()
{
if( !Parent::update() ){
return false;
}
//---choose new direction ever so often----------------------
if(mTimeOut<getTimeSeconds()){
onExpired();
}
return true;
}
JausBoolean scManagerReceiveServiceConnection(NodeManagerInterface nmi, ServiceConnection requestSc, JausMessage *message)
{
ServiceConnection prevSc;
ServiceConnection sc = nmi->scm->incommingSc;
prevSc = NULL;
while(sc)
{
if(sc->commandCode == requestSc->commandCode && sc->address->id == requestSc->address->id )
{
if(getTimeSeconds() > (sc->lastSentTime + sc->timeoutSec))
{
// Connection has Timed Out
sc->isActive = JAUS_FALSE;
while(sc->queue->size)
{
jausMessageDestroy(queuePop(sc->queue));
}
// Remove Service Connection
if(prevSc)
{
prevSc->nextSc = sc->nextSc;
sc->nextSc = NULL;
}
else
{
nmi->scm->incommingSc = sc->nextSc;
sc->nextSc = NULL;
}
nmi->scm->incommingScCount--;
return JAUS_FALSE;
}
if(sc->queue->size)
{
*message = (JausMessage)queuePop(sc->queue);
return JAUS_TRUE;
}
else
{
return JAUS_FALSE;
}
}
else
{
prevSc = sc;
sc = sc->nextSc;
}
}
cError( "libnodeManager: scManagerServiceConnectionReceive: Requested SC does not exist\n" );
return JAUS_FALSE;
}
// Function: vehicleSimShutdown
// Access: Public
// Description: This function allows the abstracted component functionality contained in this file to be stoped from an external source.
// If the component is in the running state, this function will terminate all threads running in this file
// This function will also close the Jms connection to the Node Manager and check out the component from the Node Manager
int vehicleSimShutdown(void)
{
double timeOutSec;
vehicleSimRun = FALSE;
timeOutSec = getTimeSeconds() + VEHICLE_SIM_THREAD_TIMEOUT_SEC;
while(vehicleSimThreadRunning)
{
usleep(100000);
if(getTimeSeconds() >= timeOutSec)
{
pthread_cancel(vehicleSimThreadId);
vehicleSimThreadRunning = FALSE;
cError("vehicleSim: vehicleSimThread Shutdown Improperly\n");
break;
}
}
propertiesDestroy(vehicleSimProperties);
return 0;
}
ServiceConnection scManagerGetSendList(NodeManagerInterface nmi, unsigned short commandCode)
{
SupportedScMessage supportedScMsg;
ServiceConnection sc;
ServiceConnection newSc = NULL;
ServiceConnection firstSc = NULL;
double currentTime = getTimeSeconds();
// find the SC object associated with this command code
supportedScMsg = scFindSupportedScMsgInList(nmi->scm->supportedScMsgList, commandCode);
if(supportedScMsg)
{
sc = supportedScMsg->scList;
}
else
{
return NULL;
}
while(sc)
{
// Check for update rate
if(sc->isActive && sc->lastSentTime < (currentTime - 1.0/sc->confirmedUpdateRateHz))
{
sc->lastSentTime = currentTime;
if(newSc == NULL)
{
newSc = (ServiceConnection)malloc(sizeof(ServiceConnectionStruct));
firstSc = newSc;
}
else
{
newSc->nextSc = (ServiceConnection)malloc(sizeof(ServiceConnectionStruct));
newSc = newSc->nextSc;
}
*newSc = *sc;
newSc->nextSc = NULL;
sc->sequenceNumber++;
}
sc = sc->nextSc;
}
return firstSc;
}
void scManagerReceiveMessage(NodeManagerInterface nmi, JausMessage message)
{
ServiceConnection sc = nmi->scm->incommingSc;
char string[32] = {0};
while(sc)
{
if(sc->commandCode == message->commandCode && sc->address->id == message->source->id )
{
if(sc->isActive)
{
sc->lastSentTime = getTimeSeconds();
if(sc->queueSize && sc->queueSize == sc->queue->size)
{
jausMessageDestroy(queuePop(sc->queue));
queuePush(sc->queue, (void *)message);
}
else
{
queuePush(sc->queue, (void *)message);
}
// cDebug(3, "Queue Size: %d\n", sc->queue->size);
}
else
{
// TODO: Error? received a message for inactive SC
jausMessageDestroy(message);
}
return;
}
sc = sc->nextSc;
}
jausAddressToString(message->source, string);
cError("libnodeManager: scManangerReceiveMessage: No SC for: %s, from: %s\n", jausMessageCommandCodeString(message), string);
jausMessageDestroy(message);
}
void InfluenceWander::init(AIFishPlayer *_fish, const char *_name)
{
Parent::init(_fish, _name);
mDirection = mFish->getHeading();
mTimeOut = gRandGen.randF(mFish->mDataBlock->wanderTimeMin, mFish->mDataBlock->wanderTimeMax) + getTimeSeconds();
mTurnSpeed = mFish->mDataBlock->wanderTurnScale;
mAlarm.setInterval(0.1f, true);
}
void InfluenceWander::reset(Point3F *d)
{
mTimeOut = gRandGen.randF(mFish->mDataBlock->wanderTimeMin, mFish->mDataBlock->wanderTimeMax) + getTimeSeconds();
mDirection = randomVectorRotation(d? *d : mFish->getHeading());
mMoveSpeed = gRandGen.randF( mFish->mDataBlock->wanderSpeedScaleLo,
mFish->mDataBlock->wanderSpeedScaleHi);
}
// Function: cmptThread
// Access: Private
// Description: All core component functionality is contained in this thread.
// All of the JAUS component state machine code can be found here.
void *cmptThread(void *threadData)
{
JausMessage rxMessage;
double time, prevTime, nextExcecuteTime = 0.0;
struct timespec sleepTime;
cmptThreadRunning = TRUE;
sleepTime.tv_sec = 0;
sleepTime.tv_nsec = 1000;
time = getTimeSeconds();
cmpt->state = JAUS_INITIALIZE_STATE; // Set JAUS state to INITIALIZE
cmptStartupState();
while(cmptRun) // Execute state machine code while not in the SHUTDOWN state
{
do
{
if(nodeManagerReceive(cmptNmi, &rxMessage))
{
cDebug(4, "CMPT: Got message: %s from %d.%d.%d.%d\n", jausMessageCommandCodeString(rxMessage), rxMessage->source->subsystem, rxMessage->source->node, rxMessage->source->component, rxMessage->source->instance);
cmptProcessMessage(rxMessage);
}
else
{
if(getTimeSeconds() > nextExcecuteTime)
{
break;
}
else
{
nanosleep(&sleepTime, NULL);
}
}
}while(getTimeSeconds() < nextExcecuteTime);
prevTime = time;
time = getTimeSeconds();
cmptThreadHz = 1.0/(time-prevTime); // Compute the update rate of this thread
switch(cmpt->state) // Switch component behavior based on which state the machine is in
{
case JAUS_INITIALIZE_STATE:
cmptInitState();
break;
case JAUS_STANDBY_STATE:
cmptStandbyState();
break;
case JAUS_READY_STATE:
cmptReadyState();
break;
case JAUS_EMERGENCY_STATE:
cmptEmergencyState();
break;
case JAUS_FAILURE_STATE:
cmptFailureState();
break;
case JAUS_SHUTDOWN_STATE:
cmptRun = FALSE;
break;
default:
cmpt->state = JAUS_FAILURE_STATE; // The default case JAUS_is undefined, therefore go into Failure State
break;
}
cmptAllState();
nodeManagerSendCoreServiceConnections(cmptNmi);
nextExcecuteTime = 2.0 * time + 1.0/CMPT_THREAD_DESIRED_RATE_HZ - getTimeSeconds();
}
cmptShutdownState();
usleep(50000); // Sleep for 50 milliseconds and then exit
cmptThreadRunning = FALSE;
return NULL;
}
void wdInitState(void)
{
static double nextRequestTimeSec = 0.0;
RequestComponentControlMessage requestControl = NULL;
JausMessage txMessage = NULL;
char buf[64] = {0};
// Check for critcal service connections or conditions here
if(gposSc->isActive && vssSc->isActive && pdWrenchSc->isActive && pdStatusSc->isActive && wdInControl && wdRequestControl)
{
// Transition to Standby
wd->state = JAUS_STANDBY_STATE;
//cDebug(4, "wd: Switching to STANDBY State\n");
}
else if(getTimeSeconds() > nextRequestTimeSec)
{
if(!gposSc->isActive)
{
if(scManagerCreateServiceConnection(wdNmi, gposSc))
{
//cDebug(4, "wd: Sent GPOS SC Request\n");
}
}
if(!vssSc->isActive)
{
if(scManagerCreateServiceConnection(wdNmi, vssSc))
{
//cDebug(4, "wd: Sent VSS SC Request\n");
}
}
if(!pdWrenchSc->isActive && wdInControl)
{
if(scManagerCreateServiceConnection(wdNmi, pdWrenchSc))
{
//cDebug(4, "wd: Sent PD Wrench SC Request\n");
}
}
if(!pdStatusSc->isActive && wdInControl)
{
if(scManagerCreateServiceConnection(wdNmi, pdStatusSc))
{
//cDebug(4, "wd: Sent PD Status SC Request\n");
}
}
nextRequestTimeSec = getTimeSeconds() + REQUEST_TIMEOUT_SEC;
}
if(!wdInControl && wdRequestControl)
{
if(!pd->address->node)
{
pd->address->subsystem = wd->address->subsystem;
if(nodeManagerLookupAddress(wdNmi, pd->address))
{
jausAddressCopy(wdWrench->destination, pd->address);
}
}
if(pd->address->node)
{
jausAddressToString(pd->address, buf);
//cDebug(4, "wd: Requesting control of PD %s\n", buf);
requestControl = requestComponentControlMessageCreate();
jausAddressCopy(requestControl->source, wd->address);
jausAddressCopy(requestControl->destination, pd->address);
requestControl->authorityCode = wd->authority;
txMessage = requestComponentControlMessageToJausMessage(requestControl);
nodeManagerSend(wdNmi, txMessage);
jausMessageDestroy(txMessage);
requestComponentControlMessageDestroy(requestControl);
if(scManagerCreateServiceConnection(wdNmi, pdStatusSc))
{
//cDebug(4, "wd: Sent PD Status SC Request\n");
}
if(scManagerCreateServiceConnection(wdNmi, pdWrenchSc))
{
//cDebug(4, "wd: Sent PD Wrench SC Request\n");
}
nextRequestTimeSec = getTimeSeconds() + REQUEST_TIMEOUT_SEC;
}
}
}
void wdExcecuteControl(VehicleState vehicleState)
{
static double speedCommand = 0;
static double prevSpeedError = 0;
static double dampedSpeedErrorDerivative = 0;
static double acceleration = ACCELERATION_MPSPS; //Mpsps
static double decceleration = DECCELERATION_MPSPS; //Mpsps
static double lastExcecuteTime = -1;
static double dt;
static double linearEffortInt = 0.0;
double linearEffort;
double speedError;
JausMessage txMessage;
if(lastExcecuteTime < 0)
{
lastExcecuteTime = getTimeSeconds();
dt = 1.0 / WD_THREAD_DESIRED_RATE_HZ;
}
else
{
dt = getTimeSeconds() - lastExcecuteTime;
lastExcecuteTime = getTimeSeconds();
}
if(speedCommand < vehicleState->desiredSpeedMps)
{
speedCommand += acceleration * dt;
if(speedCommand > vehicleState->desiredSpeedMps)
{
speedCommand = vehicleState->desiredSpeedMps;
}
}
else
{
speedCommand -= decceleration * dt;
if(speedCommand < vehicleState->desiredSpeedMps)
{
speedCommand = vehicleState->desiredSpeedMps;
}
}
speedError = speedCommand - vehicleState->speedMps;
linearEffortInt += speedError * dt;
if(linearEffortInt > LINEAR_EFFORT_I_LIM)
{
linearEffortInt = LINEAR_EFFORT_I_LIM;
}
if(linearEffortInt < -LINEAR_EFFORT_I_LIM)
{
linearEffortInt = -LINEAR_EFFORT_I_LIM;
}
if(dt > 0.001)
{
dampedSpeedErrorDerivative = 0.9 * dampedSpeedErrorDerivative + 0.1 * (speedError - prevSpeedError) / dt;
prevSpeedError = speedError;
}
linearEffort = LINEAR_EFFORT_K_FF * speedCommand + LINEAR_EFFORT_BIAS_FF;
linearEffort += LINEAR_EFFORT_K_P * speedError;
linearEffort += LINEAR_EFFORT_K_I * linearEffortInt;
linearEffort += LINEAR_EFFORT_K_D * dampedSpeedErrorDerivative;
// Pitch feed forward
//linearEffort += PITCH_FF_EFFORT_PER_RAD * sin(wdDampedPitchRad); // Pitch feed forward effort
// Steering feed forward
if(wdReportWrench)
{
linearEffort += STEERING_FF_EFFORT * (1.0/cos(wdReportWrench->propulsiveRotationalEffortZPercent * WHEEL_ROTATION_RAD_PER_EFFORT) - 1.0);
}
// Sticktion feed forward
if(speedCommand < WD_DEFAULT_MIN_SPEED_MPS)
{
linearEffort += STICKTION_EFFORT;
}
if(linearEffort > 0)
{
wdWrench->propulsiveLinearEffortXPercent = THROTTLE_K * linearEffort;
wdWrench->resistiveLinearEffortXPercent = 0;
}
else
{
wdWrench->propulsiveLinearEffortXPercent = 0;
wdWrench->resistiveLinearEffortXPercent = BRAKE_K * linearEffort;
}
if(wdWrench->propulsiveLinearEffortXPercent > 100.0)
{
wdWrench->propulsiveLinearEffortXPercent = 100.0;
}
if(wdWrench->propulsiveLinearEffortXPercent < 0.0)
{
wdWrench->propulsiveLinearEffortXPercent = 0.0;
}
if(wdWrench->resistiveLinearEffortXPercent > 100.0)
{
wdWrench->resistiveLinearEffortXPercent = 100.0;
}
if(wdWrench->resistiveLinearEffortXPercent < 0.0)
{
wdWrench->resistiveLinearEffortXPercent = 0.0;
}
wdWrench->propulsiveRotationalEffortZPercent = vehicleState->desiredPhiEffort;
if(wdWrench->propulsiveRotationalEffortZPercent > 100.0)
{
wdWrench->propulsiveRotationalEffortZPercent = 100.0;
}
if(wdWrench->propulsiveRotationalEffortZPercent < -100.0)
{
wdWrench->propulsiveRotationalEffortZPercent = -100.0;
}
jausAddressCopy(wdWrench->destination, pd->address);
txMessage = setWrenchEffortMessageToJausMessage(wdWrench);
nodeManagerSend(wdNmi, txMessage);
jausMessageDestroy(txMessage);
return;
}
void scManagerProcessConfirmEvent(NodeManagerInterface nmi, ConfirmEventMessage message)
{
ServiceConnection prevSc = NULL;
ServiceConnection sc = nmi->scm->incommingSc;
CancelEventMessage cancelEvent;
JausMessage txMessage;
// Loop through all incomming SC's
while(sc)
{
// Test if this incoming SC matches the one replied to
if( sc->commandCode == message->messageCode && sc->address->id == message->source->id )
{
// Success
if( message->responseCode == SUCCESSFUL_RESPONSE )
{
sc->confirmedUpdateRateHz = message->confirmedUpdateRate;
sc->instanceId = message->eventId;
sc->isActive = JAUS_TRUE;
sc->sequenceNumber = 65535;
sc->lastSentTime = getTimeSeconds();
}
else // Failure
{
// Set SC Inactive
sc->isActive = JAUS_FALSE;
// Remove Service Connection
if(prevSc)
{
prevSc->nextSc = sc->nextSc;
sc->nextSc = NULL;
}
else
{
nmi->scm->incommingSc = sc->nextSc;
sc->nextSc = NULL;
}
nmi->scm->incommingScCount--;
}
return;
}
prevSc = sc;
sc = sc->nextSc;
}
// The SC was not found, so send a terminate to prevent streaming
if( message->responseCode == SUCCESSFUL_RESPONSE )
{
cancelEvent = cancelEventMessageCreate();
cancelEvent->source->id = nmi->cmpt->address->id;
cancelEvent->destination->id = message->source->id;
cancelEvent->messageCode = message->messageCode;
cancelEvent->eventId = message->eventId;
txMessage = cancelEventMessageToJausMessage(cancelEvent);
nodeManagerSend(nmi, txMessage);
jausMessageDestroy(txMessage);
cancelEventMessageDestroy(cancelEvent);
}
}
// Function: vehicleSimThread
// Access: Private
// Description: All core component functionality is contained in this thread.
// All of the JAUS component state machine code can be found here.
void *vehicleSimThread(void *threadData)
{
double time, prevTime, dt;
double steeringRate;
double motorForce;
double brakeForce;
double dragForce;
double turningCurvature;
double deltaH;
vehicleSimThreadRunning = TRUE;
// Initialize the UTM engine
utmConversionInit(vehiclePosLla);
vehiclePosUtm = pointLlaToPointUtm(vehiclePosLla);
time = getTimeSeconds();
while(vehicleSimRun) // Execute state machine code while not in the SHUTDOWN state
{
prevTime = time;
time = getTimeSeconds();
dt = time - prevTime;
vehicleSimThreadHz = 1.0/dt; // Compute the update rate of this thread
steeringRate = K_STEERING * (vehicleDesiredRotationalEffort - vehicleRotationalEffort);
if(steeringRate > MAX_STEERING_RATE)
{
steeringRate = MAX_STEERING_RATE;
}
else if(steeringRate < -MAX_STEERING_RATE)
{
steeringRate = -MAX_STEERING_RATE;
}
vehicleRotationalEffort += steeringRate * dt;
turningCurvature = -vehicleRotationalEffort / 100.0 / MIN_TURNING_RADIUS_M;
vehicleLinearEffortX = vehicleDesiredLinearEffortX;
motorForce = K_THROTTLE * vehicleLinearEffortX + IDLE_FORCE_N;
vehicleResistiveEffortX += BRAKE_TAU * (vehicleDesiredResistiveEffortX - vehicleResistiveEffortX);
if(vehicleSpeed > 0.005)
{
brakeForce = K_DYNAMIC_BRAKE * vehicleResistiveEffortX;
}
else
{
brakeForce = K_STATIC_BRAKE * vehicleResistiveEffortX;
if(brakeForce > motorForce)
{
brakeForce = motorForce + 1.0;
}
}
dragForce = DRAG_COEFF_N_PER_MPS_SQ * vehicleSpeed * vehicleSpeed;
vehicleSpeed += ( motorForce - brakeForce - dragForce) / VEHICLE_MASS_KG * dt;
if(vehicleSpeed < 0)
{
vehicleSpeed = 0;
}
deltaH = turningCurvature * vehicleSpeed * dt;
vehiclePosUtm->xMeters += cos(vehicleH + deltaH / 2.0) * vehicleSpeed * dt;
vehiclePosUtm->yMeters += sin(vehicleH + deltaH / 2.0) * vehicleSpeed * dt;
vehicleH += deltaH;
vehicleH = fmod(vehicleH, 2*M_PI);
if(vehiclePosLla)
{
pointLlaDestroy(vehiclePosLla);
}
vehiclePosLla = pointUtmToPointLla(vehiclePosUtm);
usleep(25000);
}
usleep(50000); // Sleep for 50 milliseconds and then exit
vehicleSimThreadRunning = FALSE;
return NULL;
//pthread_exit(NULL);
}
