コード例 #1
0
ファイル: jaus.cpp プロジェクト: UML-Robotics-Club/igvc-2012
/*
 * 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;
}
コード例 #2
0
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;
}
コード例 #3
0
ファイル: Tutorial02.cpp プロジェクト: jarekAIM/IGVC2015
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;
}
コード例 #4
0
ファイル: server.cpp プロジェクト: jarekAIM/IGVC2015
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;
}
コード例 #5
0
ファイル: server.cpp プロジェクト: johnjsb/IGVC2015
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;
}
コード例 #6
0
ファイル: jaus.cpp プロジェクト: UML-Robotics-Club/igvc-2012
/*
 * 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;
}
コード例 #7
0
ファイル: Tutorial01.cpp プロジェクト: jarekAIM/IGVC2015
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;
}