// 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); }