////////////////////////////////////////////////////////////////////////////////////
///
///   \brief Generates fake waypoints to simulate control unit for testing.
///
////////////////////////////////////////////////////////////////////////////////////
void RobotSim::UseFakeWaypoints()
{
    JAUS::Address controllerID(42, 1, 1);
    mLocalWaypointDriverComponent.AccessControlService()->SetControllerID(controllerID);
    mLocalWaypointDriverComponent.ManagementService()->SetStatus(JAUS::Management::Status::Ready);

    JAUS::SetElement command(mLocalWaypointDriverComponent.GetComponentID(), controllerID);
    command.SetRequestID(1);
    JAUS::Point3D::List localWaypoints;
    // Add more waypoints for testing as needed.
    localWaypoints.push_back(JAUS::Point3D(10, 0, 0));
    localWaypoints.push_back(JAUS::Point3D(10, 10, 0));
    localWaypoints.push_back(JAUS::Point3D(0, 10, 0));
    localWaypoints.push_back(JAUS::Point3D(0, 0, 0));

    for(unsigned int i = 0; i < (unsigned int)localWaypoints.size(); i++)
    {
        JAUS::Element e;
        // Set the element ID.
        e.mID = i + 1;
        // If this is the last element (and we are not looping) set the
        // next ID to 0, otherwise the value of the next element ID.
        if(i < (unsigned int)localWaypoints.size() - 1)
        {
            e.mNextID = e.mID + 1;
        }
        // Set previous element.
        e.mPrevID = i;
        // Populate the element message which is the command to be executed.
        JAUS::SetLocalWaypoint* message = new JAUS::SetLocalWaypoint();
        message->SetX(localWaypoints[i].mX);
        message->SetY(localWaypoints[i].mY);
        // Save the pointer
        e.mpElement = message;
        // Push completed element onto the list.
        command.GetElementList()->push_back(e);
    }

    // Set the command.
    mLocalWaypointDriverComponent.TransportService()->Receive(&command);
    // Now tell it to start executing.
    JAUS::ExecuteList executeCommand(mLocalWaypointDriverComponent.GetComponentID(), controllerID);
    executeCommand.SetElementUID(1); // Start at beginning.
    executeCommand.SetSpeed(2.0);    // Maximum speed.
    mLocalWaypointDriverComponent.TransportService()->Receive(&executeCommand);
}
Пример #2
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;
}
Пример #3
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;
}
Пример #4
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;
}
Пример #5
0
////////////////////////////////////////////////////////////////////////////////////
///
///   \brief Simple simulation of a component that subscribes to any Local
///          Pose Sensor data on network using JTCP.
///
///   \param[in] subsystemID Subsystem ID number.
///   \param[in] nodeID Node ID number.
///
////////////////////////////////////////////////////////////////////////////////////
int RunClient(JAUS::UShort subsystemID, JAUS::Byte nodeID)
{
    JAUS::Component component;
    JAUS::Address id(subsystemID, nodeID, 1);

    if(id.IsValid() == false)
    {
        std::cout << "Invalid JAUS ID: " << id.ToString() << std::endl;
    }

    // Setup identification data.
    component.DiscoveryService()->SetSubsystemIdentification(JAUS::Subsystem::Vehicle,
                                                             "Subscriber JTCP Tutorial");

    //  Change the transport service to use JTCP
    component.AddService(new JAUS::JTCPClient());
    //((JAUS::JTCPClient*)component.TransportService())->AddConnection("10.171.190.61", JAUS::Address(subsystemID, 1, 1));
    //((JAUS::JTCPClient*)component.TransportService())->AddConnection("127.0.0.1", JAUS::Address(subsystemID, 1, 1));

    // Add support for reading Local Pose messages without having to
    // add a custom service.
    component.TransportService()->AddMessageTemplate(new JAUS::ReportLocalPose());
    component.TransportService()->AddMessageTemplate(new JAUS::QueryLocalPose());

    // Add a callback to get messages when they arrive.
    LocalPoseCallback localPoseCallback;
    component.TransportService()->RegisterCallback(JAUS::REPORT_LOCAL_POSE, &localPoseCallback);

    // Initialize component and communication.
    if(component.Initialize(id) == false)
    {
        std::cout << "Failed to initialize JAUS component with ID: " << id.ToString() << std::endl;
        return 0;
    }

    JAUS::Time printTime(true);

    while(CxUtils::GetChar() != 27 && component.ManagementService()->GetStatus() != JAUS::Management::Status::Shutdown)
    {
        // Look for any subsystems.
        JAUS::Subsystem::Map subsystems;
        JAUS::Subsystem::Map::iterator subsystem;

        component.DiscoveryService()->GetSubsystems(subsystems);

        for(subsystem = subsystems.begin();
            subsystem != subsystems.end();
            subsystem++)
        {
            // Look for local pose sensors to subscribe to.
            JAUS::Address::List sensors = subsystem->second->GetComponentsWithService(JAUS::LocalPoseSensor::Name);
            if(sensors.size() > 0)
            {
                JAUS::Address::List::iterator id;
                for(id = sensors.begin();
                    id != sensors.end();
                    id++)
                {
                    if(*id != component.GetComponentID() && 
                       component.EventsService()->HaveSubscription(JAUS::REPORT_LOCAL_POSE, *id) == false)
                    {
                        std::cout << "Found new Local Pose Sensors on " << id->ToString() << std::endl;
                        JAUS::QueryLocalPose queryLocalPose;
                        // Request all fields.
                        queryLocalPose.SetPresenceVector(queryLocalPose.GetPresenceVectorMask());
                        component.EventsService()->RequestPeriodicEvent(*id, &queryLocalPose, 5);
                    }
                }
            }
        }

        JAUS::Subsystem::DeleteSubsystemMap(subsystems);
        // Only print to screen every now and then.
        if(JAUS::Time(true) - printTime > .5)
        {
            //std::cout << "=======================================================\n";
            //component.EventsService()->PrintStatus();
            //printTime.SetCurrentTime();
        }
        // Don't overload the CPU
        CxUtils::SleepMs(1);
    }

    // Do shutdown
    component.Shutdown();

    return 0;
}
Пример #6
0
int main(int argc, char **argv)
{
    SICK::Sick laser;
    MessageCallback cb;

    JAUS::Component component;
    // Add the services we want our component to have
    // beyond the core set.

    JAUS::RangeSensor* rangeSensor = new JAUS::RangeSensor();
    // Setup the configuration of a range sensing device.
    JAUS::RangeSensorConfig rangeSensorConfig;
    rangeSensorConfig.mID = 0;
    rangeSensorConfig.mName = "LIDAR";
    rangeSensorConfig.mMinRange = 0.0;
    rangeSensorConfig.mMaxRange = 9.0;                              // 9 meter range.
    rangeSensorConfig.mUnitType = JAUS::RangeSensorConfig::MM;      // Range values are in mm.
    rangeSensorConfig.mScanAngle = CxUtils::CxToRadians(180);       // [-90, 90] degrees.
    rangeSensorConfig.mAngleIncrement = CxUtils::CxToRadians(0.5);  // 0.5 degree increment.
    rangeSensor->AddRangeDevice(rangeSensorConfig);
    // Add to component.
    component.AddService(rangeSensor);

    cb.mpRangeSensor = rangeSensor;

    // Register a callback to process the received
    // messages.
    laser.RegisterCallback(&cb);

    cout << "Connecting to SICK...";
    laser.InitializeWithDefaults("/dev/ttyUSB0", 38400);
    cout << "Success!\n";


    // In this example, we are simulating a component on part of a simulated
    // robot.
    component.DiscoveryService()->SetSubsystemIdentification(JAUS::Subsystem::Vehicle,
                                                             "Simulation");
    // Initialize component component with component given ID.
    if(component.InitializeWithUniqueID(2000) == false)
    {
        std::cout << "Failed to Initialize JAUS Component.\n";
        return 0;
    }

    std::cout << "JAUS Component [" << component.GetComponentID().ToString() << "] Initialized!\n";

    while ( true )
    {
        if (CxUtils::GetChar() == 27)
        {
            break;
        }
        CxUtils::SleepMs(1);
    }

    // Make sure to shutdown the laser.
    laser.Shutdown();
    component.Shutdown();

    return 0;
}