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