// 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; }
void TutorialComponent::run(OjCmpt cmpt) { ojCmptRun(cmpt); TutorialData* data = (TutorialData *)ojCmptGetUserData(cmpt); //<ROS2JAUS> ROSComponent roscomp; roscomp.data = data; *(data->subscriber) = data->nodeHandle->subscribe<std_msgs::Int32>("chatter", 1000, &ROSComponent::chatterCallback, &roscomp); while (data->running == JAUS_TRUE) { // doing nothing... ros::spinOnce(); //</ROS2JAUS> ojSleepMsec(100); } }
void TutorialComponent::run(OjCmpt cmpt) { std::cout << "[RUNNING]" << std::endl; ojCmptRun(cmpt); //<ROS2JAUS> ROSComponent roscomp; roscomp.cmpt = &cmpt; TutorialData* data = (TutorialData *)ojCmptGetUserData(cmpt); *(data->velocitySub) = data->nodeHandle->subscribe<std_msgs::Float64>("reportVelocity", 1000, &ROSComponent::reportVelocity, &roscomp); *(data->wrenchSub) = data->nodeHandle->subscribe<beginner_tutorials::wrenchData>("reportWrench", 1000, &ROSComponent::reportWrench, &roscomp); while (data->running == JAUS_TRUE) { ros::spinOnce(); //</ROS2JAUS> } }
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; }
int main(void) { OjCmpt XGV_CCU; int *xgv_ccu_service_connections; XGV_CCU = create_xgv_ccu_component(XGV_CCU_NAME, XGV_CCU_COMPONENTE_ID, XGV_CCU_STATE_MACHINE_UPDATE_RATE); register_xgv_ccu_messages_callbacks(XGV_CCU); ojCmptSetAuthority(XGV_CCU, 6); ojCmptRun(XGV_CCU); // Begin running the XGV_CCU state machine xgv_ccu_service_connections = create_xgv_ccu_service_connections(XGV_CCU); user_interface(XGV_CCU); terminate_xgv_ccu_service_connections(XGV_CCU, xgv_ccu_service_connections); ojCmptDestroy(XGV_CCU); // Shutdown and destroy component return (0); }
/** * Método público que inicia los artefactos JAUS que la clase contiene en sus * atributos y puesta en marcha de los mismos */ void RosNode_Communications::initJAUS() { try { configData = new FileLoader("/home/atica/catkin_ws/src/CITIUS/CITIUS_Control_Communication/bin/nodeManager.conf"); handler = new JausHandler(); nm = new NodeManager(this->configData, this->handler); } catch (...) { ROS_INFO("[Control] Communications - No se ha podido inicializar JAUS"); } //Creacion de componentes // Mission Spooler missionSpoolerComponent = ojCmptCreate((char *) "Mission Spooler", JAUS_MISSION_SPOOLER, 1); if (missionSpoolerComponent == NULL) { ROS_INFO("[Control] Communication - No se ha podido crear el componente MISSION SPOOLER"); exit(0); } else { ojCmptAddServiceOutputMessage(missionSpoolerComponent, JAUS_MISSION_SPOOLER, JAUS_REPORT_MISSION_STATUS, 0xFF); ojCmptAddServiceInputMessage(missionSpoolerComponent, JAUS_MISSION_SPOOLER, JAUS_RUN_MISSION, 0xFF); ojCmptSetMessageCallback(missionSpoolerComponent, JAUS_RUN_MISSION, fcn_receive_run_mission); } // Primitive Driver primitiveDriverComponent = ojCmptCreate((char *) "Primitive Driver", JAUS_PRIMITIVE_DRIVER, 1); if (primitiveDriverComponent == NULL) { ROS_INFO("[Control] Communication - No se ha podido crear el componente PRIMITIVE DRIVER"); exit(0); } else { ojCmptAddServiceOutputMessage(primitiveDriverComponent, JAUS_PRIMITIVE_DRIVER, JAUS_REPORT_WRENCH_EFFORT, 0xFF); ojCmptAddServiceOutputMessage(primitiveDriverComponent, JAUS_PRIMITIVE_DRIVER, JAUS_REPORT_DISCRETE_DEVICES, 0xFF); ojCmptAddServiceOutputMessage(primitiveDriverComponent, JAUS_PRIMITIVE_DRIVER, JAUS_UGV_INFO_12, 0xFF); ojCmptAddServiceInputMessage(primitiveDriverComponent, JAUS_PRIMITIVE_DRIVER, JAUS_SET_WRENCH_EFFORT, 0xFF); ojCmptAddServiceInputMessage(primitiveDriverComponent, JAUS_PRIMITIVE_DRIVER, JAUS_SET_DISCRETE_DEVICES, 0xFF); ojCmptSetMessageCallback(primitiveDriverComponent, JAUS_SET_WRENCH_EFFORT, fcn_receive_set_wrench_effort); ojCmptSetMessageCallback(primitiveDriverComponent, JAUS_SET_DISCRETE_DEVICES, fcn_receive_set_discrete_devices); } // Visual Sensor visualSensorComponent = ojCmptCreate((char *) "Visual Sensor", JAUS_VISUAL_SENSOR, 1); if (visualSensorComponent == NULL) { ROS_INFO("[Control] Communication - No se ha podido crear el componente VISUAL SENSOR"); exit(0); } else { ojCmptAddServiceOutputMessage(visualSensorComponent, JAUS_VISUAL_SENSOR, JAUS_REPORT_CAMERA_POSE, 0xFF); ojCmptAddServiceOutputMessage(visualSensorComponent, JAUS_VISUAL_SENSOR, JAUS_REPORT_SIGNALING_ELEMENTS_25, 0xFF); ojCmptAddServiceOutputMessage(visualSensorComponent, JAUS_VISUAL_SENSOR, JAUS_REPORT_POSITIONER_20, 0xFF); ojCmptAddServiceOutputMessage(visualSensorComponent, JAUS_VISUAL_SENSOR, JAUS_REPORT_DAY_TIME_CAMERA_22, 0xFF); ojCmptAddServiceOutputMessage(visualSensorComponent, JAUS_VISUAL_SENSOR, JAUS_REPORT_NIGHT_TIME_CAMERA_24, 0xFF); ojCmptAddServiceOutputMessage(visualSensorComponent, JAUS_VISUAL_SENSOR, JAUS_SELECT_CAMERA, 0xFF); ojCmptAddServiceInputMessage(visualSensorComponent, JAUS_VISUAL_SENSOR, JAUS_SET_CAMERA_POSE, 0xFF); ojCmptAddServiceInputMessage(visualSensorComponent, JAUS_VISUAL_SENSOR, JAUS_SET_SIGNALING_ELEMENTS_18, 0xFF); ojCmptAddServiceInputMessage(visualSensorComponent, JAUS_VISUAL_SENSOR, JAUS_SET_POSITIONER_19, 0xFF); ojCmptAddServiceInputMessage(visualSensorComponent, JAUS_VISUAL_SENSOR, JAUS_SET_DAY_TIME_CAMERA_21, 0xFF); ojCmptAddServiceInputMessage(visualSensorComponent, JAUS_VISUAL_SENSOR, JAUS_SET_NIGHT_TIME_CAMERA_23, 0xFF); ojCmptSetMessageCallback(visualSensorComponent, JAUS_SET_CAMERA_POSE, fcn_receive_set_camera_pose); ojCmptSetMessageCallback(visualSensorComponent, JAUS_SET_SIGNALING_ELEMENTS_18, fcn_receive_set_signaling_elements); ojCmptSetMessageCallback(visualSensorComponent, JAUS_SET_POSITIONER_19, fcn_receive_set_positioner); ojCmptSetMessageCallback(visualSensorComponent, JAUS_SET_DAY_TIME_CAMERA_21, fcn_receive_set_day_time_camera); ojCmptSetMessageCallback(visualSensorComponent, JAUS_SET_NIGHT_TIME_CAMERA_23, fcn_receive_set_night_time_camera); } // Platform Sensor platformSensorComponent = ojCmptCreate((char *) "Platform Sensor", JAUS_PLATFORM_SENSOR, 1); if (platformSensorComponent == NULL) { ROS_INFO("[Control] Communication - No se ha podido crear el componente PLATFORM SENSOR"); exit(0); } else { ojCmptAddServiceOutputMessage(platformSensorComponent, JAUS_PLATFORM_SENSOR, JAUS_REPORT_TELEMETER_27, 0xFF); ojCmptAddServiceInputMessage(platformSensorComponent, JAUS_PLATFORM_SENSOR, JAUS_SET_TELEMETER_26, 0xFF); ojCmptSetMessageCallback(platformSensorComponent, JAUS_SET_TELEMETER_26, fcn_receive_set_telemeter); } // Velocity State Sensor velocityStateSensorComponent = ojCmptCreate((char *) "Velocity State Sensor", JAUS_VELOCITY_STATE_SENSOR, 1); if (velocityStateSensorComponent == NULL) { ROS_INFO("[Control] Communication - No se ha podido crear el componente VELOCITY STATE SENSOR"); exit(0); } else { ojCmptAddServiceOutputMessage(velocityStateSensorComponent, JAUS_VELOCITY_STATE_SENSOR, JAUS_REPORT_TRAVEL_SPEED, 0xFF); ojCmptAddServiceOutputMessage(velocityStateSensorComponent, JAUS_VELOCITY_STATE_SENSOR, JAUS_REPORT_VELOCITY_STATE, 0xFF); ojCmptAddServiceInputMessage(velocityStateSensorComponent, JAUS_VELOCITY_STATE_SENSOR, JAUS_SET_TRAVEL_SPEED, 0xFF); ojCmptSetMessageCallback(velocityStateSensorComponent, JAUS_SET_TRAVEL_SPEED, fcn_receive_set_travel_speed); } // Global Pose Sensor globalPoseSensorComponent = ojCmptCreate((char *) "Global Pose Sensor", JAUS_GLOBAL_POSE_SENSOR, 1); if (globalPoseSensorComponent == NULL) { ROS_INFO("[Control] Communication - No se ha podido crear el componente GLOBAL POSE SENSOR"); exit(0); } else { ojCmptAddServiceOutputMessage(globalPoseSensorComponent, JAUS_GLOBAL_POSE_SENSOR, JAUS_REPORT_GLOBAL_POSE, 0xFF); ojCmptAddServiceOutputMessage(globalPoseSensorComponent, JAUS_GLOBAL_POSE_SENSOR, JAUS_ADITIONAL_GPSINS_INFO_4, 0xFF); } // HeartBeat Information heartBeatInformationComponent = ojCmptCreate((char *) "HeartBeat Information", JAUS_HEARTBEAT_INFORMATION, 1); if (heartBeatInformationComponent == NULL) { ROS_INFO("[Control] Communication - No se ha podido crear el componente HEARTBEAT INFORMATION"); exit(0); } else { ojCmptAddServiceOutputMessage(heartBeatInformationComponent, JAUS_HEARTBEAT_INFORMATION, JAUS_HEARTBEAT_POSITION_INFO_17, 0xFF); } // Run de componentes ojCmptRun(missionSpoolerComponent); ojCmptRun(primitiveDriverComponent); ojCmptRun(visualSensorComponent); ojCmptRun(platformSensorComponent); ojCmptRun(velocityStateSensorComponent); ojCmptRun(globalPoseSensorComponent); ojCmptRun(heartBeatInformationComponent); }