// Prepares the planarity test and the planar embedding
// Parallel edges: do not need to be ignored, they can be handled
// by the planarity test.
// Selfloops: need to be ignored.
bool PlanarModule::preparation(Graph &G,bool embed)
{
if (G.numberOfEdges() < 9 && !embed)
return true;
else if (G.numberOfEdges() < 3 && embed)
return true;
node v;
edge e;
SListPure<node> selfLoops;
makeLoopFree(G,selfLoops);
prepareParallelEdges(G);
int isolated = 0;
forall_nodes(v,G)
if (v->degree() == 0)
isolated++;
if (((G.numberOfNodes()-isolated) > 2) &&
((3*(G.numberOfNodes()-isolated) -6) < (G.numberOfEdges() - m_parallelCount)))
return false;
bool planar = true;
NodeArray<node> tableNodes(G,0);
EdgeArray<edge> tableEdges(G,0);
NodeArray<bool> mark(G,0);
EdgeArray<int> componentID(G);
// Determine Biconnected Components
int bcCount = biconnectedComponents(G,componentID);
// Determine edges per biconnected component
Array<SList<edge> > blockEdges(0,bcCount-1);
forall_edges(e,G)
{
blockEdges[componentID[e]].pushFront(e);
}
int main(int argc, char* argv[])
{
JAUS::Component component;
// Add services to a component is done using the AddService
// method. Services can only be added before a
// component is initialized using the Initialize method.
// Any attempt to add after will fail. Finally, you
// cannot add multiple of the same service.
// Create a Global Pose Sensor service.
JAUS::GlobalPoseSensor* globalPoseSensor = new JAUS::GlobalPoseSensor();
// Set the update rate of the sensor (Hz). This
// is used to determine what type of periodic
// events the sensor can support.
globalPoseSensor->SetSensorUpdateRate(25);
// Set some global pose values.
JAUS::GlobalPose globalPose;
globalPose.SetLatitude(34.12345);
globalPose.SetLongitude(-116.12345);
globalPose.SetAltitude(100);
globalPose.SetTimeStamp(JAUS::Time(true));
// Set the values.
globalPoseSensor->SetGlobalPose(globalPose);
// Add the sensor service to the component. Remember
// this must be done before initialization. Also,
// the component will delete the service for us
// automatically, so we don't have to!
component.AddService(globalPoseSensor);
// 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, 6);
// 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)
{
// To "simulate" a real sensor, lets modify
// the latitude like the robot is moving north.
globalPose.SetLatitude(globalPose.GetLatitude() + 0.00001);
globalPose.SetTimeStamp(JAUS::Time(true));
// Update the sensor with the new data. This will
// automatically trigger events if someone (i.e. another
// component) is subscribing to the data from our sensor.
// this is because the Global Pose Sensor class inherits
// from the Events service and has implemented all the
// methods needed to support subscriptions.
globalPoseSensor->SetGlobalPose(globalPose);
JAUS::Management* managementService = NULL;
managementService = component.ManagementService();
if(managementService->GetStatus() == JAUS::Management::Status::Shutdown)
{
// Exit program.
break;
}
if(JAUS::Time::GetUtcTimeMs() - displayStatusTimeMs > 500)
{
// Print out status of the service.
component.GetService(JAUS::GlobalPoseSensor::Name)->PrintStatus(); std::cout << std::endl;
displayStatusTimeMs = JAUS::Time::GetUtcTimeMs();
}
if(CxUtils::GetChar() == 27)
{
break;
}
CxUtils::SleepMs(100);
}
// Shutdown your component completely. Any
// services added or belonging to the component
// will be deleted.
component.Shutdown();
return 0;
}
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;
}
int main(int argc, char* argv[]) {
ros::init(argc, argv, "jaus");
ros::NodeHandle self;
JAUS::Component component;
JAUS::Discovery* discoveryService = nullptr;
JAUS::Transport* transportService = nullptr;
localPoseSensor = new JAUS::LocalPoseSensor();
velocityStateSensor = new JAUS::VelocityStateSensor();
LocalWaypointDriver* localWaypointDriver = new LocalWaypointDriver(self.advertise<sb_msgs::MoveCommand>(MOVE_COMMAND_TOPIC,100),localPoseSensor,nullptr);
LocalWaypointListDriver* localWaypointListDriver = new LocalWaypointListDriver(localWaypointDriver);
localWaypointDriver->setListDriver(localWaypointListDriver);
{
JAUS::ReportLocalPose localPose;
localPose.SetX(1.0);
localPose.SetY(1.0);
localPose.SetYaw(0.1);
localPoseSensor->SetLocalPose(localPose);
}
{
JAUS::ReportVelocityState state;
state.SetVelocityX(2);
state.SetYawRate(0.1);
velocityStateSensor->SetVelocityState(state);
}
self.subscribe("robot_state",100,onRobotStateChange);
self.subscribe("GPS_COORD",100,onNewWaypoint);
component.AddService(localPoseSensor);
component.AddService(localWaypointDriver);
component.AddService(velocityStateSensor);
discoveryService = (JAUS::Discovery*)component.GetService(JAUS::Discovery::Name);
discoveryService->SetSubsystemIdentification(JAUS::Subsystem::Vehicle,"Snowbots");
discoveryService->SetNodeIdentification("Main");
discoveryService->SetComponentIdentification("Baseline");
int comp_id = 5000;
JAUS::Address componentID(comp_id,1,1);
discoveryService->EnableDebugMessages(true);
while(component.Initialize(componentID)==false) {
std::cout << "Failed to initialize [" << componentID.ToString() << "]" << std::endl;
comp_id++;
componentID(comp_id,1,1);
}
std::cout << "Success!" << std::endl;
transportService = (JAUS::Transport*) component.GetService(JAUS::Transport::Name);
transportService->LoadSettings("services.xml");
const JAUS::Address comp_address_id = component.GetComponentID();
if(!transportService->IsInitialized()) {
transportService->Initialize(comp_address_id);
}
JAUS::Management* managementService = nullptr;
managementService = (JAUS::Management*)component.GetService(JAUS::Management::Name);
JAUS::Time::Stamp displayStatusTimeMs = JAUS::Time::GetUtcTimeMs();
ros::Rate loop_rate(10);
while(ros::ok()) {
if(managementService->GetStatus() == JAUS::Management::Status::Shutdown) {
break;
}
if(JAUS::Time::GetUtcTimeMs() - displayStatusTimeMs > 500) {
std::cout << "==================" << std::endl;
managementService->PrintStatus();
discoveryService->PrintStatus();
transportService->PrintStatus();
std::cout << std::endl;
displayStatusTimeMs = JAUS::Time::GetUtcTimeMs();
}
ros::spinOnce();
loop_rate.sleep();
}
component.Shutdown();
return 0;
}
//
// Prepare planarity test for one cluster
//
bool CconnectClusterPlanar::preparation(
Graph &G,
cluster &cl,
node superSink)
{
int bcIdSuperSink = -1; // ID of biconnected component that contains superSink
// Initialization with -1 necessary for assertion
bool cPlanar = true;
NodeArray<node> tableNodes(G, nullptr);
EdgeArray<edge> tableEdges(G, nullptr);
NodeArray<bool> mark(G, 0);
EdgeArray<int> componentID(G);
// Determine Biconnected Components
int bcCount = biconnectedComponents(G, componentID);
// Determine edges per biconnected component
Array<SList<edge> > blockEdges(0, bcCount - 1);
for (edge e : G.edges) {
blockEdges[componentID[e]].pushFront(e);
}
// Determine nodes per biconnected component.
Array<SList<node> > blockNodes(0, bcCount - 1);
for (int i = 0; i < bcCount; i++)
{
for (edge e : blockEdges[i])
{
if (!mark[e->source()])
{
blockNodes[i].pushBack(e->source());
mark[e->source()] = true;
}
if (!mark[e->target()])
{
blockNodes[i].pushBack(e->target());
mark[e->target()] = true;
}
}
if (superSink && mark[superSink]) {
OGDF_ASSERT(bcIdSuperSink == -1);
bcIdSuperSink = i;
}
for (node v : blockNodes[i]) {
if (mark[v])
mark[v] = false;
else {
OGDF_ASSERT(mark[v]); // v has been placed two times on the list.
}
}
}
// Perform planarity test for every biconnected component
if (bcCount == 1)
{
// Compute st-numbering
NodeArray<int> numbering(G,0);
#ifdef OGDF_DEBUG
int n =
#endif
(superSink) ? stNumber(G,numbering,nullptr,superSink) : stNumber(G,numbering);
OGDF_ASSERT_IF(dlConsistencyChecks,testSTnumber(G,numbering,n))
EdgeArray<edge> backTableEdges(G,nullptr);
for(edge e : G.edges)
backTableEdges[e] = e;
cPlanar = doTest(G,numbering,cl,superSink,backTableEdges);
}
else
{
for (int i = 0; i < bcCount; i++)
{
#ifdef OGDF_DEBUG
if(int(ogdf::debugLevel)>=int(dlHeavyChecks)){
cout<<endl<<endl<<"-----------------------------------";
cout<<endl<<endl<<"Component "<<i<<endl;}
#endif
Graph C;
for (node v : blockNodes[i])
{
node w = C.newNode();
tableNodes[v] = w;
#ifdef OGDF_DEBUG
if (int(ogdf::debugLevel) >= int(dlHeavyChecks)){
cout << "Original: " << v << " New: " << w << endl;
}
#endif
}
NodeArray<node> backTableNodes(C,nullptr);
for (edge e : blockEdges[i])
{
edge f = C.newEdge(tableNodes[e->source()],tableNodes[e->target()]);
tableEdges[e] = f;
}
EdgeArray<edge> backTableEdges(C,nullptr);
for (edge e : blockEdges[i])
backTableEdges[tableEdges[e]] = e;
// Compute st-numbering
NodeArray<int> numbering(C,0);
if (bcIdSuperSink == i) {
#ifdef OGDF_DEBUG
int n =
#endif
stNumber(C,numbering,nullptr,tableNodes[superSink]);
OGDF_ASSERT_IF(dlConsistencyChecks,testSTnumber(C,numbering,n))
cPlanar = doTest(C,numbering,cl,tableNodes[superSink],backTableEdges);
} else {
#ifdef OGDF_DEBUG
int n =
#endif
stNumber(C,numbering);
OGDF_ASSERT_IF(dlConsistencyChecks,testSTnumber(C,numbering,n))
cPlanar = doTest(C,numbering,cl,nullptr,backTableEdges);
}
if (!cPlanar)
break;
}
}
return cPlanar;
}
int main(int argc, char* argv[])
{
// Create a component. By default a component
// has all the services of the Core Service set:
// - Transport (JUDP)
// - Control
// - Discovery
// - Events
// - Liveness
// - Time
// - Management
JAUS::Component component;
// The Transport service is used to send
// and receive messages to other JAUS components. All
// other services use the Transport service. The
// default transport type for JAUS++ is UDP communication
// using the JUDP class.
// The Discovery service is used to find
// other JAUS components and services on the
// network using the Transport service. In JAUS++
// Discovery will automatically find these components,
// make connections to them, and keep track of what
// services they have.
// The first thing we must do for a component is
// configure its identification. This is done by
// using the Discovery Service. Get a pointer
// to the service:
JAUS::Discovery* discoveryService = NULL;
discoveryService = (JAUS::Discovery*)component.GetService(JAUS::Discovery::Name);
// Alternative method:
// discoveryService = component.DiscoveryService();
// Set the type of subsystem the component is for. Subsystem
// types available are currently Vehicle, or OCU. The string
// name "Robot" represents the type or category of platform.
// You must set the subsystem identification before you will be
// able to initialize your component.
discoveryService->SetSubsystemIdentification(JAUS::Subsystem::Vehicle,
"Robot");
// You can also set identification information for the component
// and node that it is on.
discoveryService->SetNodeIdentification("Primary Computer");
discoveryService->SetComponentIdentification("Baseline");
// Now that we have setup our identification information we
// can initialize our component. First, create the
// component ID.
JAUS::Address componentID(1000, 1, 1);
// 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)
{
// Let's check the "state" of our component. This
// is done using the Management service.
// A component can be in the following states:
// - Initialized
// - Ready
// - Standby
// - Shutdown
// - Failure
// - Emergency
JAUS::Management* managementService = NULL;
managementService = (JAUS::Management*)component.GetService(JAUS::Management::Name);
// Alternative method:
//managementService = component.ManagementService();
if(managementService->GetStatus() == JAUS::Management::Status::Shutdown)
{
// Exit program.
break;
}
if(JAUS::Time::GetUtcTimeMs() - displayStatusTimeMs > 500)
{
std::cout << "======================================================\n";
// Print status of the service.
managementService->PrintStatus(); std::cout << std::endl;
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;
}