/**
* Método público que envía información de heartbeat (mensaje JAUS Heartbeat -
* Position Info) a los nodos JAUS de Communication Management de todos los
* subsistemas
*/
void RosNode_Communications::informHeartbeatPositionInfo() {
JausMessage jMsg = NULL;
JausAddress jAdd = jausAddressCreate();
jAdd->subsystem = JAUS_SUBSYSTEM_UGV;
jAdd->node = JAUS_NODE_COMM_MNG;
jAdd->component = JAUS_HEARTBEAT_INFORMATION;
HeartbeatPositionInfo17Message hpi = heartbeatPositionInfo17MessageCreate();
hpi->latitude = hbPosition.latitude;
hpi->longitude = hbPosition.longitude;
hpi->altitude = hbPosition.altitude;
hpi->heading = hbPosition.heading;
hpi->speed = hbPosition.speed;
// Hacia UGV
jausAddressCopy(hpi->destination, jAdd);
jMsg = heartbeatPositionInfo17MessageToJausMessage(hpi);
if (jMsg != NULL) {
ojCmptSendMessage(instance->heartBeatInformationComponent, jMsg);
} else {
ROS_INFO("[Control] Communications - No se ha posdido generar mensaje JAUS con informacion heartbeat hacia UGV");
}
// Hacia MyC
jAdd->subsystem = JAUS_SUBSYSTEM_MYC;
jausAddressCopy(hpi->destination, jAdd);
jMsg = heartbeatPositionInfo17MessageToJausMessage(hpi);
if (jMsg != NULL) {
ojCmptSendMessage(instance->heartBeatInformationComponent, jMsg);
} else {
ROS_INFO("[Control] Communications - No se ha posdido generar mensaje JAUS con informacion heartbeat hacia MyC");
}
// Hacia USV
jAdd->subsystem = JAUS_SUBSYSTEM_USV;
jausAddressCopy(hpi->destination, jAdd);
jMsg = heartbeatPositionInfo17MessageToJausMessage(hpi);
if (jMsg != NULL) {
ojCmptSendMessage(instance->heartBeatInformationComponent, jMsg);
} else {
ROS_INFO("[Control] Communications - No se ha posdido generar mensaje JAUS con informacion heartbeat hacia USV");
}
heartbeatPositionInfo17MessageDestroy(hpi);
jausAddressDestroy(jAdd);
jausMessageDestroy(jMsg);
}
void gposReadyState(OjCmpt gpos)
{
JausMessage txMessage;
ServiceConnection scList;
ServiceConnection sc;
PointLla vehiclePosLla;
ReportGlobalPoseMessage message;
message = (ReportGlobalPoseMessage)ojCmptGetUserData(gpos);
vehiclePosLla = vehicleSimGetPositionLla();
if(vehiclePosLla)
{
message->latitudeDegrees = vehiclePosLla->latitudeRadians * DEG_PER_RAD;
message->longitudeDegrees = vehiclePosLla->longitudeRadians * DEG_PER_RAD;
}
message->yawRadians = -(vehicleSimGetH() - M_PI/2.0);
message->elevationMeters = 0;
message->positionRmsMeters = 1.0;
message->rollRadians = 0.0;
message->pitchRadians = 0.0;
message->attitudeRmsRadians = 0.05;
jausTimeSetCurrentTime(message->time);
// Send message
if(ojCmptIsOutgoingScActive(gpos, JAUS_REPORT_GLOBAL_POSE))
{
scList = ojCmptGetScSendList(gpos, JAUS_REPORT_GLOBAL_POSE);
sc = scList;
while(sc)
{
jausAddressCopy(message->destination, sc->address);
message->presenceVector = sc->presenceVector;
message->sequenceNumber = sc->sequenceNumber;
message->properties.scFlag = JAUS_SERVICE_CONNECTION_MESSAGE;
txMessage = reportGlobalPoseMessageToJausMessage(message);
ojCmptSendMessage(gpos, txMessage);
jausMessageDestroy(txMessage);
sc = sc->nextSc;
}
ojCmptDestroySendList(scList);
}
}
//This function was pulled from another JAUS component for use with ROS2JAUS.
//It allows for the safe use of our service connections.
void sendServiceMessage( JausMessage message, OjCmpt cmpt )
{
ServiceConnection scList;
ServiceConnection sc;
if (ojCmptIsOutgoingScActive(cmpt, message->commandCode)) {
printf("Our outgoing SC is active...\n");
scList = ojCmptGetScSendList(cmpt, message->commandCode);
sc = scList;
while (sc) {
jausAddressCopy(message->destination, sc->address);
message->sequenceNumber = sc->sequenceNumber;
message->properties.scFlag = JAUS_SERVICE_CONNECTION_MESSAGE;
ojCmptSendMessage(cmpt, message);
jausMessageDestroy(message);
sc = sc->nextSc;
}
ojCmptDestroySendList(scList);
}
}
/**
* Método público que obtiene el modo de operación activo de la máquina de
* estados, lo traduce a JAUS (Report Mission Status) y lo envía al controlador
*/
void RosNode_Communications::informStatus() {
// Obtencion del estado
int status;
ros::NodeHandle nh;
nh.getParam("vehicleStatus", status);
JausMessage jMsg = NULL;
JausAddress jAdd = jausAddressCreate();
jAdd->subsystem = instance->subsystemController;
jAdd->node = instance->nodeController;
jAdd->component = JAUS_MISSION_SPOOLER;
jAdd->instance = JAUS_DESTINANTION_INSTANCE;
ReportMissionStatusMessage rmsm = reportMissionStatusMessageCreate();
if (status == OPERATION_MODE_LOCAL) {
rmsm->missionId = JAUS_OPERATION_MODE_LOCAL;
} else if (status == OPERATION_MODE_CONDUCCION) {
rmsm->missionId = JAUS_OPERATION_MODE_CONDUCCION;
} else if (status == OPERATION_MODE_OBSERVACION) {
rmsm->missionId = JAUS_OPERATION_MODE_OBSERVACION;
} else {
rmsm->missionId = JAUS_OPERATION_MODE_INICIANDO;
}
rmsm->status = JAUS_CONTROLLER_NONE;
if(subsystemController == JAUS_SUBSYSTEM_MYC && nodeController == JAUS_NODE_CONTROL)
rmsm->status = JAUS_CONTROLLER_C2;
else if(subsystemController == JAUS_SUBSYSTEM_UGV && nodeController == JAUS_NODE_TABLET)
rmsm->status = JAUS_CONTROLLER_TABLET;
jausAddressCopy(rmsm->destination, jAdd);
jMsg = reportMissionStatusMessageToJausMessage(rmsm);
if (jMsg != NULL) {
ojCmptSendMessage(instance->missionSpoolerComponent, jMsg);
} else {
ROS_INFO("[Control] Communications - No se ha posdido generar mensaje JAUS con informacion electrica de vehiculo");
}
reportMissionStatusMessageDestroy(rmsm);
jausAddressDestroy(jAdd);
jausMessageDestroy(jMsg);
}
void TutorialComponent::processMessage(OjCmpt cmpt, JausMessage msg) {
//<ROS2JAUS>
TutorialData *data = NULL;
data = (TutorialData *)ojCmptGetUserData(cmpt);
SetWrenchEffortMessage wrMsg;
beginner_tutorials::wrenchData rosWrenchMsg;
switch (msg->commandCode) {
case JAUS_SET_WRENCH_EFFORT:
//If we recieve a set wrench message from someone, publish the contents of the message to ROS
wrMsg = setWrenchEffortMessageFromJausMessage(msg);
rosWrenchMsg.throttle = wrMsg->propulsiveLinearEffortXPercent;
rosWrenchMsg.brake = wrMsg->resistiveLinearEffortXPercent;
rosWrenchMsg.steering = wrMsg->propulsiveRotationalEffortZPercent;
data->wrenchPub->publish(rosWrenchMsg);
case JAUS_REQUEST_COMPONENT_CONTROL:
//Hardcode an outgoing address, only necessary if the message doesn't provide a source
JausAddress addr;
addr = jausAddressCreate();
addr->subsystem = 1;
addr->node = 1;
addr->component = JAUS_EXPERIMENTAL_MOTION_PROFILE_DRIVER;
addr->instance = 1;
//Spoof a message to confirm control of the primitive driver
ConfirmComponentControlMessage confirmComponentControl;
confirmComponentControl = confirmComponentControlMessageCreate();
jausAddressCopy(confirmComponentControl->source, ojCmptGetAddress(cmpt));
jausAddressCopy(confirmComponentControl->destination, addr);
confirmComponentControl->responseCode = JAUS_CONTROL_ACCEPTED;
JausMessage confirmControl;
confirmControl = confirmComponentControlMessageToJausMessage(confirmComponentControl);
ojCmptSendMessage(cmpt, confirmControl);
//Spoof a message to report the status of the component
ReportComponentStatusMessage reportComponentStatus;
reportComponentStatus = reportComponentStatusMessageCreate();
reportComponentStatus->primaryStatusCode = JAUS_READY_STATE;
jausAddressCopy(reportComponentStatus->source, ojCmptGetAddress(cmpt));
jausAddressCopy(reportComponentStatus->destination, addr);
JausMessage reportStatus;
reportStatus = reportComponentStatusMessageToJausMessage(reportComponentStatus);
ojCmptSendMessage(cmpt, reportStatus);
ojCmptDefaultMessageProcessor(cmpt, msg);
//</ROS2JAUS>
default:
//if we recieve an unfamiliar message, print its command code
if (msg->commandCode != JAUS_REPORT_HEARTBEAT_PULSE && msg->commandCode != JAUS_CREATE_SERVICE_CONNECTION) {
std::cout << "Received message..." << std::endl;
std::cout << "Command code=" << jausCommandCodeString(msg->commandCode) << std::endl;
}
ojCmptDefaultMessageProcessor(cmpt, msg);
}
}
/**
* 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);
}
// Function: pdProcessMessage
// Access: Private
// Description: This function is responsible for handling incoming JAUS messages from the Node Manager.
// Incoming messages are processed according to message type.
void pdProcessMessage(OjCmpt pd, JausMessage message)
{
ReportComponentStatusMessage reportComponentStatus;
ReportPlatformSpecificationsMessage reportPlatformSpecifications;
SetWrenchEffortMessage setWrenchEffort;
SetDiscreteDevicesMessage setDiscreteDevices;
QueryPlatformSpecificationsMessage queryPlatformSpecifications;
JausAddress address;
JausMessage txMessage;
PdData *data;
data = (PdData*)ojCmptGetUserData(pd);
// This block of code is intended to reject commands from non-controlling components
if(ojCmptHasController(pd) && jausMessageIsRejectableCommand(message) )
{
address = ojCmptGetControllerAddress(pd);
if(!jausAddressEqual(message->source, address))
{
//jausAddressToString(message->source, buf);
//cError("pd: Received command message %s from non-controlling component (%s).\n", jausMessageCommandCodeString(message), buf);
jausAddressDestroy(address);
return;
}
jausAddressDestroy(address);
}
switch(message->commandCode) // Switch the processing algorithm according to the JAUS message type
{
case JAUS_REPORT_COMPONENT_STATUS:
reportComponentStatus = reportComponentStatusMessageFromJausMessage(message);
if(reportComponentStatus)
{
address = ojCmptGetControllerAddress(pd);
if(jausAddressEqual(reportComponentStatus->source, address))
{
reportComponentStatusMessageDestroy(data->controllerStatus);
data->controllerStatus = reportComponentStatus;
}
else
{
reportComponentStatusMessageDestroy(reportComponentStatus);
}
jausAddressDestroy(address);
}
break;
case JAUS_SET_WRENCH_EFFORT:
setWrenchEffort = setWrenchEffortMessageFromJausMessage(message);
if(setWrenchEffort)
{
setWrenchEffortMessageDestroy(data->setWrenchEffort);
data->setWrenchEffort = setWrenchEffort;
}
break;
case JAUS_SET_DISCRETE_DEVICES:
setDiscreteDevices = setDiscreteDevicesMessageFromJausMessage(message);
if(setDiscreteDevices)
{
setDiscreteDevicesMessageDestroy(data->setDiscreteDevices);
data->setDiscreteDevices = setDiscreteDevices;
}
break;
case JAUS_QUERY_PLATFORM_SPECIFICATIONS:
queryPlatformSpecifications = queryPlatformSpecificationsMessageFromJausMessage(message);
if(queryPlatformSpecifications)
{
reportPlatformSpecifications = reportPlatformSpecificationsMessageCreate();
jausAddressCopy(reportPlatformSpecifications->destination, queryPlatformSpecifications->source);
jausAddressCopy(reportPlatformSpecifications->source, queryPlatformSpecifications->destination);
reportPlatformSpecifications->maximumVelocityXMps = 10.0;
txMessage = reportPlatformSpecificationsMessageToJausMessage(reportPlatformSpecifications);
ojCmptSendMessage(pd, txMessage);
jausMessageDestroy(txMessage);
reportPlatformSpecificationsMessageDestroy(reportPlatformSpecifications);
queryPlatformSpecificationsMessageDestroy(queryPlatformSpecifications);
}
break;
default:
ojCmptDefaultMessageProcessor(pd, message);
break;
}
}
