jaus++/2.0/src/jaus/extras/controllers/controldevice.cpp

Go to the documentation of this file.

#include "jaus/extras/controllers/controldevice.h"
#include "jaus/mobility/drivers/primitivedriver.h"
#include "jaus/extras/mcu/microcontroller.h"
#include "jaus/extras/mcu/setmicrocontrollerstate.h"
#include <cmath>
#include <tinyxml/tinyxml.h>
#include <cxutils/fileio.h>
#include <algorithm>

using namespace JAUS;

const std::string ControlDevice::Name = "urn:jaus:jss:jpp:extras:ControlDevice";


ControlDevice::ControlDevice() : Management::Child(Service::ID(ControlDevice::Name), 
                                                   Service::ID(Management::Name))
{
    mTakeDriveControlFlag = false;
    mAutoBrakingFlag = true;
    mSubsystemID = 0;
    // Initialize limits.
    for(unsigned int i = 0; i < 11; i++)
    {
        mLimitsMapping[(WrenchEffort)i] = 100.0;
    }
}


ControlDevice::~ControlDevice()
{

}


bool ControlDevice::LoadSettings(const std::string& filename)
{
    TiXmlDocument xml;

    if(xml.LoadFile(filename.c_str()) == false)
    {
        return false;
    }

    // Clear all mappings.
    mControlDevice.Lock();
    mWrenchMapping.clear();
    mDeadzoneMapping.clear();
    mInvertMapping.clear();
    mButtonMapping.clear();
    mLimitsMapping.clear();
    mControlDevice.Unlock();

    TiXmlHandle doc(&xml);
    TiXmlNode* node;
    TiXmlElement* element;
    element = doc.FirstChild("JAUS").FirstChild("ControlDevice").FirstChild("Wrenches").FirstChild("WrenchEffort").ToElement();
    while(element)
    {
        if(element->Attribute("wrench") && element->Attribute("input"))
        {
            WrenchEffort wrench;
            int input;
            double deadzone = 0.0;
            double limit = 100.0;
            bool invert = false;
            wrench = (WrenchEffort)atoi(element->Attribute("wrench"));
            input = atoi(element->Attribute("input"));
            if(element->Attribute("deadzone"))
            {
                deadzone = atof(element->Attribute("deadzone"));
            }
            if(element->Attribute("limit"))
            {
                limit = atof(element->Attribute("limit"));
            }
            if(element->Attribute("invert"))
            {
                std::string value = element->Attribute("invert");
                if(value == "true" || value == "false")
                {
                    if(value == "true")
                    {
                        invert = true;
                    }
                }
                else
                {
                    invert = atoi(value.c_str()) > 0 ? true : false;
                }
            }
            MapInputToWrench(input, wrench, deadzone, invert, limit);
        }
        element = element->NextSiblingElement();
    }

    element = doc.FirstChild("JAUS").FirstChild("ControlDevice").FirstChild("Microcontroller").FirstChild("AnalogOut").ToElement();
    while(element)
    {
        if(element->Attribute("name") && element->Attribute("input"))
        {
            std::string name;
            int input;
            double limit = 100.0;
            bool invert = false;
            name = element->Attribute("name");
            input = atoi(element->Attribute("input"));
            if(element->Attribute("limit"))
            {
                limit = atof(element->Attribute("limit"));
            }
            if(element->Attribute("invert"))
            {
                std::string value = element->Attribute("invert");
                if(value == "true" || value == "false")
                {
                    if(value == "true")
                    {
                        invert = true;
                    }
                }
                else
                {
                    invert = atoi(value.c_str()) > 0 ? true : false;
                }
            }
            MapInputToAnalogOut(input, name, invert, limit);
        }
        element = element->NextSiblingElement("AnalogOut");
    }

    element = doc.FirstChild("JAUS").FirstChild("ControlDevice").FirstChild("Microcontroller").FirstChild("DigitalOut").ToElement();
    while(element)
    {
        if(element->Attribute("name") && element->Attribute("button"))
        {
            std::string name;
            int button;
            bool onPress = true;
            name = element->Attribute("name");
            button = atoi(element->Attribute("button"));
            if(element->Attribute("press"))
            {
                std::string value = element->Attribute("press");
                if(value == "1" || value == "true")
                {
                    onPress = 1;
                }
                else
                {
                    onPress = 0;
                }
            }
            MapButtonToDigitalOut(button, name, onPress);
        }
        element = element->NextSiblingElement("AnalogOut");
    }

    node = doc.FirstChild("JAUS").FirstChild("ControlDevice").FirstChild("Buttons").FirstChild("RequestDriveControlButton").ToNode();
    if(node && node->FirstChild())
    {
        MapButtonToAction(atoi(node->FirstChild()->Value()), RequestDriveControl);
    }
    node = doc.FirstChild("JAUS").FirstChild("ControlDevice").FirstChild("Buttons").FirstChild("ReleaseDriveControlButton").ToNode();
    if(node && node->FirstChild())
    {
        MapButtonToAction(atoi(node->FirstChild()->Value()), ReleaseDriveControl);
    }
    
    return true;
}


