jaus++/2.0/src/jaus/mobility/drivers/globalwaypointdriver.cpp

Go to the documentation of this file.

#include "jaus/core/component.h"
#include "jaus/mobility/drivers/globalwaypointdriver.h"

using namespace JAUS;

const std::string GlobalWaypointDriver::Name = "urn:jaus:jss:mobility:GlobalWaypointDriver";


GlobalWaypointDriver::GlobalWaypointDriver() : Management::Child(Service::ID(GlobalWaypointDriver::Name), 
                                                                 Service::ID(Management::Name))
{
    mWaypointAchievedFlag = false;
    mpGlobalPoseSensor = NULL;
    mpVelocityStateSensor = NULL;
    mGlobalWaypointTime;
}


GlobalWaypointDriver::~GlobalWaypointDriver()
{
}


bool GlobalWaypointDriver::SetGlobalWaypoint(const JAUS::SetGlobalWaypoint *command)
{
    Mutex::ScopedLock lock(&mGlobalWaypointDriverMutex);
    mGlobalWaypoint = *command;
    mGlobalWaypoint.SetSourceID(GetComponentID());
    mWaypointAchievedFlag = false;
    return true;
}


bool GlobalWaypointDriver::SetDesiredTravelSpeed(const double speed)
{
    Mutex::ScopedLock lock(&mGlobalWaypointDriverMutex);
    mTravelSpeedTime.SetCurrentTime();
    mDesiredTravelSpeed.SetSourceID(GetComponentID());
    mDesiredTravelSpeed.SetSpeed(speed);
    return true;
}


bool GlobalWaypointDriver::SetDriverToControl(const Address& driver)
{
    //Disallow control of drivers on the same component as this one.
    if(driver == GetComponentID())
    {
        return false;
    }

    Mutex::ScopedLock lock(&mGlobalWaypointDriverMutex);
    mControlledDriverID = driver;
    return true;
}


SetGlobalWaypoint GlobalWaypointDriver::GetGlobalWaypoint() const
{
    Mutex::ScopedLock lock(&mGlobalWaypointDriverMutex);
    return mGlobalWaypoint;
}


GlobalPose GlobalWaypointDriver::GetGlobalPose() const
{
    Mutex::ScopedLock lock(&mGlobalWaypointDriverMutex);
    return mGlobalPose;
}


VelocityState GlobalWaypointDriver::GetVelocityState() const
{
    Mutex::ScopedLock lock(&mGlobalWaypointDriverMutex);
    return mVelocityState;
}


SetTravelSpeed GlobalWaypointDriver::GetDesiredTravelSpeed() const
{
    Mutex::ScopedLock lock(&mGlobalWaypointDriverMutex);
    return mDesiredTravelSpeed;
}


Time GlobalWaypointDriver::GetGlobalWaypointTime() const
{
    Mutex::ScopedLock lock(&mGlobalWaypointDriverMutex);
    return mGlobalWaypointTime;
}


Time GlobalWaypointDriver::GetTravelSpeedTime() const
{
    Mutex::ScopedLock lock(&mGlobalWaypointDriverMutex);
    return mTravelSpeedTime;
}


Address GlobalWaypointDriver::GetControlledDriverID() const
{
    Mutex::ScopedLock lock(&mGlobalWaypointDriverMutex);
    return mControlledDriverID;
}


bool GlobalWaypointDriver::GenerateEvent(const Events::Subscription& info) const
{
    if(info.mpQueryMessage->GetMessageCode() == QUERY_GLOBAL_WAYPOINT)
    {
        Mutex::ScopedLock lock(&mGlobalWaypointDriverMutex);
        const QueryGlobalWaypoint* query = dynamic_cast<const QueryGlobalWaypoint*>(info.mpQueryMessage);

        if(query == NULL)
        {
            return false;
        }

        ReportGlobalWaypoint report;
        CreateReportFromQuery(query, report);
        SendEvent(info, &report);

        return true;
    }
    else if(info.mpQueryMessage->GetMessageCode() == QUERY_TRAVEL_SPEED)
    {
        Mutex::ScopedLock lock(&mGlobalWaypointDriverMutex);
        const QueryTravelSpeed* query = dynamic_cast<const QueryTravelSpeed*>(info.mpQueryMessage);

        if(query == NULL)
        {
            return false;
        }

        ReportTravelSpeed report;
        CreateReportFromQuery(query, report);
        SendEvent(info, &report);

        return true;
    }

    return false;
}


