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);
}
}
char* jausCommandToString(JausCommand command)
{
char* buffer = (char*)malloc(sizeof(char)*300);
if(command)
sprintf(buffer, "Command Code: 0X%X %s Presence Vector: 0X%X", command->commandCode, jausCommandCodeString(command->commandCode), command->presenceVector);
else
strcpy(buffer, "No Jaus Command");
return buffer;
}
int jausSubsystemTableToDetailedString(JausSubsystem subsystem, char *buf)
{
int i = 0;
int j = 0;
int k = 0;
char tempBuf[256] = {0};
JausNode node;
JausComponent comp;
JausService service;
JausCommand command;
jausSubsystemToString(subsystem, tempBuf);
sprintf(buf, "%s\n", tempBuf);
for(i = 0; i < subsystem->nodes->elementCount; i++)
{
node = (JausNode)subsystem->nodes->elementData[i];
jausNodeToString(node, tempBuf);
strcat(buf, " ");
strcat(buf, tempBuf);
strcat(buf, "\n");
for(j = 0; j < node->components->elementCount; j++)
{
comp = (JausComponent)node->components->elementData[j];
memset(tempBuf, 0, 256);
jausComponentToString(comp, tempBuf);
strcat(buf, " ");
strcat(buf, tempBuf);
strcat(buf, "\n");
for(k = 0; k < comp->services->elementCount; k++)
{
service = (JausService) comp->services->elementData[k];
sprintf(tempBuf, " Service Type: %d\n", service->type);
strcat(buf, tempBuf);
strcat(buf, " INPUT:\n");
command = service->inputCommandList;
while(command)
{
sprintf(tempBuf, " %s (0x%04X)\n", jausCommandCodeString(command->commandCode), command->presenceVector);
strcat(buf, tempBuf);
command = command->next;
}
strcat(buf, " OUTPUT:\n");
command = service->outputCommandList;
while(command)
{
sprintf(tempBuf, " %s (0x%04X)\n", jausCommandCodeString(command->commandCode), command->presenceVector);
strcat(buf, tempBuf);
command = command->next;
};
}
}
}
return (int)strlen(buf);
}