bool ControlDevice::LoadSettingsForVehicle(const std::string& vehicleIdentificationName,
                                           const std::string& directory)
{
    bool result = false;
    if(vehicleIdentificationName == "")
    {
        return result;
    }
    std::vector<std::string> filenames;
    if(CxUtils::FileIO::GetFiles(filenames, "*.xml", directory, false, true) > 0)
    {
        std::string ident = vehicleIdentificationName;
        std::transform(ident.begin(), ident.end(), ident.begin(), tolower);
        for(unsigned int i = 0; i < (unsigned int)filenames.size(); i++)
        {
            std::string copy = filenames[i];
            std::transform(copy.begin(), copy.end(), copy.begin(), tolower);
            if(strstr(copy.c_str(), ident.c_str()) > 0)
            {
                result = this->LoadSettings(filenames[i]);
                break;
            }
        }
        if(result == false)
        {
            for(unsigned int i = 0; i < (unsigned int)filenames.size(); i++)
            {
                std::string copy = filenames[i];
                std::transform(copy.begin(), copy.end(), copy.begin(), tolower);
                if(strstr(copy.c_str(), "default") > 0)
                {
                    result = this->LoadSettings(filenames[i]);
                    break;
                }
            }
        }
    }

    return result;
}


void ControlDevice::Shutdown()
{
    TakeDriveControl(false);
    Mutex::ScopedLock lock(&mControlDevice);
    if(mPrimitiveDriverID.IsValid())
    {
        if(GetComponent()->AccessControlService()->HaveControl(mPrimitiveDriverID))
        {
            ReleaseComponentControl(mPrimitiveDriverID, true);
        }
    }

    if(mMicrocontrollerID.IsValid())
    {
        if(GetComponent()->AccessControlService()->HaveControl(mMicrocontrollerID))
        {
            ReleaseComponentControl(mMicrocontrollerID, true);
        }
    }
}


void ControlDevice::SetSubsystemToControl(const UShort id)
{
    Mutex::ScopedLock lock(&mControlDevice);
    mSubsystemID = id;
    if(mPrimitiveDriverID.IsValid())
    {
        // Release control if we were controlling a component.
        if(mPrimitiveDriverID.mSubsystem != mSubsystemID)
        {
            if(mTakeDriveControlFlag == true &&
               GetComponent()->AccessControlService()->HaveControl(mPrimitiveDriverID))
            {
                ReleaseComponentControl(mPrimitiveDriverID, true);
            }
            mPrimitiveDriverID.Clear();
        }
    }
    if(mMicrocontrollerID.IsValid())
    {
        // Release control if we were controlling a component.
        if(mMicrocontrollerID.mSubsystem != mSubsystemID)
        {
            if(mTakeDriveControlFlag == true &&
               GetComponent()->AccessControlService()->HaveControl(mMicrocontrollerID))
            {
                ReleaseComponentControl(mMicrocontrollerID, true);
            }
            mMicrocontrollerID.Clear();
        }
    }
    // Make sure the Discovery service is enabled.
    if(GetComponent()->DiscoveryService()->IsEnabled() == false)
    {
        GetComponent()->DiscoveryService()->EnableService(true);
    }
}


void ControlDevice::TakeDriveControl(const bool enable)
{
    Mutex::ScopedLock lock(&mControlDevice);
    mTakeDriveControlFlag = enable;
}


bool ControlDevice::HaveDriveControl() const
{
    Mutex::ScopedLock lock(&mControlDevice);
    if(mPrimitiveDriverID.IsValid() && 
       GetComponent()->AccessControlService()->HaveControl(mPrimitiveDriverID))
    {
        return true;
    }
    return false;
}



