void ojCmptDestroy(OjCmpt ojCmpt)
{
RejectComponentControlMessage rejectComponentControl;
JausMessage txMessage;
int i = 0;
if(ojCmpt->run == TRUE)
{
ojCmpt->run = FALSE;
pthread_cond_signal(&ojCmpt->nmi->recvCondition);
pthread_join(ojCmpt->thread, NULL);
}
if(ojCmpt->jaus->controller.active)
{
// Terminate control of current component
rejectComponentControl = rejectComponentControlMessageCreate();
jausAddressCopy(rejectComponentControl->source, ojCmpt->jaus->address);
jausAddressCopy(rejectComponentControl->destination, ojCmpt->jaus->controller.address);
txMessage = rejectComponentControlMessageToJausMessage(rejectComponentControl);
nodeManagerSend(ojCmpt->nmi, txMessage);
jausMessageDestroy(txMessage);
rejectComponentControlMessageDestroy(rejectComponentControl);
}
for(i=0; i<OJ_CMPT_MAX_INCOMING_SC_COUNT; i++)
{
if(ojCmpt->inConnection[i])
{
if(ojCmpt->inConnection[i]->isActive)
{
scManagerTerminateServiceConnection(ojCmpt->nmi, ojCmpt->inConnection[i]);
}
serviceConnectionDestroy(ojCmpt->inConnection[i]);
}
}
if(ojCmpt->messageCallback)
{
free(ojCmpt->messageCallback);
}
if(ojCmpt->nmi)
{
nodeManagerClose(ojCmpt->nmi); // Close Node Manager Connection
}
free(ojCmpt->jaus->identification);
ojCmpt->jaus->identification = NULL;
jausComponentDestroy(ojCmpt->jaus);
free(ojCmpt);
};
int ojCmptTerminateSc(OjCmpt ojCmpt, int scIndex)
{
if(scIndex < 0 || scIndex >= OJ_CMPT_MAX_INCOMING_SC_COUNT || ojCmpt->inConnection[scIndex] == NULL)
{
return FALSE;
}
if(ojCmpt->inConnection[scIndex]->isActive)
{
scManagerTerminateServiceConnection(ojCmpt->nmi, ojCmpt->inConnection[scIndex]);
}
serviceConnectionDestroy(ojCmpt->inConnection[scIndex]);
ojCmpt->inConnection[scIndex] = NULL;
return TRUE;
}
void wdAllState(void)
{
JausMessage message = NULL;
ServiceConnection scList;
ServiceConnection sc;
ReportComponentStatusMessage reportStatus;
ReleaseComponentControlMessage releaseControl = NULL;
// Retrieve the Gpos SC Message
if(gposSc && gposSc->isActive)
{
if(scManagerReceiveServiceConnection(wdNmi, gposSc, &message))
{
wdProcessMessage(message);
}
}
if(vssSc && vssSc->isActive)
{
if(scManagerReceiveServiceConnection(wdNmi, vssSc, &message))
{
wdProcessMessage(message);
}
}
if(pdWrenchSc && pdWrenchSc->isActive)
{
if(scManagerReceiveServiceConnection(wdNmi, pdWrenchSc, &message))
{
wdProcessMessage(message);
}
}
if(pdStatusSc && pdStatusSc->isActive)
{
if(scManagerReceiveServiceConnection(wdNmi, pdStatusSc, &message))
{
wdProcessMessage(message);
}
}
// Respond to a ReportComponentStatus Service Connection
scList = scManagerGetSendList(wdNmi, JAUS_REPORT_COMPONENT_STATUS);
if(scList)
{
reportStatus = reportComponentStatusMessageCreate();
jausAddressCopy(reportStatus->source, wd->address);
reportStatus->properties.scFlag = JAUS_SERVICE_CONNECTION_MESSAGE;
reportStatus->primaryStatusCode = wd->state;
sc = scList;
while(sc)
{
jausAddressCopy(reportStatus->destination, sc->address);
reportStatus->sequenceNumber = sc->sequenceNumber;
message = reportComponentStatusMessageToJausMessage(reportStatus);
nodeManagerSend(wdNmi, message);
jausMessageDestroy(message);
sc = sc->nextSc;
}
reportComponentStatusMessageDestroy(reportStatus);
}
scManagerDestroySendList(scList);
// Check Control
if(!wdRequestControl && wdInControl)
{
// Release Control
//cDebug(4, "wd: Releasing control of PD\n");
releaseControl = releaseComponentControlMessageCreate();
jausAddressCopy(releaseControl->source, wd->address);
jausAddressCopy(releaseControl->destination, pd->address);
message = releaseComponentControlMessageToJausMessage(releaseControl);
nodeManagerSend(wdNmi, message);
jausMessageDestroy(message);
releaseComponentControlMessageDestroy(releaseControl);
wdInControl = JAUS_FALSE;
pd->state = JAUS_UNKNOWN_STATE;
if(pdStatusSc->isActive)
{
scManagerTerminateServiceConnection(wdNmi, pdStatusSc);
}
if(pdWrenchSc->isActive)
{
scManagerTerminateServiceConnection(wdNmi, pdWrenchSc);
}
wd->state = JAUS_INITIALIZE_STATE;
}
}
void wdShutdownState(void)
{
RejectComponentControlMessage rejectComponentControl;
ReleaseComponentControlMessage releaseControl = NULL;
JausMessage txMessage;
scManagerRemoveSupportedMessage(wdNmi, JAUS_REPORT_COMPONENT_STATUS);
if(wd->controller.active)
{
// Terminate control of current component
rejectComponentControl = rejectComponentControlMessageCreate();
jausAddressCopy(rejectComponentControl->source, wd->address);
jausAddressCopy(rejectComponentControl->destination, wd->controller.address);
txMessage = rejectComponentControlMessageToJausMessage(rejectComponentControl);
nodeManagerSend(wdNmi, txMessage);
jausMessageDestroy(txMessage);
rejectComponentControlMessageDestroy(rejectComponentControl);
}
// USER: Insert Shutdown Code Here, NOTE: Terminate any active service connections
// Release Control
if(wdInControl)
{
//cDebug(4, "wd: Releasing control of PD\n");
releaseControl = releaseComponentControlMessageCreate();
jausAddressCopy(releaseControl->source, wd->address);
jausAddressCopy(releaseControl->destination, pd->address);
txMessage = releaseComponentControlMessageToJausMessage(releaseControl);
nodeManagerSend(wdNmi, txMessage);
jausMessageDestroy(txMessage);
releaseComponentControlMessageDestroy(releaseControl);
wdInControl = JAUS_FALSE;
wd->state = JAUS_INITIALIZE_STATE;
}
if(pdStatusSc->isActive)
{
scManagerTerminateServiceConnection(wdNmi, pdStatusSc);
serviceConnectionDestroy(pdStatusSc);
}
if(pdWrenchSc->isActive)
{
scManagerTerminateServiceConnection(wdNmi, pdWrenchSc);
serviceConnectionDestroy(pdWrenchSc);
}
if(gposSc->isActive)
{
scManagerTerminateServiceConnection(wdNmi, gposSc);
serviceConnectionDestroy(gposSc);
}
if(vssSc->isActive)
{
scManagerTerminateServiceConnection(wdNmi, vssSc);
serviceConnectionDestroy(vssSc);
}
// Destory Global Messages
if(wdSpeed)
{
setTravelSpeedMessageDestroy(wdSpeed);
}
if(wdReportGpos)
{
reportGlobalPoseMessageDestroy(wdReportGpos);
}
if(wdReportVss)
{
reportVelocityStateMessageDestroy(wdReportVss);
}
if(wdReportWrench)
{
reportWrenchEffortMessageDestroy(wdReportWrench);
}
// Destroy Global Variables
vehicleStateDestroy(vehicleState);
jausArrayDestroy(wdWaypoints, (void *)setGlobalWaypointMessageDestroy);
setWrenchEffortMessageDestroy(wdWrench);
jausComponentDestroy(pd);
}
