void scManagerProcessCancelEvent(NodeManagerInterface nmi, CancelEventMessage message)
{
SupportedScMessage supportedScMsg;
ServiceConnection sc;
ServiceConnection prevSc;
// Find the outbound SC that matches the cancel event
supportedScMsg = scFindSupportedScMsgInList(nmi->scm->supportedScMsgList, message->messageCode);
if(supportedScMsg == NULL)
{
// Cancel Event message for SC type not found
return;
}
if( supportedScMsg->scList == NULL )
{
// Cancel Event message for SC not found
return;
}
sc = supportedScMsg->scList;
if( sc->address->id == message->source->id &&
sc->commandCode == message->messageCode &&
sc->instanceId == message->eventId )
{
// Remove sc from list
supportedScMsg->scList = sc->nextSc;
serviceConnectionDestroy(sc);
nmi->scm->outgoingScCount--;
return;
}
prevSc = sc;
sc = prevSc->nextSc;
while(sc)
{
if( sc->address->id == message->source->id &&
sc->commandCode == message->messageCode &&
sc->instanceId == message->eventId )
{
// Remove sc from list
prevSc->nextSc = sc->nextSc;
serviceConnectionDestroy(sc);
nmi->scm->outgoingScCount--;
return;
}
prevSc = sc;
sc = sc->nextSc;
}
}
void scManagerProcessTerminateScMessage(NodeManagerInterface nmi, TerminateServiceConnectionMessage message)
{
SupportedScMessage supportedScMsg;
ServiceConnection sc;
ServiceConnection prevSc;
supportedScMsg = scFindSupportedScMsgInList(nmi->scm->supportedScMsgList, message->serviceConnectionCommandCode);
if(supportedScMsg == NULL)
{
return;
}
if( supportedScMsg->scList == NULL )
{
return;
}
sc = supportedScMsg->scList;
if( sc->address->id == message->source->id &&
sc->commandCode == message->serviceConnectionCommandCode &&
sc->instanceId == message->instanceId )
{
// Remove sc from list
supportedScMsg->scList = sc->nextSc;
serviceConnectionDestroy(sc);
nmi->scm->outgoingScCount--;
return;
}
prevSc = sc;
sc = prevSc->nextSc;
while(sc)
{
if( sc->address->id == message->source->id &&
sc->commandCode == message->serviceConnectionCommandCode &&
sc->instanceId == message->instanceId )
{
// Remove sc from list
prevSc->nextSc = sc->nextSc;
serviceConnectionDestroy(sc);
nmi->scm->outgoingScCount--;
return;
}
prevSc = sc;
sc = sc->nextSc;
}
}
void scManagerDestroy(ServiceConnectionManager scm)
{
SupportedScMessage supportedScMsg;
ServiceConnection sc;
// Only attempt to destroy the scm if it is a non-NULL pointer
if(scm == NULL)
{
return;
}
// Free the supported message list
while(scm->supportedScMsgList)
{
supportedScMsg = scm->supportedScMsgList;
scm->supportedScMsgList = supportedScMsg->nextSupportedScMsg;
// Free all the service connections
while(supportedScMsg->scList)
{
sc = supportedScMsg->scList;
supportedScMsg->scList = sc->nextSc;
serviceConnectionDestroy(sc);
}
free(supportedScMsg);
}
free(scm);
}
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 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);
}
void scManagerRemoveSupportedMessage(NodeManagerInterface nmi, unsigned short commandCode)
{
SupportedScMessage prevSupportedScMsg = NULL;
SupportedScMessage supportedScMsg = nmi->scm->supportedScMsgList;
ServiceConnection sc;
TerminateServiceConnectionMessage terminateSc;
CancelEventMessage cancelEvent;
JausMessage txMessage;
while(supportedScMsg)
{
if(supportedScMsg->commandCode == commandCode)
{
// Remove Service Connection
if(prevSupportedScMsg)
{
prevSupportedScMsg->nextSupportedScMsg = supportedScMsg->nextSupportedScMsg;
}
else
{
nmi->scm->supportedScMsgList = supportedScMsg->nextSupportedScMsg;
}
// Terminate and free all the service connections
while(supportedScMsg->scList)
{
sc = supportedScMsg->scList;
supportedScMsg->scList = sc->nextSc;
// This test will indicate if it is the new style using event messages or not
if(sc->serviceConnectionType == SC_EVENT_TYPE)
{
cancelEvent = cancelEventMessageCreate();
cancelEvent->source->id = nmi->cmpt->address->id;
cancelEvent->destination->id = sc->address->id;
cancelEvent->messageCode = sc->commandCode;
cancelEvent->eventId = sc->instanceId;
txMessage = cancelEventMessageToJausMessage(cancelEvent);
nodeManagerSend(nmi, txMessage);
jausMessageDestroy(txMessage);
cancelEventMessageDestroy(cancelEvent);
jausMessageDestroy(sc->queryMessage);
}
else
{
terminateSc = terminateServiceConnectionMessageCreate();
terminateSc->source->id = nmi->cmpt->address->id;
terminateSc->destination->id = sc->address->id;
terminateSc->serviceConnectionCommandCode = sc->commandCode;
terminateSc->instanceId = sc->instanceId;
txMessage = terminateServiceConnectionMessageToJausMessage(terminateSc);
nodeManagerSend(nmi, txMessage);
jausMessageDestroy(txMessage);
terminateServiceConnectionMessageDestroy(terminateSc);
}
serviceConnectionDestroy(sc);
}
free(supportedScMsg);
nmi->scm->supportedScMsgCount--;
return;
}
prevSupportedScMsg = supportedScMsg;
supportedScMsg = supportedScMsg->nextSupportedScMsg;
}
}
void scManagerProcessUpdatedSubystem(NodeManagerInterface nmi, JausSubsystem subsystem)
{
SupportedScMessage supportedScMsg;
ServiceConnection sc;
ServiceConnection prevSc;
supportedScMsg = nmi->scm->supportedScMsgList;
while(supportedScMsg)
{
prevSc = NULL;
sc = supportedScMsg->scList;
while(sc)
{
//if(!ssManagerCheckAddress(subsystem, sc->address) )
if( !nodeManagerVerifyAddress(nmi, sc->address) )
{
// Remove sc from list
if(prevSc)
{
prevSc->nextSc = sc->nextSc;
serviceConnectionDestroy(sc);
sc = prevSc->nextSc;
}
else
{
supportedScMsg->scList = sc->nextSc;
serviceConnectionDestroy(sc);
sc = supportedScMsg->scList;
}
nmi->scm->outgoingScCount--;
}
else
{
prevSc = sc;
sc = sc->nextSc;
}
}
supportedScMsg = supportedScMsg->nextSupportedScMsg;
}
prevSc = NULL;
sc = nmi->scm->incommingSc;
while(sc)
{
//if(!ssManagerCheckAddress(subsystem, sc->address) )
if( !nodeManagerVerifyAddress(nmi, sc->address) )
{
// Remove sc from list
sc->isActive = JAUS_FALSE;
// Clear out Inbound Queue
while(sc->queue->size)
{
jausMessageDestroy(queuePop(sc->queue));
}
if(prevSc)
{
prevSc->nextSc = sc->nextSc;
sc->nextSc = NULL;
sc = prevSc->nextSc;
}
else
{
nmi->scm->incommingSc = sc->nextSc;
sc->nextSc = NULL;
sc = nmi->scm->incommingSc;
}
nmi->scm->incommingScCount--;
}
else
{
prevSc = sc;
sc = sc->nextSc;
}
}
}
