// 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;
}
/**
* Método público que hace de destructor de componentes JAUS de la clase para
* su finalización
*/
void RosNode_Communications::finishJAUS() {
ojCmptDestroy(missionSpoolerComponent);
ojCmptDestroy(primitiveDriverComponent);
ojCmptDestroy(visualSensorComponent);
ojCmptDestroy(platformSensorComponent);
ojCmptDestroy(velocityStateSensorComponent);
ojCmptDestroy(globalPoseSensorComponent);
ojCmptDestroy(heartBeatInformationComponent);
}
// Function: gposShutdown
// Access: Public
// Description: This function allows the abstracted component functionality contained in this file to be stoped from an external source.
// If the component is in the running state, this function will terminate all threads running in this file
// This function will also close the Jms connection to the Node Manager and check out the component from the Node Manager
void gposDestroy(OjCmpt gpos)
{
ReportGlobalPoseMessage message;
message = (ReportGlobalPoseMessage)ojCmptGetUserData(gpos);
// Remove support for ReportGlobalPose Service Connections
ojCmptRemoveSupportedSc(gpos, JAUS_REPORT_GLOBAL_POSE);
ojCmptDestroy(gpos);
reportGlobalPoseMessageDestroy(message);
}
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;
}
int
main(void)
{
OjCmpt XGV_CCU;
int *xgv_ccu_service_connections;
XGV_CCU = create_xgv_ccu_component(XGV_CCU_NAME, XGV_CCU_COMPONENTE_ID, XGV_CCU_STATE_MACHINE_UPDATE_RATE);
register_xgv_ccu_messages_callbacks(XGV_CCU);
ojCmptSetAuthority(XGV_CCU, 6);
ojCmptRun(XGV_CCU); // Begin running the XGV_CCU state machine
xgv_ccu_service_connections = create_xgv_ccu_service_connections(XGV_CCU);
user_interface(XGV_CCU);
terminate_xgv_ccu_service_connections(XGV_CCU, xgv_ccu_service_connections);
ojCmptDestroy(XGV_CCU); // Shutdown and destroy component
return (0);
}
void pdDestroy(OjCmpt pd)
{
PdData *data;
data = (PdData*)ojCmptGetUserData(pd);
if(ojCmptIsIncomingScActive(pd, data->controllerSc))
{
ojCmptTerminateSc(pd, data->controllerSc);
}
ojCmptRemoveSupportedSc(pd, JAUS_REPORT_WRENCH_EFFORT);
ojCmptDestroy(pd);
setWrenchEffortMessageDestroy(data->setWrenchEffort);
setDiscreteDevicesMessageDestroy(data->setDiscreteDevices);
reportWrenchEffortMessageDestroy(data->reportWrenchEffort);
reportComponentStatusMessageDestroy(data->controllerStatus);
free(data);
}
