/*
* Discover the Judges' COP. Will not return until discovery or failure.
*/
JAUS::Subsystem* DiscoverJudgeSubsystem(JAUS::Component* c)
{
JAUS::Subsystem* judgeSubsystem = NULL;
#ifdef REAL_JUDGES
// now that we are initialized, create a direct connection to the Judges' COP
// (Multicast may not be supported)
JAUS::JTCPClient* transportService = NULL;
transportService = (JAUS::JTCPClient*) component->TransportService();
transportService->AddConnection(JudgeIPString,
JAUS::Address(JudgeSubsystemID, JudgeNodeID, JudgeComponentID));
#endif
// get a pointer to our Management Service
JAUS::Management* managementService = c->ManagementService();
JAUS::Time::Stamp timeMs = JAUS::Time::GetUtcTimeMs();
// try to find the judges' COP
while (managementService->GetStatus() != JAUS::Management::Status::Shutdown)
{
CxUtils::SleepMs(10); // save some CPU time
if (JAUS::Time::GetUtcTimeMs() - timeMs < 1000)
continue;
// now look at discovered subsystems (for the judges' COP)
JAUS::Discovery* discoveryService = c->DiscoveryService();
JAUS::Subsystem::Map discoveredSubsystems;
discoveryService->GetSubsystems(discoveredSubsystems);
JAUS::Subsystem::Map::iterator subsystem;
// NOTE: the map is indexed by the subsystem number
for (subsystem = discoveredSubsystems.begin();
subsystem != discoveredSubsystems.end();
subsystem++)
{
if (subsystem->first == JudgeSubsystemID)
{
judgeSubsystem = subsystem->second;
break;
}
}
JAUS::Subsystem::DeleteSubsystemMap(discoveredSubsystems);
timeMs = JAUS::Time::GetUtcTimeMs();
if (judgeSubsystem != NULL)
break;
}
return judgeSubsystem;
}
int main(int argc, char* argv[])
{
JAUS::Component component;
// Setup identification info. For questions about this,
// see the previous tutorial(s).
JAUS::Discovery* discoveryService = NULL;
discoveryService = component.DiscoveryService();
discoveryService->SetSubsystemIdentification(JAUS::Subsystem::Vehicle,
"Robot");
discoveryService->SetNodeIdentification("Primary Computer");
discoveryService->SetComponentIdentification("Baseline");
JAUS::Address componentID(1000, 1, 2);
// Initialize!
std::cout << "Initializing component...";
if(component.Initialize(componentID) == false)
{
std::cout << "Failed to initialize component [" << componentID.ToString() << "]\n";
return 0;
}
std::cout << "Success!\n";
// Now go into your main computer loop until the
// component has been told to shutdown.
JAUS::Time::Stamp displayStatusTimeMs = JAUS::Time::GetUtcTimeMs();
while(true)
{
JAUS::Management* managementService = NULL;
managementService = component.ManagementService();
if(managementService->GetStatus() == JAUS::Management::Status::Shutdown)
{
// Exit program.
break;
}
if(JAUS::Time::GetUtcTimeMs() - displayStatusTimeMs > 500)
{
// Use the discovery service to get a list of
// discovered subsystems. (see below for how to clear map).
JAUS::Subsystem::Map discoveredSubsystems;
discoveryService->GetSubsystems(discoveredSubsystems);
std::cout << "======================================================\n";
JAUS::Subsystem::Map::iterator subsystem;
// The map is indexed by the subsystem number.
for(subsystem = discoveredSubsystems.begin();
subsystem != discoveredSubsystems.end();
subsystem++)
{
std::cout << "Subsystem: "
<< subsystem->first
<< " Identification: "
<< subsystem->second->mIdentification
<< std::endl;
// Lets see if it has specific service we
// want to communicate with. For this tutorial
// let's check for the Liveness service, which
// supports the Query Heartbeat Pulse and
// Report Heartbeat Pulse messages.
// We can use the subsystem data structure to
// get a list of components on the subsystem that
// have the service, so we can send a message to it.
JAUS::Address::List componentsWithLiveness;
componentsWithLiveness = subsystem->second->GetComponentsWithService(JAUS::Liveness::Name);
JAUS::Address::List::iterator c;
for(c = componentsWithLiveness.begin();
c != componentsWithLiveness.end();
c++)
{
// First, make sure it is not the
// component we are using, because we
// want to talk to a different one.
if( (*c) != component.GetComponentID())
{
// Now that we have the ID of
// component with the Liveness
// service, lets send a query message
// and wait for the response.
// Setup the query message to send.
JAUS::QueryHeartbeatPulse query;
query.SetDestinationID( (*c) );
query.SetSourceID(component.GetComponentID());
// This is the response message we want.
JAUS::ReportHeartbeatPulse response;
// Send and see if we got a
// response, but only wait up to 1 second
// for a response. Default value for waiting
// is 100 ms.
std::cout << "\tSending Query to " << c->ToString() << std::endl;
if(component.Send(&query, &response, 1000))
{
std::cout << "\tReceived Response Message!\n\t";
response.Print();
}
}
}
// The above steps can be used to find any component
// with a specific service you wish to communicate with.
}
// Make sure you delete the subsystem map when
// you are done with it, otherwise you will have a
// memory leak.
JAUS::Subsystem::DeleteSubsystemMap(discoveredSubsystems);
displayStatusTimeMs = JAUS::Time::GetUtcTimeMs();
}
if(CxUtils::GetChar() == 27)
{
break;
}
CxUtils::SleepMs(1);
}
// Shutdown your component completely. Any
// services added or belonging to the component
// will be deleted.
component.Shutdown();
return 0;
}
////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Simple simulation of a component that subscribes to any Local
/// Pose Sensor data on network using JTCP.
///
/// \param[in] subsystemID Subsystem ID number.
/// \param[in] nodeID Node ID number.
///
////////////////////////////////////////////////////////////////////////////////////
int RunClient(JAUS::UShort subsystemID, JAUS::Byte nodeID)
{
JAUS::Component component;
JAUS::Address id(subsystemID, nodeID, 1);
if(id.IsValid() == false)
{
std::cout << "Invalid JAUS ID: " << id.ToString() << std::endl;
}
// Setup identification data.
component.DiscoveryService()->SetSubsystemIdentification(JAUS::Subsystem::Vehicle,
"Subscriber JTCP Tutorial");
// Change the transport service to use JTCP
component.AddService(new JAUS::JTCPClient());
//((JAUS::JTCPClient*)component.TransportService())->AddConnection("10.171.190.61", JAUS::Address(subsystemID, 1, 1));
//((JAUS::JTCPClient*)component.TransportService())->AddConnection("127.0.0.1", JAUS::Address(subsystemID, 1, 1));
// Add support for reading Local Pose messages without having to
// add a custom service.
component.TransportService()->AddMessageTemplate(new JAUS::ReportLocalPose());
component.TransportService()->AddMessageTemplate(new JAUS::QueryLocalPose());
// Add a callback to get messages when they arrive.
LocalPoseCallback localPoseCallback;
component.TransportService()->RegisterCallback(JAUS::REPORT_LOCAL_POSE, &localPoseCallback);
// Initialize component and communication.
if(component.Initialize(id) == false)
{
std::cout << "Failed to initialize JAUS component with ID: " << id.ToString() << std::endl;
return 0;
}
JAUS::Time printTime(true);
while(CxUtils::GetChar() != 27 && component.ManagementService()->GetStatus() != JAUS::Management::Status::Shutdown)
{
// Look for any subsystems.
JAUS::Subsystem::Map subsystems;
JAUS::Subsystem::Map::iterator subsystem;
component.DiscoveryService()->GetSubsystems(subsystems);
for(subsystem = subsystems.begin();
subsystem != subsystems.end();
subsystem++)
{
// Look for local pose sensors to subscribe to.
JAUS::Address::List sensors = subsystem->second->GetComponentsWithService(JAUS::LocalPoseSensor::Name);
if(sensors.size() > 0)
{
JAUS::Address::List::iterator id;
for(id = sensors.begin();
id != sensors.end();
id++)
{
if(*id != component.GetComponentID() &&
component.EventsService()->HaveSubscription(JAUS::REPORT_LOCAL_POSE, *id) == false)
{
std::cout << "Found new Local Pose Sensors on " << id->ToString() << std::endl;
JAUS::QueryLocalPose queryLocalPose;
// Request all fields.
queryLocalPose.SetPresenceVector(queryLocalPose.GetPresenceVectorMask());
component.EventsService()->RequestPeriodicEvent(*id, &queryLocalPose, 5);
}
}
}
}
JAUS::Subsystem::DeleteSubsystemMap(subsystems);
// Only print to screen every now and then.
if(JAUS::Time(true) - printTime > .5)
{
//std::cout << "=======================================================\n";
//component.EventsService()->PrintStatus();
//printTime.SetCurrentTime();
}
// Don't overload the CPU
CxUtils::SleepMs(1);
}
// Do shutdown
component.Shutdown();
return 0;
}