void LocalWaypointListDriver_ReceiveFSM::SendAction(std::string arg0, Receive::Body::ReceiveRec transportData)
{
// Extract the sender information
JausAddress sender(transportData.getSrcSubsystemID(),
transportData.getSrcNodeID(),
transportData.getSrcComponentID());
if (arg0 == "ReportActiveElement")
{
ReportActiveElement active_element_msg;
active_element_msg.getBody()->getActiveElementRec()->setElementUID(activeWaypoint);
sendJausMessage( active_element_msg, sender );
}
else if (arg0 == "ConfirmElementRequest")
{
ConfirmElementRequest confirm_element_msg;
// this should actually be populated with the request ID
sendJausMessage( confirm_element_msg, sender );
}
else if (arg0 == "ReportLocalWaypoint")
{
// Get the current waypoint if active, otherwise first in list
ReportLocalWaypoint waypoint;
waypoint.decode(findElement((activeWaypoint == 0) ? 1 : activeWaypoint)->getElementData()->getData());
ReportLocalWaypoint report_waypoint_msg;
report_waypoint_msg.getBody()->getLocalWaypointRec()->setX(waypoint.getBody()->getLocalWaypointRec()->getX());
report_waypoint_msg.getBody()->getLocalWaypointRec()->setY(waypoint.getBody()->getLocalWaypointRec()->getY());
sendJausMessage( report_waypoint_msg, sender );
}
}
void DiscoveryClient_DiscoveryClientFSM::sendQueryMessageAction(GuiControlEntered msg)
{
/// Insert User Code HERE
cout << "Sending query:" << msg.getGuiControlEnteredBody()->getGuiControlEnteredRecord()->getCommand() << endl;
// see which query message I should send and send it
// Cmd 1 - Query Identification
// Cmd 2 - Query Services
// Cmd 3 - Query Configuration
// Cmd 4 - Query Subsystem List
// Cmd 5 - Query Services List
switch (msg.getGuiControlEnteredBody()->getGuiControlEnteredRecord()->getCommand()) {
case QUERYIDENTIFICATION: {
QueryIdentification qry;
JausAddress recipient(msg.getGuiControlEnteredBody()->getGuiControlEnteredRecord()->getValue());
cout << "Send QueryIdentification to: " << hex << (unsigned int)recipient.getSubsystemID() << ":"
<< (unsigned int)recipient.getNodeID() << ":" << (unsigned int)recipient.getComponentID() << endl;
sendJausMessage( qry, recipient);
}
break;
case QUERYSERVICES: {
cout << "Send QueryServices." << endl;
QueryServices qry;
// broadcast query to specified discovery service
JausAddress recipient(msg.getGuiControlEnteredBody()->getGuiControlEnteredRecord()->getValue());
sendJausMessage( qry, recipient);
}
break;
case QUERYCONFIGURATION:
cout << "Send QueryConfiguration." << endl;
break;
case QUERYSUBSYSTEMLIST:
cout << "Send QuerySubsystemList." << endl;
break;
case QUERYSERVICESLIST:
cout << "Send QueryServicesList." << endl;
break;
default:
cout << "Unknown GUI command." << endl;
break;
}
}
void LocalPoseSensor_ReceiveFSM::SendAction(std::string arg0, Receive::Body::ReceiveRec transportData)
{
// Extract the sender information
JausAddress sender(transportData.getSrcSubsystemID(),
transportData.getSrcNodeID(),
transportData.getSrcComponentID());
// Formulate the requested response
if (arg0 == "ReportLocalPose")
{
ReportLocalPose pose_msg;
pose_msg.getBody()->getLocalPoseRec()->setX( SharedData::get()->getX() );
pose_msg.getBody()->getLocalPoseRec()->setY( SharedData::get()->getY() );
pose_msg.getBody()->getLocalPoseRec()->setYaw( SharedData::get()->getYaw() );
sendJausMessage(pose_msg, sender);
}
}
void AdditionServerServiceDef_additionServerFSM::sendReportAdditionAction(QueryAddition msg, unsigned int sender)
{
// Pull out the data.
int A1 = msg.getAdditionInputBody()->getAdditionInput()->getA1();
int A2 = msg.getAdditionInputBody()->getAdditionInput()->getA2();
// Now, lets pull out the two numbers we received
std::cout << " Need to add " << A1 << " + " << A2 << std::endl;
// Now let's formulate a response
int answer;
answer = A1 + A2;
ReportAddition theAnswer;
theAnswer.getAdditionOutputBody()->getAdditionOutput()->setAdditionResult(answer);
// Encode the response and send it back to the requestor.
sendJausMessage( theAnswer, JausAddress(sender) );
std::cout << "answer sent to client\n";
}
bool JausOpcUdpInterface::processMessage(JausMessage message)
{
switch(this->type)
{
case SUBSYSTEM_INTERFACE:
// if subs==BROADCAST send multicast
if(message->destination->subsystem == JAUS_BROADCAST_SUBSYSTEM_ID)
{
if(this->multicast)
{
// Send multicast packet
sendJausMessage(this->multicastData, message);
jausMessageDestroy(message);
return true;
}
else
{
// Unicast to all known subsystems
HASH_MAP<int, OpcUdpTransportData>::iterator iter;
for(iter = addressMap.begin(); iter != addressMap.end(); iter++)
{
sendJausMessage(iter->second, message);
}
jausMessageDestroy(message);
return true;
}
}
else
{
// Unicast
if(addressMap.find(message->destination->subsystem) != addressMap.end())
{
sendJausMessage(addressMap.find(message->destination->subsystem)->second, message);
jausMessageDestroy(message);
return true;
}
else
{
// Don't know how to send this message
jausMessageDestroy(message);
return false;
}
}
break;
case NODE_INTERFACE:
if( message->destination->subsystem == mySubsystemId ||
message->destination->subsystem == JAUS_BROADCAST_SUBSYSTEM_ID )
{
// if node==BROADCAST send multicast
if(message->destination->node == JAUS_BROADCAST_NODE_ID)
{
if(this->multicast)
{
// Send multicast packet
sendJausMessage(this->multicastData, message);
jausMessageDestroy(message);
return true;
}
else
{
// Unicast to all known nodes
HASH_MAP<int, OpcUdpTransportData>::iterator iter;
for(iter = addressMap.begin(); iter != addressMap.end(); iter++)
{
sendJausMessage(iter->second, message);
}
jausMessageDestroy(message);
return true;
}
}
else
{
// Unicast
if(addressMap.find(message->destination->node) != addressMap.end())
{
sendJausMessage(addressMap.find(message->destination->node)->second, message);
jausMessageDestroy(message);
return true;
}
else
{
// Don't know how to send this message
jausMessageDestroy(message);
return false;
}
}
}
else
{
// Message for other subsystem
if(subsystemGatewayDiscovered)
{
sendJausMessage(subsystemGatewayData, message);
jausMessageDestroy(message);
return true;
}
else
{
// Don't know how to send this message
jausMessageDestroy(message);
return false;
}
}
break;
case COMPONENT_INTERFACE:
// if cmpt == BROADCAST || inst == BROADCAST send unicast to all components
if( message->destination->component == JAUS_BROADCAST_COMPONENT_ID ||
message->destination->instance == JAUS_BROADCAST_INSTANCE_ID )
{
if(this->multicast)
{
// Send multicast packet
sendJausMessage(this->multicastData, message);
jausMessageDestroy(message);
return true;
}
else
{
// Unicast to all known subsystems
HASH_MAP<int, OpcUdpTransportData>::iterator iter;
for(iter = addressMap.begin(); iter != addressMap.end(); iter++)
{
sendJausMessage(iter->second, message);
}
jausMessageDestroy(message);
return true;
}
}
else
{
// Unicast
if(addressMap.find(jausAddressHash(message->destination)) != addressMap.end())
{
sendJausMessage(addressMap.find(jausAddressHash(message->destination))->second, message);
jausMessageDestroy(message);
return true;
}
else
{
// Don't know how to send this message
jausMessageDestroy(message);
return false;
}
}
break;
default:
// Unknown type
// No routing behavior
jausMessageDestroy(message);
return false;
}
}
void PingClient_PingClientFSM::QueryHeartBeatPulseAction()
{
// Send the QueryHeartbeat message to the local component
QueryHeartbeatPulse query;
sendJausMessage( query, *jausRouter->getJausAddress());
}
