/** Loop for generating drive commands. */
void GlobalCommand::CommandThread()
{
    JAUS::AccessControl* control = NULL;
    JAUS::Management* management = NULL;
    control = GetComponent()->AccessControlService();
    management = GetComponent()->ManagementService();
    while(mCommandThread.QuitThreadFlag() == false && control)
    {
        
        JAUS::Address baselineComponentID;
        JAUS::Address driverToControl;
        JAUS::Message* driveCommandToSend = NULL;
        
        if(baselineComponentID.IsValid() == false)
        {
            //JAUS::Address::List idList = GetComponent()->DiscoveryService()->GetSubsystem(GetComponentID().mSubsystem)->GetComponentsWithService(JAUS::PrimitiveDriver::Name);
            JAUS::Address::List idList = GetComponent()->DiscoveryService()->GetSubsystem(GetComponentID().mSubsystem)->GetComponentsWithService(JAUS::Microcontroller::Name);
            if(idList.size() > 0)
            {
                baselineComponentID = idList.front();
            }
        }
        if(baselineComponentID.IsValid() == false)
        {
            CxUtils::SleepMs(1);
            //std::cout << "Baseline Invalid\n";
            continue;
        }
        
        // Take control of the correct component based on the
        // type of control mode we are.  If Power, control Primitive Driver
        // component, and release control of Velocity State Driver.  Do the
        // reverse for Velocity State Driver
        
        JAUS::SetVelocityVector setVelocityVector;
        JAUS::SetMicrocontrollerState setMicrocontrollerState;
        mCommandMutex.Lock();
        if(mControlReady==true)
        {
            //Control the three orientation angles
            setVelocityVector.SetRoll(mDesiredRoll);
            setVelocityVector.SetPitch(mDesiredPitch);
            setVelocityVector.SetYaw(mDesiredYaw);
            
            //Set the depth in meters, if surface called, force surface
            if(mSurfaceFlag)
            {
                setVelocityVector.SetPositionZ(0);
            }
            else
            {
                setVelocityVector.SetPositionZ(mDesiredDepth);
            }
            //Set the velocities
            setVelocityVector.SetVelocityX(mDesiredAxialVel);
            setVelocityVector.SetVelocityY(mDesiredLateralVel);
        }
        mCommandMutex.Unlock();
        setVelocityVector.SetSourceID(mGlobalInfo->GetComponentID());
        setVelocityVector.SetDestinationID(baselineComponentID);
        //setVelocityVector.Print();
        
        mCommandMutex.Lock();
        (*setMicrocontrollerState.GetDigitalStates())["Torpedo1"] = FireUpdate(mTorpedo1,mTorpedo1Ticks);
        (*setMicrocontrollerState.GetDigitalStates())["Torpedo2"] = FireUpdate(mTorpedo2,mTorpedo2Ticks);
        (*setMicrocontrollerState.GetDigitalStates())["Dropper1"] = FireUpdate(mDropper1,mDropper1Ticks);
        (*setMicrocontrollerState.GetDigitalStates())["Dropper2"] = FireUpdate(mDropper2,mDropper2Ticks);
        (*setMicrocontrollerState.GetDigitalStates())["GrabberArm1"] = FireUpdate(mGrabber1,mGrabber1Ticks);
        (*setMicrocontrollerState.GetDigitalStates())["GrabberArm2"] = FireUpdate(mGrabber2,mGrabber2Ticks);
        
        //setMicrocontrollerState
        mCommandMutex.Unlock();
        
        mCommandMutex.Lock();
        bool estop;
        mGlobalInfo->GetInfo(GlobalInfo::EStop, estop);
        if (!estop)
        {
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Upper_Left_Red"] = 255;
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Upper_Left_Green"] = 0;
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Upper_Left_Blue"] = 0;
            
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Forward_Arrow_Red"] = 255;
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Forward_Arrow_Green"] = 0;
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Forward_Arrow_Blue"] = 0;
            
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Upper_Right_Red"] = 255;
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Upper_Right_Green"] = 0;
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Upper_Right_Blue"] = 0;
            
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Left_Arrow_Red"] = 255;
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Left_Arrow_Green"] = 0;
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Left_Arrow_Blue"] = 0;
            
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Center_Red"] = 255;
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Center_Green"] = 0;
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Center_Blue"] = 0;
            
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Right_Arrow_Red"] = 255;
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Right_Arrow_Green"] = 0;
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Right_Arrow_Blue"] = 0;
            
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Lower_Left_Red"] = 255;
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Lower_Left_Green"] = 0;
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Lower_Left_Blue"] = 0;
            
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Back_Arrow_Red"] = 255;
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Back_Arrow_Green"] = 0;
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Back_Arrow_Blue"] = 0;
            
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Lower_Right_Red"] = 255;
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Lower_Right_Green"] = 0;
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Lower_Right_Blue"] = 0;
        }
        else
        {
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Upper_Left_Red"] = mUpperLeftRed;
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Upper_Left_Green"] = mUpperLeftGreen;
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Upper_Left_Blue"] = mUpperLeftBlue;
            
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Forward_Arrow_Red"] = mForwardArrowRed;
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Forward_Arrow_Green"] = mForwardArrowGreen;
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Forward_Arrow_Blue"] = mForwardArrowBlue;
            
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Upper_Right_Red"] = mUpperRightRed;
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Upper_Right_Green"] = mUpperRightGreen;
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Upper_Right_Blue"] = mUpperRightBlue;
            
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Left_Arrow_Red"] = mLeftArrowRed;
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Left_Arrow_Green"] = mLeftArrowGreen;
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Left_Arrow_Blue"] = mLeftArrowBlue;
            
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Center_Red"] = mCenterRed;
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Center_Green"] = mCenterGreen;
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Center_Blue"] = mCenterBlue;
            
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Right_Arrow_Red"] = mRightArrowRed;
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Right_Arrow_Green"] = mRightArrowGreen;
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Right_Arrow_Blue"] = mRightArrowBlue;
            
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Lower_Left_Red"] = mLowerLeftRed;
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Lower_Left_Green"] = mLowerLeftGreen;
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Lower_Left_Blue"] = mLowerLeftBlue;
            
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Back_Arrow_Red"] = mBackArrowRed;
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Back_Arrow_Green"] = mBackArrowGreen;
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Back_Arrow_Blue"] = mBackArrowBlue;
            
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Lower_Right_Red"] = mLowerRightRed;
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Lower_Right_Green"] = mLowerRightGreen;
            (*setMicrocontrollerState.GetAnalogStates())["Disp_Lower_Right_Blue"] = mLowerRightBlue;
        }
        mCommandMutex.Unlock();
        
        setMicrocontrollerState.SetSourceID(mGlobalInfo->GetComponentID());
        setMicrocontrollerState.SetDestinationID(baselineComponentID);
        
        bool mHaveControl;
        
        //release in surface
        double currDepth;
        mGlobalInfo->GetInfo(GlobalInfo::DepthSensor, currDepth);
        if(mSurfaceFlag && currDepth<mSurfaceDepth)
        {
            AccessControlService()->ReleaseComponentControl(baselineComponentID);
        }
        else if(!AccessControlService()->HaveControl(baselineComponentID))
        {
            AccessControlService()->RequestComponentControl(baselineComponentID);
        }
        
        
        mHaveControl &= SendCommand(&setMicrocontrollerState, true);
        //mHaveControl &= SendCommand(&setWrenchEffort, true);
        if(mControlReady==true)
        {
            mHaveControl &= SendCommand(&setVelocityVector, true);
        }
        
        //std::cout << "Here: " << std::endl;
        //commandLoop++;
        
        CxUtils::SleepMs(mDelayTimeMs);
        
        
        /*if(mCommandMode == AI::GlobalCommand::Power)
        {
            driverToControl = mPrimitiveDriverComponentID;
            if(control->HaveControl(mVelocityStateDriverComponentID))
            {
                control->ReleaseComponentControl(mVelocityStateDriverComponentID);
            }
            if(!control->HaveControl(driverToControl))
            {
                this->RequestComponentControl(driverToControl, true, true);
            }
            driveCommandToSend = &mWrenchEffort;
        }
        else if(mCommandMode == AI::GlobalCommand::Velocity)
        {
            driverToControl = mVelocityStateDriverComponentID;
            if(control->HaveControl(mPrimitiveDriverComponentID))
            {
                control->ReleaseComponentControl(mPrimitiveDriverComponentID);
            }
            if(!control->HaveControl(driverToControl))
            {
                this->RequestComponentControl(driverToControl, true, true);
            }
            if(mStandardsModeFlag)
            {
                driveCommandToSend = &mVelocityCommand;
            }
            else
            {
                driveCommandToSend = &mVelocityVector;
            }
        }*/
        
        /*******driverToControl = mVelocityStateDriverComponentID;
        if(control->HaveControl(mPrimitiveDriverComponentID))
        {
            control->ReleaseComponentControl(mPrimitiveDriverComponentID);
        }
        if(!control->HaveControl(driverToControl))
        {
            this->RequestComponentControl(driverToControl, true, true);
        }
        
        driveCommandToSend = &mVelocityVector;******/
        
        
        /*if(mStandardsModeFlag)
        {
            driveCommandToSend = &mVelocityCommand;
        }
        
        if(control->HaveControl(mVelocityStateDriverComponentID))
        {
            driveCommandToSend = &mVelocityVector;
        }*/
        
        // Send drive command.
        /******if(driveCommandToSend)
        {
            CxUtils::Mutex::ScopedLock lock(&mCommandMutex);
            driveCommandToSend->SetSourceID(GetComponentID());
            driveCommandToSend->SetDestinationID(driverToControl);
            Send(driveCommandToSend);
        }
        
        CxUtils::SleepMs(mDelayTimeMs);*****/
    }
}
Beispiel #2
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;
}
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;
}