void gposStartupState(void)
{
JausService service;
// Populate Core Service
if(!jausServiceAddCoreServices(gpos->services))
{
cError("gpos: Addition of Core Services FAILED! Switching to FAILURE_STATE\n");
gpos->state = JAUS_FAILURE_STATE;
}
// USER: Add the rest of your component specific service(s) here
service = jausServiceCreate(gpos->address->component);
if(!service)
{
cError("gpos:%d: Creation of JausService FAILED! Switching to FAILURE_STATE\n", __LINE__);
gpos->state = JAUS_FAILURE_STATE;
}
jausServiceAddService(gpos->services, service);
jausServiceAddInputCommand(service, JAUS_QUERY_GLOBAL_POSE, 0xFF);
jausServiceAddOutputCommand(service, JAUS_REPORT_GLOBAL_POSE, 0xFF);
gposMessage = reportGlobalPoseMessageCreate();
gposMessage->source->id = gpos->address->id;
//add support for ReportGlovalPose Service Connections
scManagerAddSupportedMessage(gposNmi, JAUS_REPORT_GLOBAL_POSE);
}
// 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;
}
/**
* Método privado consumidor de topic asociado a información recopilada de los
* sensores de Posición/Orientación. Traduce la información a JAUS (Report
* Global Pose/Report Velocity State/Additional GPS/INS Info) y la envía al
* controlador
* @param[in] msg Mensaje ROS con información de posición/orientación
*/
void RosNode_Communications::fnc_subs_posOriInfo(CITIUS_Control_Communication::msg_posOriInfo msg) {
JausMessage jMsg = NULL;
JausAddress jAdd = jausAddressCreate();
jAdd->subsystem = subsystemController;
jAdd->node = nodeController;
jAdd->component = JAUS_GLOBAL_POSE_SENSOR;
jAdd->instance = JAUS_DESTINANTION_INSTANCE;
// Report Global Pose
ReportGlobalPoseMessage rgpm = reportGlobalPoseMessageCreate();
rgpm->presenceVector = 0x0077;
rgpm->latitudeDegrees = msg.latitude;
hbPosition.latitude = msg.latitude;
rgpm->longitudeDegrees = msg.longitude;
hbPosition.longitude = msg.longitude;
rgpm->attitudeRmsRadians = msg.altitude;
hbPosition.altitude = msg.altitude;
rgpm->rollRadians = msg.roll;
rgpm->pitchRadians = msg.pitch;
rgpm->yawRadians = msg.yaw;
hbPosition.heading = msg.yaw;
jausAddressCopy(rgpm->destination, jAdd);
jMsg = reportGlobalPoseMessageToJausMessage(rgpm);
reportGlobalPoseMessageDestroy(rgpm);
if (jMsg != NULL) {
ojCmptSendMessage(globalPoseSensorComponent, jMsg);
}
// Report Velocity State
ReportVelocityStateMessage rvsm = reportVelocityStateMessageCreate();
rvsm->presenceVector = 0x0007;
rvsm->velocityXMps = msg.velX;
rvsm->velocityYMps = msg.velY;
rvsm->velocityZMps = msg.velZ;
hbPosition.speed = sqrt(pow(msg.velX,2)+pow(msg.velY,2)+pow(msg.velZ,2));
jAdd->component = JAUS_VELOCITY_STATE_SENSOR;
jAdd->instance = JAUS_DESTINANTION_INSTANCE;
jausAddressCopy(rvsm->destination, jAdd);
jMsg = reportVelocityStateMessageToJausMessage(rvsm);
reportVelocityStateMessageDestroy(rvsm);
if (jMsg != NULL) {
ojCmptSendMessage(velocityStateSensorComponent, jMsg);
}
// Additional GPS/INS Info
AditionalGPSINSInfo4Message agim = aditionalGPSINSInfo4MessageCreate();
agim->presenceVector = 0x0007;
agim->longitudinal_acc = msg.accX;
agim->lateral_acc = msg.accY;
agim->vertical_acc = msg.accZ;
// Estado IMU y GPS
// TODO!!!!!!!!!!!!!!!!!!!!!!!!
jAdd->component = JAUS_GLOBAL_POSE_SENSOR;
jAdd->instance = JAUS_DESTINANTION_INSTANCE;
jausAddressCopy(agim->destination, jAdd);
jMsg = aditionalGPSINSInfo4MessageToJausMessage(agim);
aditionalGPSINSInfo4MessageDestroy(agim);
if (jMsg != NULL) {
ojCmptSendMessage(velocityStateSensorComponent, jMsg);
}
jausAddressDestroy(jAdd);
jausMessageDestroy(jMsg);
}