void ControlDevice::SignalButtonPress(const int buttonNumber)
{
    Mutex::ScopedLock lock(&mControlDevice);
    std::map<int, ButtonActions>::iterator b;
    b = mButtonMapping.find(buttonNumber);
    if(b != mButtonMapping.end())
    {
        switch(b->second)
        {
        case RequestDriveControl:
            mTakeDriveControlFlag = true;
            break;
        case ReleaseDriveControl:
            mTakeDriveControlFlag = false;
            if(mPrimitiveDriverID.IsValid())
            {      
                ReleaseComponentControl(mPrimitiveDriverID, true);
            }
            if(mMicrocontrollerID.IsValid())
            {              
                ReleaseComponentControl(mMicrocontrollerID, true);
            }
            break;
        default:
            break;
        }
    }

    std::map<int, std::string>::iterator digital;
    digital = mButtonDigitalMapping.begin();
    if(mMicrocontrollerID.IsValid() && 
       digital != mButtonDigitalMapping.end())
    {
        SetMicrocontrollerState command(mMicrocontrollerID, GetComponentID());
        (*command.GetDigitalStates())[digital->second] = mDigitalButtonState[digital->second];
        Send(&command);
    }
}


void ControlDevice::SignalButtonRelease(const int buttonNumber)
{
    Mutex::ScopedLock lock(&mControlDevice);
    std::map<int, ButtonActions>::iterator b;
    b = mButtonMapping.find(buttonNumber);
    if(b != mButtonMapping.end())
    {
        // Currently no off button mappings.
    }

    std::map<int, std::string>::iterator digital;
    digital = mButtonDigitalMapping.begin();
    if(mMicrocontrollerID.IsValid() && 
       digital != mButtonDigitalMapping.end())
    {
        SetMicrocontrollerState command(mMicrocontrollerID, GetComponentID());
        (*command.GetDigitalStates())[digital->second] = !mDigitalButtonState[digital->second];
        Send(&command);
    }
}


void ControlDevice::UpdateWrench(const int inputID, const double percentEffort)
{
    Mutex::ScopedLock lock(&mControlDevice);
    std::map<int, WrenchEffort>::iterator w;
    w = mWrenchMapping.find(inputID);
    if(w != mWrenchMapping.end())
    {
        double value = 0.0;
        std::map<int, double>::iterator scale = mLimitsMapping.find(inputID);        
        std::map<int, double>::iterator deadzone = mDeadzoneMapping.find(inputID);

        // Apply deadzone filter.
        if(deadzone != mDeadzoneMapping.end())
        {
            if(fabs(percentEffort) > deadzone->second)
            {
                value = (fabs(percentEffort) - deadzone->second)*100.0/(100.0 - deadzone->second);
                if(percentEffort < 0)
                {
                    value *= -1;
                }
            }
            else
            {
                value = 0.0;
            }
        }
        
        // Apply scaling.
        value = value*scale->second/100.0;

        // Apply invert filter
        std::map<int, bool>::iterator invert = mInvertMapping.find(inputID);
        if(invert != mInvertMapping.end())
        {
            if(invert->second)
            {
                value *= -1;
            }
        }
       
        // Set wrench value.
        switch(w->second)
        {
        case PropulsiveLinearEffortX:
            mWrenchEffort.SetPropulsiveLinearEffortX(value);
            if(fabs(value) < 0.001 && mAutoBrakingFlag)
            {
                mWrenchEffort.SetResistiveLinearEffortX(100.0);
            }
            else if(mAutoBrakingFlag)
            {
                mWrenchEffort.SetResistiveLinearEffortX(0.0);
            }
            break;
        case PropulsiveLinearEffortY:
            mWrenchEffort.SetPropulsiveLinearEffortY(value);
            if(fabs(value) < 0.001 && mAutoBrakingFlag)
            {
                mWrenchEffort.SetResistiveLinearEffortY(100.0);
            }
            else if(mAutoBrakingFlag)
            {
                mWrenchEffort.SetResistiveLinearEffortY(0.0);
            }
            break;
        case PropulsiveLinearEffortZ:
            mWrenchEffort.SetPropulsiveLinearEffortZ(value);
            if(fabs(value) < 0.001 && mAutoBrakingFlag)
            {
                mWrenchEffort.SetResistiveLinearEffortZ(100.0);
            }
            else if(mAutoBrakingFlag)
            {
                mWrenchEffort.SetResistiveLinearEffortZ(0.0);
            }
            break;
        case PropulsiveRotationalEffortX:
            mWrenchEffort.SetPropulsiveRotationalEffortX(value);
            break;
        case PropulsiveRotationalEffortY:
            mWrenchEffort.SetPropulsiveRotationalEffortY(value);
            break;
        case PropulsiveRotationalEffortZ:
            mWrenchEffort.SetPropulsiveRotationalEffortZ(value);
            break;
        case ResistiveLinearEffortX:
            mWrenchEffort.SetResistiveLinearEffortX(value);
            break;
        case ResistiveLinearEffortY:
            mWrenchEffort.SetResistiveLinearEffortY(value);
            break;
        case ResistiveLinearEffortZ:
            mWrenchEffort.SetResistiveLinearEffortZ(value);
            break;
        case ResistiveRotationalEffortX:
            mWrenchEffort.SetResistiveRotationalEffortX(value);
            break;
        case ResistiveRotationalEffortY:
            mWrenchEffort.SetResistiveRotationalEffortY(value);
            break;
        case ResistiveRotationalEffortZ:
            mWrenchEffort.SetResistiveRotationalEffortZ(value);
            break;
        default:
            break;
        }              
    }
}


