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 Wrench Effort". 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_wrench_effort(OjCmpt cmp, JausMessage msg) { printf("Set Wrench Effort\n"); SetWrenchEffortMessage sWrenchEffort = setWrenchEffortMessageFromJausMessage(msg); CITIUS_Control_Communication::msg_command command; if ((sWrenchEffort->presenceVector & PRESENCE_VECTOR_STEER) == PRESENCE_VECTOR_STEER) { command.id_device = STEERING; command.value = sWrenchEffort->propulsiveRotationalEffortZPercent; instance->pubCommand.publish(command); } if ((sWrenchEffort->presenceVector & PRESENCE_VECTOR_THROTTLE) == PRESENCE_VECTOR_THROTTLE) { command.id_device = THROTTLE; command.value = sWrenchEffort->propulsiveLinearEffortXPercent; instance->pubCommand.publish(command); } if ((sWrenchEffort->presenceVector & PRESENCE_VECTOR_BRAKE) == PRESENCE_VECTOR_BRAKE) { command.id_device = BRAKE; command.value = sWrenchEffort->resistiveLinearEffortXPercent; instance->pubCommand.publish(command); } setWrenchEffortMessageDestroy(sWrenchEffort); }
void wdShutdownState(void) { RejectComponentControlMessage rejectComponentControl; ReleaseComponentControlMessage releaseControl = NULL; JausMessage txMessage; scManagerRemoveSupportedMessage(wdNmi, JAUS_REPORT_COMPONENT_STATUS); if(wd->controller.active) { // Terminate control of current component rejectComponentControl = rejectComponentControlMessageCreate(); jausAddressCopy(rejectComponentControl->source, wd->address); jausAddressCopy(rejectComponentControl->destination, wd->controller.address); txMessage = rejectComponentControlMessageToJausMessage(rejectComponentControl); nodeManagerSend(wdNmi, txMessage); jausMessageDestroy(txMessage); rejectComponentControlMessageDestroy(rejectComponentControl); } // USER: Insert Shutdown Code Here, NOTE: Terminate any active service connections // Release Control if(wdInControl) { //cDebug(4, "wd: Releasing control of PD\n"); releaseControl = releaseComponentControlMessageCreate(); jausAddressCopy(releaseControl->source, wd->address); jausAddressCopy(releaseControl->destination, pd->address); txMessage = releaseComponentControlMessageToJausMessage(releaseControl); nodeManagerSend(wdNmi, txMessage); jausMessageDestroy(txMessage); releaseComponentControlMessageDestroy(releaseControl); wdInControl = JAUS_FALSE; wd->state = JAUS_INITIALIZE_STATE; } if(pdStatusSc->isActive) { scManagerTerminateServiceConnection(wdNmi, pdStatusSc); serviceConnectionDestroy(pdStatusSc); } if(pdWrenchSc->isActive) { scManagerTerminateServiceConnection(wdNmi, pdWrenchSc); serviceConnectionDestroy(pdWrenchSc); } if(gposSc->isActive) { scManagerTerminateServiceConnection(wdNmi, gposSc); serviceConnectionDestroy(gposSc); } if(vssSc->isActive) { scManagerTerminateServiceConnection(wdNmi, vssSc); serviceConnectionDestroy(vssSc); } // Destory Global Messages if(wdSpeed) { setTravelSpeedMessageDestroy(wdSpeed); } if(wdReportGpos) { reportGlobalPoseMessageDestroy(wdReportGpos); } if(wdReportVss) { reportVelocityStateMessageDestroy(wdReportVss); } if(wdReportWrench) { reportWrenchEffortMessageDestroy(wdReportWrench); } // Destroy Global Variables vehicleStateDestroy(vehicleState); jausArrayDestroy(wdWaypoints, (void *)setGlobalWaypointMessageDestroy); setWrenchEffortMessageDestroy(wdWrench); jausComponentDestroy(pd); }
// 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; } }