int main(int argc, char* argv[])
{
if (argc < 3)
{
std::cout << "Error: Arguments must include IP and Address" << std::endl;
return 1;
}
char* ip_address = argv[1];
char* comp_address = argv[2];
cout << "ip_address: " << ip_address << " comp_address: " << comp_address << std::endl;
JAUS::Component component;
JAUS::Discovery* discoveryService = nullptr;
JAUS::Transport* transportService = nullptr;
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);
}
// Create connection to OCP for the JAUS Interoperability Challenge using JUDP.
/*transportService->AddNetworkConnection(JAUS::Address(8000, 1, 1),
std::string(ip_address),
3794);*/
JAUS::Management* managementService = nullptr;
managementService = (JAUS::Management*)component.GetService(JAUS::Management::Name);
JAUS::Time::Stamp displayStatusTimeMs = JAUS::Time::GetUtcTimeMs();
while(true)
{
if(managementService->GetStatus() == JAUS::Management::Status::Shutdown)
{
break;
}
if(JAUS::Time::GetUtcTimeMs() - displayStatusTimeMs > 500)
{
std::cout << "==================" << std::endl;
managementService->PrintStatus();
discoveryService->PrintStatus(); //std::cout << std::endl;
transportService->PrintStatus();
std::cout << std::endl;
displayStatusTimeMs = JAUS::Time::GetUtcTimeMs();
}
CxUtils::SleepMs(1);
}
component.Shutdown();
return 0;
}
/*
* Main program entry point.
*/
int main(int argc, char* argv[])
{
std::cout << "Initializing ROS component... ";
// ROS initialization stuff
ros::init(argc, argv, "jaus");
ros::NodeHandle n;
// subscriptions
subscriberCmdVel = n.subscribe(ROSTopicCmdVelocity, 1000, ROSVelocityCallback);
subscriberOdom = n.subscribe(ROSTopicOdometry, 1000, ROSOdometryCallback);
// publications
publisherSetLocalPose = n.advertise<nav_msgs::Odometry>(ROSTopicSetLocalPose, 1000);
// -- we just use std_msgs::String for waypoints for now, until we can all agree on
// some custom ROS messages for setting local pose, and setting waypoints...
publisherSetWaypoints = n.advertise<std_msgs::String>(ROSTopicSetWaypoints, 1000);
std::cout << "OK" << std::endl;
// JAUS initialization stuff
JAUS::Component component;
JAUS::Subsystem* judgeSubsystem;
std::cout << "Initializing JAUS component... ";
if (InitializeJAUSComponent(&component) < 0)
{
std::cout << "Failed!" << std::endl;
return -1;
}
std::cout << "OK" << std::endl;
std::cout << "Searching for Judges' COP... ";
if ((judgeSubsystem = DiscoverJudgeSubsystem(&component)) == NULL)
{
std::cout << "Failed!" << std::endl;
return -1;
}
std::cout << "Success (Identification: " << judgeSubsystem->mIdentification << ")" << std::endl;
/*
* Set initial states for services
*/
// Set an initial local pose
JAUS::LocalPose localPose;
localPose.SetX(0.0);
localPose.SetY(0.0);
localPose.SetTimeStamp(JAUS::Time::GetUtcTime());
localPoseSensor->SetLocalPose(localPose);
// Set an initial velocity state
JAUS::VelocityState velocityState;
velocityState.SetVelocityX(0.0);
velocityState.SetYawRate(0.0);
velocityState.SetTimeStamp(JAUS::Time::GetUtcTime());
velocityStateSensor->SetVelocityState(velocityState);
// now loop forever
std::cout << "READY, We are now listening to ROS topics and updating current JAUS state..." << std::endl;
JAUS::Management* managementService = (JAUS::Management*)component.GetService(JAUS::Management::Name);
JAUS::Time::Stamp displayStatusTimeMs = JAUS::Time::GetUtcTimeMs();
while (true) // (managementService->GetStatus() != JAUS::Management::Status::Shutdown)
{
if (CxUtils::GetChar() == 'q')
break;
if (JAUS::Time::GetUtcTimeMs() - displayStatusTimeMs > 2000)
{
managementService->PrintStatus(); // display a status message
displayStatusTimeMs = JAUS::Time::GetUtcTimeMs();
}
ros::spinOnce(); // process ROS callbacks
CxUtils::SleepMs(10); // save some CPU time
}
component.Shutdown(); // this will take care of cleanup
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;
}
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;
}