bool GlobalWaypointDriver::IsEventSupported(const Events::Type type,
                                            const double requestedPeriodicRate,
                                            const Message* queryMessage,
                                            double& confirmedPeriodicRate,
                                            std::string& errorMessage) const
{
    // Support any type of event for QUERY_GLOBAL_WAYPOINT data.
    if(queryMessage->GetMessageCode() == QUERY_GLOBAL_WAYPOINT)
    {
        confirmedPeriodicRate = requestedPeriodicRate;
        return true;
    }
    // Support any type of event for QUERY_TRAVEL_SPEED data.
    else if(queryMessage->GetMessageCode() == QUERY_TRAVEL_SPEED)
    {
        confirmedPeriodicRate = requestedPeriodicRate;
        return true;
    }
    return false;
}


void GlobalWaypointDriver::Receive(const Message *message)
{
    switch(message->GetMessageCode())
    {
    case QUERY_GLOBAL_WAYPOINT:
        {
            const JAUS::QueryGlobalWaypoint* query = dynamic_cast<const JAUS::QueryGlobalWaypoint*>(message);
            if(query)
            {
                ReportGlobalWaypoint report;
                CreateReportFromQuery(query, report);
                Send(&report);
            }
        }
        break;
    case QUERY_TRAVEL_SPEED:
        {
            const JAUS::QueryTravelSpeed* query = dynamic_cast<const JAUS::QueryTravelSpeed*>(message);
            if(query)
            {
                ReportTravelSpeed report;
                CreateReportFromQuery(query, report);
                Send(&report);
            }
        }
        break;
    case SET_GLOBAL_WAYPOINT:
        {
            const JAUS::SetGlobalWaypoint* command = dynamic_cast<const JAUS::SetGlobalWaypoint*>(message);
            if(command)
            {
                Mutex::ScopedLock lock(&mGlobalWaypointDriverMutex);
                if(SetGlobalWaypoint(command))
                {
                    mGlobalWaypoint = *command;
                    mGlobalWaypointTime.SetCurrentTime();
                    SignalEvent(REPORT_GLOBAL_WAYPOINT);
                }
            }
        }
        break;
    case SET_TRAVEL_SPEED:
        {
            const JAUS::SetTravelSpeed* command = dynamic_cast<const JAUS::SetTravelSpeed*>(message);
            if(command)
            {
                Mutex::ScopedLock lock(&mGlobalWaypointDriverMutex);
                if(SetDesiredTravelSpeed(command))
                {
                    mDesiredTravelSpeed = *command;
                    mTravelSpeedTime.SetCurrentTime();
                    SignalEvent(REPORT_TRAVEL_SPEED);
                }
            }
        }
        break;
    default:
        break;
    }
}


Message* GlobalWaypointDriver::CreateMessage(const UShort messageCode) const
{
    Message* message = NULL;
    switch(messageCode)
    {
    case QUERY_GLOBAL_WAYPOINT:
        message = new JAUS::QueryGlobalWaypoint();
        break;
    case QUERY_TRAVEL_SPEED:
        message = new JAUS::QueryTravelSpeed();
        break;
    case REPORT_GLOBAL_WAYPOINT:
        message = new JAUS::ReportGlobalWaypoint();
        break;
    case REPORT_TRAVEL_SPEED:
        message = new JAUS::ReportTravelSpeed();
        break;
    case SET_GLOBAL_WAYPOINT:
        message = new JAUS::SetGlobalWaypoint();
        break;
    case SET_TRAVEL_SPEED:
        message = new JAUS::SetTravelSpeed();
        break;
    default:
        message = NULL;
        break;
    };
    return message;
}


