// 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; }
OjCmpt create_xgv_ccu_component(char *xgv_ccu_name, int xgv_ccu_component_id, double xgv_ccu_state_machine_update_rate) { OjCmpt XGV_CCU; // Variable that will store the component reference XGV_CCU = ojCmptCreate(xgv_ccu_name, xgv_ccu_component_id, xgv_ccu_state_machine_update_rate); // Create the component if (XGV_CCU == NULL) { printf("Error creating %s component\n", xgv_ccu_name); exit(1); } ojCmptAddService(XGV_CCU, xgv_ccu_component_id); // Add XGV_CCU service type add_xgv_ccu_component_service_messages(XGV_CCU, xgv_ccu_component_id); //TODO função abaixo foi comentada pois gera 50Htz de mensagens desnecessarias de xgv_ccu_state_machine. //ojCmptSetStateCallback(XGV_CCU, JAUS_READY_STATE, xgv_ccu_state_machine); // Set ready state callback //ojCmptSetState(XGV_CCU, JAUS_READY_STATE); // Set the current state to ready return (XGV_CCU); }
OjCmpt TutorialComponent::create(std::string prettyName) { printf("creating\n"); OjCmpt result; TutorialData *data = NULL; // 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> //First spoof the comand line arguments to ROS char **argw = (char**)malloc(sizeof(char*)*2); char *str = new char[8]; strcpy(str,"tutorial"); argw[0] = str; argw[1] = NULL; int one = 1; ros::init(one, argw, "bridge"); //Initialize the data struct and set its values data = (TutorialData *)malloc(sizeof(TutorialData)); data->gotVelocity = false; data->gotWrench = false; data->nodeHandle = new ros::NodeHandle; data->wrenchPub = new ros::Publisher; data->wrenchSub = new ros::Subscriber; data->velocitySub = new ros::Subscriber; *(data->wrenchPub) = data->nodeHandle->advertise<beginner_tutorials::wrenchData>("setWrench", 1000); //Set up service connections ojCmptAddService(result, JAUS_PRIMITIVE_DRIVER); ojCmptAddService(result, JAUS_VELOCITY_STATE_SENSOR); ojCmptAddService(result, JAUS_EXPERIMENTAL_MOTION_PROFILE_DRIVER); ojCmptAddServiceOutputMessage(result, JAUS_PRIMITIVE_DRIVER, JAUS_REPORT_WRENCH_EFFORT, 0xFF); ojCmptAddServiceOutputMessage(result, JAUS_PRIMITIVE_DRIVER, JAUS_REPORT_COMPONENT_STATUS, 0xFF); ojCmptAddServiceOutputMessage(result, JAUS_EXPERIMENTAL_MOTION_PROFILE_DRIVER, JAUS_REPORT_COMPONENT_STATUS, 0xFF); ojCmptAddServiceOutputMessage(result, JAUS_VELOCITY_STATE_SENSOR, JAUS_REPORT_VELOCITY_STATE, 0xFF); ojCmptAddSupportedSc(result, JAUS_REPORT_WRENCH_EFFORT); ojCmptAddSupportedSc(result, JAUS_REPORT_COMPONENT_STATUS); ojCmptAddSupportedSc(result, JAUS_REPORT_VELOCITY_STATE); //</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); data->running = JAUS_TRUE; ojCmptSetUserData(result, (void *)data); } return result; }