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 Discrete Devices".
* 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_discrete_devices(OjCmpt cmp, JausMessage msg) {
printf("Set Discrete Device\n");
SetDiscreteDevicesMessage sDiscreteDevice = setDiscreteDevicesMessageFromJausMessage(msg);
CITIUS_Control_Communication::msg_command command;
if ((sDiscreteDevice->presenceVector & PRESENCE_VECTOR_PARKING_BRAKE) == PRESENCE_VECTOR_PARKING_BRAKE) {
command.id_device = HANDBRAKE;
command.value = sDiscreteDevice->parkingBrake;
instance->pubCommand.publish(command);
/*if(sDiscreteDevice->parkingBrake)
printf("Handbrake ON\n");
else
printf("Handbrake OFF\n");*/
}
if ((sDiscreteDevice->presenceVector & PRESENCE_VECTOR_GEAR) == PRESENCE_VECTOR_GEAR) {
command.id_device = GEAR;
command.value = sDiscreteDevice->gear-1;
//printf("Recibida marcha := %d\n",command.value+1);
instance->pubCommand.publish(command);
}
setDiscreteDevicesMessageDestroy(sDiscreteDevice);
}
// 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;
}
}
