Example #1
0
File: ojCmpt.c Project: pfg/qgc
void ojCmptDestroy(OjCmpt ojCmpt)
{
	RejectComponentControlMessage rejectComponentControl;
	JausMessage txMessage;
	int i = 0;
	
	if(ojCmpt->run == TRUE)
	{
		ojCmpt->run = FALSE;
		pthread_cond_signal(&ojCmpt->nmi->recvCondition);
		pthread_join(ojCmpt->thread, NULL);
	}

	if(ojCmpt->jaus->controller.active)
	{
		// Terminate control of current component
		rejectComponentControl = rejectComponentControlMessageCreate();
		jausAddressCopy(rejectComponentControl->source, ojCmpt->jaus->address);
		jausAddressCopy(rejectComponentControl->destination, ojCmpt->jaus->controller.address);

		txMessage = rejectComponentControlMessageToJausMessage(rejectComponentControl);
		nodeManagerSend(ojCmpt->nmi, txMessage);
		jausMessageDestroy(txMessage);

		rejectComponentControlMessageDestroy(rejectComponentControl);
	}
	
	for(i=0; i<OJ_CMPT_MAX_INCOMING_SC_COUNT; i++)
	{
		if(ojCmpt->inConnection[i])
		{	
			if(ojCmpt->inConnection[i]->isActive)
			{
				scManagerTerminateServiceConnection(ojCmpt->nmi, ojCmpt->inConnection[i]);
			}
			serviceConnectionDestroy(ojCmpt->inConnection[i]);		
		}
	}
	
	if(ojCmpt->messageCallback)
	{
		free(ojCmpt->messageCallback);
	}
	
	if(ojCmpt->nmi)
	{
		nodeManagerClose(ojCmpt->nmi); // Close Node Manager Connection
	}
	
	free(ojCmpt->jaus->identification);
	ojCmpt->jaus->identification = NULL;
	jausComponentDestroy(ojCmpt->jaus);
	free(ojCmpt);	
};
Example #2
0
void cmptShutdownState(void)
{
	RejectComponentControlMessage rejectComponentControl;
	JausMessage txMessage;
		
	if(cmpt->controller.active)
	{
		// Terminate control of current component
		rejectComponentControl = rejectComponentControlMessageCreate();
		rejectComponentControl->source->id = cmpt->address->id;
		rejectComponentControl->destination->id = cmpt->controller.address->id;

		txMessage = rejectComponentControlMessageToJausMessage(rejectComponentControl);
		nodeManagerSend(cmptNmi, txMessage);
		jausMessageDestroy(txMessage);

		rejectComponentControlMessageDestroy(rejectComponentControl);
	}
	
	// USER: Insert Shutdown Code Here, NOTE: Terminate any active service connections
}
Example #3
0
void gposShutdownState(void)
{
	RejectComponentControlMessage rejectComponentControl;
	JausMessage txMessage;
		
	if(gpos->controller.active)
	{
		// Terminate control of current component
		rejectComponentControl = rejectComponentControlMessageCreate();
		rejectComponentControl->source->id = gpos->address->id;
		rejectComponentControl->destination->id = gpos->controller.address->id;

		txMessage = rejectComponentControlMessageToJausMessage(rejectComponentControl);
		nodeManagerSend(gposNmi, txMessage);
		jausMessageDestroy(txMessage);

		rejectComponentControlMessageDestroy(rejectComponentControl);
	}
	
	// Remove support for ReportGlovalPose Service Connections
	scManagerRemoveSupportedMessage(gposNmi, JAUS_REPORT_GLOBAL_POSE);

	reportGlobalPoseMessageDestroy(gposMessage);
}
Example #4
0
// Function: cmptProcessMessage
// 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 cmptProcessMessage(JausMessage message)
{
	ConfirmComponentControlMessage confirmComponentControl;
	RejectComponentControlMessage rejectComponentControl;
	ReportComponentAuthorityMessage reportComponentAuthority;
	ReportComponentStatusMessage reportComponentStatus;

	// This block of code is intended to reject commands from non-controlling components
	if(cmpt->controller.active && message->source->id != cmpt->controller.address->id && jausMessageIsRejectableCommand(message) )
	{
		cError("cmpt: Received command message %s from non-controlling component.\n", jausMessageCommandCodeString(message));
		jausMessageDestroy(message); // Ignore this message
		return;		
	}	

	switch(message->commandCode) // Switch the processing algorithm according to the JAUS message type
	{
		case JAUS_CONFIRM_COMPONENT_CONTROL:
			confirmComponentControl = confirmComponentControlMessageFromJausMessage(message);
			if(confirmComponentControl)
			{
				// USER: Insert code here to handle the confirm component control message if needed
				confirmComponentControlMessageDestroy(confirmComponentControl);
			}
			else
			{
				cError("cmpt: Error unpacking %s message.\n", jausMessageCommandCodeString(message));
			}
			jausMessageDestroy(message);
			break;
			
		case JAUS_REJECT_COMPONENT_CONTROL:
			rejectComponentControl = rejectComponentControlMessageFromJausMessage(message);
			if(rejectComponentControl)
			{
				// USER: Insert code here to handle the reject component control message if needed
				rejectComponentControlMessageDestroy(rejectComponentControl);
			}
			else
			{
				cError("cmpt: Error unpacking %s message.\n", jausMessageCommandCodeString(message));
			}
			jausMessageDestroy(message);
			break;
			
		case JAUS_REPORT_COMPONENT_AUTHORITY:
			reportComponentAuthority = reportComponentAuthorityMessageFromJausMessage(message);
			if(reportComponentAuthority)
			{
				// USER: Insert code here to handle the report component authority message if needed
				reportComponentAuthorityMessageDestroy(reportComponentAuthority);
			}
			else
			{
				cError("cmpt: Error unpacking %s message.\n", jausMessageCommandCodeString(message));
			}
			jausMessageDestroy(message);
			break;
			
		case JAUS_REPORT_COMPONENT_STATUS:
			reportComponentStatus = reportComponentStatusMessageFromJausMessage(message);
			if(reportComponentStatus)
			{
				// USER: Insert code here to handle the report component status message if needed
				reportComponentStatusMessageDestroy(reportComponentStatus);
			}
			else
			{
				cError("cmpt: Error unpacking %s message.\n", jausMessageCommandCodeString(message));
			}
			jausMessageDestroy(message);
			break;
		
		default:
			defaultJausMessageProcessor(message, cmptNmi, cmpt);
			break;
	}
}
Example #5
0
// Function: wdProcessMessage
// 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 wdProcessMessage(JausMessage message)
{
	JausMessage txMessage;
	ConfirmComponentControlMessage confirmComponentControl;
	RejectComponentControlMessage rejectComponentControl;
	ReportComponentStatusMessage reportComponentStatus;
	QueryGlobalWaypointMessage queryGlobalWaypointMessage;
	ReportGlobalWaypointMessage reportGlobalWaypointMessage;
	QueryWaypointCountMessage queryWaypointCountMessage;
	ReportWaypointCountMessage reportWaypointCountMessage;
	int i;
	char buf[64] = {0};

	// This block of code is intended to reject commands from non-controlling components
	if(wd->controller.active && !jausAddressEqual(message->source, wd->controller.address) && jausMessageIsRejectableCommand(message))
	{
		//cError("wd: Received command message %s from non-controlling component.\n", jausMessageCommandCodeString(message));
		jausMessageDestroy(message); // Ignore this message
		return;		
	}	

	switch(message->commandCode) // Switch the processing algorithm according to the JAUS message type
	{	
		case JAUS_REPORT_COMPONENT_STATUS:
			reportComponentStatus = reportComponentStatusMessageFromJausMessage(message);
			if(reportComponentStatus)
			{
				if(jausAddressEqual(reportComponentStatus->source, pd->address))
				{
					pd->state = reportComponentStatus->primaryStatusCode;
				}
				reportComponentStatusMessageDestroy(reportComponentStatus);
			}
			else
			{
				//cError("wd: Error unpacking %s message.\n", jausMessageCommandCodeString(message));
			}
			jausMessageDestroy(message);
			break;

		case JAUS_CONFIRM_COMPONENT_CONTROL:
			confirmComponentControl = confirmComponentControlMessageFromJausMessage(message);
			if(confirmComponentControl)
			{
				if(jausAddressEqual(confirmComponentControl->source, pd->address))
				{
					//cDebug(4,"wd: Confirmed control of PD\n");
					wdInControl = JAUS_TRUE;
				}
				confirmComponentControlMessageDestroy(confirmComponentControl);
			}
			else
			{
				//cError("wd: Error unpacking %s message.\n", jausMessageCommandCodeString(message));
			}
			jausMessageDestroy(message);
			break;
			
		case JAUS_REJECT_COMPONENT_CONTROL:
			rejectComponentControl = rejectComponentControlMessageFromJausMessage(message);
			if(rejectComponentControl)
			{
				if(jausAddressEqual(rejectComponentControl->source, pd->address))
				{			
					//cDebug(4,"wd: Lost control of PD\n");
					wdInControl = JAUS_FALSE;
				}
				rejectComponentControlMessageDestroy(rejectComponentControl);
			}
			else
			{
				//cError("wd: Error unpacking %s message.\n", jausMessageCommandCodeString(message));
			}
			jausMessageDestroy(message);
			break;
			
		case JAUS_SET_TRAVEL_SPEED:
			if(wdSpeed)
			{
				setTravelSpeedMessageDestroy(wdSpeed);
			}
			
			wdSpeed = setTravelSpeedMessageFromJausMessage(message);
			if(!wdSpeed)
			{
				//cError("wd: Error unpacking %s message.\n", jausMessageCommandCodeString(message));
			}
			jausMessageDestroy(message);
			break;

		case JAUS_REPORT_GLOBAL_POSE:
			if(wdReportGpos)
			{
				reportGlobalPoseMessageDestroy(wdReportGpos);
			}
			
			wdReportGpos = reportGlobalPoseMessageFromJausMessage(message);
			if(wdReportGpos)
			{
				// Nothing to do
				jausAddressToString(message->source, buf);
				//cDebug(9, "Recv GPOS msg from %s\n", buf);
				
			}
			else
			{
				//cError("wd: Error unpacking %s message.\n", jausMessageCommandCodeString(message));
			}

			if(currentWaypointIndex < wdWaypoints->elementCount)
			{
				// update waypoint index
				SetGlobalWaypointMessage tempGlobalWaypoint;
				tempGlobalWaypoint = (SetGlobalWaypointMessage) wdWaypoints->elementData[currentWaypointIndex];
				wdWaypointDistance = greatCircleDistance(wdReportGpos->latitudeDegrees * RAD_PER_DEG, wdReportGpos->longitudeDegrees * RAD_PER_DEG, tempGlobalWaypoint->latitudeDegrees * RAD_PER_DEG, tempGlobalWaypoint->longitudeDegrees * RAD_PER_DEG);
								
				if(wdWaypointDistance < WAYPOINT_POP_DISTANCE_M)
				{
					//cError("wd: popping waypoint: %d\n",currentWaypointIndex); 
					currentWaypointIndex++;
				}
			}
//			else
//			{
//				if(wdSpeed) wdSpeed->speedMps = 0;
//			}
			
			jausMessageDestroy(message);
			break;

		case JAUS_REPORT_VELOCITY_STATE:
			if(wdReportVss)
			{
				reportVelocityStateMessageDestroy(wdReportVss);
			}
			
			wdReportVss = reportVelocityStateMessageFromJausMessage(message);
			if(!wdReportVss)
			{
				//cError("wd: Error unpacking %s message.\n", jausMessageCommandCodeString(message));
			}
			jausMessageDestroy(message);
			break;

		case JAUS_REPORT_WRENCH_EFFORT:
			if(wdReportWrench)
			{
				reportWrenchEffortMessageDestroy(wdReportWrench);
			}
			
			wdReportWrench = reportWrenchEffortMessageFromJausMessage(message);
			if(!wdReportWrench)
			{
				//cError("wd: Error unpacking %s message.\n", jausMessageCommandCodeString(message));
			}
			jausMessageDestroy(message);
			break;
			
		case JAUS_SET_GLOBAL_WAYPOINT:
			wdGlobalWaypoint = setGlobalWaypointMessageFromJausMessage(message);
			if(!wdGlobalWaypoint)
			{
				//cError("wd: Error unpacking %s message.\n", jausMessageCommandCodeString(message));
			}
			jausMessageDestroy(message);
			jausArrayAdd(wdWaypoints, wdGlobalWaypoint);
			break;

		case JAUS_QUERY_GLOBAL_WAYPOINT:
			queryGlobalWaypointMessage = queryGlobalWaypointMessageFromJausMessage(message);
			if(!queryGlobalWaypointMessage)
			{
				//cError("wd: Error unpacking %s message.\n", jausMessageCommandCodeString(message));
			}
			// loop thru waypoints to find the one that matches the request
			// if there's a match, prepare/send the report, else whine

			for(i = 0; i < wdWaypoints->elementCount; i++)
			{
				SetGlobalWaypointMessage tempGlobalWaypoint;
				tempGlobalWaypoint = (SetGlobalWaypointMessage) wdWaypoints->elementData[i];
				if(tempGlobalWaypoint->waypointNumber == queryGlobalWaypointMessage->waypointNumber)
				{
					reportGlobalWaypointMessage = reportGlobalWaypointMessageCreate();
					jausAddressCopy(reportGlobalWaypointMessage->destination, queryGlobalWaypointMessage->source);
					jausAddressCopy(reportGlobalWaypointMessage->source, wd->address);
					reportGlobalWaypointMessage->presenceVector = NO_PRESENCE_VECTOR;
					reportGlobalWaypointMessage->waypointNumber = tempGlobalWaypoint->waypointNumber;
					reportGlobalWaypointMessage->latitudeDegrees = tempGlobalWaypoint->latitudeDegrees;
					reportGlobalWaypointMessage->longitudeDegrees = tempGlobalWaypoint->longitudeDegrees;
					txMessage = reportGlobalWaypointMessageToJausMessage(reportGlobalWaypointMessage);
					reportGlobalWaypointMessageDestroy(reportGlobalWaypointMessage);
					nodeManagerSend(wdNmi, txMessage);
					jausMessageDestroy(txMessage);
				}
			}
			queryGlobalWaypointMessageDestroy(queryGlobalWaypointMessage);
			jausMessageDestroy(message);
			break;
			
		case JAUS_QUERY_WAYPOINT_COUNT:
			queryWaypointCountMessage = queryWaypointCountMessageFromJausMessage(message);
			if(!queryWaypointCountMessage)
			{
				//cError("wd: Error unpacking %s message.\n", jausMessageCommandCodeString(message));
			}
			reportWaypointCountMessage = reportWaypointCountMessageCreate();
			jausAddressCopy(reportWaypointCountMessage->destination, queryWaypointCountMessage->source);
			jausAddressCopy(reportWaypointCountMessage->source, wd->address);
			reportWaypointCountMessage->waypointCount = wdWaypoints->elementCount;
			txMessage = reportWaypointCountMessageToJausMessage(reportWaypointCountMessage);
			reportWaypointCountMessageDestroy(reportWaypointCountMessage);
			nodeManagerSend(wdNmi, txMessage);
			queryWaypointCountMessageDestroy(queryWaypointCountMessage);
			jausMessageDestroy(message);
			break;
		
		default:
			defaultJausMessageProcessor(message, wdNmi, wd);
			break;
	}
}
Example #6
0
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);
}