Ejemplo n.º 1
0
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);
}
Ejemplo n.º 3
0
// 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;
	}
}