void jausComponentDestroy(JausComponent component)
{
if(component)
{
jausServicesDestroy(component->services);
jausAddressDestroy(component->controller.address);
jausAddressDestroy(component->address);
if(component->identification) free(component->identification);
free(component);
}
}
// Function: gposStartup
// Access: Public
// Description: This function allows the abstracted component functionality contained in this file to be started from an external source.
// It must be called first before the component state machine and node manager interaction will begin
// Each call to "gposStartup" should be followed by one call to the "gposShutdown" function
OjCmpt gposCreate(void)
{
OjCmpt cmpt;
ReportGlobalPoseMessage message;
JausAddress gposAddr;
cmpt = ojCmptCreate("gpos", JAUS_GLOBAL_POSE_SENSOR, GPOS_THREAD_DESIRED_RATE_HZ);
ojCmptAddService(cmpt, JAUS_GLOBAL_POSE_SENSOR);
ojCmptAddServiceInputMessage(cmpt, JAUS_GLOBAL_POSE_SENSOR, JAUS_QUERY_GLOBAL_POSE, 0xFF);
ojCmptAddServiceOutputMessage(cmpt, JAUS_GLOBAL_POSE_SENSOR, JAUS_REPORT_GLOBAL_POSE, 0xFF);
ojCmptSetStateCallback(cmpt, JAUS_READY_STATE, gposReadyState);
ojCmptSetMessageCallback(cmpt, JAUS_QUERY_GLOBAL_POSE, gposQueryGlobalPoseCallback);
ojCmptAddSupportedSc(cmpt, JAUS_REPORT_GLOBAL_POSE);
message = reportGlobalPoseMessageCreate();
gposAddr = ojCmptGetAddress(cmpt);
jausAddressCopy(message->source, gposAddr);
jausAddressDestroy(gposAddr);
ojCmptSetUserData(cmpt, (void *)message);
ojCmptSetState(cmpt, JAUS_READY_STATE);
if(ojCmptRun(cmpt))
{
ojCmptDestroy(cmpt);
return NULL;
}
return cmpt;
}
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);
}
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;
}
void serviceConnectionDestroy(ServiceConnection sc)
{
jausAddressDestroy(sc->address);
if(sc->serviceConnectionType == SC_EVENT_TYPE)
{
jausMessageDestroy(sc->queryMessage);
}
queueDestroy(sc->queue, (void *)jausMessageDestroy);
free(sc);
}
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;
}
}
OjCmpt pdCreate(void)
{
OjCmpt cmpt;
PdData *data;
JausAddress pdAddr;
cmpt = ojCmptCreate("pd", JAUS_PRIMITIVE_DRIVER, PD_THREAD_DESIRED_RATE_HZ);
ojCmptAddService(cmpt, JAUS_PRIMITIVE_DRIVER);
ojCmptAddServiceInputMessage(cmpt, JAUS_PRIMITIVE_DRIVER, JAUS_SET_WRENCH_EFFORT, 0xFF);
ojCmptAddServiceInputMessage(cmpt, JAUS_PRIMITIVE_DRIVER, JAUS_SET_DISCRETE_DEVICES, 0xFF);
ojCmptAddServiceInputMessage(cmpt, JAUS_PRIMITIVE_DRIVER, JAUS_QUERY_PLATFORM_SPECIFICATIONS, 0xFF);
ojCmptAddServiceInputMessage(cmpt, JAUS_PRIMITIVE_DRIVER, JAUS_QUERY_WRENCH_EFFORT, 0xFF);
ojCmptAddServiceOutputMessage(cmpt, JAUS_PRIMITIVE_DRIVER, JAUS_REPORT_PLATFORM_SPECIFICATIONS, 0xFF);
ojCmptAddServiceOutputMessage(cmpt, JAUS_PRIMITIVE_DRIVER, JAUS_REPORT_WRENCH_EFFORT, 0xFF);
ojCmptAddSupportedSc(cmpt, JAUS_REPORT_WRENCH_EFFORT);
ojCmptSetMessageProcessorCallback(cmpt, pdProcessMessage);
ojCmptSetStateCallback(cmpt, JAUS_STANDBY_STATE, pdStandbyState);
ojCmptSetStateCallback(cmpt, JAUS_READY_STATE, pdReadyState);
ojCmptSetState(cmpt, JAUS_STANDBY_STATE);
pdAddr = ojCmptGetAddress(cmpt);
data = (PdData*)malloc(sizeof(PdData));
data->setWrenchEffort = setWrenchEffortMessageCreate();
data->setDiscreteDevices = setDiscreteDevicesMessageCreate();
data->reportWrenchEffort = reportWrenchEffortMessageCreate();
data->controllerStatus = reportComponentStatusMessageCreate();
data->controllerSc = -1;
jausAddressCopy(data->reportWrenchEffort->source, pdAddr);
jausAddressDestroy(pdAddr);
ojCmptSetUserData(cmpt, (void *)data);
if(ojCmptRun(cmpt))
{
ojCmptDestroy(cmpt);
return NULL;
}
return cmpt;
}
/**
* 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);
}
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;
}
void pdReadyState(OjCmpt pd)
{
PdData *data;
JausAddress address;
data = (PdData*)ojCmptGetUserData(pd);
pdSendReportWrenchEffort(pd);
if( vehicleSimGetState() != VEHICLE_SIM_READY_STATE )
{
ojCmptSetState(pd, JAUS_STANDBY_STATE);
return;
}
if(ojCmptHasController(pd))
{
if(data->controllerSc == -1)
{
address = ojCmptGetControllerAddress(pd);
data->controllerSc = ojCmptEstablishSc( pd,
JAUS_REPORT_COMPONENT_STATUS,
CONTROLLER_STATUS_PRESENCE_VECTOR,
address,
CONTROLLER_STATUS_UPDATE_RATE_HZ,
CONTROLLER_STATUS_TIMEOUT_SEC,
CONTROLLER_STATUS_QUEUE_SIZE);
jausAddressDestroy(address);
}
if(ojCmptIsIncomingScActive(pd, data->controllerSc))
{
if(data->controllerStatus->primaryStatusCode == JAUS_READY_STATE || data->controllerStatus->primaryStatusCode == JAUS_STANDBY_STATE)
{
if(vehicleSimGetRunPause() == VEHICLE_SIM_RUN)
{
vehicleSimSetCommand( data->setWrenchEffort->propulsiveLinearEffortXPercent,
data->setWrenchEffort->resistiveLinearEffortXPercent,
data->setWrenchEffort->propulsiveRotationalEffortZPercent
);
}
else
{
vehicleSimSetCommand(0, 80, data->setWrenchEffort->propulsiveRotationalEffortZPercent);
}
}
else
{
vehicleSimSetCommand(0, 80, 0);
}
}
else
{
vehicleSimSetCommand(0, 80, 0);
}
}
else
{
if(data->controllerSc > -1)
{
ojCmptTerminateSc(pd, data->controllerSc);
data->controllerSc = -1;
}
data->controllerStatus->primaryStatusCode = JAUS_UNKNOWN_STATE;
vehicleSimSetCommand(0, 80, 0);
}
}
// Function: pdProcessMessage
// 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 pdProcessMessage(OjCmpt pd, JausMessage message)
{
ReportComponentStatusMessage reportComponentStatus;
ReportPlatformSpecificationsMessage reportPlatformSpecifications;
SetWrenchEffortMessage setWrenchEffort;
SetDiscreteDevicesMessage setDiscreteDevices;
QueryPlatformSpecificationsMessage queryPlatformSpecifications;
JausAddress address;
JausMessage txMessage;
PdData *data;
data = (PdData*)ojCmptGetUserData(pd);
// This block of code is intended to reject commands from non-controlling components
if(ojCmptHasController(pd) && jausMessageIsRejectableCommand(message) )
{
address = ojCmptGetControllerAddress(pd);
if(!jausAddressEqual(message->source, address))
{
//jausAddressToString(message->source, buf);
//cError("pd: Received command message %s from non-controlling component (%s).\n", jausMessageCommandCodeString(message), buf);
jausAddressDestroy(address);
return;
}
jausAddressDestroy(address);
}
switch(message->commandCode) // Switch the processing algorithm according to the JAUS message type
{
case JAUS_REPORT_COMPONENT_STATUS:
reportComponentStatus = reportComponentStatusMessageFromJausMessage(message);
if(reportComponentStatus)
{
address = ojCmptGetControllerAddress(pd);
if(jausAddressEqual(reportComponentStatus->source, address))
{
reportComponentStatusMessageDestroy(data->controllerStatus);
data->controllerStatus = reportComponentStatus;
}
else
{
reportComponentStatusMessageDestroy(reportComponentStatus);
}
jausAddressDestroy(address);
}
break;
case JAUS_SET_WRENCH_EFFORT:
setWrenchEffort = setWrenchEffortMessageFromJausMessage(message);
if(setWrenchEffort)
{
setWrenchEffortMessageDestroy(data->setWrenchEffort);
data->setWrenchEffort = setWrenchEffort;
}
break;
case JAUS_SET_DISCRETE_DEVICES:
setDiscreteDevices = setDiscreteDevicesMessageFromJausMessage(message);
if(setDiscreteDevices)
{
setDiscreteDevicesMessageDestroy(data->setDiscreteDevices);
data->setDiscreteDevices = setDiscreteDevices;
}
break;
case JAUS_QUERY_PLATFORM_SPECIFICATIONS:
queryPlatformSpecifications = queryPlatformSpecificationsMessageFromJausMessage(message);
if(queryPlatformSpecifications)
{
reportPlatformSpecifications = reportPlatformSpecificationsMessageCreate();
jausAddressCopy(reportPlatformSpecifications->destination, queryPlatformSpecifications->source);
jausAddressCopy(reportPlatformSpecifications->source, queryPlatformSpecifications->destination);
reportPlatformSpecifications->maximumVelocityXMps = 10.0;
txMessage = reportPlatformSpecificationsMessageToJausMessage(reportPlatformSpecifications);
ojCmptSendMessage(pd, txMessage);
jausMessageDestroy(txMessage);
reportPlatformSpecificationsMessageDestroy(reportPlatformSpecifications);
queryPlatformSpecificationsMessageDestroy(queryPlatformSpecifications);
}
break;
default:
ojCmptDefaultMessageProcessor(pd, message);
break;
}
}
// Refresh screen in curses mode
void updateScreen(int keyboardLock, int keyPress)
{
int row = 0;
int col = 0;
char string[256] = {0};
PointLla vehiclePosLla;
static int lastChoice = '1';
JausAddress address;
if(!keyboardLock && keyPress != -1 && keyPress != 27 && keyPress != 12) //Magic Numbers: 27 = ESC, 12 = Ctrl+l
{
switch(keyPress)
{
case ' ':
wdToggleRequestControl(wd);
break;
case 'S':
wdSetSpeed(wd);
break;
case 'W':
wdCreateWaypoint(wd);
break;
default:
lastChoice = keyPress;
}
}
clear();
mvprintw(row,35,"Keyboard Lock: %s", keyboardLock?"ON, Press ctrl+L to unlock":"OFF, Press ctrl+L to lock");
mvprintw(row++,0,"+---------------------------+");
mvprintw(row++,0,"| Component Menu |");
mvprintw(row++,0,"| |");
mvprintw(row++,0,"| 1. Vehicle Sim |");
mvprintw(row++,0,"| 2. Primitive Driver |");
mvprintw(row++,0,"| 3. GPOS / VSS |");
mvprintw(row++,0,"| 4. Waypoint Driver |");
mvprintw(row++,0,"| |");
mvprintw(row++,0,"| ESC to Exit |");
mvprintw(row++,0,"+---------------------------+");
row = 2;
col = 40;
switch(lastChoice)
{
case '1':
mvprintw(row++,col,"Vehicle Simulator");
mvprintw(row++,col,"VS Update Rate: %7.2f", vehicleSimGetUpdateRate());
mvprintw(row++,col,"VS Run/Pause: %s", vehicleSimGetRunPause() == VEHICLE_SIM_PAUSE ? "Pause" : "Run");
row++;
mvprintw(row++,col,"VS Vehicle X:\t%9.2f", vehicleSimGetX());
mvprintw(row++,col,"VS Vehicle Y:\t%9.2f", vehicleSimGetY());
mvprintw(row++,col,"VS Vehicle H:\t%9.2f", vehicleSimGetH());
mvprintw(row++,col,"VS Vehicle Speed: %7.2f", vehicleSimGetSpeed());
row++;
mvprintw(row++,col,"VS Throttle:\t%9.2f", vehicleSimGetLinearEffortX());
mvprintw(row++,col,"VS Brake:\t%9.2f", vehicleSimGetResistiveEffortX());
mvprintw(row++,col,"VS Steering:\t%9.2f", vehicleSimGetRotationalEffort());
row++;
vehiclePosLla = vehicleSimGetPositionLla();
mvprintw(row++,col,"VS Vehicle Latitude: %+10.7f", vehiclePosLla? vehiclePosLla->latitudeRadians*DEG_PER_RAD : 0.0);
mvprintw(row++,col,"VS Vehicle Longitude: %+10.7f", vehiclePosLla? vehiclePosLla->longitudeRadians*DEG_PER_RAD : 0.0);
break;
case '2':
mvprintw(row++,col,"Primitive Driver");
mvprintw(row++,col,"PD Update Rate: %5.2f", ojCmptGetRateHz(pd));
address = ojCmptGetAddress(pd);
jausAddressToString(address, string);
jausAddressDestroy(address);
mvprintw(row++,col,"PD Address:\t%s", string);
mvprintw(row++,col,"PD State:\t%s", jausStateGetString(ojCmptGetState(pd)));
row++;
if(ojCmptHasController(pd))
{
address = ojCmptGetControllerAddress(pd);
jausAddressToString(address, string);
jausAddressDestroy(address);
mvprintw(row++,col,"PD Controller: %s", string);
}
else
{
mvprintw(row++,col,"PD Controller: None");
}
mvprintw(row++,col,"PD Controller SC: %s", pdGetControllerScStatus(pd)?"Active":"Inactive");
mvprintw(row++,col,"PD Controller State: %s", jausStateGetString(pdGetControllerState(pd)));
row++;
mvprintw(row++,col,"PD Prop Effort X: %0.0lf", pdGetWrenchEffort(pd)? pdGetWrenchEffort(pd)->propulsiveLinearEffortXPercent:-1.0);
mvprintw(row++,col,"PD Rstv Effort X: %0.0lf", pdGetWrenchEffort(pd)? pdGetWrenchEffort(pd)->resistiveLinearEffortXPercent:-1.0);
mvprintw(row++,col,"PD Rtat Effort Z: %0.0lf", pdGetWrenchEffort(pd)? pdGetWrenchEffort(pd)->propulsiveRotationalEffortZPercent:-1.0);
break;
case '3':
mvprintw(row++,col,"Global Pose Sensor");
mvprintw(row++,col,"GPOS Update Rate: %7.2f", ojCmptGetRateHz(gpos));
address = ojCmptGetAddress(gpos);
jausAddressToString(address, string );
jausAddressDestroy(address);
mvprintw(row++,col,"GPOS Address:\t %s", string);
mvprintw(row++,col,"GPOS State:\t %s", jausStateGetString(ojCmptGetState(gpos)));
mvprintw(row++,col,"GPOS SC State:\t %s", gposGetScActive(gpos)? "Active" : "Inactive");
row++;
mvprintw(row++,col,"Velocity State Sensor");
mvprintw(row++,col,"VSS Update Rate: %7.2f", ojCmptGetRateHz(vss));
address = ojCmptGetAddress(vss);
jausAddressToString(address, string );
jausAddressDestroy(address);
mvprintw(row++,col,"VSS Address:\t %s", string);
mvprintw(row++,col,"VSS State:\t %s", jausStateGetString(ojCmptGetState(vss)));
mvprintw(row++,col,"VSS SC State:\t %s", vssGetScActive(vss)? "Active" : "Inactive");
break;
case '4':
mvprintw(row++,col,"Waypoint Driver");
mvprintw(row++,col,"WD Update Rate: %7.2f", ojCmptGetRateHz(wd));
address = ojCmptGetAddress(wd);
jausAddressToString(address, string );
jausAddressDestroy(address);
mvprintw(row++,col,"WD Address:\t %s", string);
mvprintw(row++,col,"WD State:\t %s", jausStateGetString(ojCmptGetState(wd)));
address = ojCmptGetControllerAddress(wd);
if(address)
{
jausAddressToString(address, string);
mvprintw(row++,col,"WD Controller:\t %s", string);
jausAddressDestroy(address);
}
else
{
mvprintw(row++,col,"WD Controller:\t None");
}
row = 11;
col = 2;
mvprintw(row++,col,"GPOS SC:\t %s", wdGetGposScStatus(wd)? "Active" : "Inactive");
mvprintw(row++,col,"VSS SC:\t %s", wdGetVssScStatus(wd)? "Active" : "Inactive");
mvprintw(row++,col,"PD Wrench SC:\t %s", wdGetPdWrenchScStatus(wd)? "Active" : "Inactive");
mvprintw(row++,col,"PD State SC:\t %s", wdGetPdStatusScStatus(wd)? "Active" : "Inactive");
row++;
mvprintw(row++,col,"WD Request Control:\t%s", wdGetRequestControl(wd)? "True" : "False");
mvprintw(row++,col,"(Space to Toggle)");
mvprintw(row++,col,"WD Control:\t\t%s", wdGetInControlStatus(wd)? "True" : "False");
mvprintw(row++,col,"PD State:\t\t%s", jausStateGetString(wdGetPdState(wd)));
row = 11;
col = 40;
if(wdGetGlobalWaypoint(wd))
{
mvprintw(row++,col,"Global Waypoint: (%9.7lf,%9.7lf)", wdGetGlobalWaypoint(wd)->latitudeDegrees, wdGetGlobalWaypoint(wd)->longitudeDegrees);
}
else
{
mvprintw(row++,col,"Global Waypoint: None");
}
if(wdGetTravelSpeed(wd))
{
mvprintw(row++,col,"Travel Speed: %7.2f", wdGetTravelSpeed(wd)->speedMps);
}
else
{
mvprintw(row++,col,"Travel Speed: None");
}
mvprintw(row++,col,"dSpeedMps: %7.2f", wdGetDesiredVehicleState(wd)? wdGetDesiredVehicleState(wd)->desiredSpeedMps : 0.0);
mvprintw(row++,col,"dPhi: %7.2f", wdGetDesiredVehicleState(wd)? wdGetDesiredVehicleState(wd)->desiredPhiEffort : 0.0);
break;
default:
mvprintw(row++,col,"NONE.");
break;
}
move(24,0);
refresh();
}
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 lmListDestroy(LargeMessageList msgList)
{
vectorDestroy(msgList->messages, (void *)jausMessageDestroy);
jausAddressDestroy(msgList->source);
free(msgList);
}
/**
* 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);
}
// ***********************************************************
// 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 serviceConnectionDestroyNoMutex(ServiceConnection sc)
{
jausAddressDestroy(sc->address);
queueDestroy(sc->queue, (void *)jausMessageDestroy);
free(sc);
}
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;
}
}
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);
}
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);
}
