// 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); }
void gposShutdownState(void) { RejectComponentControlMessage rejectComponentControl; JausMessage txMessage; if(gpos->controller.active) { // Terminate control of current component rejectComponentControl = rejectComponentControlMessageCreate(); rejectComponentControl->source->id = gpos->address->id; rejectComponentControl->destination->id = gpos->controller.address->id; txMessage = rejectComponentControlMessageToJausMessage(rejectComponentControl); nodeManagerSend(gposNmi, txMessage); jausMessageDestroy(txMessage); rejectComponentControlMessageDestroy(rejectComponentControl); } // Remove support for ReportGlovalPose Service Connections scManagerRemoveSupportedMessage(gposNmi, JAUS_REPORT_GLOBAL_POSE); reportGlobalPoseMessageDestroy(gposMessage); }
/** * 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); }
// Function: wdProcessMessage // 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 wdProcessMessage(JausMessage message) { JausMessage txMessage; ConfirmComponentControlMessage confirmComponentControl; RejectComponentControlMessage rejectComponentControl; ReportComponentStatusMessage reportComponentStatus; QueryGlobalWaypointMessage queryGlobalWaypointMessage; ReportGlobalWaypointMessage reportGlobalWaypointMessage; QueryWaypointCountMessage queryWaypointCountMessage; ReportWaypointCountMessage reportWaypointCountMessage; int i; char buf[64] = {0}; // This block of code is intended to reject commands from non-controlling components if(wd->controller.active && !jausAddressEqual(message->source, wd->controller.address) && jausMessageIsRejectableCommand(message)) { //cError("wd: Received command message %s from non-controlling component.\n", jausMessageCommandCodeString(message)); jausMessageDestroy(message); // Ignore this message return; } switch(message->commandCode) // Switch the processing algorithm according to the JAUS message type { case JAUS_REPORT_COMPONENT_STATUS: reportComponentStatus = reportComponentStatusMessageFromJausMessage(message); if(reportComponentStatus) { if(jausAddressEqual(reportComponentStatus->source, pd->address)) { pd->state = reportComponentStatus->primaryStatusCode; } reportComponentStatusMessageDestroy(reportComponentStatus); } else { //cError("wd: Error unpacking %s message.\n", jausMessageCommandCodeString(message)); } jausMessageDestroy(message); break; case JAUS_CONFIRM_COMPONENT_CONTROL: confirmComponentControl = confirmComponentControlMessageFromJausMessage(message); if(confirmComponentControl) { if(jausAddressEqual(confirmComponentControl->source, pd->address)) { //cDebug(4,"wd: Confirmed control of PD\n"); wdInControl = JAUS_TRUE; } confirmComponentControlMessageDestroy(confirmComponentControl); } else { //cError("wd: Error unpacking %s message.\n", jausMessageCommandCodeString(message)); } jausMessageDestroy(message); break; case JAUS_REJECT_COMPONENT_CONTROL: rejectComponentControl = rejectComponentControlMessageFromJausMessage(message); if(rejectComponentControl) { if(jausAddressEqual(rejectComponentControl->source, pd->address)) { //cDebug(4,"wd: Lost control of PD\n"); wdInControl = JAUS_FALSE; } rejectComponentControlMessageDestroy(rejectComponentControl); } else { //cError("wd: Error unpacking %s message.\n", jausMessageCommandCodeString(message)); } jausMessageDestroy(message); break; case JAUS_SET_TRAVEL_SPEED: if(wdSpeed) { setTravelSpeedMessageDestroy(wdSpeed); } wdSpeed = setTravelSpeedMessageFromJausMessage(message); if(!wdSpeed) { //cError("wd: Error unpacking %s message.\n", jausMessageCommandCodeString(message)); } jausMessageDestroy(message); break; case JAUS_REPORT_GLOBAL_POSE: if(wdReportGpos) { reportGlobalPoseMessageDestroy(wdReportGpos); } wdReportGpos = reportGlobalPoseMessageFromJausMessage(message); if(wdReportGpos) { // Nothing to do jausAddressToString(message->source, buf); //cDebug(9, "Recv GPOS msg from %s\n", buf); } else { //cError("wd: Error unpacking %s message.\n", jausMessageCommandCodeString(message)); } if(currentWaypointIndex < wdWaypoints->elementCount) { // update waypoint index SetGlobalWaypointMessage tempGlobalWaypoint; tempGlobalWaypoint = (SetGlobalWaypointMessage) wdWaypoints->elementData[currentWaypointIndex]; wdWaypointDistance = greatCircleDistance(wdReportGpos->latitudeDegrees * RAD_PER_DEG, wdReportGpos->longitudeDegrees * RAD_PER_DEG, tempGlobalWaypoint->latitudeDegrees * RAD_PER_DEG, tempGlobalWaypoint->longitudeDegrees * RAD_PER_DEG); if(wdWaypointDistance < WAYPOINT_POP_DISTANCE_M) { //cError("wd: popping waypoint: %d\n",currentWaypointIndex); currentWaypointIndex++; } } // else // { // if(wdSpeed) wdSpeed->speedMps = 0; // } jausMessageDestroy(message); break; case JAUS_REPORT_VELOCITY_STATE: if(wdReportVss) { reportVelocityStateMessageDestroy(wdReportVss); } wdReportVss = reportVelocityStateMessageFromJausMessage(message); if(!wdReportVss) { //cError("wd: Error unpacking %s message.\n", jausMessageCommandCodeString(message)); } jausMessageDestroy(message); break; case JAUS_REPORT_WRENCH_EFFORT: if(wdReportWrench) { reportWrenchEffortMessageDestroy(wdReportWrench); } wdReportWrench = reportWrenchEffortMessageFromJausMessage(message); if(!wdReportWrench) { //cError("wd: Error unpacking %s message.\n", jausMessageCommandCodeString(message)); } jausMessageDestroy(message); break; case JAUS_SET_GLOBAL_WAYPOINT: wdGlobalWaypoint = setGlobalWaypointMessageFromJausMessage(message); if(!wdGlobalWaypoint) { //cError("wd: Error unpacking %s message.\n", jausMessageCommandCodeString(message)); } jausMessageDestroy(message); jausArrayAdd(wdWaypoints, wdGlobalWaypoint); break; case JAUS_QUERY_GLOBAL_WAYPOINT: queryGlobalWaypointMessage = queryGlobalWaypointMessageFromJausMessage(message); if(!queryGlobalWaypointMessage) { //cError("wd: Error unpacking %s message.\n", jausMessageCommandCodeString(message)); } // loop thru waypoints to find the one that matches the request // if there's a match, prepare/send the report, else whine for(i = 0; i < wdWaypoints->elementCount; i++) { SetGlobalWaypointMessage tempGlobalWaypoint; tempGlobalWaypoint = (SetGlobalWaypointMessage) wdWaypoints->elementData[i]; if(tempGlobalWaypoint->waypointNumber == queryGlobalWaypointMessage->waypointNumber) { reportGlobalWaypointMessage = reportGlobalWaypointMessageCreate(); jausAddressCopy(reportGlobalWaypointMessage->destination, queryGlobalWaypointMessage->source); jausAddressCopy(reportGlobalWaypointMessage->source, wd->address); reportGlobalWaypointMessage->presenceVector = NO_PRESENCE_VECTOR; reportGlobalWaypointMessage->waypointNumber = tempGlobalWaypoint->waypointNumber; reportGlobalWaypointMessage->latitudeDegrees = tempGlobalWaypoint->latitudeDegrees; reportGlobalWaypointMessage->longitudeDegrees = tempGlobalWaypoint->longitudeDegrees; txMessage = reportGlobalWaypointMessageToJausMessage(reportGlobalWaypointMessage); reportGlobalWaypointMessageDestroy(reportGlobalWaypointMessage); nodeManagerSend(wdNmi, txMessage); jausMessageDestroy(txMessage); } } queryGlobalWaypointMessageDestroy(queryGlobalWaypointMessage); jausMessageDestroy(message); break; case JAUS_QUERY_WAYPOINT_COUNT: queryWaypointCountMessage = queryWaypointCountMessageFromJausMessage(message); if(!queryWaypointCountMessage) { //cError("wd: Error unpacking %s message.\n", jausMessageCommandCodeString(message)); } reportWaypointCountMessage = reportWaypointCountMessageCreate(); jausAddressCopy(reportWaypointCountMessage->destination, queryWaypointCountMessage->source); jausAddressCopy(reportWaypointCountMessage->source, wd->address); reportWaypointCountMessage->waypointCount = wdWaypoints->elementCount; txMessage = reportWaypointCountMessageToJausMessage(reportWaypointCountMessage); reportWaypointCountMessageDestroy(reportWaypointCountMessage); nodeManagerSend(wdNmi, txMessage); queryWaypointCountMessageDestroy(queryWaypointCountMessage); jausMessageDestroy(message); break; default: defaultJausMessageProcessor(message, wdNmi, wd); break; } }
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); }