void GlobalWaypointDriver::PrintStatus() const
{
    //Mutex::ScopedLock lock(&mGlobalWaypointDriverMutex);
    
    if(mGlobalWaypoint.GetSourceID().IsValid())
    {
        std::cout << "[" << GetServiceID().ToString() << "] - Driving to Waypoint:\n";

        mGlobalWaypointDriverMutex.Lock();
        JAUS::SetGlobalWaypoint wp = mGlobalWaypoint;
        JAUS::SetTravelSpeed speed = mDesiredTravelSpeed;
        mGlobalWaypointDriverMutex.Unlock();

        wp.PrintMessageBody();
        if(speed.GetSourceID().IsValid())
        {
            speed.PrintMessageBody();
        }
    }
    else
    {
        std::cout << "[" << GetServiceID().ToString() << "] - Idle\n";
    }
    
}


bool GlobalWaypointDriver::Resume()
{
    return (CheckDriver() &&  CheckSensors());
}


bool GlobalWaypointDriver::Reset()
{
    bool success = ReleaseResources();

    mWaypointAchievedFlag = false;
    mControlledDriverID.Clear();
    mpGlobalPoseSensor = NULL;
    mpVelocityStateSensor = NULL;
    mGlobalWaypointTime.Clear();
    mGlobalWaypoint.ClearMessage();
    mDesiredTravelSpeed.ClearMessage();

    return success;
}


bool GlobalWaypointDriver::Standby()
{
    return ReleaseResources();
}


bool GlobalWaypointDriver::SetEmergency()
{
    bool success = true;
    Message* command = NULL;
    command = GenerateIdleDriveCommand(GetStatus());
    if(command)
    {
        success &= Send(command);
    }
    success &= ReleaseResources();

    return success;
}


bool GlobalWaypointDriver::ClearEmergency()
{
    return Resume();
}


bool GlobalWaypointDriver::ReleaseControl()
{
    return true;
}


bool GlobalWaypointDriver::ReleaseResources()
{
    Mutex::ScopedLock lock(&mGlobalWaypointDriverMutex);

    if(GetComponent()->AccessControlService()->HaveControl(mControlledDriverID))
    {
        GetComponent()->AccessControlService()->ReleaseComponentControl(mControlledDriverID);
    }

    return true;
}


void GlobalWaypointDriver::CheckServiceStatus(const unsigned int timeSinceLastCheckMs)
{
    Byte status = GetComponent()->ManagementService()->GetStatus();
    switch(status)
    {
    case Management::Status::Ready:
        {
            Mutex::ScopedLock lock(&mGlobalWaypointDriverMutex);
            //If all in order, generate and send a drive command, delete after use.
            if(CheckDriver())
            {
                Message *command = NULL;
                //Check whether sensor Data is valid.
                if(CheckSensors() &&
                   mGlobalWaypoint.GetSourceID().IsValid())
                {
                    //Send Idle command if Waypoint is achieved.
                    if(IsWaypointAchieved(mGlobalPose, mGlobalWaypoint))
                    {
                        if(!mWaypointAchievedFlag)
                        {
                            mWaypointAchievedFlag = true;
                            WaypointAchieved(mGlobalWaypoint);
                            // Flush out the ID so we don't keep driving to
                            // the waypoint.
                            mGlobalWaypoint.ClearMessage();
                        }

                        command = GenerateIdleDriveCommand(status);
                    }
                    else
                    {
                        command = GenerateDriveCommand(status);
                    }
                }
                else
                {
                    command = GenerateIdleDriveCommand(status);
                }

                if(command)
                {
                    command->SetDestinationID(mControlledDriverID);
                    command->SetSourceID(this->GetComponentID());
                    Send(command);
                    delete command;
                }
            }
        }
        break;
    case Management::Status::Standby:
        // Switching to standby mode automatically releases control and halts subscription(s).
        break;
    default:
        break;
    };

    WaypointDriverUpdateEvent(timeSinceLastCheckMs);
}


