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;
}
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;
}
