void gposReadyState(OjCmpt gpos)
{
JausMessage txMessage;
ServiceConnection scList;
ServiceConnection sc;
PointLla vehiclePosLla;
ReportGlobalPoseMessage message;
message = (ReportGlobalPoseMessage)ojCmptGetUserData(gpos);
vehiclePosLla = vehicleSimGetPositionLla();
if(vehiclePosLla)
{
message->latitudeDegrees = vehiclePosLla->latitudeRadians * DEG_PER_RAD;
message->longitudeDegrees = vehiclePosLla->longitudeRadians * DEG_PER_RAD;
}
message->yawRadians = -(vehicleSimGetH() - M_PI/2.0);
message->elevationMeters = 0;
message->positionRmsMeters = 1.0;
message->rollRadians = 0.0;
message->pitchRadians = 0.0;
message->attitudeRmsRadians = 0.05;
jausTimeSetCurrentTime(message->time);
// Send message
if(ojCmptIsOutgoingScActive(gpos, JAUS_REPORT_GLOBAL_POSE))
{
scList = ojCmptGetScSendList(gpos, JAUS_REPORT_GLOBAL_POSE);
sc = scList;
while(sc)
{
jausAddressCopy(message->destination, sc->address);
message->presenceVector = sc->presenceVector;
message->sequenceNumber = sc->sequenceNumber;
message->properties.scFlag = JAUS_SERVICE_CONNECTION_MESSAGE;
txMessage = reportGlobalPoseMessageToJausMessage(message);
ojCmptSendMessage(gpos, txMessage);
jausMessageDestroy(txMessage);
sc = sc->nextSc;
}
ojCmptDestroySendList(scList);
}
}
//This function was pulled from another JAUS component for use with ROS2JAUS.
//It allows for the safe use of our service connections.
void sendServiceMessage( JausMessage message, OjCmpt cmpt )
{
ServiceConnection scList;
ServiceConnection sc;
if (ojCmptIsOutgoingScActive(cmpt, message->commandCode)) {
printf("Our outgoing SC is active...\n");
scList = ojCmptGetScSendList(cmpt, message->commandCode);
sc = scList;
while (sc) {
jausAddressCopy(message->destination, sc->address);
message->sequenceNumber = sc->sequenceNumber;
message->properties.scFlag = JAUS_SERVICE_CONNECTION_MESSAGE;
ojCmptSendMessage(cmpt, message);
jausMessageDestroy(message);
sc = sc->nextSc;
}
ojCmptDestroySendList(scList);
}
}
int gposGetScActive(OjCmpt gpos)
{
return ojCmptIsOutgoingScActive(gpos, JAUS_REPORT_GLOBAL_POSE);
}
