Ejemplo n.º 1
0
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;
}
Ejemplo n.º 2
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;
}