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

Go to the documentation of this file.

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

using namespace JAUS;

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


LocalWaypointDriver::LocalWaypointDriver() : Management::Child(Service::ID(LocalWaypointDriver::Name), 
                                                               Service::ID(Management::Name))
{
    mWaypointAchievedFlag = false;
    mpLocalPoseSensor = NULL;
    mpVelocityStateSensor = NULL;
    mLocalWaypointTime;
}


LocalWaypointDriver::~LocalWaypointDriver()
{
}


bool LocalWaypointDriver::SetLocalWaypoint(const JAUS::SetLocalWaypoint *command)
{
    Mutex::ScopedLock lock(&mLocalWaypointDriverMutex);
    mLocalWaypoint = *command;
    mLocalWaypoint.SetSourceID(GetComponentID());
    mWaypointAchievedFlag = false;
    return true;
}


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


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

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


SetLocalWaypoint LocalWaypointDriver::GetLocalWaypoint() const
{
    Mutex::ScopedLock lock(&mLocalWaypointDriverMutex);
    return mLocalWaypoint;
}


LocalPose LocalWaypointDriver::GetLocalPose() const
{
    Mutex::ScopedLock lock(&mLocalWaypointDriverMutex);
    return mLocalPose;
}


VelocityState LocalWaypointDriver::GetVelocityState() const
{
    Mutex::ScopedLock lock(&mLocalWaypointDriverMutex);
    return mVelocityState;
}


SetTravelSpeed LocalWaypointDriver::GetDesiredTravelSpeed() const
{
    Mutex::ScopedLock lock(&mLocalWaypointDriverMutex);
    return mDesiredTravelSpeed;
}


Time LocalWaypointDriver::GetLocalWaypointTime() const
{
    Mutex::ScopedLock lock(&mLocalWaypointDriverMutex);
    return mLocalWaypointTime;
}


Time LocalWaypointDriver::GetTravelSpeedTime() const
{
    Mutex::ScopedLock lock(&mLocalWaypointDriverMutex);
    return mTravelSpeedTime;
}


Address LocalWaypointDriver::GetControlledDriverID() const
{
    Mutex::ScopedLock lock(&mLocalWaypointDriverMutex);
    return mControlledDriverID;
}


