void TutorialComponent::initState(OjCmpt cmpt) {
std::cout << "[INITIALIZING]" << std::endl;
TutorialData *data = NULL;
data = (TutorialData *)ojCmptGetUserData(cmpt);
if (data) {
data->oneNumberMessage->number++;
JausAddress destination = jausAddressCreate();
// this is the address that we want to send it to
if (data->instance == 1) {
destination->instance = 2;
} else {
destination->instance = 1;
}
destination->component = JAUS_EXPERIMENTAL_TUTORIAL_COMPONENT;
destination->node = 1;
destination->subsystem = 1;
data->oneNumberMessage->source = ojCmptGetAddress(cmpt);
data->oneNumberMessage->destination = destination;
JausMessage txMsg = oneNumberMessageToJausMessage(data->oneNumberMessage);
ojCmptSendMessage(cmpt, txMsg);
ojCmptSetState(cmpt, JAUS_READY_STATE);
} else {
// bad things, man...
}
}
void scManagerProcessSuspendScMessage(NodeManagerInterface nmi, SuspendServiceConnectionMessage message)
{
SupportedScMessage supportedScMsg;
ServiceConnection sc;
ServiceConnection messageSc;
supportedScMsg = scFindSupportedScMsgInList(nmi->scm->supportedScMsgList, message->serviceConnectionCommandCode);
if(supportedScMsg == NULL)
{
return;
}
messageSc = (ServiceConnection)malloc( sizeof(ServiceConnectionStruct) );
if(messageSc == NULL)
{
return;
}
messageSc->address = jausAddressCreate();
messageSc->address->id = message->source->id;
messageSc->instanceId = message->instanceId;
sc = scFindScInList(supportedScMsg->scList, messageSc);
if(sc != NULL)
{
sc->isActive = JAUS_FALSE;
}
jausAddressDestroy(messageSc->address);
free(messageSc);
}
/**
* Método público que envía información sobre la cámara de la que debe enviar el
* streaming el nodo JAUS de Communication Management
*/
void RosNode_Communications::informCameraToStream(){
int status;
ros::NodeHandle nh;
nh.getParam("vehicleStatus", status);
JausMessage jMsg = NULL;
JausAddress jAdd = jausAddressCreate();
jAdd->subsystem = JAUS_SUBSYSTEM_UGV;
jAdd->node = JAUS_NODE_COMM_MNG;
jAdd->component = JAUS_VISUAL_SENSOR;
SelectCameraMessage scm = selectCameraMessageCreate();
if (status == OPERATION_MODE_OBSERVACION) {
scm->cameraID = currentObservationCamera;
} else {
scm->cameraID = currentDrivingCamera;
}
jausAddressCopy(scm->destination, jAdd);
jMsg = selectCameraMessageToJausMessage(scm);
if (jMsg != NULL) {
ojCmptSendMessage(instance->visualSensorComponent, jMsg);
} else {
ROS_INFO("[Control] Communications - No se ha posdido generar mensaje JAUS con informacion electrica de vehiculo");
}
selectCameraMessageDestroy(scm);
jausAddressDestroy(jAdd);
jausMessageDestroy(jMsg);
}
void TutorialComponent::standbyState(OjCmpt cmpt) {
//std::cout << "[STANDBY]" << std::endl;
TutorialData *data = (TutorialData *)ojCmptGetUserData(cmpt);
if (data) {
data->oneNumberMessage->number++;
JausAddress destination = jausAddressCreate();
// this is the address that we want to send it to
if (data->instance == 1) {
destination->instance = 2;
} else {
destination->instance = 1;
}
destination->component = JAUS_EXPERIMENTAL_TUTORIAL_COMPONENT;
destination->node = 1;
destination->subsystem = 1;
data->oneNumberMessage->source = ojCmptGetAddress(cmpt);
data->oneNumberMessage->destination = destination;
JausMessage txMsg = oneNumberMessageToJausMessage(data->oneNumberMessage);
ojCmptSendMessage(cmpt, txMsg);
if (data->oneNumberMessage->number >= data->limit) {
ojCmptSetState(cmpt, JAUS_SHUTDOWN_STATE);
} else {
ojCmptSetState(cmpt, JAUS_READY_STATE);
}
} else {
// bad stuff
}
}
JausComponent jausComponentCreate(void)
{
JausComponent component;
component = (JausComponent) calloc( 1, sizeof(JausComponentStruct) );
if(component == NULL)
{
return NULL;
}
// Init Values
component->identification = NULL;
component->address = jausAddressCreate();
if(!component->address)
{
free(component);
return NULL;
}
component->node = NULL;
component->state = JAUS_UNDEFINED_STATE;
component->authority = JAUS_DEFAULT_AUTHORITY;
component->services = jausServicesCreate();
if(!component->services)
{
jausAddressDestroy(component->address);
free(component);
return NULL;
}
component->controller.active = JAUS_FALSE;
component->controller.authority = JAUS_DEFAULT_AUTHORITY;
component->controller.state = JAUS_UNDEFINED_STATE;
component->controller.address = jausAddressCreate();
if(!component->controller.address)
{
jausServicesDestroy(component->services);
jausAddressDestroy(component->address);
free(component);
return NULL;
}
jausComponentUpdateTimestamp(component);
return component;
}
LargeMessageList lmListCreate(void)
{
LargeMessageList msgList = (LargeMessageList)malloc(sizeof(LargeMessageListStruct));
msgList->commandCode = 0;
msgList->source = jausAddressCreate();
msgList->messages = vectorCreate();
return msgList;
}
JausAddress ojCmptGetControllerAddress(OjCmpt ojCmpt)
{
if(ojCmptHasController(ojCmpt))
{
return jausAddressClone(ojCmpt->jaus->controller.address);
}
else
{
return jausAddressCreate();
}
}
JausComponent jausComponentClone(JausComponent component)
{
JausComponent clone;
size_t stringLength = 0;
clone = (JausComponent)malloc( sizeof(JausComponentStruct) );
if(clone == NULL)
{
return NULL;
}
// Init Values
if(component->identification)
{
stringLength = strlen(component->identification) + 1;
clone->identification = malloc(stringLength);
sprintf(clone->identification, "%s", component->identification);
}
else
{
clone->identification = NULL;
}
clone->address = jausAddressCreate();
jausAddressCopy(clone->address, component->address);
clone->node = component->node;
clone->state = component->state;
clone->authority = component->authority;
clone->controller.active = component->controller.active;
clone->controller.authority = component->controller.authority;
clone->controller.state = component->controller.state;
clone->controller.address = jausAddressCreate();
jausAddressCopy(clone->controller.address, component->controller.address);
clone->services = jausServicesClone(component->services);
jausComponentUpdateTimestamp(clone);
return clone;
}
ServiceConnection serviceConnectionCreate(void)
{
ServiceConnection sc;
sc = (ServiceConnection) malloc(sizeof(ServiceConnectionStruct));
if(sc)
{
sc->requestedUpdateRateHz = 0;
sc->confirmedUpdateRateHz = 0;
sc->lastSentTime = 0;
sc->timeoutSec = 0;
sc->nextRequestTimeSec = 0;
sc->presenceVector = 0;
sc->commandCode = 0;
sc->sequenceNumber = 0;
sc->instanceId = -1;
sc->isActive = JAUS_FALSE;
sc->queueSize = 0;
sc->nextSc = NULL;
sc->address = jausAddressCreate();
if(!sc->address)
{
// Failed to init address
free(sc);
return NULL;
}
sc->queue = queueCreate();
if(!sc->queue)
{
// Failed to init queue
jausAddressDestroy(sc->address);
free(sc);
return NULL;
}
return sc;
}
else
{
return NULL;
}
}
ServiceConnection serviceConnectionCreate(void)
{
ServiceConnection sc;
sc = (ServiceConnection) malloc(sizeof(ServiceConnectionStruct));
if(sc)
{
sc->address = jausAddressCreate();
sc->queryMessage = NULL;
sc->queue = queueCreate();
sc->serviceConnectionType = 0;
return sc;
}
else
{
return NULL;
}
}
/**
* Método público que envía información de heartbeat (mensaje JAUS Heartbeat -
* Position Info) a los nodos JAUS de Communication Management de todos los
* subsistemas
*/
void RosNode_Communications::informHeartbeatPositionInfo() {
JausMessage jMsg = NULL;
JausAddress jAdd = jausAddressCreate();
jAdd->subsystem = JAUS_SUBSYSTEM_UGV;
jAdd->node = JAUS_NODE_COMM_MNG;
jAdd->component = JAUS_HEARTBEAT_INFORMATION;
HeartbeatPositionInfo17Message hpi = heartbeatPositionInfo17MessageCreate();
hpi->latitude = hbPosition.latitude;
hpi->longitude = hbPosition.longitude;
hpi->altitude = hbPosition.altitude;
hpi->heading = hbPosition.heading;
hpi->speed = hbPosition.speed;
// Hacia UGV
jausAddressCopy(hpi->destination, jAdd);
jMsg = heartbeatPositionInfo17MessageToJausMessage(hpi);
if (jMsg != NULL) {
ojCmptSendMessage(instance->heartBeatInformationComponent, jMsg);
} else {
ROS_INFO("[Control] Communications - No se ha posdido generar mensaje JAUS con informacion heartbeat hacia UGV");
}
// Hacia MyC
jAdd->subsystem = JAUS_SUBSYSTEM_MYC;
jausAddressCopy(hpi->destination, jAdd);
jMsg = heartbeatPositionInfo17MessageToJausMessage(hpi);
if (jMsg != NULL) {
ojCmptSendMessage(instance->heartBeatInformationComponent, jMsg);
} else {
ROS_INFO("[Control] Communications - No se ha posdido generar mensaje JAUS con informacion heartbeat hacia MyC");
}
// Hacia USV
jAdd->subsystem = JAUS_SUBSYSTEM_USV;
jausAddressCopy(hpi->destination, jAdd);
jMsg = heartbeatPositionInfo17MessageToJausMessage(hpi);
if (jMsg != NULL) {
ojCmptSendMessage(instance->heartBeatInformationComponent, jMsg);
} else {
ROS_INFO("[Control] Communications - No se ha posdido generar mensaje JAUS con informacion heartbeat hacia USV");
}
heartbeatPositionInfo17MessageDestroy(hpi);
jausAddressDestroy(jAdd);
jausMessageDestroy(jMsg);
}
/**
* Método público que obtiene el modo de operación activo de la máquina de
* estados, lo traduce a JAUS (Report Mission Status) y lo envía al controlador
*/
void RosNode_Communications::informStatus() {
// Obtencion del estado
int status;
ros::NodeHandle nh;
nh.getParam("vehicleStatus", status);
JausMessage jMsg = NULL;
JausAddress jAdd = jausAddressCreate();
jAdd->subsystem = instance->subsystemController;
jAdd->node = instance->nodeController;
jAdd->component = JAUS_MISSION_SPOOLER;
jAdd->instance = JAUS_DESTINANTION_INSTANCE;
ReportMissionStatusMessage rmsm = reportMissionStatusMessageCreate();
if (status == OPERATION_MODE_LOCAL) {
rmsm->missionId = JAUS_OPERATION_MODE_LOCAL;
} else if (status == OPERATION_MODE_CONDUCCION) {
rmsm->missionId = JAUS_OPERATION_MODE_CONDUCCION;
} else if (status == OPERATION_MODE_OBSERVACION) {
rmsm->missionId = JAUS_OPERATION_MODE_OBSERVACION;
} else {
rmsm->missionId = JAUS_OPERATION_MODE_INICIANDO;
}
rmsm->status = JAUS_CONTROLLER_NONE;
if(subsystemController == JAUS_SUBSYSTEM_MYC && nodeController == JAUS_NODE_CONTROL)
rmsm->status = JAUS_CONTROLLER_C2;
else if(subsystemController == JAUS_SUBSYSTEM_UGV && nodeController == JAUS_NODE_TABLET)
rmsm->status = JAUS_CONTROLLER_TABLET;
jausAddressCopy(rmsm->destination, jAdd);
jMsg = reportMissionStatusMessageToJausMessage(rmsm);
if (jMsg != NULL) {
ojCmptSendMessage(instance->missionSpoolerComponent, jMsg);
} else {
ROS_INFO("[Control] Communications - No se ha posdido generar mensaje JAUS con informacion electrica de vehiculo");
}
reportMissionStatusMessageDestroy(rmsm);
jausAddressDestroy(jAdd);
jausMessageDestroy(jMsg);
}
void scManagerProcessActivateScMessage(NodeManagerInterface nmi, ActivateServiceConnectionMessage message)
{
SupportedScMessage supportedScMsg;
ServiceConnection sc;
ServiceConnection messageSc;
pthread_mutex_lock(&nmi->scm->mutex);
supportedScMsg = scFindSupportedScMsgInList(nmi->scm->supportedScMsgList, message->serviceConnectionCommandCode);
if(supportedScMsg == NULL)
{
pthread_mutex_unlock(&nmi->scm->mutex);
return;
}
messageSc = (ServiceConnection)malloc( sizeof(ServiceConnectionStruct) );
if(messageSc == NULL)
{
pthread_mutex_unlock(&nmi->scm->mutex);
return;
}
messageSc->address = jausAddressCreate();
jausAddressCopy(messageSc->address, message->source);
messageSc->instanceId = message->instanceId;
sc = scFindScInList(supportedScMsg->scList, messageSc);
if(sc != NULL)
{
sc->isActive = JAUS_TRUE;
}
jausAddressDestroy(messageSc->address);
free(messageSc);
pthread_mutex_unlock(&nmi->scm->mutex);
}
// ***********************************************************
// 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");
}
}
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 OjUdpComponentInterface::run()
{
DatagramPacket packet;
JausAddress address, currentAddress;
JausAddress lookupAddress;
int bytesRecv = 0;
char buf[256] = {0};
int componentId = 0;
int commandCode = 0;
int serviceType = 0;
packet = datagramPacketCreate();
packet->bufferSizeBytes = OJ_UDP_INTERFACE_MESSAGE_SIZE_BYTES;
packet->buffer = (unsigned char *) calloc(packet->bufferSizeBytes, 1);
while(this->running)
{
bytesRecv = datagramSocketReceive(this->socket, packet);
if(bytesRecv == OJ_UDP_INTERFACE_MESSAGE_SIZE_BYTES)
{
// This is to ensure we are using a valid NM pointer (this occurs
this->nodeManager = ((JausComponentCommunicationManager *)this->commMngr)->getNodeManagerComponent();
switch(packet->buffer[0])
{
case CHECK_IN:
componentId = (packet->buffer[1] & 0xFF);
if(componentId < JAUS_MINIMUM_COMPONENT_ID || componentId > JAUS_MAXIMUM_COMPONENT_ID)
{
sprintf(buf, "Invalid Component Id (%d) trying to check in.", componentId);
ErrorEvent *e = new ErrorEvent(ErrorEvent::Configuration, __FUNCTION__, __LINE__, buf);
this->eventHandler->handleEvent(e);
return;
}
address = this->nodeManager->checkInLocalComponent(componentId);
if(!address || !jausAddressIsValid(address))
{
sprintf(buf, "Cannot add local component with Id: %d.", componentId);
ErrorEvent *e = new ErrorEvent(ErrorEvent::Warning, __FUNCTION__, __LINE__, buf);
this->eventHandler->handleEvent(e);
// TODO: Send back checkin error reply (no available instance)
break;
}
memset(packet->buffer, 0, OJ_UDP_INTERFACE_MESSAGE_SIZE_BYTES);
packet->buffer[0] = REPORT_ADDRESS;
packet->buffer[1] = (unsigned char)(address->instance & 0xFF);
packet->buffer[2] = (unsigned char)(address->component & 0xFF);
packet->buffer[3] = (unsigned char)(address->node & 0xFF);
packet->buffer[4] = (unsigned char)(address->subsystem & 0xFF);
datagramSocketSend(this->socket, packet);
jausAddressDestroy(address);
break;
case CHECK_OUT:
nodeManager->checkOutLocalComponent(packet->buffer[4], packet->buffer[3], packet->buffer[2], packet->buffer[1]);
break;
case VERIFY_ADDRESS:
lookupAddress = jausAddressCreate();
lookupAddress->instance = (packet->buffer[1] & 0xFF);
lookupAddress->component = (packet->buffer[2] & 0xFF);
lookupAddress->node = (packet->buffer[3] & 0xFF);
lookupAddress->subsystem = (packet->buffer[4] & 0xFF);
memset(packet->buffer, 0, OJ_UDP_INTERFACE_MESSAGE_SIZE_BYTES);
packet->buffer[0] = ADDRESS_VERIFIED;
if(this->systemTree->hasComponent(lookupAddress))
{
packet->buffer[1] = JAUS_TRUE;
}
else
{
packet->buffer[1] = JAUS_FALSE;
}
datagramSocketSend(this->socket, packet);
jausAddressDestroy(lookupAddress);
break;
case GET_COMPONENT_ADDRESS_LIST:
componentId = packet->buffer[1] & 0xFF;
address = systemTree->lookUpAddress(JAUS_ADDRESS_WILDCARD_OCTET, JAUS_ADDRESS_WILDCARD_OCTET, componentId, JAUS_ADDRESS_WILDCARD_OCTET);
memset(packet->buffer, 0, OJ_UDP_INTERFACE_MESSAGE_SIZE_BYTES);
packet->buffer[0] = COMPONENT_ADDRESS_LIST_RESPONSE;
//
if(!address || !jausAddressIsValid(address))
{
packet->buffer[1] = (unsigned char)(JAUS_INVALID_INSTANCE_ID & 0xFF);
packet->buffer[2] = (unsigned char)(JAUS_INVALID_COMPONENT_ID & 0xFF);
packet->buffer[3] = (unsigned char)(JAUS_INVALID_NODE_ID & 0xFF);
packet->buffer[4] = (unsigned char)(JAUS_INVALID_SUBSYSTEM_ID & 0xFF);
}
else
{
packet->buffer[1] = (unsigned char)(address->instance & 0xFF);
packet->buffer[2] = (unsigned char)(address->component & 0xFF);
packet->buffer[3] = (unsigned char)(address->node & 0xFF);
packet->buffer[4] = (unsigned char)(address->subsystem & 0xFF);
}
datagramSocketSend(this->socket, packet);
jausAddressDestroy(address);
break;
case LOOKUP_ADDRESS:
lookupAddress = jausAddressCreate();
lookupAddress->instance = (packet->buffer[1] & 0xFF);
lookupAddress->component = (packet->buffer[2] & 0xFF);
lookupAddress->node = (packet->buffer[3] & 0xFF);
lookupAddress->subsystem = (packet->buffer[4] & 0xFF);
memset(packet->buffer, 0, OJ_UDP_INTERFACE_MESSAGE_SIZE_BYTES);
packet->buffer[0] = LOOKUP_ADDRESS_RESPONSE;
address = systemTree->lookUpAddress(lookupAddress);
if(address && jausAddressIsValid(address))
{
packet->buffer[5] = (unsigned char) JAUS_TRUE;
}
else
{
packet->buffer[5] = (unsigned char) JAUS_FALSE;
}
packet->buffer[1] = (unsigned char) (address->instance & 0xFF);
packet->buffer[2] = (unsigned char) (address->component & 0xFF);
packet->buffer[3] = (unsigned char) (address->node & 0xFF);
packet->buffer[4] = (unsigned char) (address->subsystem & 0xFF);
datagramSocketSend(this->socket, packet);
while(address) // NMJ
{ // NMJ
currentAddress = address; // NMJ
address = address->next; // NMJ
jausAddressDestroy(currentAddress); // NMJ
} // NMJ
jausAddressDestroy(lookupAddress);
break;
case LOOKUP_SERVICE_ADDRESS:
lookupAddress = jausAddressCreate();
lookupAddress->instance = (packet->buffer[1] & 0xFF);
lookupAddress->component = (packet->buffer[2] & 0xFF);
lookupAddress->node = (packet->buffer[3] & 0xFF);
lookupAddress->subsystem = (packet->buffer[4] & 0xFF);
commandCode = (packet->buffer[5] & 0xFF) + ((packet->buffer[6] & 0xFF) << 8);
serviceType = (packet->buffer[7] & 0xFF);
memset(packet->buffer, 0, OJ_UDP_INTERFACE_MESSAGE_SIZE_BYTES);
packet->buffer[0] = LOOKUP_SERVICE_ADDRESS_RESPONSE;
address = systemTree->lookUpServiceInSystem(commandCode, serviceType);
if(address && jausAddressIsValid(address))
{
packet->buffer[5] = (unsigned char) JAUS_TRUE;
packet->buffer[1] = (unsigned char) (address->instance & 0xFF);
packet->buffer[2] = (unsigned char) (address->component & 0xFF);
packet->buffer[3] = (unsigned char) (address->node & 0xFF);
packet->buffer[4] = (unsigned char) (address->subsystem & 0xFF);
}
else
{
packet->buffer[5] = (unsigned char) JAUS_FALSE;
}
datagramSocketSend(this->socket, packet);
if(address)
{
jausAddressDestroy(address);
jausAddressDestroy(lookupAddress);
}
break;
//case InterfaceMessage.NODE_MANAGER_LOOKUP_SERVICE_ADDRESS_LIST:
// lookupAddress = jausAddressCreate();
// lookupAddress->instance(packet->buffer[1] & 0xFF);
// lookupAddress->component(packet->buffer[2] & 0xFF);
// lookupAddress->node(packet->buffer[3] & 0xFF);
// lookupAddress->subsystem(packet->buffer[4] & 0xFF);
//
// commandCode = (packet->buffer[5] & 0xFF) + ((packet->buffer[6] & 0xFF) << 8);
// serviceType = (packet->buffer[7] & 0xFF);
// memset(packet->buffer, 0, OJ_UDP_INTERFACE_MESSAGE_SIZE_BYTES);
// packet->buffer[0] = LOOKUP_SERVICE_ADDRESS_LIST_RESPONSE;
// //System.out.println("CmptInterface: Get address for component ID: " + componentId);
// //address = subsystemTable.lookupServiceAddressList(address, commandCode, serviceCommandType);
//
// if(lookupAddress->subsystem ==
// if(address == null)
// {
// replyMessage.getData()[0] = (byte)(0 & 0xff);
// replyMessage.getData()[1] = (byte)(0 & 0xff);
// replyMessage.getData()[2] = (byte)(0 & 0xff);
// replyMessage.getData()[3] = (byte)(0 & 0xff);
// replyMessage.getData()[4] = (byte)0;
// //System.out.println("CmptInterface: Get address for component ID: " + componentId + " returning: 0.0.0.0");
// }
// else
// {
// replyMessage.getData()[0] = (byte)(address.getInstance() & 0xff);
// replyMessage.getData()[1] = (byte)(address.getComponent() & 0xff);
// replyMessage.getData()[2] = (byte)(address.getNode() & 0xff);
// replyMessage.getData()[3] = (byte)(address.getSubsystem() & 0xff);
// replyMessage.getData()[4] = (byte)1;
// //System.out.println("CmptInterface: Get address for component ID: " + componentId + " returning: " + address);
// }
//
// replyBuffer = new byte[8];
// replyMessage.pack(replyBuffer);
// outPacket = new DatagramPacket(replyBuffer, replyBuffer.length, packet.getAddress(), packet.getPort());
// socket.send(outPacket);
// break;
default:
sprintf(buf, "Unknown Interface Message Received. CC: 0x%02X\n", packet->buffer[0]);
ErrorEvent *e = new ErrorEvent(ErrorEvent::Warning, __FUNCTION__, __LINE__, buf);
this->eventHandler->handleEvent(e);
break;
}
}
}
free(packet->buffer);
datagramPacketDestroy(packet);
}
JausAddress SystemTree::lookUpAddress(int lookupSubs, int lookupNode, int lookupCmpt, int lookupInst)
{
JausSubsystem subs = NULL;
JausNode node = NULL;
JausComponent cmpt = NULL;
JausAddress address = jausAddressCreate();
if(!address)
{
// TODO: Log Error. Throw Exception.
return NULL;
}
address->subsystem = JAUS_INVALID_SUBSYSTEM_ID;
address->node = JAUS_INVALID_NODE_ID;
address->component = JAUS_INVALID_COMPONENT_ID;
address->instance = JAUS_INVALID_INSTANCE_ID;
address->next = NULL;
JausAddress returnAddress = address;
// Find this address
// If the subsystem is a wild card
if(lookupSubs == JAUS_ADDRESS_WILDCARD_OCTET)
{
// Look through all subsystems
for(int i = JAUS_MINIMUM_SUBSYSTEM_ID; i < JAUS_MAXIMUM_SUBSYSTEM_ID; i++)
{
subs = system[i];
if(!subs)
{
continue;
}
// If the node is a wild card
if(lookupNode == JAUS_ADDRESS_WILDCARD_OCTET)
{
// Look through all nodes
for(int j = 0; j < subs->nodes->elementCount; j++)
{
node = (JausNode) subs->nodes->elementData[j];
// If the component or the instance is a wild card
if(lookupCmpt == JAUS_ADDRESS_WILDCARD_OCTET || lookupInst == JAUS_ADDRESS_WILDCARD_OCTET)
{
// Look through all components
for(int k = 0; k < node->components->elementCount; k++)
{
cmpt = (JausComponent) node->components->elementData[k];
// If the component is wild card and this is the instance we are looking for
if( lookupCmpt == JAUS_ADDRESS_WILDCARD_OCTET && cmpt->address->instance == lookupInst)
{
// If the address is valid (it should never be)
if(jausAddressIsValid(address))
{
address->next = jausAddressClone(cmpt->address); // Why do we clone if the address is valid??
if(!address->next)
{
return returnAddress;
}
else
{
address = address->next;
}
}
// Address was not valid
else
{
jausAddressCopy(address, cmpt->address); // Copy the address of the component we are looking at
address->next = jausAddressCreate(); // Create a new address. Gets created even if we don't do anything else?
if(!address->next)
{
return returnAddress;
}
else
{
address = address->next;
}
}
}
// If the instance is a wild card and this is the component we are looking for
else if(lookupInst == JAUS_ADDRESS_WILDCARD_OCTET && cmpt->address->component == lookupCmpt)
{
if(jausAddressIsValid(address))
{
address->next = jausAddressClone(cmpt->address);
if(!address->next)
{
return returnAddress;
}
else
{
address = address->next;
}
}
else
{
jausAddressCopy(address, cmpt->address);
address->next = jausAddressCreate();
if(!address->next)
{
return returnAddress;
}
else
{
address = address->next;
}
}
}
}
}
else
{
cmpt = findComponent(subs->id, node->id, lookupCmpt, lookupInst);
if(cmpt)
{
if(jausAddressIsValid(address))
{
address->next = jausAddressClone(cmpt->address);
if(!address->next)
{
return returnAddress;
}
else
{
address = address->next;
}
}
else
{
jausAddressCopy(address, cmpt->address);
address->next = jausAddressCreate();
if(!address->next)
{
return returnAddress;
}
else
{
address = address->next;
}
}
}
}
}
}
else
{
node = findNode(i, lookupNode);
if(lookupCmpt == JAUS_ADDRESS_WILDCARD_OCTET || lookupInst == JAUS_ADDRESS_WILDCARD_OCTET)
{
// Look through all components
for(int k = 0; k < node->components->elementCount; k++)
{
cmpt = (JausComponent) node->components->elementData[k];
if( lookupCmpt == JAUS_ADDRESS_WILDCARD_OCTET &&
cmpt->address->instance == lookupInst)
{
if(jausAddressIsValid(address))
{
address->next = jausAddressClone(cmpt->address);
if(!address->next)
{
return returnAddress;
}
else
{
address = address->next;
}
}
else
{
jausAddressCopy(address, cmpt->address);
address->next = jausAddressCreate();
if(!address->next)
{
return returnAddress;
}
else
{
address = address->next;
}
}
}
else if(lookupInst == JAUS_ADDRESS_WILDCARD_OCTET &&
cmpt->address->component == lookupCmpt)
{
if(jausAddressIsValid(address))
{
address->next = jausAddressClone(cmpt->address);
if(!address->next)
{
return returnAddress;
}
else
{
address = address->next;
}
}
else
{
jausAddressCopy(address, cmpt->address);
address->next = jausAddressCreate();
if(!address->next)
{
return returnAddress;
}
else
{
address = address->next;
}
}
}
}
}
else
{
cmpt = findComponent(subs->id, node->id, lookupCmpt, lookupInst);
if(cmpt)
{
if(jausAddressIsValid(address))
{
address->next = jausAddressClone(cmpt->address);
if(!address->next)
{
return returnAddress;
}
else
{
address = address->next;
}
}
else
{
jausAddressCopy(address, cmpt->address);
address->next = jausAddressCreate();
if(!address->next)
{
return returnAddress;
}
else
{
address = address->next;
}
}
}
}
}
}
}
// We are looking within a specific subsystem
else
{
subs = system[lookupSubs];
if(!subs)
{
// Subsystem not found
// TODO: Log Error. Throw Exception.
return address;
}
if(lookupNode == JAUS_ADDRESS_WILDCARD_OCTET)
{
// Look through all nodes
for(int j = 0; j < subs->nodes->elementCount; j++)
{
node = (JausNode) subs->nodes->elementData[j];
if(lookupCmpt == JAUS_ADDRESS_WILDCARD_OCTET || lookupInst == JAUS_ADDRESS_WILDCARD_OCTET)
{
// Look through all components
for(int k = 0; k < node->components->elementCount; k++)
{
cmpt = (JausComponent) node->components->elementData[k];
if( lookupCmpt == JAUS_ADDRESS_WILDCARD_OCTET && cmpt->address->instance == lookupInst)
{
if(jausAddressIsValid(address))
{
address->next = jausAddressClone(cmpt->address);
if(!address->next)
{
return returnAddress;
}
else
{
address = address->next;
}
}
else
{
jausAddressCopy(address, cmpt->address);
address->next = jausAddressCreate();
if(!address->next)
{
return returnAddress;
}
else
{
address = address->next;
}
}
}
else if(lookupInst == JAUS_ADDRESS_WILDCARD_OCTET && cmpt->address->component == lookupCmpt)
{
if(jausAddressIsValid(address))
{
address->next = jausAddressClone(cmpt->address);
if(!address->next)
{
return returnAddress;
}
else
{
address = address->next;
}
}
else
{
jausAddressCopy(address, cmpt->address);
address->next = jausAddressCreate();
if(!address->next)
{
return returnAddress;
}
else
{
address = address->next;
}
}
}
}
}
else
{
cmpt = findComponent(subs->id, node->id, lookupCmpt, lookupInst);
if(cmpt)
{
if(jausAddressIsValid(address))
{
address->next = jausAddressClone(cmpt->address);
if(!address->next)
{
return returnAddress;
}
else
{
address = address->next;
}
}
else
{
jausAddressCopy(address, cmpt->address);
address->next = jausAddressCreate();
if(!address->next)
{
return returnAddress;
}
else
{
address = address->next;
}
}
}
}
}
}
else
{
node = findNode(subs->id, lookupNode);
if(lookupCmpt == JAUS_ADDRESS_WILDCARD_OCTET || lookupInst == JAUS_ADDRESS_WILDCARD_OCTET)
{
// Look through all components
for(int k = 0; k < node->components->elementCount; k++)
{
cmpt = (JausComponent) node->components->elementData[k];
if( lookupCmpt == JAUS_ADDRESS_WILDCARD_OCTET &&
cmpt->address->instance == lookupInst)
{
if(jausAddressIsValid(address))
{
address->next = jausAddressClone(cmpt->address);
if(!address->next)
{
return returnAddress;
}
else
{
address = address->next;
}
}
else
{
jausAddressCopy(address, cmpt->address);
address->next = jausAddressCreate();
if(!address->next)
{
return returnAddress;
}
else
{
address = address->next;
}
}
}
else if(lookupInst == JAUS_ADDRESS_WILDCARD_OCTET &&
cmpt->address->component == lookupCmpt)
{
if(jausAddressIsValid(address))
{
address->next = jausAddressClone(cmpt->address);
if(!address->next)
{
return returnAddress;
}
else
{
address = address->next;
}
}
else
{
jausAddressCopy(address, cmpt->address);
address->next = jausAddressCreate();
if(!address->next)
{
return returnAddress;
}
else
{
address = address->next;
}
}
}
}
}
else
{
cmpt = findComponent(subs->id, node->id, lookupCmpt, lookupInst);
if(cmpt)
{
if(jausAddressIsValid(address))
{
address->next = jausAddressClone(cmpt->address);
if(!address->next)
{
return returnAddress;
}
else
{
address = address->next;
}
}
else
{
jausAddressCopy(address, cmpt->address);
address->next = jausAddressCreate();
if(!address->next)
{
return returnAddress;
}
else
{
address = address->next;
}
}
}
}
}
}
return returnAddress;
}
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);
}
void TutorialComponent::processMessage(OjCmpt cmpt, JausMessage msg) {
//<ROS2JAUS>
TutorialData *data = NULL;
data = (TutorialData *)ojCmptGetUserData(cmpt);
SetWrenchEffortMessage wrMsg;
beginner_tutorials::wrenchData rosWrenchMsg;
switch (msg->commandCode) {
case JAUS_SET_WRENCH_EFFORT:
//If we recieve a set wrench message from someone, publish the contents of the message to ROS
wrMsg = setWrenchEffortMessageFromJausMessage(msg);
rosWrenchMsg.throttle = wrMsg->propulsiveLinearEffortXPercent;
rosWrenchMsg.brake = wrMsg->resistiveLinearEffortXPercent;
rosWrenchMsg.steering = wrMsg->propulsiveRotationalEffortZPercent;
data->wrenchPub->publish(rosWrenchMsg);
case JAUS_REQUEST_COMPONENT_CONTROL:
//Hardcode an outgoing address, only necessary if the message doesn't provide a source
JausAddress addr;
addr = jausAddressCreate();
addr->subsystem = 1;
addr->node = 1;
addr->component = JAUS_EXPERIMENTAL_MOTION_PROFILE_DRIVER;
addr->instance = 1;
//Spoof a message to confirm control of the primitive driver
ConfirmComponentControlMessage confirmComponentControl;
confirmComponentControl = confirmComponentControlMessageCreate();
jausAddressCopy(confirmComponentControl->source, ojCmptGetAddress(cmpt));
jausAddressCopy(confirmComponentControl->destination, addr);
confirmComponentControl->responseCode = JAUS_CONTROL_ACCEPTED;
JausMessage confirmControl;
confirmControl = confirmComponentControlMessageToJausMessage(confirmComponentControl);
ojCmptSendMessage(cmpt, confirmControl);
//Spoof a message to report the status of the component
ReportComponentStatusMessage reportComponentStatus;
reportComponentStatus = reportComponentStatusMessageCreate();
reportComponentStatus->primaryStatusCode = JAUS_READY_STATE;
jausAddressCopy(reportComponentStatus->source, ojCmptGetAddress(cmpt));
jausAddressCopy(reportComponentStatus->destination, addr);
JausMessage reportStatus;
reportStatus = reportComponentStatusMessageToJausMessage(reportComponentStatus);
ojCmptSendMessage(cmpt, reportStatus);
ojCmptDefaultMessageProcessor(cmpt, msg);
//</ROS2JAUS>
default:
//if we recieve an unfamiliar message, print its command code
if (msg->commandCode != JAUS_REPORT_HEARTBEAT_PULSE && msg->commandCode != JAUS_CREATE_SERVICE_CONNECTION) {
std::cout << "Received message..." << std::endl;
std::cout << "Command code=" << jausCommandCodeString(msg->commandCode) << std::endl;
}
ojCmptDefaultMessageProcessor(cmpt, msg);
}
}
/**
* Método privado consumidor de topic asociado a información recopilada de los
* sensores de Posición/Orientación. Traduce la información a JAUS (Report
* Global Pose/Report Velocity State/Additional GPS/INS Info) y la envía al
* controlador
* @param[in] msg Mensaje ROS con información de posición/orientación
*/
void RosNode_Communications::fnc_subs_posOriInfo(CITIUS_Control_Communication::msg_posOriInfo msg) {
JausMessage jMsg = NULL;
JausAddress jAdd = jausAddressCreate();
jAdd->subsystem = subsystemController;
jAdd->node = nodeController;
jAdd->component = JAUS_GLOBAL_POSE_SENSOR;
jAdd->instance = JAUS_DESTINANTION_INSTANCE;
// Report Global Pose
ReportGlobalPoseMessage rgpm = reportGlobalPoseMessageCreate();
rgpm->presenceVector = 0x0077;
rgpm->latitudeDegrees = msg.latitude;
hbPosition.latitude = msg.latitude;
rgpm->longitudeDegrees = msg.longitude;
hbPosition.longitude = msg.longitude;
rgpm->attitudeRmsRadians = msg.altitude;
hbPosition.altitude = msg.altitude;
rgpm->rollRadians = msg.roll;
rgpm->pitchRadians = msg.pitch;
rgpm->yawRadians = msg.yaw;
hbPosition.heading = msg.yaw;
jausAddressCopy(rgpm->destination, jAdd);
jMsg = reportGlobalPoseMessageToJausMessage(rgpm);
reportGlobalPoseMessageDestroy(rgpm);
if (jMsg != NULL) {
ojCmptSendMessage(globalPoseSensorComponent, jMsg);
}
// Report Velocity State
ReportVelocityStateMessage rvsm = reportVelocityStateMessageCreate();
rvsm->presenceVector = 0x0007;
rvsm->velocityXMps = msg.velX;
rvsm->velocityYMps = msg.velY;
rvsm->velocityZMps = msg.velZ;
hbPosition.speed = sqrt(pow(msg.velX,2)+pow(msg.velY,2)+pow(msg.velZ,2));
jAdd->component = JAUS_VELOCITY_STATE_SENSOR;
jAdd->instance = JAUS_DESTINANTION_INSTANCE;
jausAddressCopy(rvsm->destination, jAdd);
jMsg = reportVelocityStateMessageToJausMessage(rvsm);
reportVelocityStateMessageDestroy(rvsm);
if (jMsg != NULL) {
ojCmptSendMessage(velocityStateSensorComponent, jMsg);
}
// Additional GPS/INS Info
AditionalGPSINSInfo4Message agim = aditionalGPSINSInfo4MessageCreate();
agim->presenceVector = 0x0007;
agim->longitudinal_acc = msg.accX;
agim->lateral_acc = msg.accY;
agim->vertical_acc = msg.accZ;
// Estado IMU y GPS
// TODO!!!!!!!!!!!!!!!!!!!!!!!!
jAdd->component = JAUS_GLOBAL_POSE_SENSOR;
jAdd->instance = JAUS_DESTINANTION_INSTANCE;
jausAddressCopy(agim->destination, jAdd);
jMsg = aditionalGPSINSInfo4MessageToJausMessage(agim);
aditionalGPSINSInfo4MessageDestroy(agim);
if (jMsg != NULL) {
ojCmptSendMessage(velocityStateSensorComponent, jMsg);
}
jausAddressDestroy(jAdd);
jausMessageDestroy(jMsg);
}
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;
}
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;
}
}
