jaus++/2.0/src/jaus/extras/command/subsystemcommand.cpp

Go to the documentation of this file.

#include "jaus/extras/command/subsystemcommand.h"
#include "jaus/mobility/sensors/globalposesensor.h"
#include "jaus/mobility/sensors/velocitystatesensor.h"
#include <cmath>
#include <tinyxml/tinyxml.h>

using namespace JAUS;

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


SubsystemCommand::SubsystemCommand(const Service::ID& id) : Management::Child(id,
                                                                              Service::ID(Management::Name))
{
    mOnChangeEventsFlag = false;
    mDesiredPeriodicRateHz = 25;
    mpGlobalPoseCallback = NULL;
}


SubsystemCommand::~SubsystemCommand()
{
    if(mpGlobalPoseCallback)
    {
        delete mpGlobalPoseCallback;
        mpGlobalPoseCallback = NULL;
    }
}


void SubsystemCommand::Initialize()
{
    DiscoveryService()->RegisterCallback(this, true);
}


void SubsystemCommand::SetGlobalPoseSubscriptionType(const bool everyChange,
                                                     const double desiredPeriodicRate)
{
    if(GetComponent() && GetComponent()->IsInitialized() == false)
    {
        mOnChangeEventsFlag = everyChange;
        mDesiredPeriodicRateHz = desiredPeriodicRate;
    }
}


const Discovery* SubsystemCommand::DiscoveryService() const
{
    return GetComponent()->DiscoveryService();
}


Discovery* SubsystemCommand::DiscoveryService()
{
    return GetComponent()->DiscoveryService();
}


const AccessControl* SubsystemCommand::AccessControlService() const
{
    return GetComponent()->AccessControlService();
}


AccessControl* SubsystemCommand::AccessControlService()
{
    return GetComponent()->AccessControlService();
}


bool SubsystemCommand::SendCommand(const Message* commandMessage,
                                   const bool sendResumeFlag,
                                   const unsigned int waitTimeMs)
{
    Address dest = commandMessage->GetDestinationID();
    // Make sure we have control.
    RequestComponentControl(dest, sendResumeFlag, true, waitTimeMs);
    return Send(commandMessage);
}


void SubsystemCommand::Receive(const Message* message)
{
}


Message* SubsystemCommand::CreateMessage(const UShort messageCode) const
{
    Message* message = NULL;
    switch(messageCode)
    {
    case REPORT_GLOBAL_POSE:
        message = new ReportGlobalPose();
        break;
    case REPORT_VELOCITY_STATE:
        message = new ReportVelocityState();
        break;
    default:
        message = NULL;
        break;
    }
    return message;
}


void SubsystemCommand::CheckServiceStatus(const unsigned int timeSinceLastCheckMs)
{
}


void SubsystemCommand::PrintStatus() const
{
    Vehicle::Map vehicles;

    GetComponent()->DiscoveryService()->GetVehicles(vehicles);

    Vehicle::Map::const_iterator subsystem;
    for(subsystem = vehicles.begin();
        subsystem != vehicles.end();
        subsystem++)
    {
        std::cout << "========================================================================\n";
        std::cout << "Vehicle [" << subsystem->first << "] - " << subsystem->second->mIdentification << std::endl;
        Subsystem::Configuration::const_iterator node;
        for(node = subsystem->second->mConfiguration.begin();
            node != subsystem->second->mConfiguration.end();
            node++)
        {
            Subsystem::Component::Set::const_iterator component;
            for(component = node->second.begin();
                component != node->second.end();
                component++)
            {
                if(GetComponent()->AccessControlService()->HaveControl(component->mID))
                {
                    std::cout << "    Controlling: " << component->mIdentification << " - " << component->mID.ToString() << std::endl;
                }
                else
                {
                    std::cout << "    " << component->mIdentification << " - " << component->mID.ToString() << std::endl;
                }                
            }
        }
        std::cout << "    ";
        subsystem->second->mPosition.Print();
        double x, y, z;
        x = fabs(subsystem->second->mAttitude.mX) < .001 ? 0.0 : subsystem->second->mAttitude.mX;
        y = fabs(subsystem->second->mAttitude.mY) < .001 ? 0.0 : subsystem->second->mAttitude.mY;
        z = fabs(subsystem->second->mAttitude.mZ) < .001 ? 0.0 : subsystem->second->mAttitude.mZ;
        std::cout << "    A <" << x << ", " << y << ", " << z << ">\n    ";
        x = fabs(subsystem->second->mLinearVelocity.mX) < .001 ? 0.0 : subsystem->second->mLinearVelocity.mX;
        y = fabs(subsystem->second->mLinearVelocity.mY) < .001 ? 0.0 : subsystem->second->mLinearVelocity.mY;
        z = fabs(subsystem->second->mLinearVelocity.mZ) < .001 ? 0.0 : subsystem->second->mLinearVelocity.mZ;
        std::cout << "    V <" << x << ", " << y << ", " << z << ">\n    ";
        std::cout << subsystem->second->mUpdateTime.ToString() << std::endl;
    }  

    Vehicle::DeleteVehicleMap(vehicles);
}


