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