bool LocalWaypointDriver::GenerateEvent(const Events::Subscription& info) const
{
    if(info.mpQueryMessage->GetMessageCode() == QUERY_LOCAL_WAYPOINT)
    {
        Mutex::ScopedLock lock(&mLocalWaypointDriverMutex);
        const QueryLocalWaypoint* query = dynamic_cast<const QueryLocalWaypoint*>(info.mpQueryMessage);

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

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

        return true;
    }
    else if(info.mpQueryMessage->GetMessageCode() == QUERY_TRAVEL_SPEED)
    {
        Mutex::ScopedLock lock(&mLocalWaypointDriverMutex);
        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 LocalWaypointDriver::IsEventSupported(const Events::Type type,
                                            const double requestedPeriodicRate,
                                            const Message* queryMessage,
                                            double& confirmedPeriodicRate,
                                            std::string& errorMessage) const
{
    // Support any type of event for QUERY_LOCAL_WAYPOINT data.
    if(queryMessage->GetMessageCode() == QUERY_LOCAL_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 LocalWaypointDriver::Receive(const Message *message)
{
    switch(message->GetMessageCode())
    {
    case QUERY_LOCAL_WAYPOINT:
        {
            const JAUS::QueryLocalWaypoint* query = dynamic_cast<const JAUS::QueryLocalWaypoint*>(message);
            if(query)
            {
                ReportLocalWaypoint 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_LOCAL_WAYPOINT:
        {
            const JAUS::SetLocalWaypoint* command = dynamic_cast<const JAUS::SetLocalWaypoint*>(message);
            if(command)
            {
                Mutex::ScopedLock lock(&mLocalWaypointDriverMutex);
                if(SetLocalWaypoint(command))
                {
                    mLocalWaypoint = *command;
                    mLocalWaypointTime.SetCurrentTime();
                    SignalEvent(REPORT_LOCAL_WAYPOINT);
                }
            }
        }
        break;
    case SET_TRAVEL_SPEED:
        {
            const JAUS::SetTravelSpeed* command = dynamic_cast<const JAUS::SetTravelSpeed*>(message);
            if(command)
            {
                Mutex::ScopedLock lock(&mLocalWaypointDriverMutex);
                if(SetDesiredTravelSpeed(command))
                {
                    mDesiredTravelSpeed = *command;
                    mTravelSpeedTime.SetCurrentTime();
                    SignalEvent(REPORT_TRAVEL_SPEED);
                }
            }
        }
        break;
    default:
        break;
    }
}


Message* LocalWaypointDriver::CreateMessage(const UShort messageCode) const
{
    Message* message = NULL;
    switch(messageCode)
    {
    case QUERY_LOCAL_WAYPOINT:
        message = new JAUS::QueryLocalWaypoint();
        break;
    case QUERY_TRAVEL_SPEED:
        message = new JAUS::QueryTravelSpeed();
        break;
    case REPORT_LOCAL_WAYPOINT:
        message = new JAUS::ReportLocalWaypoint();
        break;
    case REPORT_TRAVEL_SPEED:
        message = new JAUS::ReportTravelSpeed();
        break;
    case SET_LOCAL_WAYPOINT:
        message = new JAUS::SetLocalWaypoint();
        break;
    case SET_TRAVEL_SPEED:
        message = new JAUS::SetTravelSpeed();
        break;
    default:
        message = NULL;
        break;
    };
    return message;
}


void LocalWaypointDriver::PrintStatus() const
{
    Mutex::ScopedLock lock(&mLocalWaypointDriverMutex);
    if(mLocalWaypoint.GetSourceID().IsValid())
    {
        std::cout << "[" << GetServiceID().ToString() << "] - Driving to Waypoint:\n";
        mLocalWaypoint.PrintMessageBody();
        if(mDesiredTravelSpeed.GetSourceID().IsValid())
        {
            mDesiredTravelSpeed.PrintMessageBody();
        }
    }
    else
    {
        std::cout << "[" << GetServiceID().ToString() << "] - Idle\n";
    }
}


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


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

    mWaypointAchievedFlag = false;
    mControlledDriverID.Clear();
    mpLocalPoseSensor = NULL;
    mpVelocityStateSensor = NULL;
    mLocalWaypointTime.Clear();
    mLocalWaypoint.ClearMessage();
    mDesiredTravelSpeed.ClearMessage();

    return success;
}


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


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

    return success;
}


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


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


bool LocalWaypointDriver::ReleaseResources()
{
    Mutex::ScopedLock lock(&mLocalWaypointDriverMutex);

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

    return true;
}


void LocalWaypointDriver::CheckServiceStatus(const unsigned int timeSinceLastCheckMs)
{
    Byte status = GetComponent()->ManagementService()->GetStatus();
    switch(status)
    {
    case Management::Status::Ready:
        {
            Mutex::ScopedLock lock(&mLocalWaypointDriverMutex);
            //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() &&
                   mLocalWaypoint.GetSourceID().IsValid())
                {
                    //Send Idle command if Waypoint is achieved.
                    if(IsWaypointAchieved(mLocalPose, mLocalWaypoint))
                    {
                        if(!mWaypointAchievedFlag)
                        {
                            mWaypointAchievedFlag = true;
                            WaypointAchieved(mLocalWaypoint);
                            // Flush out the ID so we don't keep driving to
                            // the waypoint.
                            mLocalWaypoint.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 LocalWaypointDriver::CreateReportFromQuery(const QueryLocalWaypoint* query,
                                                 ReportLocalWaypoint& report) const
{
    Mutex::ScopedLock lock(&mLocalWaypointDriverMutex);
    report.ClearMessage();
    report.SetDestinationID(query->GetSourceID());
    report.SetSourceID(GetComponentID());
    UInt pv1 = query->GetPresenceVector();
    UInt pv2 = mLocalWaypoint.GetPresenceVector();

    report.SetX(mLocalWaypoint.GetX());
    report.SetY(mLocalWaypoint.GetY());
    if( (pv2 & (pv1 & LocalWaypoint::PresenceVector::Z)) > 0) { report.SetZ(mLocalWaypoint.GetZ()); }
    if( (pv2 & (pv1 & LocalWaypoint::PresenceVector::Roll)) > 0) { report.SetRoll(mLocalWaypoint.GetRoll()); }
    if( (pv2 & (pv1 & LocalWaypoint::PresenceVector::Pitch)) > 0) { report.SetPitch(mLocalWaypoint.GetPitch()); }
    if( (pv2 & (pv1 & LocalWaypoint::PresenceVector::Yaw)) > 0) { report.SetYaw(mLocalWaypoint.GetYaw()); }
    if( (pv2 & (pv1 & LocalWaypoint::PresenceVector::WaypointTolerance)) > 0) { report.SetWaypointTolerance(mLocalWaypoint.GetWaypointTolerance()); }
    if( (pv2 & (pv1 & LocalWaypoint::PresenceVector::PathTolerance)) > 0) { report.SetPathTolerance(mLocalWaypoint.GetPathTolerance()); }    
}


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


bool LocalWaypointDriver::CheckDriver()
{
    bool success = false;
    Mutex::ScopedLock lock(&mLocalWaypointDriverMutex);

    //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 LocalWaypointDriver::CheckSensors()
{
    bool success = true;
    Mutex::ScopedLock lock(&mLocalWaypointDriverMutex);

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

    if(mpLocalPoseSensor)
    {
        mLocalPose = mpLocalPoseSensor->GetLocalPose();
        if(mLocalPose.AreFieldsPresent(ReportLocalPose::PresenceVector::X | 
                                       ReportLocalPose::PresenceVector::Y ) == 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 */