Ejemplo n.º 1
0
////////////////////////////////////////////////////////////////////////////////////
///
///   \brief Entry point of jaus_challenge_2010.cpp
///
///   This program demonstrates everything required to complete the JAUS
///   Interoperability Challenge for 2010.  In this program a simulated robot
///   is created which can drive to local waypoints.  This program has
///   run against the JAUS Validation Tool (JVT) and the OCP of the
///   JAUS Interopability Challange during the Autonomous Surface Vehicle
///   Competition, passing all requirements.
///
///   All you have to do is integrate on your own robot, providing real
///   sensor values and generating real control of your bot!
///
////////////////////////////////////////////////////////////////////////////////////
int main(int argc, char* argv[])
{
    JAUS::Component component;

    //component.AddService(new JAUS::JTCPClient());

    // Setting timeout for control to 0 (disables timeout of control).
    // This is done because the JVT and OCP do not query for the timeout period
    // and may not send messages to re-acquire/maintain control within the
    // normal 2 second timeout window.
    component.AccessControlService()->SetTimeoutPeriod(0);

    // Add the services we want/need for our component. By
    // default, the component object already has the Core Service set
    // (e.g. Discovery, Events, Access Control, Management).
    // The Discovery service automatically handles finding other JAUS
    // components, so you do not need to generate any Query Messages
    // (e.g. Query Identification) yourself, because Discovery does it
    // for you.

    // In this test program, we are making a simulated robot, which
    // requires the following mobility services.

    JAUS::GlobalPoseSensor* globalPoseSensor = new JAUS::GlobalPoseSensor();
    globalPoseSensor->SetSensorUpdateRate(25);      // Updates at 25 Hz (used for periodic events).
    component.AddService(globalPoseSensor);

    JAUS::LocalPoseSensor* localPoseSensor = new JAUS::LocalPoseSensor();
    localPoseSensor->SetSensorUpdateRate(25);       // Updates at 25 Hz (used for periodic events).
    component.AddService(localPoseSensor);

    JAUS::VelocityStateSensor* velocityStateSensor = new JAUS::VelocityStateSensor();
    velocityStateSensor->SetSensorUpdateRate(25);   // Updates at 25 Hz (used for periodic events).
    component.AddService(velocityStateSensor);

    // Need a List Manager Service for Local Waypoint List Driver.  You can
    // ignore this service for the Underwater Vehicle Competition.
    component.AddService(new JAUS::ListManager());
    JAUS::LocalWaypointListDriver* localWaypointListDriver = new JAUS::LocalWaypointListDriver();
    component.AddService(localWaypointListDriver);

    // Set Vehicle Identification Information.  Available options for the
    // parameters are JAUS::Subsystem::OCU and JAUS::Subsystem::Vehicle.  Since
    // we are not an OCU, we use Vehicle.  Finally, the string represents the type
    // of robot you have (e.g. Segway RMP, XUV).  This is different than the
    // name of your vehicle, but you can use that if you want.  Your subsystem number
    // is your unique identifier.
    component.DiscoveryService()->SetSubsystemIdentification(JAUS::Subsystem::Vehicle,
                                                             "Simulation");
   
    // Initialize component with a given ID.  Remember, you must
    // add all your services before you initialize the component, because you 
    // cannot add them after initialization.  All services will be deleted by
    // the component on program exit.
    if(component.Initialize(JAUS::Address(gSubsystemID, gNodeID, gComponentID)) == false)
    {
        std::cout << "Failed to Initialize Component.\n";
        return 0;
    }

    // Set state to Standby since we have initialized OK.
    component.ManagementService()->SetStatus(JAUS::Management::Status::Standby);

    // Now that we are initialized, lets create a fixed connection to the
    // JVT for testing.  Since the JVT and the OCP of the JAUS 
    // Interoperability Challenge do not support multicast for Discovery, you
    // must make a direct connection to them.
    JAUS::JTCPClient* transportService = NULL;
    transportService = (JAUS::JTCPClient*)component.TransportService();
    /*
    // Create connection to JAUS Validation Tool (JVT)
    transportService->AddConnection("24.42.140.203", JAUS::Address(90, 1, 1));
    // Create connection to OCP for the JAUS Interoperability Challenge.
    transportService->AddConnection("192.168.1.42", JAUS::Address(42, 1, 1));
    */
    
    // Set an initial global pose.
    JAUS::GlobalPose globalPose;
    globalPose.SetLatitude(32.703356);
    globalPose.SetLongitude(-117.253919);
    globalPose.SetAltitude(300);
    globalPose.SetPositionRMS(0.0);
    globalPose.SetRoll(0.0);
    globalPose.SetPitch(0.0);
    globalPose.SetYaw(CxUtils::CxToRadians(45));
    globalPose.SetAttitudeRMS(0.0);
    globalPose.SetTimeStamp(JAUS::Time::GetUtcTime());
    // Save the data to the service.
    globalPoseSensor->SetGlobalPose(globalPose);
    // You can set local pose data using global pose
    // as it is automatically converted to local pose data for you.
    // How convenient!
    localPoseSensor->SetLocalPose(globalPose);

    // Set an initial velocity state.
    JAUS::VelocityState velocityState;
    velocityState.SetVelocityX(0.0);
    velocityState.SetYawRate(0.0);
    velocityState.SetVelocityRMS(0.0);
    velocityState.SetTimeStamp(JAUS::Time::GetUtcTime());
    // Save the data to the service.
    velocityStateSensor->SetVelocityState(velocityState);

    JAUS::Time::Stamp printTimeMs = 0;

    double timeDiff = 0.33; // Used for simulation of robot physics.
    
    while(CxUtils::GetChar() != 27 && 
          component.ManagementService()->GetStatus() != JAUS::Management::Status::Shutdown)
    {
        double thrust = 25;
        double steering = 0;

        // Simulate some simple physics based on 
        // desired thrust and steering 
        // to reach local waypoints.
        
        double linearDistance = timeDiff*thrust*0.025;
        double linearVelocity = linearDistance/timeDiff;
        double rotation = timeDiff*steering*CxUtils::CxToRadians(1);
        double rotationRate = rotation/timeDiff;
        double yaw = globalPose.GetYaw();
        
        JAUS::Point3D localPosChange(linearDistance, 0, 0);        
        localPosChange = localPosChange.Rotate(yaw, JAUS::Point3D::Z);
        
        CxUtils::Wgs worldPositionWgs(globalPose.GetLatitude(),
                                      globalPose.GetLongitude(),
                                      globalPose.GetAltitude());
        CxUtils::Utm worldPositionUtm(worldPositionWgs);
        
        worldPositionUtm.mNorthing += localPosChange.mX;
        worldPositionUtm.mEasting += localPosChange.mY;
        yaw = CxUtils::Orientation::AddToAngle(yaw, rotation);
        // Convert to WGS
        worldPositionWgs << worldPositionUtm;
        
        // Save newly calculated position and orientation.
        globalPose.SetYaw(yaw);
        globalPose.SetLatitude(worldPositionWgs.mLatitude);
        globalPose.SetLongitude(worldPositionWgs.mLongitude);
        globalPose.SetAltitude(worldPositionWgs.mElevation);
        globalPose.SetTimeStamp(JAUS::Time(true));
        
        velocityState.SetVelocityX(linearVelocity);
        velocityState.SetYawRate(rotationRate);
        velocityState.SetTimeStamp(JAUS::Time(true));
        
        // Save global pose information.
        globalPoseSensor->SetGlobalPose(globalPose);
        // The Local Pose Sensor service supports converting
        // global pose data to local pose data.  It automatically
        // calculates offsets and relative changes for you.
        localPoseSensor->SetLocalPose(globalPose);
        // Save velocity state information.
        velocityStateSensor->SetVelocityState(velocityState);

        if(JAUS::Time::GetUtcTimeMs() - printTimeMs > 500)
        {
            // Print status of services.
            std::cout << "\n======================================================\n";
            component.AccessControlService()->PrintStatus(); std::cout << std::endl;
            component.ManagementService()->PrintStatus(); std::cout << std::endl;
            globalPoseSensor->PrintStatus(); std::cout << std::endl;
            localPoseSensor->PrintStatus(); std::cout << std::endl;
            velocityStateSensor->PrintStatus(); std::cout << std::endl;
            localWaypointListDriver->PrintStatus();
            printTimeMs = JAUS::Time::GetUtcTimeMs();
        }

        // Exit if escape key pressed.
        if(CxUtils::GetChar() == 27)
        {
            break;
        }

        CxUtils::SleepMs((unsigned int)(timeDiff*1000.0));
    }

    // Don't delete services, they are
    // deleted by the Component class.

    // Shutdown any components associated with our subsystem.
    component.Shutdown();

    return 0;
}
Ejemplo n.º 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;
}
Ejemplo n.º 3
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;
}
Ejemplo n.º 4
0
////////////////////////////////////////////////////////////////////////////////////
///
///   \brief Simple simulation of a component with a Local Pose Sensor using
///          JTCP.
///
///   \param[in] subsystemID Subsystem ID number.
///   \param[in] nodeID Node ID number.
///
////////////////////////////////////////////////////////////////////////////////////
int RunServer(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,
                                                             "Local Pose JTCP Tutorial");

    //  Change the transport service to use JTCP
    component.AddService(new JAUS::JTCPClient());

    // Add any other services.  In this case local pose.
    JAUS::LocalPoseSensor* localPoseSensor = new JAUS::LocalPoseSensor();
    component.AddService(localPoseSensor);


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

    // Configure local pose information.
    JAUS::LocalPose localPose;
    localPose.SetX(0);
    localPose.SetY(0);
    localPose.SetZ(0);
    localPose.SetYaw(0);
    localPose.SetTimeStamp(JAUS::Time(true));

    JAUS::Time printTime(true);
    while(CxUtils::GetChar() != 27 && component.ManagementService()->GetStatus() != JAUS::Management::Status::Shutdown)
    {
        // Fake a position change.
        localPose.SetY(localPose.GetY() + .01);
        localPose.SetTimeStamp(JAUS::Time(true));
        // Save the data.  Anyone who is subscribing will be
        // notified of the change automatically.
        localPoseSensor->SetLocalPose(localPose);

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

    // Do shutdown
    component.Shutdown();

    return 0;
}
Ejemplo n.º 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;
}
Ejemplo n.º 6
0
int main(int argc, char* argv[])
{
    JAUS::Component component; 

    // Testing prioritized message data
    component.TransportService()->AddPriorityMessage(JAUS::REPORT_GLOBAL_POSE);
    
    // Add the services our component needs.  This has to
    // be done before component initialization.
    // Second, you cannot add the same service twice.

    JAUS::GlobalPoseSensor* globalPoseSensor = new JAUS::GlobalPoseSensor();
    JAUS::LocalPoseSensor* localPoseSensor = new JAUS::LocalPoseSensor();
    JAUS::VelocityStateSensor* velocityStateSensor = new JAUS::VelocityStateSensor();
    JAUS::AccelerationStateSensor* accelerationStateSensor = new JAUS::AccelerationStateSensor();
    FakeMicrocontroller* mcu = new FakeMicrocontroller();

    // Add to our component.
    component.AddService(globalPoseSensor);
    component.AddService(localPoseSensor);
    component.AddService(velocityStateSensor);
    component.AddService(accelerationStateSensor);
    component.AddService(mcu);

    // Tell our sensor services what component we want them
    // to sync there data with (i.e. who to subscribe to.
    JAUS::Address syncID(gSubsystemID, gSyncNodeID, gSyncComponentID);
    globalPoseSensor->SynchronizeToComponent(syncID);
    localPoseSensor->SynchronizeToComponent(syncID);
    velocityStateSensor->SynchronizeToComponent(syncID);
    accelerationStateSensor->SynchronizeToComponent(syncID);
    mcu->SynchronizeToComponent(syncID);

    // Example of how to use events callback
    EventCallback callback;
    component.EventsService()->RegisterCallback(&callback);


    // Try load settings files.
    // These files determine your UDP network 
    // settings, what Services to turn on/off 
    // or any Service specific settings. See the
    // example file for settings file format.
    if(component.LoadSettings("settings/services.xml") == false)
    {
        // Working directory probably not set (or not running from output directory), so
        // use default values.
        component.DiscoveryService()->SetSubsystemIdentification(JAUS::Subsystem::Vehicle,
                                                                 "Simulation");
        // Set optional identification info for this component.
        component.DiscoveryService()->SetComponentIdentification("Synchronize");
    }

    // Initialize component with component given ID.
    if(component.Initialize(JAUS::Address(gSubsystemID, gNodeID, gComponentID)) == false)
    {
        std::cout << "Failed to Initialize Component.\n";
        return 0;
    }
    
    bool foundSyncID = false;
    CxUtils::Time::Stamp printTimeMs = 0;
    while(CxUtils::GetChar() != 27)
    {
        // This is a short example code to dynamically lookup a
        // component ID for the service you want to synchronize on your
        // subsystem (like in your AI program if the services may run on different computers).
#if 0
        // Enable synchronization for each sensor.
        if(globalPoseSensor->IsSynchronizing() == false)
        {
            // Find sensor to sync to.
            JAUS::Address::List idList = component.DiscoveryService()->GetSubsystem(gSubsystemID)->GetComponentsWithService(JAUS::GlobalPoseSensor::Name);
            if(idList.size() > 0)
            {
                globalPoseSensor->SynchronizeToComponent(idList.front());
            }
        }
#endif
        if(CxUtils::Time::GetUtcTimeMs() - printTimeMs > 500)
        {
            // Print status of services.
            std::cout << "\n======================================================\n";
            globalPoseSensor->PrintStatus(); std::cout << std::endl;
            localPoseSensor->PrintStatus(); std::cout << std::endl;
            velocityStateSensor->PrintStatus(); std::cout << std::endl;
            accelerationStateSensor->PrintStatus(); std::cout << std::endl;
            mcu->PrintStatus(); std::cout << std::endl;
            printTimeMs = CxUtils::Time::GetUtcTimeMs();
        }
        CxUtils::SleepMs(1);
    }

    // Don't delete services, they are
    // deleted by the Component class.

    // Shutdown any components associated with our subsystem.
    component.Shutdown();

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