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