/** * 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 TutorialComponent::processMessage(OjCmpt cmpt, JausMessage msg) { //<ROS2JAUS> TutorialData *data = NULL; data = (TutorialData *)ojCmptGetUserData(cmpt); SetWrenchEffortMessage wrMsg; beginner_tutorials::wrenchData rosWrenchMsg; switch (msg->commandCode) { case JAUS_SET_WRENCH_EFFORT: //If we recieve a set wrench message from someone, publish the contents of the message to ROS wrMsg = setWrenchEffortMessageFromJausMessage(msg); rosWrenchMsg.throttle = wrMsg->propulsiveLinearEffortXPercent; rosWrenchMsg.brake = wrMsg->resistiveLinearEffortXPercent; rosWrenchMsg.steering = wrMsg->propulsiveRotationalEffortZPercent; data->wrenchPub->publish(rosWrenchMsg); case JAUS_REQUEST_COMPONENT_CONTROL: //Hardcode an outgoing address, only necessary if the message doesn't provide a source JausAddress addr; addr = jausAddressCreate(); addr->subsystem = 1; addr->node = 1; addr->component = JAUS_EXPERIMENTAL_MOTION_PROFILE_DRIVER; addr->instance = 1; //Spoof a message to confirm control of the primitive driver ConfirmComponentControlMessage confirmComponentControl; confirmComponentControl = confirmComponentControlMessageCreate(); jausAddressCopy(confirmComponentControl->source, ojCmptGetAddress(cmpt)); jausAddressCopy(confirmComponentControl->destination, addr); confirmComponentControl->responseCode = JAUS_CONTROL_ACCEPTED; JausMessage confirmControl; confirmControl = confirmComponentControlMessageToJausMessage(confirmComponentControl); ojCmptSendMessage(cmpt, confirmControl); //Spoof a message to report the status of the component ReportComponentStatusMessage reportComponentStatus; reportComponentStatus = reportComponentStatusMessageCreate(); reportComponentStatus->primaryStatusCode = JAUS_READY_STATE; jausAddressCopy(reportComponentStatus->source, ojCmptGetAddress(cmpt)); jausAddressCopy(reportComponentStatus->destination, addr); JausMessage reportStatus; reportStatus = reportComponentStatusMessageToJausMessage(reportComponentStatus); ojCmptSendMessage(cmpt, reportStatus); ojCmptDefaultMessageProcessor(cmpt, msg); //</ROS2JAUS> default: //if we recieve an unfamiliar message, print its command code if (msg->commandCode != JAUS_REPORT_HEARTBEAT_PULSE && msg->commandCode != JAUS_CREATE_SERVICE_CONNECTION) { std::cout << "Received message..." << std::endl; std::cout << "Command code=" << jausCommandCodeString(msg->commandCode) << std::endl; } ojCmptDefaultMessageProcessor(cmpt, msg); } }
// 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; } }