Esempio n. 1
0
void TutorialComponent::standbyState(OjCmpt cmpt) {
	//std::cout << "[STANDBY]" << std::endl;
	
	TutorialData *data = (TutorialData *)ojCmptGetUserData(cmpt);
	if (data) {
		data->oneNumberMessage->number++;
		JausAddress destination = jausAddressCreate();
		
		// this is the address that we want to send it to
		if (data->instance == 1) {
			destination->instance = 2;
		} else {
			destination->instance = 1;
		}
		destination->component = JAUS_EXPERIMENTAL_TUTORIAL_COMPONENT;
		destination->node = 1;
		destination->subsystem = 1;
		
		data->oneNumberMessage->source = ojCmptGetAddress(cmpt);
		data->oneNumberMessage->destination = destination;
		
		JausMessage txMsg = oneNumberMessageToJausMessage(data->oneNumberMessage);
		ojCmptSendMessage(cmpt, txMsg);
		
		if (data->oneNumberMessage->number >= data->limit) {
			ojCmptSetState(cmpt, JAUS_SHUTDOWN_STATE);
		} else {
			ojCmptSetState(cmpt, JAUS_READY_STATE);
		}
	} else {
		// bad stuff
	}
}
Esempio n. 2
0
OjCmpt TutorialComponent::create(std::string prettyName) {
	OjCmpt result;
	TutorialData *data = NULL;
	JausAddress tutorialAddr;
	// it is unbelieveable that I have to do this...what a HACK
	char szName[256];
	strcpy(szName, prettyName.c_str());
	// now, we create it using the global (groan) methods
	result = ojCmptCreate(szName, JAUS_EXPERIMENTAL_TUTORIAL_COMPONENT, TUTORIAL_THREAD_DESIRED_RATE_HZ);

	if (result == NULL) {
		// something bad happened...
		std::cout << "Error starting controller...aborting." << std::endl;
		return result;
	} else {
//<ROS2JAUS>
		//Spoof the comand line arguments to ROS
		char **argw = (char**)malloc(sizeof(char*)*2);
		char *str = (char*)malloc(sizeof(char)*strlen("tutorial"));
		str = "tutorial";
		argw[0] = str;
		argw[1] = NULL;
		int one = 1;

		ros::init(one, argw, "bridge");

		data = (TutorialData *)malloc(sizeof(TutorialData));

		data->nodeHandle = new ros::NodeHandle; 
		data->publisher = new ros::Publisher;
		data->subscriber = new ros::Subscriber;

		*(data->publisher) = data->nodeHandle->advertise<std_msgs::Int32>("chat", 1000);
//</ROS2JAUS>

		// Register function callback for the process message state
		ojCmptSetMessageProcessorCallback(result, TutorialComponent::processMessage);
		// Register function callback for the standby state
		ojCmptSetStateCallback(result, JAUS_STANDBY_STATE, TutorialComponent::standbyState);
		// Register function callback for the ready state
		ojCmptSetStateCallback(result, JAUS_READY_STATE, TutorialComponent::readyState);
		// Register function callback for the initialize state
		ojCmptSetStateCallback(result, JAUS_INITIALIZE_STATE, TutorialComponent::initState);
		// Register function callback for the shutdown state
		ojCmptSetStateCallback(result, JAUS_SHUTDOWN_STATE, TutorialComponent::shutdownState);
		ojCmptSetState(result, JAUS_INITIALIZE_STATE);

		// our address
		tutorialAddr = ojCmptGetAddress(result);

		data->oneNumberMessage = oneNumberMessageCreate();
		data->instance = tutorialAddr->instance;
		data->limit = 50000;
		data->running = JAUS_TRUE;

		ojCmptSetUserData(result, (void *)data);
	}
	return result;

}
Esempio n. 3
0
void TutorialComponent::initState(OjCmpt cmpt) {
	std::cout << "[INITIALIZING]" << std::endl;
	TutorialData *data = NULL;
	data = (TutorialData *)ojCmptGetUserData(cmpt);

	if (data) {
		data->oneNumberMessage->number++;
		JausAddress destination = jausAddressCreate();
		
		// this is the address that we want to send it to
		if (data->instance == 1) {
			destination->instance = 2;
		} else {
			destination->instance = 1;
		}
		destination->component = JAUS_EXPERIMENTAL_TUTORIAL_COMPONENT;
		destination->node = 1;
		destination->subsystem = 1;
		
		data->oneNumberMessage->source = ojCmptGetAddress(cmpt);
		data->oneNumberMessage->destination = destination;
		
		JausMessage txMsg = oneNumberMessageToJausMessage(data->oneNumberMessage);
		ojCmptSendMessage(cmpt, txMsg);
		
		ojCmptSetState(cmpt, JAUS_READY_STATE);
	} else {
		// bad things, man...
	}
}
Esempio n. 4
0
// Function: 	gposStartup
// Access:		Public	
// Description: This function allows the abstracted component functionality contained in this file to be started from an external source.
// 				It must be called first before the component state machine and node manager interaction will begin
//				Each call to "gposStartup" should be followed by one call to the "gposShutdown" function
OjCmpt gposCreate(void)
{
	OjCmpt cmpt;
	ReportGlobalPoseMessage message;
	JausAddress gposAddr;
	
	cmpt = ojCmptCreate("gpos", JAUS_GLOBAL_POSE_SENSOR, GPOS_THREAD_DESIRED_RATE_HZ);

	ojCmptAddService(cmpt, JAUS_GLOBAL_POSE_SENSOR);
	ojCmptAddServiceInputMessage(cmpt, JAUS_GLOBAL_POSE_SENSOR, JAUS_QUERY_GLOBAL_POSE, 0xFF);
	ojCmptAddServiceOutputMessage(cmpt, JAUS_GLOBAL_POSE_SENSOR, JAUS_REPORT_GLOBAL_POSE, 0xFF);

	ojCmptSetStateCallback(cmpt, JAUS_READY_STATE, gposReadyState);
	ojCmptSetMessageCallback(cmpt, JAUS_QUERY_GLOBAL_POSE, gposQueryGlobalPoseCallback);
	ojCmptAddSupportedSc(cmpt, JAUS_REPORT_GLOBAL_POSE);
	
	message = reportGlobalPoseMessageCreate();
	gposAddr = ojCmptGetAddress(cmpt);
	jausAddressCopy(message->source, gposAddr);
	jausAddressDestroy(gposAddr);
	
	ojCmptSetUserData(cmpt, (void *)message);
	
	ojCmptSetState(cmpt, JAUS_READY_STATE);

	if(ojCmptRun(cmpt))
	{
		ojCmptDestroy(cmpt);
		return NULL;
	}

	return cmpt;
}
Esempio n. 5
0
OjCmpt pdCreate(void)
{
	OjCmpt cmpt;
	PdData *data;
	JausAddress pdAddr;

	cmpt = ojCmptCreate("pd", JAUS_PRIMITIVE_DRIVER, PD_THREAD_DESIRED_RATE_HZ);

	ojCmptAddService(cmpt, JAUS_PRIMITIVE_DRIVER);
	ojCmptAddServiceInputMessage(cmpt, JAUS_PRIMITIVE_DRIVER, JAUS_SET_WRENCH_EFFORT, 0xFF);
	ojCmptAddServiceInputMessage(cmpt, JAUS_PRIMITIVE_DRIVER, JAUS_SET_DISCRETE_DEVICES, 0xFF);
	ojCmptAddServiceInputMessage(cmpt, JAUS_PRIMITIVE_DRIVER, JAUS_QUERY_PLATFORM_SPECIFICATIONS, 0xFF);
	ojCmptAddServiceInputMessage(cmpt, JAUS_PRIMITIVE_DRIVER, JAUS_QUERY_WRENCH_EFFORT, 0xFF);
	ojCmptAddServiceOutputMessage(cmpt, JAUS_PRIMITIVE_DRIVER, JAUS_REPORT_PLATFORM_SPECIFICATIONS, 0xFF);
	ojCmptAddServiceOutputMessage(cmpt, JAUS_PRIMITIVE_DRIVER, JAUS_REPORT_WRENCH_EFFORT, 0xFF);
	ojCmptAddSupportedSc(cmpt, JAUS_REPORT_WRENCH_EFFORT);

	ojCmptSetMessageProcessorCallback(cmpt, pdProcessMessage);
	ojCmptSetStateCallback(cmpt, JAUS_STANDBY_STATE, pdStandbyState);
	ojCmptSetStateCallback(cmpt, JAUS_READY_STATE, pdReadyState);
	ojCmptSetState(cmpt, JAUS_STANDBY_STATE);

	pdAddr = ojCmptGetAddress(cmpt);

	data = (PdData*)malloc(sizeof(PdData));
	data->setWrenchEffort = setWrenchEffortMessageCreate();
	data->setDiscreteDevices = setDiscreteDevicesMessageCreate();
	data->reportWrenchEffort = reportWrenchEffortMessageCreate();
	data->controllerStatus = reportComponentStatusMessageCreate();
	data->controllerSc = -1;
	jausAddressCopy(data->reportWrenchEffort->source, pdAddr);

	jausAddressDestroy(pdAddr);

	ojCmptSetUserData(cmpt, (void *)data);

	if(ojCmptRun(cmpt))
	{
		ojCmptDestroy(cmpt);
		return NULL;
	}

	return cmpt;
}
Esempio n. 6
0
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);


	}

}
Esempio n. 7
0
// Refresh screen in curses mode
void updateScreen(int keyboardLock, int keyPress)
{
	int row = 0;
	int col = 0;
	char string[256] = {0};
	PointLla vehiclePosLla;
	static int lastChoice = '1';
	JausAddress address;

	if(!keyboardLock && keyPress != -1 && keyPress != 27 && keyPress != 12) //Magic Numbers: 27 = ESC, 12 = Ctrl+l
	{
		switch(keyPress)
		{
			case ' ':
				wdToggleRequestControl(wd);
				break;

			case 'S':
				wdSetSpeed(wd);
				break;

			case 'W':
				wdCreateWaypoint(wd);
				break;

			default:
				lastChoice = keyPress;
		}
	}

	clear();

	mvprintw(row,35,"Keyboard Lock:	%s", keyboardLock?"ON, Press ctrl+L to unlock":"OFF, Press ctrl+L to lock");

	mvprintw(row++,0,"+---------------------------+");
	mvprintw(row++,0,"|     Component Menu        |");
	mvprintw(row++,0,"|                           |");
	mvprintw(row++,0,"| 1. Vehicle Sim            |");
	mvprintw(row++,0,"| 2. Primitive Driver       |");
	mvprintw(row++,0,"| 3. GPOS / VSS             |");
	mvprintw(row++,0,"| 4. Waypoint Driver        |");
	mvprintw(row++,0,"|                           |");
	mvprintw(row++,0,"| ESC to Exit               |");
	mvprintw(row++,0,"+---------------------------+");

	row = 2;
	col = 40;
	switch(lastChoice)
	{
		case '1':
			mvprintw(row++,col,"Vehicle Simulator");
			mvprintw(row++,col,"VS Update Rate:	%7.2f", vehicleSimGetUpdateRate());
			mvprintw(row++,col,"VS Run/Pause:	%s", vehicleSimGetRunPause() == VEHICLE_SIM_PAUSE ? "Pause" : "Run");
			row++;
			mvprintw(row++,col,"VS Vehicle X:\t%9.2f", vehicleSimGetX());
			mvprintw(row++,col,"VS Vehicle Y:\t%9.2f", vehicleSimGetY());
			mvprintw(row++,col,"VS Vehicle H:\t%9.2f", vehicleSimGetH());
			mvprintw(row++,col,"VS Vehicle Speed: %7.2f", vehicleSimGetSpeed());

			row++;
			mvprintw(row++,col,"VS Throttle:\t%9.2f", vehicleSimGetLinearEffortX());
			mvprintw(row++,col,"VS Brake:\t%9.2f", vehicleSimGetResistiveEffortX());
			mvprintw(row++,col,"VS Steering:\t%9.2f", vehicleSimGetRotationalEffort());

			row++;
			vehiclePosLla = vehicleSimGetPositionLla();
			mvprintw(row++,col,"VS Vehicle Latitude:  %+10.7f", vehiclePosLla? vehiclePosLla->latitudeRadians*DEG_PER_RAD : 0.0);
			mvprintw(row++,col,"VS Vehicle Longitude: %+10.7f", vehiclePosLla? vehiclePosLla->longitudeRadians*DEG_PER_RAD : 0.0);
			break;

		case '2':
			mvprintw(row++,col,"Primitive Driver");
			mvprintw(row++,col,"PD Update Rate:	%5.2f", ojCmptGetRateHz(pd));
			address = ojCmptGetAddress(pd);
			jausAddressToString(address, string);
			jausAddressDestroy(address);
			mvprintw(row++,col,"PD Address:\t%s", string);
			mvprintw(row++,col,"PD State:\t%s", jausStateGetString(ojCmptGetState(pd)));

			row++;
			if(ojCmptHasController(pd))
			{
				address = ojCmptGetControllerAddress(pd);
				jausAddressToString(address, string);
				jausAddressDestroy(address);
				mvprintw(row++,col,"PD Controller:	%s", string);
			}
			else
			{
				mvprintw(row++,col,"PD Controller:	None");
			}
			mvprintw(row++,col,"PD Controller SC:	%s", pdGetControllerScStatus(pd)?"Active":"Inactive");
			mvprintw(row++,col,"PD Controller State:	%s", jausStateGetString(pdGetControllerState(pd)));

			row++;
			mvprintw(row++,col,"PD Prop Effort X: %0.0lf", pdGetWrenchEffort(pd)? pdGetWrenchEffort(pd)->propulsiveLinearEffortXPercent:-1.0);
			mvprintw(row++,col,"PD Rstv Effort X: %0.0lf", pdGetWrenchEffort(pd)? pdGetWrenchEffort(pd)->resistiveLinearEffortXPercent:-1.0);
			mvprintw(row++,col,"PD Rtat Effort Z: %0.0lf", pdGetWrenchEffort(pd)? pdGetWrenchEffort(pd)->propulsiveRotationalEffortZPercent:-1.0);
			break;

		case '3':
			mvprintw(row++,col,"Global Pose Sensor");
			mvprintw(row++,col,"GPOS Update Rate: %7.2f", ojCmptGetRateHz(gpos));
			address = ojCmptGetAddress(gpos);
			jausAddressToString(address, string );
			jausAddressDestroy(address);
			mvprintw(row++,col,"GPOS Address:\t    %s", string);
			mvprintw(row++,col,"GPOS State:\t    %s", jausStateGetString(ojCmptGetState(gpos)));
			mvprintw(row++,col,"GPOS SC State:\t    %s", gposGetScActive(gpos)? "Active" : "Inactive");

			row++;
			mvprintw(row++,col,"Velocity State Sensor");
			mvprintw(row++,col,"VSS Update Rate:  %7.2f", ojCmptGetRateHz(vss));
			address = ojCmptGetAddress(vss);
			jausAddressToString(address, string );
			jausAddressDestroy(address);
			mvprintw(row++,col,"VSS Address:\t    %s", string);
			mvprintw(row++,col,"VSS State:\t    %s", jausStateGetString(ojCmptGetState(vss)));
			mvprintw(row++,col,"VSS SC State:\t    %s", vssGetScActive(vss)? "Active" : "Inactive");
			break;

		case '4':
			mvprintw(row++,col,"Waypoint Driver");
			mvprintw(row++,col,"WD Update Rate: %7.2f", ojCmptGetRateHz(wd));

			address = ojCmptGetAddress(wd);
			jausAddressToString(address, string );
			jausAddressDestroy(address);
			mvprintw(row++,col,"WD Address:\t  %s", string);
			mvprintw(row++,col,"WD State:\t  %s", jausStateGetString(ojCmptGetState(wd)));

			address = ojCmptGetControllerAddress(wd);
			if(address)
			{
				jausAddressToString(address, string);
				mvprintw(row++,col,"WD Controller:\t  %s", string);
				jausAddressDestroy(address);
			}
			else
			{
				mvprintw(row++,col,"WD Controller:\t  None");
			}

			row = 11;
			col = 2;
			mvprintw(row++,col,"GPOS SC:\t    %s", wdGetGposScStatus(wd)? "Active" : "Inactive");
			mvprintw(row++,col,"VSS SC:\t    %s", wdGetVssScStatus(wd)? "Active" : "Inactive");
			mvprintw(row++,col,"PD Wrench SC:\t    %s", wdGetPdWrenchScStatus(wd)? "Active" : "Inactive");
			mvprintw(row++,col,"PD State SC:\t    %s", wdGetPdStatusScStatus(wd)? "Active" : "Inactive");
			row++;
			mvprintw(row++,col,"WD Request Control:\t%s", wdGetRequestControl(wd)? "True" : "False");
			mvprintw(row++,col,"(Space to Toggle)");
			mvprintw(row++,col,"WD Control:\t\t%s", wdGetInControlStatus(wd)? "True" : "False");
			mvprintw(row++,col,"PD State:\t\t%s", jausStateGetString(wdGetPdState(wd)));

			row = 11;
			col = 40;
			if(wdGetGlobalWaypoint(wd))
			{
				mvprintw(row++,col,"Global Waypoint: (%9.7lf,%9.7lf)", wdGetGlobalWaypoint(wd)->latitudeDegrees, wdGetGlobalWaypoint(wd)->longitudeDegrees);
			}
			else
			{
				mvprintw(row++,col,"Global Waypoint: None");
			}

			if(wdGetTravelSpeed(wd))
			{
				mvprintw(row++,col,"Travel Speed: %7.2f", wdGetTravelSpeed(wd)->speedMps);
			}
			else
			{
				mvprintw(row++,col,"Travel Speed: None");
			}

			mvprintw(row++,col,"dSpeedMps: %7.2f", wdGetDesiredVehicleState(wd)? wdGetDesiredVehicleState(wd)->desiredSpeedMps : 0.0);
			mvprintw(row++,col,"dPhi:      %7.2f", wdGetDesiredVehicleState(wd)? wdGetDesiredVehicleState(wd)->desiredPhiEffort : 0.0);

			break;

		default:
			mvprintw(row++,col,"NONE.");
			break;
	}

	move(24,0);
	refresh();
}