void ControlDevice::UpdateAnalogOut(const int inputID, const double signalValue)
{
    Mutex::ScopedLock lock(&mControlDevice);
    std::map<int, std::string>::iterator a;
    a = mAnalogMapping.find(inputID);
    if(a != mAnalogMapping.end())
    {
        double value = 0.0;
        std::map<std::string, double>::iterator scale = mAnalogLimitsMapping.find(a->second);        
        
        // Apply scaling.
        value = signalValue*scale->second/100.0;

        // Apply invert filter
        std::map<std::string, bool>::iterator invert = mAnalogInvertMapping.find(a->second);
        if(invert != mAnalogInvertMapping.end())
        {
            if(invert->second)
            {
                value *= -1;
            }
        }
       
        (*mMicrocontrollerState.GetAnalogStates())[a->second] = value;            
    }
}


void ControlDevice::SendWrenchEffort()
{
    Mutex::ScopedLock lock(&mControlDevice);
    if(mPrimitiveDriverID.IsValid() && mTakeDriveControlFlag)
    {
        // Request control.
        if(!GetComponent()->AccessControlService()->HaveControl(mPrimitiveDriverID))
        {
            GetComponent()->AccessControlService()->RequestComponentControl(mPrimitiveDriverID, true);
        }
        if(mPrimitiveDriverID.IsValid() &&
           GetComponent()->AccessControlService()->HaveControl(mPrimitiveDriverID))
        {
            mWrenchEffort.SetSourceID(GetComponentID());
            mWrenchEffort.SetDestinationID(mPrimitiveDriverID);
            Send(&mWrenchEffort);            
        }
    }
}


void ControlDevice::SendMicrocontrollerState()
{
    Mutex::ScopedLock lock(&mControlDevice);
    if(mMicrocontrollerID.IsValid() && mTakeDriveControlFlag)
    {
        // Request control.
        if(!GetComponent()->AccessControlService()->HaveControl(mMicrocontrollerID))
        {
            GetComponent()->AccessControlService()->RequestComponentControl(mMicrocontrollerID, true);
        }
        if(mMicrocontrollerID.IsValid() &&
           GetComponent()->AccessControlService()->HaveControl(mMicrocontrollerID))
        {
            mMicrocontrollerState.SetSourceID(GetComponentID());
            mMicrocontrollerState.SetDestinationID(mMicrocontrollerID);
            Send(&mMicrocontrollerState);            
        }
    }
}


void ControlDevice::ClearWrenchEffort()
{
    Mutex::ScopedLock lock(&mControlDevice);
    mWrenchEffort.ClearMessage();
}


void ControlDevice::ClearMicrocontrollState()
{
    Mutex::ScopedLock lock(&mControlDevice);
    mMicrocontrollerState.ClearMessage();
}


void ControlDevice::MapButtonToAction(const int buttonNumber,
                                      const ButtonActions action)
{
    Mutex::ScopedLock lock(&mControlDevice);
    mButtonMapping[buttonNumber] = action;
}