void GlobalWaypointDriver::CreateReportFromQuery(const QueryGlobalWaypoint* query,
                                                 ReportGlobalWaypoint& report) const
{
    Mutex::ScopedLock lock(&mGlobalWaypointDriverMutex);
    report.ClearMessage();
    report.SetDestinationID(query->GetSourceID());
    report.SetSourceID(GetComponentID());
    UInt pv1 = query->GetPresenceVector();
    UInt pv2 = mGlobalWaypoint.GetPresenceVector();

    report.SetLatitude(mGlobalWaypoint.GetLatitude());
    report.SetLongitude(mGlobalWaypoint.GetLongitude());
    if( (pv2 & (pv1 & ReportGlobalWaypoint::PresenceVector::Altitude)) > 0) { report.SetAltitude(mGlobalWaypoint.GetAltitude()); }
    if( (pv2 & (pv1 & ReportGlobalWaypoint::PresenceVector::Roll)) > 0) { report.SetRoll(mGlobalWaypoint.GetRoll()); }
    if( (pv2 & (pv1 & ReportGlobalWaypoint::PresenceVector::Pitch)) > 0) { report.SetPitch(mGlobalWaypoint.GetPitch()); }
    if( (pv2 & (pv1 & ReportGlobalWaypoint::PresenceVector::Yaw)) > 0) { report.SetYaw(mGlobalWaypoint.GetYaw()); }
    if( (pv2 & (pv1 & ReportGlobalWaypoint::PresenceVector::WaypointTolerance)) > 0) { report.SetWaypointTolerance(mGlobalWaypoint.GetWaypointTolerance()); }
    if( (pv2 & (pv1 & ReportGlobalWaypoint::PresenceVector::PathTolerance)) > 0) { report.SetPathTolerance(mGlobalWaypoint.GetPathTolerance()); }    
}


void GlobalWaypointDriver::CreateReportFromQuery(const QueryTravelSpeed* query,
                                                 ReportTravelSpeed& report) const
{
    Mutex::ScopedLock lock(&mGlobalWaypointDriverMutex);
    report.ClearMessage();
    report.SetDestinationID(query->GetSourceID());
    report.SetSourceID(GetComponentID());
    report.SetSpeed(mDesiredTravelSpeed.GetSpeed());
}


bool GlobalWaypointDriver::CheckDriver()
{
    bool success = false;
    Mutex::ScopedLock lock(&mGlobalWaypointDriverMutex);

    //Check if control of the driver is available, else try to acquire it
    if(GetComponent()->AccessControlService()->HaveControl(mControlledDriverID))
    {
        success = true;
    }
    else
    {
        if(GetComponent()->AccessControlService()->RequestComponentControl(mControlledDriverID, false))
        {
            success = GetComponent()->ManagementService()->Resume(mControlledDriverID);
        }
    }

    return success;
}


bool GlobalWaypointDriver::CheckSensors()
{
    bool success = true;
    Mutex::ScopedLock lock(&mGlobalWaypointDriverMutex);

    if(mpGlobalPoseSensor == NULL)
    {
        mpGlobalPoseSensor = dynamic_cast<GlobalPoseSensor*>(GetComponent()->GetService(GlobalPoseSensor::Name));
    }

    if(mpGlobalPoseSensor)
    {
        mGlobalPose = mpGlobalPoseSensor->GetGlobalPose();
        if(mGlobalPose.AreFieldsPresent(ReportGlobalPose::PresenceVector::Latitude | 
                                        ReportGlobalPose::PresenceVector::Longitude ) == false)
        {
            success = false;
        }
    }
    else
    {
        success = false;
    }

    if(mpVelocityStateSensor == NULL)
    {
        mpVelocityStateSensor = dynamic_cast<VelocityStateSensor*>(GetComponent()->GetService(VelocityStateSensor::Name));
    }

    if(mpVelocityStateSensor)
    {
        mVelocityState = mpVelocityStateSensor->GetVelocityState();
    }
    else
    {
        success = false;
    }

    return success;
}


/*  End of File */