void scManagerRemoveSupportedMessage(NodeManagerInterface nmi, unsigned short commandCode)
{
SupportedScMessage prevSupportedScMsg = NULL;
SupportedScMessage supportedScMsg;
ServiceConnection sc;
TerminateServiceConnectionMessage terminateSc;
JausMessage txMessage;
pthread_mutex_lock(&nmi->scm->mutex);
supportedScMsg = nmi->scm->supportedScMsgList;
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;
terminateSc = terminateServiceConnectionMessageCreate();
jausAddressCopy(terminateSc->source, nmi->cmpt->address);
jausAddressCopy(terminateSc->destination, sc->address);
terminateSc->serviceConnectionCommandCode = sc->commandCode;
terminateSc->instanceId = sc->instanceId;
txMessage = terminateServiceConnectionMessageToJausMessage(terminateSc);
nodeManagerSend(nmi, txMessage);
jausMessageDestroy(txMessage);
terminateServiceConnectionMessageDestroy(terminateSc);
serviceConnectionDestroyNoMutex(sc); sc = NULL;
}
free(supportedScMsg);
nmi->scm->supportedScMsgCount--;
pthread_mutex_unlock(&nmi->scm->mutex);
return;
}
prevSupportedScMsg = supportedScMsg;
supportedScMsg = supportedScMsg->nextSupportedScMsg;
}
pthread_mutex_unlock(&nmi->scm->mutex);
}
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);
};
// Function: gposProcessMessage
// 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 gposProcessMessage(JausMessage message)
{
QueryGlobalPoseMessage queryGlobalPose;
JausMessage txMessage;
// This block of code is intended to reject commands from non-controlling components
if(gpos->controller.active && message->source->id != gpos->controller.address->id && jausMessageIsRejectableCommand(message) )
{
cError("gpos: 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_QUERY_GLOBAL_POSE:
queryGlobalPose = queryGlobalPoseMessageFromJausMessage(message);
if(queryGlobalPose)
{
gposMessage->destination->id = queryGlobalPose->source->id;
gposMessage->presenceVector = queryGlobalPose->presenceVector;
gposMessage->sequenceNumber = 0;
gposMessage->scFlag = 0;
txMessage = reportGlobalPoseMessageToJausMessage(gposMessage);
nodeManagerSend(gposNmi, txMessage);
jausMessageDestroy(txMessage);
queryGlobalPoseMessageDestroy(queryGlobalPose);
}
else
{
cError("gpos: Error unpacking %s message.\n", jausMessageCommandCodeString(message));
}
jausMessageDestroy(message);
break;
default:
defaultJausMessageProcessor(message, gposNmi, gpos);
break;
}
}
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);
}
void scManagerProcessConfirmScMessage(NodeManagerInterface nmi, ConfirmServiceConnectionMessage message)
{
ServiceConnection prevSc = NULL;
ServiceConnection sc;
TerminateServiceConnectionMessage terminateSc;
JausMessage txMessage;
pthread_mutex_lock(&nmi->scm->mutex);
sc = nmi->scm->incomingSc;
while(sc)
{
if( sc->commandCode == message->serviceConnectionCommandCode && jausAddressEqual(sc->address, message->source) )
{
if( message->responseCode == JAUS_SC_SUCCESSFUL )
{
sc->confirmedUpdateRateHz = message->confirmedPeriodicUpdateRateHertz;
sc->instanceId = message->instanceId;
sc->isActive = JAUS_TRUE;
sc->sequenceNumber = 65535;
sc->lastSentTime = ojGetTimeSec();
}
else
{
// Set SC Inactive
sc->isActive = JAUS_FALSE;
// Remove Service Connection
if(prevSc)
{
prevSc->nextSc = sc->nextSc;
sc->nextSc = NULL;
}
else
{
nmi->scm->incomingSc = sc->nextSc;
sc->nextSc = NULL;
}
nmi->scm->incomingScCount--;
}
pthread_mutex_unlock(&nmi->scm->mutex);
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();
jausAddressCopy(terminateSc->source, nmi->cmpt->address);
jausAddressCopy(terminateSc->destination, message->source);
terminateSc->serviceConnectionCommandCode = message->serviceConnectionCommandCode;
terminateSc->instanceId = message->instanceId;
txMessage = terminateServiceConnectionMessageToJausMessage(terminateSc);
nodeManagerSend(nmi, txMessage);
jausMessageDestroy(txMessage);
terminateServiceConnectionMessageDestroy(terminateSc);
}
pthread_mutex_unlock(&nmi->scm->mutex);
}
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;
}
}
}
// 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 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 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);
}
JausBoolean scManagerCreateServiceConnection(NodeManagerInterface nmi, ServiceConnection sc)
{
CreateServiceConnectionMessage createSc;
JausMessage txMessage;
JausAddress localAddress;
ServiceConnection prevSc = NULL;
ServiceConnection testSc = NULL;
if(!sc)
{
return JAUS_FALSE;
}
pthread_mutex_lock(&nmi->scm->mutex);
// Check: Is this SC already on the incomingSc list? If so, remove it (otherwise it ends up on the list twice == bad)
// Remove this service connection from the list of incoming service connections
testSc = nmi->scm->incomingSc;
while(testSc)
{
if(sc == testSc)
{
if(prevSc)
{
prevSc->nextSc = testSc->nextSc;
}
else
{
nmi->scm->incomingSc = testSc->nextSc;
}
testSc = testSc->nextSc;
nmi->scm->incomingScCount--;
}
else
{
prevSc = testSc;
testSc = testSc->nextSc;
}
}
sc->confirmedUpdateRateHz = 0;
sc->lastSentTime = 0;
sc->sequenceNumber = 65535;
sc->instanceId = -1;
sc->isActive = JAUS_FALSE;
sc->nextSc = NULL;
createSc = createServiceConnectionMessageCreate();
jausAddressCopy(createSc->source, nmi->cmpt->address);
createSc->serviceConnectionCommandCode = sc->commandCode;
createSc->requestedPeriodicUpdateRateHertz = sc->requestedUpdateRateHz;
createSc->presenceVector = sc->presenceVector;
// If the subsystem for this service connection is known
if(sc->address && sc->address->subsystem != 0)
{
jausAddressCopy(createSc->destination, sc->address);
txMessage = createServiceConnectionMessageToJausMessage(createSc);
nodeManagerSend(nmi, txMessage);
jausMessageDestroy(txMessage);
createServiceConnectionMessageDestroy(createSc);
// Add the service connection to the front of the incoming service connection list
sc->nextSc = nmi->scm->incomingSc;
nmi->scm->incomingSc = sc;
nmi->scm->incomingScCount++;
pthread_mutex_unlock(&nmi->scm->mutex);
return JAUS_TRUE;
}
// otherwise the subsystem is unknown so we assume it is the same subsystem?
else
{
localAddress = jausAddressCreate();
localAddress->subsystem = nmi->cmpt->address->subsystem;
localAddress->node = 0;
localAddress->component = sc->address->component;
localAddress->instance = 0;
// Lookup Address from nodeManager
// Tests if the target component exists or not
if(nodeManagerLookupAddress(nmi, localAddress))
{
jausAddressCopy(createSc->destination, localAddress);
jausAddressCopy(sc->address, localAddress);
txMessage = createServiceConnectionMessageToJausMessage(createSc);
nodeManagerSend(nmi, txMessage);
jausMessageDestroy(txMessage);
jausAddressDestroy(localAddress);
createServiceConnectionMessageDestroy(createSc);
// Add the service connection to the front of the incoming service connection list
sc->nextSc = nmi->scm->incomingSc;
nmi->scm->incomingSc = sc;
nmi->scm->incomingScCount++;
pthread_mutex_unlock(&nmi->scm->mutex);
return JAUS_TRUE;
}
else
{
jausAddressDestroy(localAddress);
createServiceConnectionMessageDestroy(createSc);
pthread_mutex_unlock(&nmi->scm->mutex);
return JAUS_FALSE;
}
}
pthread_mutex_unlock(&nmi->scm->mutex);
return JAUS_FALSE;
}
// ***********************************************************
// Experimental Use of Event Messages for OPC 2.75
// Danny Kent (jaus AT dannykent DOT com)
// 11/07/05
// ***********************************************************
void scManagerProccessCreateEvent(NodeManagerInterface nmi, CreateEventMessage message)
{
SupportedScMessage supportedScMsg;
ServiceConnection sc;
ServiceConnection newSc;
JausMessage txMessage;
ConfirmEventMessage confirmEvent;
// Look to see if the requested commandcode exists
supportedScMsg = scFindSupportedScMsgInList(nmi->scm->supportedScMsgList, message->messageCode);
if(supportedScMsg == NULL)
{
// Command Code Not Supported
confirmEvent = confirmEventMessageCreate();
if(confirmEvent)
{
confirmEvent->source->id = nmi->cmpt->address->id;
confirmEvent->destination->id = message->source->id;
confirmEvent->messageCode = message->messageCode;
confirmEvent->eventId = 0;
confirmEvent->responseCode = MESSAGE_UNSUPPORTED_RESPONSE;
txMessage = confirmEventMessageToJausMessage(confirmEvent);
confirmEventMessageDestroy(confirmEvent);
nodeManagerSend(nmi, txMessage);
jausMessageDestroy(txMessage);
}
else
{
cError("scManagerProccessCreateEvent: Cannot create confirmEvent\n");
}
return;
}
// Attempt to create a new ServiceConnection
newSc = (ServiceConnection)malloc( sizeof(ServiceConnectionStruct) );
if(newSc == NULL)
{
// Send negative conf (could not create sc)
confirmEvent = confirmEventMessageCreate();
if(confirmEvent)
{
confirmEvent->source->id = nmi->cmpt->address->id;
confirmEvent->destination->id = message->source->id;
confirmEvent->messageCode = message->messageCode;
confirmEvent->eventId = 0;
confirmEvent->responseCode = CONNECTION_REFUSED_RESPONSE;
txMessage = confirmEventMessageToJausMessage(confirmEvent);
confirmEventMessageDestroy(confirmEvent);
nodeManagerSend(nmi, txMessage);
jausMessageDestroy(txMessage);
}
else
{
cError("scManagerProccessCreateEvent: Cannot create confirmEvent\n");
}
return;
}
// Create a copy of the QueryMessage
newSc->queryMessage = jausMessageDuplicate(message->queryMessage);
if(!newSc->queryMessage)
{
// error creating message
free(newSc);
return;
}
newSc->serviceConnectionType = SC_EVENT_TYPE;
newSc->commandCode = message->messageCode;
newSc->presenceVector = 0;
newSc->address = jausAddressCreate();
newSc->address->id = message->source->id;
// Get Instance / Event Id value
newSc->instanceId = scGetAvailableInstanceId(supportedScMsg->scList, newSc);
if(newSc->instanceId == -1)
{
// Send negative conf (could not create sc)
confirmEvent = confirmEventMessageCreate();
if(confirmEvent)
{
confirmEvent->source->id = nmi->cmpt->address->id;
confirmEvent->destination->id = message->source->id;
confirmEvent->messageCode = message->messageCode;
confirmEvent->eventId = 0;
confirmEvent->responseCode = CONNECTION_REFUSED_RESPONSE;
txMessage = confirmEventMessageToJausMessage(confirmEvent);
confirmEventMessageDestroy(confirmEvent);
nodeManagerSend(nmi, txMessage);
jausMessageDestroy(txMessage);
}
else
{
cError("scManagerProccessCreateEvent: Cannot create confirmEvent\n");
}
jausMessageDestroy(newSc->queryMessage);
jausAddressDestroy(newSc->address);
free(newSc);
return;
}
newSc->queue = NULL;
newSc->queueSize = 0;
sc = scFindScInList(supportedScMsg->scList, newSc);
if(sc == NULL) // Test to see if the sc does not already exist
{
// The sc doesent exist, so we insert the new one into the list
sc = newSc;
sc->nextSc = supportedScMsg->scList;
supportedScMsg->scList = sc;
nmi->scm->outgoingScCount++;
}
else
{
jausMessageDestroy(newSc->queryMessage);
jausAddressDestroy(newSc->address);
free(newSc);
}
// Setup the SC control parameters
sc->requestedUpdateRateHz = message->requestedUpdateRate;
sc->confirmedUpdateRateHz = message->requestedUpdateRate;
sc->lastSentTime = 0.0;
sc->sequenceNumber = 0;
sc->isActive = JAUS_TRUE;
// Send confirmation
confirmEvent = confirmEventMessageCreate();
if(confirmEvent)
{
confirmEvent->source->id = nmi->cmpt->address->id;
confirmEvent->destination->id = message->source->id;
confirmEvent->messageCode = message->messageCode;
confirmEvent->eventId = sc->instanceId;
confirmEvent->responseCode = SUCCESSFUL_RESPONSE;
txMessage = confirmEventMessageToJausMessage(confirmEvent);
confirmEventMessageDestroy(confirmEvent);
nodeManagerSend(nmi, txMessage);
jausMessageDestroy(txMessage);
}
else
{
cError("scManagerProccessCreateEvent: Cannot create confirmEvent\n");
}
}
void scManagerProcessCreateScMessage(NodeManagerInterface nmi, CreateServiceConnectionMessage message)
{
SupportedScMessage supportedScMsg;
ServiceConnection sc;
ServiceConnection newSc;
JausMessage txMessage;
ConfirmServiceConnectionMessage confScMsg;
pthread_mutex_lock(&nmi->scm->mutex);
supportedScMsg = scFindSupportedScMsgInList(nmi->scm->supportedScMsgList, message->serviceConnectionCommandCode);
if(supportedScMsg == NULL)
{
confScMsg = confirmServiceConnectionMessageCreate();
jausAddressCopy(confScMsg->source, nmi->cmpt->address);
jausAddressCopy(confScMsg->destination, message->source);
confScMsg->serviceConnectionCommandCode = message->serviceConnectionCommandCode;
confScMsg->instanceId = 0;
confScMsg->confirmedPeriodicUpdateRateHertz = 0;
confScMsg->responseCode = JAUS_SC_COMPONENT_NOT_CAPABLE;
txMessage = confirmServiceConnectionMessageToJausMessage(confScMsg);
nodeManagerSend(nmi, txMessage);
jausMessageDestroy(txMessage);
confirmServiceConnectionMessageDestroy(confScMsg);
pthread_mutex_unlock(&nmi->scm->mutex);
return;
}
newSc = (ServiceConnection)malloc( sizeof(ServiceConnectionStruct) );
if(newSc == NULL)
{
// Send negative conf (could not create sc)
confScMsg = confirmServiceConnectionMessageCreate();
jausAddressCopy(confScMsg->source, nmi->cmpt->address);
jausAddressCopy(confScMsg->destination, message->source);
confScMsg->serviceConnectionCommandCode = message->serviceConnectionCommandCode;
confScMsg->instanceId = 0;
confScMsg->confirmedPeriodicUpdateRateHertz = 0;
confScMsg->responseCode = JAUS_SC_CONNECTION_REFUSED;
txMessage = confirmServiceConnectionMessageToJausMessage(confScMsg);
nodeManagerSend(nmi, txMessage);
jausMessageDestroy(txMessage);
confirmServiceConnectionMessageDestroy(confScMsg);
pthread_mutex_unlock(&nmi->scm->mutex);
return;
}
newSc->commandCode = message->serviceConnectionCommandCode;
newSc->presenceVector = message->presenceVector;
newSc->address = jausAddressCreate();
jausAddressCopy(newSc->address, message->source);
newSc->instanceId = scGetAvailableInstanceId(supportedScMsg->scList, newSc);
if(newSc->instanceId == -1)
{
// Send negative conf (could not create sc)
confScMsg = confirmServiceConnectionMessageCreate();
jausAddressCopy(confScMsg->source, nmi->cmpt->address);
jausAddressCopy(confScMsg->destination, message->source);
confScMsg->serviceConnectionCommandCode = message->serviceConnectionCommandCode;
confScMsg->instanceId = 0;
confScMsg->confirmedPeriodicUpdateRateHertz = 0;
confScMsg->responseCode = JAUS_SC_CONNECTION_REFUSED;
txMessage = confirmServiceConnectionMessageToJausMessage(confScMsg);
nodeManagerSend(nmi, txMessage);
jausMessageDestroy(txMessage);
confirmServiceConnectionMessageDestroy(confScMsg);
jausAddressDestroy(newSc->address);
free(newSc);
pthread_mutex_unlock(&nmi->scm->mutex);
return;
}
newSc->queue = NULL;
newSc->queueSize = 0;
newSc->nextSc = NULL;
sc = scFindScInList(supportedScMsg->scList, newSc);
if(sc == NULL) // Test to see if the sc does not already exist
{
// The sc doesent exist, so we insert the new one into the list
sc = newSc;
sc->nextSc = supportedScMsg->scList;
supportedScMsg->scList = sc;
nmi->scm->outgoingScCount++;
}
else
{
jausAddressDestroy(newSc->address);
free(newSc);
}
sc->requestedUpdateRateHz = message->requestedPeriodicUpdateRateHertz;
sc->lastSentTime = 0.0;
sc->sequenceNumber = 0;
sc->isActive = JAUS_TRUE;
sc->confirmedUpdateRateHz = message->requestedPeriodicUpdateRateHertz; // TODO: calculate confirmedUpdateRateHz
confScMsg = confirmServiceConnectionMessageCreate();
jausAddressCopy(confScMsg->source, nmi->cmpt->address);
jausAddressCopy(confScMsg->destination, message->source);
confScMsg->serviceConnectionCommandCode = sc->commandCode;
confScMsg->instanceId = (JausByte)sc->instanceId;
confScMsg->confirmedPeriodicUpdateRateHertz = sc->confirmedUpdateRateHz;
confScMsg->responseCode = JAUS_SC_SUCCESSFUL;
txMessage = confirmServiceConnectionMessageToJausMessage(confScMsg);
nodeManagerSend(nmi, txMessage);
jausMessageDestroy(txMessage);
confirmServiceConnectionMessageDestroy(confScMsg);
pthread_mutex_unlock(&nmi->scm->mutex);
}
int ojCmptSendMessage(OjCmpt ojCmpt, JausMessage message)
{
jausAddressCopy(message->source, ojCmpt->jaus->address);
return nodeManagerSend(ojCmpt->nmi, message);
}
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);
}
}
JausBoolean scManagerCreateServiceConnection(NodeManagerInterface nmi, ServiceConnection sc)
{
CreateServiceConnectionMessage createSc;
JausMessage txMessage;
JausAddress localAddress;
ServiceConnection prevSc = NULL;
ServiceConnection testSc = nmi->scm->incommingSc;
if(!sc)
{
return JAUS_FALSE;
}
while(testSc)
{
if(sc == testSc)
{
if(prevSc)
{
prevSc->nextSc = testSc->nextSc;
}
else
{
nmi->scm->incommingSc = testSc->nextSc;
}
nmi->scm->incommingScCount--;
}
prevSc = testSc;
testSc = testSc->nextSc;
}
sc->confirmedUpdateRateHz = 0;
sc->lastSentTime = 0;
sc->sequenceNumber = 65535;
sc->instanceId = -1;
sc->isActive = JAUS_FALSE;
sc->nextSc = NULL;
createSc = createServiceConnectionMessageCreate();
createSc->source->id = nmi->cmpt->address->id;
createSc->serviceConnectionCommandCode = sc->commandCode;
createSc->requestedPeriodicUpdateRateHertz = sc->requestedUpdateRateHz;
createSc->presenceVector = sc->presenceVector;
if(sc->address && sc->address->subsystem != 0)
{
createSc->destination->id = sc->address->id;
txMessage = createServiceConnectionMessageToJausMessage(createSc);
nodeManagerSend(nmi, txMessage);
jausMessageDestroy(txMessage);
createServiceConnectionMessageDestroy(createSc);
sc->nextSc = nmi->scm->incommingSc;
nmi->scm->incommingSc = sc;
nmi->scm->incommingScCount++;
return JAUS_TRUE;
}
else
{
localAddress = jausAddressCreate();
localAddress->subsystem = nmi->cmpt->address->subsystem;
localAddress->node = 0;
localAddress->component = sc->address->component;
localAddress->instance = 0;
// Lookup Address from nodeManager
// Tests if the target component exists or not
if(nodeManagerLookupAddress(nmi, localAddress))
{
createSc->destination->id = localAddress->id;
sc->address->id = localAddress->id;
txMessage = createServiceConnectionMessageToJausMessage(createSc);
nodeManagerSend(nmi, txMessage);
jausMessageDestroy(txMessage);
jausAddressDestroy(localAddress);
createServiceConnectionMessageDestroy(createSc);
sc->nextSc = nmi->scm->incommingSc;
nmi->scm->incommingSc = sc;
nmi->scm->incommingScCount++;
return JAUS_TRUE;
}
else
{
jausAddressDestroy(localAddress);
createServiceConnectionMessageDestroy(createSc);
return JAUS_FALSE;
}
}
}
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;
}
}
JausBoolean scManagerCreatePeriodicEvent(NodeManagerInterface nmi, ServiceConnection sc)
{
CreateEventMessage createEvent;
JausMessage txMessage;
JausAddress localAddress;
ServiceConnection prevSc = NULL;
ServiceConnection testSc = nmi->scm->incommingSc;
// Test to see if the SC requested is already on the incomming list
// The sc sent to this function exists in the developer space, therefore
// do not destroy it. This happens if a component attempts to create the same
// service connection twice without removing it from the list first.
while(testSc)
{
if(sc == testSc)
{
if(prevSc)
{
prevSc->nextSc = testSc->nextSc;
}
else
{
nmi->scm->incommingSc = testSc->nextSc;
}
nmi->scm->incommingScCount--;
}
prevSc = testSc;
testSc = testSc->nextSc;
}
// Setup SC Control Parameters
sc->confirmedUpdateRateHz = 0;
sc->lastSentTime = 0;
sc->sequenceNumber = 65535;
sc->instanceId = -1;
sc->isActive = JAUS_FALSE;
sc->nextSc = NULL;
// Setup the Create Event Message
createEvent = createEventMessageCreate();
createEvent->source->id = nmi->cmpt->address->id;
jausBytePresenceVectorSetBit(&createEvent->presenceVector, CREATE_EVENT_PV_REQUESTED_RATE_BIT); // Set for Periodic Event, Specify the Requested Rate
jausBytePresenceVectorSetBit(&createEvent->presenceVector, CREATE_EVENT_PV_MINIMUM_RATE_BIT); // Set for Periodic Event, Specify the Requested Rate
createEvent->messageCode = sc->commandCode;
createEvent->eventType = EVENT_PERIODIC_TYPE;
createEvent->requestedMinimumRate = sc->requestedUpdateRateHz;
createEvent->requestedUpdateRate = sc->requestedUpdateRateHz;
jausMessageDestroy(createEvent->queryMessage);
createEvent->queryMessage = jausMessageDuplicate(sc->queryMessage);
localAddress = jausAddressCreate();
localAddress->subsystem = nmi->cmpt->address->subsystem;
localAddress->node = 0;
localAddress->component = sc->address->component;
localAddress->instance = 0;
// Retrieve AddressList from nodeManager
// Tests if the target component exists or not
if(localAddress)
{
createEvent->destination->id = localAddress->id;
sc->address->id = localAddress->id;
txMessage = createEventMessageToJausMessage(createEvent);
nodeManagerSend(nmi, txMessage);
jausMessageDestroy(txMessage);
jausAddressDestroy(localAddress);
createEventMessageDestroy(createEvent);
sc->nextSc = nmi->scm->incommingSc;
nmi->scm->incommingSc = sc;
nmi->scm->incommingScCount++;
return JAUS_TRUE;
}
else
{
jausAddressDestroy(localAddress);
createEventMessageDestroy(createEvent);
return JAUS_FALSE;
}
}