void ControlDevice::MapButtonToDigitalOut(const int buttonNumber,
                                          const std::string& digital,
                                          const bool onPress)
{
    Mutex::ScopedLock lock(&mControlDevice);
    mButtonDigitalMapping[buttonNumber] = digital;
    mDigitalButtonState[digital] = onPress;
}


void ControlDevice::MapInputToWrench(const int inputID,
                                     const WrenchEffort wrenchID,
                                     const double deadzone,
                                     const bool invert,
                                     const double limit)
{
    Mutex::ScopedLock lock(&mControlDevice);
    mWrenchMapping[inputID] = wrenchID;
    if(fabs(deadzone) < 100)
    {
        mDeadzoneMapping[inputID] = fabs(deadzone);
    }
    mInvertMapping[inputID] = invert;
    if(fabs(limit) > 0 && fabs(limit) <= 100.0)
    {
        mLimitsMapping[inputID] = fabs(limit);
    }
}


void ControlDevice::MapInputToAnalogOut(const int inputID,
                                        const std::string& analog,
                                        const bool invert,
                                        const double limit)
{
    Mutex::ScopedLock lock(&mControlDevice);
    mAnalogMapping[inputID] = analog;
    mAnalogInvertMapping[analog] = invert;
    if(fabs(limit) > 0 && fabs(limit) <= 100.0)
    {
        mAnalogLimitsMapping[analog] = fabs(limit);
    }
}


void ControlDevice::CheckServiceStatus(const unsigned int timeSinceLastCheckMs)
{
    // See if we need to take control of a component.
    Mutex::ScopedLock lock(&mControlDevice);
    if(mTakeDriveControlFlag && mSubsystemID != 0)
    {
        if(mPrimitiveDriverID.IsValid() == false)
        {
            // Lookup a primitive driver on the subsystem.
            GetComponent()->DiscoveryService()->GetSubsystem(mSubsystemID)->HaveService(PrimitiveDriver::Name, 
                                                                                        &mPrimitiveDriverID);
        }
        if(mMicrocontrollerID.IsValid() == false)
        {
            // Lookup a primitive driver on the subsystem.
            GetComponent()->DiscoveryService()->GetSubsystem(mSubsystemID)->HaveService(Microcontroller::Name, 
                                                                                        &mMicrocontrollerID);
        }
    }    
    if(mTakeDriveControlFlag == false)
    {
        if(mPrimitiveDriverID.IsValid() && GetComponent()->AccessControlService()->HaveControl(mPrimitiveDriverID))
        {
            ReleaseComponentControl(mPrimitiveDriverID, true);
        }
        if(mMicrocontrollerID.IsValid() && GetComponent()->AccessControlService()->HaveControl(mMicrocontrollerID))
        {
            ReleaseComponentControl(mMicrocontrollerID, true);
        }
    }
}


void ControlDevice::ProcessAcquisitionOfControl(const Address& controlledComponent)
{
    Mutex::ScopedLock lock(&mControlDevice);
    // If we just too control, or re-acquired control of a vehicle
    // we are operating, we must put it in a Resume/Ready state.
    if(mTakeDriveControlFlag && controlledComponent == mPrimitiveDriverID)
    {
        GetComponent()->ManagementService()->Resume(mPrimitiveDriverID);
    }
    if(mTakeDriveControlFlag && controlledComponent == mMicrocontrollerID)
    {
        GetComponent()->ManagementService()->Resume(mMicrocontrollerID);
    }
}


void ControlDevice::Receive(const Message* message)
{
    // Currently service doesn't receive any messages.
}


Message* ControlDevice::CreateMessage(const UShort messageCode) const
{
    Message* message = NULL;
    // Currently service doesn't receive any messages.
    //switch(messageCode)
    //{
    //default:
    //    message = NULL;
    //    break;
    //};
    return message;
}


void ControlDevice::PrintStatus() const
{
    Mutex::ScopedLock lock(&mControlDevice);
    if(mPrimitiveDriverID.IsValid() && GetComponent()->AccessControlService()->HaveControl(mPrimitiveDriverID))
    {
        std::cout << "[Control Device] - Controlling Primitive Driver [" << mPrimitiveDriverID.ToString() << "]\n";
        
    }
    std::cout << "[Control Device] - Current Control Wrench:\n";
    mWrenchEffort.PrintMessageBody();
}

/*  End of File */