/** * Método privado que recibe mensajes JAUS de tipo "Set Travel Speed". 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 vehículo * @param[in] cmp Componente JAUS emisor * @param[in] msg Mensaje JAUS capturado */ void RosNode_Communications::fcn_receive_set_travel_speed(OjCmpt cmp, JausMessage msg) { printf("Set Travel Speed\n"); SetTravelSpeedMessage sTravelSpeed = setTravelSpeedMessageFromJausMessage(msg); CITIUS_Control_Communication::msg_command command; command.id_device = CRUISING_SPEED; command.value = sTravelSpeed->speedMps; instance->pubCommand.publish(command); setTravelSpeedMessageDestroy(sTravelSpeed); }
// 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; } }