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); }
/** * Método privado que recibe mensajes JAUS de tipo "Set Discrete Devices". * Traduce la información a mensaje ROS (msg_command) y la publica en el topic * correspondiente para la ejecución de comandos de control del vehiculo * @param[in] cmp Componente JAUS emisor * @param[in] msg Mensaje JAUS capturado */ void RosNode_Communications::fcn_receive_set_discrete_devices(OjCmpt cmp, JausMessage msg) { printf("Set Discrete Device\n"); SetDiscreteDevicesMessage sDiscreteDevice = setDiscreteDevicesMessageFromJausMessage(msg); CITIUS_Control_Communication::msg_command command; if ((sDiscreteDevice->presenceVector & PRESENCE_VECTOR_PARKING_BRAKE) == PRESENCE_VECTOR_PARKING_BRAKE) { command.id_device = HANDBRAKE; command.value = sDiscreteDevice->parkingBrake; instance->pubCommand.publish(command); /*if(sDiscreteDevice->parkingBrake) printf("Handbrake ON\n"); else printf("Handbrake OFF\n");*/ } if ((sDiscreteDevice->presenceVector & PRESENCE_VECTOR_GEAR) == PRESENCE_VECTOR_GEAR) { command.id_device = GEAR; command.value = sDiscreteDevice->gear-1; //printf("Recibida marcha := %d\n",command.value+1); instance->pubCommand.publish(command); } setDiscreteDevicesMessageDestroy(sDiscreteDevice); }
// 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; } }