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);
};
void cmptShutdownState(void)
{
RejectComponentControlMessage rejectComponentControl;
JausMessage txMessage;
if(cmpt->controller.active)
{
// Terminate control of current component
rejectComponentControl = rejectComponentControlMessageCreate();
rejectComponentControl->source->id = cmpt->address->id;
rejectComponentControl->destination->id = cmpt->controller.address->id;
txMessage = rejectComponentControlMessageToJausMessage(rejectComponentControl);
nodeManagerSend(cmptNmi, txMessage);
jausMessageDestroy(txMessage);
rejectComponentControlMessageDestroy(rejectComponentControl);
}
// USER: Insert Shutdown Code Here, NOTE: Terminate any active service connections
}
void gposShutdownState(void)
{
RejectComponentControlMessage rejectComponentControl;
JausMessage txMessage;
if(gpos->controller.active)
{
// Terminate control of current component
rejectComponentControl = rejectComponentControlMessageCreate();
rejectComponentControl->source->id = gpos->address->id;
rejectComponentControl->destination->id = gpos->controller.address->id;
txMessage = rejectComponentControlMessageToJausMessage(rejectComponentControl);
nodeManagerSend(gposNmi, txMessage);
jausMessageDestroy(txMessage);
rejectComponentControlMessageDestroy(rejectComponentControl);
}
// Remove support for ReportGlovalPose Service Connections
scManagerRemoveSupportedMessage(gposNmi, JAUS_REPORT_GLOBAL_POSE);
reportGlobalPoseMessageDestroy(gposMessage);
}
// Function: cmptProcessMessage
// Access: Private
// Description: This function is responsible for handling incoming JAUS messages from the Node Manager.
// Incoming messages are processed according to message type.
void cmptProcessMessage(JausMessage message)
{
ConfirmComponentControlMessage confirmComponentControl;
RejectComponentControlMessage rejectComponentControl;
ReportComponentAuthorityMessage reportComponentAuthority;
ReportComponentStatusMessage reportComponentStatus;
// This block of code is intended to reject commands from non-controlling components
if(cmpt->controller.active && message->source->id != cmpt->controller.address->id && jausMessageIsRejectableCommand(message) )
{
cError("cmpt: Received command message %s from non-controlling component.\n", jausMessageCommandCodeString(message));
jausMessageDestroy(message); // Ignore this message
return;
}
switch(message->commandCode) // Switch the processing algorithm according to the JAUS message type
{
case JAUS_CONFIRM_COMPONENT_CONTROL:
confirmComponentControl = confirmComponentControlMessageFromJausMessage(message);
if(confirmComponentControl)
{
// USER: Insert code here to handle the confirm component control message if needed
confirmComponentControlMessageDestroy(confirmComponentControl);
}
else
{
cError("cmpt: Error unpacking %s message.\n", jausMessageCommandCodeString(message));
}
jausMessageDestroy(message);
break;
case JAUS_REJECT_COMPONENT_CONTROL:
rejectComponentControl = rejectComponentControlMessageFromJausMessage(message);
if(rejectComponentControl)
{
// USER: Insert code here to handle the reject component control message if needed
rejectComponentControlMessageDestroy(rejectComponentControl);
}
else
{
cError("cmpt: Error unpacking %s message.\n", jausMessageCommandCodeString(message));
}
jausMessageDestroy(message);
break;
case JAUS_REPORT_COMPONENT_AUTHORITY:
reportComponentAuthority = reportComponentAuthorityMessageFromJausMessage(message);
if(reportComponentAuthority)
{
// USER: Insert code here to handle the report component authority message if needed
reportComponentAuthorityMessageDestroy(reportComponentAuthority);
}
else
{
cError("cmpt: Error unpacking %s message.\n", jausMessageCommandCodeString(message));
}
jausMessageDestroy(message);
break;
case JAUS_REPORT_COMPONENT_STATUS:
reportComponentStatus = reportComponentStatusMessageFromJausMessage(message);
if(reportComponentStatus)
{
// USER: Insert code here to handle the report component status message if needed
reportComponentStatusMessageDestroy(reportComponentStatus);
}
else
{
cError("cmpt: Error unpacking %s message.\n", jausMessageCommandCodeString(message));
}
jausMessageDestroy(message);
break;
default:
defaultJausMessageProcessor(message, cmptNmi, cmpt);
break;
}
}
// Function: wdProcessMessage
// Access: Private
// Description: This function is responsible for handling incoming JAUS messages from the Node Manager.
// Incoming messages are processed according to message type.
void wdProcessMessage(JausMessage message)
{
JausMessage txMessage;
ConfirmComponentControlMessage confirmComponentControl;
RejectComponentControlMessage rejectComponentControl;
ReportComponentStatusMessage reportComponentStatus;
QueryGlobalWaypointMessage queryGlobalWaypointMessage;
ReportGlobalWaypointMessage reportGlobalWaypointMessage;
QueryWaypointCountMessage queryWaypointCountMessage;
ReportWaypointCountMessage reportWaypointCountMessage;
int i;
char buf[64] = {0};
// This block of code is intended to reject commands from non-controlling components
if(wd->controller.active && !jausAddressEqual(message->source, wd->controller.address) && jausMessageIsRejectableCommand(message))
{
//cError("wd: Received command message %s from non-controlling component.\n", jausMessageCommandCodeString(message));
jausMessageDestroy(message); // Ignore this message
return;
}
switch(message->commandCode) // Switch the processing algorithm according to the JAUS message type
{
case JAUS_REPORT_COMPONENT_STATUS:
reportComponentStatus = reportComponentStatusMessageFromJausMessage(message);
if(reportComponentStatus)
{
if(jausAddressEqual(reportComponentStatus->source, pd->address))
{
pd->state = reportComponentStatus->primaryStatusCode;
}
reportComponentStatusMessageDestroy(reportComponentStatus);
}
else
{
//cError("wd: Error unpacking %s message.\n", jausMessageCommandCodeString(message));
}
jausMessageDestroy(message);
break;
case JAUS_CONFIRM_COMPONENT_CONTROL:
confirmComponentControl = confirmComponentControlMessageFromJausMessage(message);
if(confirmComponentControl)
{
if(jausAddressEqual(confirmComponentControl->source, pd->address))
{
//cDebug(4,"wd: Confirmed control of PD\n");
wdInControl = JAUS_TRUE;
}
confirmComponentControlMessageDestroy(confirmComponentControl);
}
else
{
//cError("wd: Error unpacking %s message.\n", jausMessageCommandCodeString(message));
}
jausMessageDestroy(message);
break;
case JAUS_REJECT_COMPONENT_CONTROL:
rejectComponentControl = rejectComponentControlMessageFromJausMessage(message);
if(rejectComponentControl)
{
if(jausAddressEqual(rejectComponentControl->source, pd->address))
{
//cDebug(4,"wd: Lost control of PD\n");
wdInControl = JAUS_FALSE;
}
rejectComponentControlMessageDestroy(rejectComponentControl);
}
else
{
//cError("wd: Error unpacking %s message.\n", jausMessageCommandCodeString(message));
}
jausMessageDestroy(message);
break;
case JAUS_SET_TRAVEL_SPEED:
if(wdSpeed)
{
setTravelSpeedMessageDestroy(wdSpeed);
}
wdSpeed = setTravelSpeedMessageFromJausMessage(message);
if(!wdSpeed)
{
//cError("wd: Error unpacking %s message.\n", jausMessageCommandCodeString(message));
}
jausMessageDestroy(message);
break;
case JAUS_REPORT_GLOBAL_POSE:
if(wdReportGpos)
{
reportGlobalPoseMessageDestroy(wdReportGpos);
}
wdReportGpos = reportGlobalPoseMessageFromJausMessage(message);
if(wdReportGpos)
{
// Nothing to do
jausAddressToString(message->source, buf);
//cDebug(9, "Recv GPOS msg from %s\n", buf);
}
else
{
//cError("wd: Error unpacking %s message.\n", jausMessageCommandCodeString(message));
}
if(currentWaypointIndex < wdWaypoints->elementCount)
{
// update waypoint index
SetGlobalWaypointMessage tempGlobalWaypoint;
tempGlobalWaypoint = (SetGlobalWaypointMessage) wdWaypoints->elementData[currentWaypointIndex];
wdWaypointDistance = greatCircleDistance(wdReportGpos->latitudeDegrees * RAD_PER_DEG, wdReportGpos->longitudeDegrees * RAD_PER_DEG, tempGlobalWaypoint->latitudeDegrees * RAD_PER_DEG, tempGlobalWaypoint->longitudeDegrees * RAD_PER_DEG);
if(wdWaypointDistance < WAYPOINT_POP_DISTANCE_M)
{
//cError("wd: popping waypoint: %d\n",currentWaypointIndex);
currentWaypointIndex++;
}
}
// else
// {
// if(wdSpeed) wdSpeed->speedMps = 0;
// }
jausMessageDestroy(message);
break;
case JAUS_REPORT_VELOCITY_STATE:
if(wdReportVss)
{
reportVelocityStateMessageDestroy(wdReportVss);
}
wdReportVss = reportVelocityStateMessageFromJausMessage(message);
if(!wdReportVss)
{
//cError("wd: Error unpacking %s message.\n", jausMessageCommandCodeString(message));
}
jausMessageDestroy(message);
break;
case JAUS_REPORT_WRENCH_EFFORT:
if(wdReportWrench)
{
reportWrenchEffortMessageDestroy(wdReportWrench);
}
wdReportWrench = reportWrenchEffortMessageFromJausMessage(message);
if(!wdReportWrench)
{
//cError("wd: Error unpacking %s message.\n", jausMessageCommandCodeString(message));
}
jausMessageDestroy(message);
break;
case JAUS_SET_GLOBAL_WAYPOINT:
wdGlobalWaypoint = setGlobalWaypointMessageFromJausMessage(message);
if(!wdGlobalWaypoint)
{
//cError("wd: Error unpacking %s message.\n", jausMessageCommandCodeString(message));
}
jausMessageDestroy(message);
jausArrayAdd(wdWaypoints, wdGlobalWaypoint);
break;
case JAUS_QUERY_GLOBAL_WAYPOINT:
queryGlobalWaypointMessage = queryGlobalWaypointMessageFromJausMessage(message);
if(!queryGlobalWaypointMessage)
{
//cError("wd: Error unpacking %s message.\n", jausMessageCommandCodeString(message));
}
// loop thru waypoints to find the one that matches the request
// if there's a match, prepare/send the report, else whine
for(i = 0; i < wdWaypoints->elementCount; i++)
{
SetGlobalWaypointMessage tempGlobalWaypoint;
tempGlobalWaypoint = (SetGlobalWaypointMessage) wdWaypoints->elementData[i];
if(tempGlobalWaypoint->waypointNumber == queryGlobalWaypointMessage->waypointNumber)
{
reportGlobalWaypointMessage = reportGlobalWaypointMessageCreate();
jausAddressCopy(reportGlobalWaypointMessage->destination, queryGlobalWaypointMessage->source);
jausAddressCopy(reportGlobalWaypointMessage->source, wd->address);
reportGlobalWaypointMessage->presenceVector = NO_PRESENCE_VECTOR;
reportGlobalWaypointMessage->waypointNumber = tempGlobalWaypoint->waypointNumber;
reportGlobalWaypointMessage->latitudeDegrees = tempGlobalWaypoint->latitudeDegrees;
reportGlobalWaypointMessage->longitudeDegrees = tempGlobalWaypoint->longitudeDegrees;
txMessage = reportGlobalWaypointMessageToJausMessage(reportGlobalWaypointMessage);
reportGlobalWaypointMessageDestroy(reportGlobalWaypointMessage);
nodeManagerSend(wdNmi, txMessage);
jausMessageDestroy(txMessage);
}
}
queryGlobalWaypointMessageDestroy(queryGlobalWaypointMessage);
jausMessageDestroy(message);
break;
case JAUS_QUERY_WAYPOINT_COUNT:
queryWaypointCountMessage = queryWaypointCountMessageFromJausMessage(message);
if(!queryWaypointCountMessage)
{
//cError("wd: Error unpacking %s message.\n", jausMessageCommandCodeString(message));
}
reportWaypointCountMessage = reportWaypointCountMessageCreate();
jausAddressCopy(reportWaypointCountMessage->destination, queryWaypointCountMessage->source);
jausAddressCopy(reportWaypointCountMessage->source, wd->address);
reportWaypointCountMessage->waypointCount = wdWaypoints->elementCount;
txMessage = reportWaypointCountMessageToJausMessage(reportWaypointCountMessage);
reportWaypointCountMessageDestroy(reportWaypointCountMessage);
nodeManagerSend(wdNmi, txMessage);
queryWaypointCountMessageDestroy(queryWaypointCountMessage);
jausMessageDestroy(message);
break;
default:
defaultJausMessageProcessor(message, wdNmi, wd);
break;
}
}
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);
}