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

Go to the documentation of this file.

#include "jaus/mobility/drivers/localwaypointlistdriver.h"
#include "jaus/core/component.h"
#include <iostream>
#include <iomanip>

using namespace JAUS;

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

LocalWaypointListDriver::LocalWaypointListDriver() : ListDriver(Service::ID(LocalWaypointListDriver::Name),
                                                                Service::ID(ListManager::Name))
{
    
}


LocalWaypointListDriver::~LocalWaypointListDriver()
{
}


void LocalWaypointListDriver::ExecuteList(const double speedMetersPerSecond)
{
    Mutex::ScopedLock lock(&mListDriverMutex);
    if(speedMetersPerSecond > 0.0)
    {
        mSpeedMetersPerSecond = speedMetersPerSecond;
    }
}


bool LocalWaypointListDriver::IsElementSupported(const JAUS::Message* message) const
{
    const JAUS::SetLocalWaypoint* command = dynamic_cast<const JAUS::SetLocalWaypoint*>(message);
    return command != NULL ? true : false;
}


void LocalWaypointListDriver::Receive(const Message* message)
{
    switch(message->GetMessageCode())
    {
    case QUERY_LOCAL_WAYPOINT:
        {
            Mutex::ScopedLock lock(&mListDriverMutex);
            const QueryLocalWaypoint* query = dynamic_cast<const QueryLocalWaypoint*>(message);

            if(query == NULL)
            {
                return;
            }

            ReportLocalWaypoint report(message->GetSourceID(), GetComponentID());
            Element current = GetActiveListElement();
            if(current.mpElement)
            {
                SetLocalWaypoint* wp = dynamic_cast<SetLocalWaypoint*>(current.mpElement);
                if(wp)
                {
                    UInt pv1 = query->GetPresenceVector();
                    UInt pv2 = wp->GetPresenceVector();

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


Message* LocalWaypointListDriver::CreateMessage(const UShort messageCode) const
{
    switch(messageCode)
    {
    case SET_LOCAL_WAYPOINT:
        return new JAUS::SetLocalWaypoint();
        break;
    case QUERY_LOCAL_WAYPOINT:
        return new QueryLocalWaypoint();
        break;
    case REPORT_LOCAL_WAYPOINT:
        return new ReportLocalWaypoint();
        break;
    default:
        return ListDriver::CreateMessage(messageCode);
        break;
    }
    return NULL;
}



bool LocalWaypointListDriver::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;
}


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

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

        ReportLocalWaypoint report;
        Element current = GetActiveListElement();
        if(current.mpElement)
        {
            SetLocalWaypoint* wp = dynamic_cast<SetLocalWaypoint*>(current.mpElement);
            if(wp)
            {
                UInt pv1 = query->GetPresenceVector();
                UInt pv2 = wp->GetPresenceVector();

                report.SetX(wp->GetX());
                report.SetY(wp->GetY());
                if( (pv2 & (pv1 & LocalWaypoint::PresenceVector::Z)) > 0) { report.SetZ(wp->GetZ()); }
                if( (pv2 & (pv1 & LocalWaypoint::PresenceVector::Roll)) > 0) { report.SetRoll(wp->GetRoll()); }
                if( (pv2 & (pv1 & LocalWaypoint::PresenceVector::Pitch)) > 0) { report.SetPitch(wp->GetPitch()); }
                if( (pv2 & (pv1 & LocalWaypoint::PresenceVector::Yaw)) > 0) { report.SetYaw(wp->GetYaw()); }
                if( (pv2 & (pv1 & LocalWaypoint::PresenceVector::WaypointTolerance)) > 0) { report.SetWaypointTolerance(wp->GetWaypointTolerance()); }
                if( (pv2 & (pv1 & LocalWaypoint::PresenceVector::PathTolerance)) > 0) { report.SetPathTolerance(wp->GetPathTolerance()); }    
            }
        }
        SendEvent(info, &report);
        return true;
    }
    else if(info.mpQueryMessage->GetMessageCode() == QUERY_TRAVEL_SPEED)
    {
        Mutex::ScopedLock lock(&mListDriverMutex);
        const QueryTravelSpeed* query = dynamic_cast<const QueryTravelSpeed*>(info.mpQueryMessage);
        if(query == NULL)
        {
            return false;
        }
        ReportTravelSpeed report;
        report.SetSpeed(mSpeedMetersPerSecond);
        SendEvent(info, &report);
        return true;
    }

    return false;
}


JAUS::SetLocalWaypoint LocalWaypointListDriver::GetCurrentWaypoint() const
{
    Element current = GetActiveListElement();
    if(current.mpElement)
    {
        JAUS::SetLocalWaypoint* cmd = dynamic_cast<JAUS::SetLocalWaypoint*>(current.mpElement);
        if(cmd)
        {
            return *cmd;
        }
    }
    return JAUS::SetLocalWaypoint();
}


std::vector<JAUS::SetLocalWaypoint> LocalWaypointListDriver::GetWaypointList() const
{
    // Get local waypoint list.
    JAUS::Element::Map elementList = GetElementList();
    // Convert to SetLocalWaypointCommands
    JAUS::Element::Map::iterator listElement;
    std::vector<JAUS::SetLocalWaypoint> commandList;
    for(listElement = elementList.begin();
        listElement != elementList.end();
        listElement++)
    {
        if(listElement->second.mpElement->GetMessageCode() == JAUS::SET_LOCAL_WAYPOINT)
        {
            commandList.push_back(*( (JAUS::SetLocalWaypoint *)(listElement->second.mpElement)) );
        }
    }
    return commandList;
}


void LocalWaypointListDriver::PrintStatus() const
{
    std::vector<JAUS::SetLocalWaypoint> waypoints = GetWaypointList();
    if(IsExecuting() == true)
    {
        std::cout << "[" << GetServiceID().ToString() << "] - Driving to Waypoint [" << GetActiveListElementID() << "]:\n";
        std::cout << "There are " << waypoints.size() << " Waypoints in the List.\n";
        std::cout << "Execution Speed (m/s): " << std::fixed << std::setprecision(2) << mSpeedMetersPerSecond << std::endl;
    }
    else
    {
        std::cout << "[" << GetServiceID().ToString() << "] - Idle\n";
        std::cout << "There are " << waypoints.size() << " Waypoints in the List.\n";
    }
}


/*  End of File */