/**
* 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;
}
}
