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