void SubsystemCommand::ProcessSystemState(const Subsystem::Map& system)
{
    if(mpGlobalPoseCallback == NULL)
    {
        mpGlobalPoseCallback = new GlobalPoseCallback();
        mpGlobalPoseCallback->mpComponent = GetComponent();
        GetComponent()->TransportService()->RegisterCallback(REPORT_GLOBAL_POSE, mpGlobalPoseCallback);
    }
    // See if we need to make any requests for Global Pose data.
    Subsystem::Map::const_iterator s;
    for(s = system.begin();
        s != system.end();
        s++)
    {
        Address id;
        if(s->second->HaveService(GlobalPoseSensor::Name, &id))
        {
            if(EventsService()->HaveSubscription(REPORT_GLOBAL_POSE, id) == false)
            {
                QueryGlobalPose query;
                query.SetPresenceVector(query.GetPresenceVectorMask()); // Try all data.
                if(mOnChangeEventsFlag)
                {
                    EventsService()->RequestEveryChangeEvent(id, &query);
                }
                else
                {
                    EventsService()->RequestPeriodicEvent(id, &query, mDesiredPeriodicRateHz);
                }
            }
        }
        if(s->second->HaveService(VelocityStateSensor::Name, &id))
        {
            if(EventsService()->HaveSubscription(REPORT_VELOCITY_STATE, id) == false)
            {
                QueryVelocityState query;
                query.SetPresenceVector(query.GetPresenceVectorMask()); // Try all data.
                if(mOnChangeEventsFlag)
                {
                    EventsService()->RequestEveryChangeEvent(id, &query);
                }
                else
                {
                    EventsService()->RequestPeriodicEvent(id, &query, mDesiredPeriodicRateHz);
                }
            }
        }
    }
}


void SubsystemCommand::GlobalPoseCallback::ProcessMessage(const Message* message)
{
    if(mpComponent == NULL)
    {
        return;
    }
    switch(message->GetMessageCode())
    {
    case REPORT_GLOBAL_POSE:
        {
            const ReportGlobalPose* report = dynamic_cast<const ReportGlobalPose*>(message);
            if(report)
            {
                Wgs wgs;
                Attitude att;
                Time timestamp;

                wgs = mpComponent->DiscoveryService()->GetVehicle(report->GetSourceID().mSubsystem)->mPosition;
                att = mpComponent->DiscoveryService()->GetVehicle(report->GetSourceID().mSubsystem)->mAttitude;

                // Build on top of the current values.

                if(report->IsFieldPresent(ReportGlobalPose::PresenceVector::Latitude))
                {
                    wgs.mLatitude = report->GetLatitude();
                }
                if(report->IsFieldPresent(ReportGlobalPose::PresenceVector::Longitude))
                {
                    wgs.mLongitude = report->GetLongitude();
                }
                if(report->IsFieldPresent(ReportGlobalPose::PresenceVector::Altitude))
                {
                    wgs.mElevation = report->GetAltitude();
                }
                if(report->IsFieldPresent(ReportGlobalPose::PresenceVector::Roll))
                {
                    att.mX = report->GetRoll();
                }
                if(report->IsFieldPresent(ReportGlobalPose::PresenceVector::Pitch))
                {
                    att.mY = report->GetPitch();
                }
                if(report->IsFieldPresent(ReportGlobalPose::PresenceVector::Yaw))
                {
                    att.mZ = report->GetYaw();
                }
                if(report->IsFieldPresent(ReportGlobalPose::PresenceVector::TimeStamp))
                {
                    timestamp = report->GetTimeStamp();
                }
                else
                {
                    timestamp.SetCurrentTime();
                }

                // Save the results.
                mpComponent->DiscoveryService()->GetVehicle(report->GetSourceID().mSubsystem)->Update(wgs, att, timestamp);

                // Trigger callbacks to signal change in data.
                mpComponent->DiscoveryService()->TriggerDiscoveryCallbacks();
            }
        }
        break;
    case REPORT_VELOCITY_STATE:
        {
            const ReportVelocityState* report = dynamic_cast<const ReportVelocityState*>(message);
            if(report)
            {
                Point3D linearVelocity;
                linearVelocity.Set(report->GetVelocityX(),
                                   report->GetVelocityY(),
                                   report->GetVelocityZ());
                // Save the results.
                mpComponent->DiscoveryService()->GetVehicle(report->GetSourceID().mSubsystem)->Update(linearVelocity, Time(true));
                // Trigger callbacks to signal change in data.
                mpComponent->DiscoveryService()->TriggerDiscoveryCallbacks();
            }
        }
        break;
    default:
        break;
    }
}


/*  End of File */