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