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);
}
}
void wdAllState(void)
{
JausMessage message = NULL;
ServiceConnection scList;
ServiceConnection sc;
ReportComponentStatusMessage reportStatus;
ReleaseComponentControlMessage releaseControl = NULL;
// Retrieve the Gpos SC Message
if(gposSc && gposSc->isActive)
{
if(scManagerReceiveServiceConnection(wdNmi, gposSc, &message))
{
wdProcessMessage(message);
}
}
if(vssSc && vssSc->isActive)
{
if(scManagerReceiveServiceConnection(wdNmi, vssSc, &message))
{
wdProcessMessage(message);
}
}
if(pdWrenchSc && pdWrenchSc->isActive)
{
if(scManagerReceiveServiceConnection(wdNmi, pdWrenchSc, &message))
{
wdProcessMessage(message);
}
}
if(pdStatusSc && pdStatusSc->isActive)
{
if(scManagerReceiveServiceConnection(wdNmi, pdStatusSc, &message))
{
wdProcessMessage(message);
}
}
// Respond to a ReportComponentStatus Service Connection
scList = scManagerGetSendList(wdNmi, JAUS_REPORT_COMPONENT_STATUS);
if(scList)
{
reportStatus = reportComponentStatusMessageCreate();
jausAddressCopy(reportStatus->source, wd->address);
reportStatus->properties.scFlag = JAUS_SERVICE_CONNECTION_MESSAGE;
reportStatus->primaryStatusCode = wd->state;
sc = scList;
while(sc)
{
jausAddressCopy(reportStatus->destination, sc->address);
reportStatus->sequenceNumber = sc->sequenceNumber;
message = reportComponentStatusMessageToJausMessage(reportStatus);
nodeManagerSend(wdNmi, message);
jausMessageDestroy(message);
sc = sc->nextSc;
}
reportComponentStatusMessageDestroy(reportStatus);
}
scManagerDestroySendList(scList);
// Check Control
if(!wdRequestControl && wdInControl)
{
// Release Control
//cDebug(4, "wd: Releasing control of PD\n");
releaseControl = releaseComponentControlMessageCreate();
jausAddressCopy(releaseControl->source, wd->address);
jausAddressCopy(releaseControl->destination, pd->address);
message = releaseComponentControlMessageToJausMessage(releaseControl);
nodeManagerSend(wdNmi, message);
jausMessageDestroy(message);
releaseComponentControlMessageDestroy(releaseControl);
wdInControl = JAUS_FALSE;
pd->state = JAUS_UNKNOWN_STATE;
if(pdStatusSc->isActive)
{
scManagerTerminateServiceConnection(wdNmi, pdStatusSc);
}
if(pdWrenchSc->isActive)
{
scManagerTerminateServiceConnection(wdNmi, pdWrenchSc);
}
wd->state = JAUS_INITIALIZE_STATE;
}
}
