jaus++/2.0/include/jaus/mobility/drivers/globalwaypointdriver.h

Go to the documentation of this file.

#ifndef __JAUS_MOBILITY_GLOBAL_WAYPOINT_DRIVER__H
#define __JAUS_MOBILITY_GLOBAL_WAYPOINT_DRIVER__H

#include "jaus/core/management/management.h"
#include "jaus/mobility/drivers/settravelspeed.h"
#include "jaus/mobility/drivers/setglobalwaypoint.h"
#include "jaus/mobility/drivers/querytravelspeed.h"
#include "jaus/mobility/drivers/queryglobalwaypoint.h"
#include "jaus/mobility/drivers/reporttravelspeed.h"
#include "jaus/mobility/drivers/reportglobalwaypoint.h"
#include "jaus/mobility/sensors/globalposesensor.h"
#include "jaus/mobility/sensors/velocitystatesensor.h"

namespace JAUS
{
    typedef JAUS::ReportTravelSpeed TravelSpeed;

    //    The function of the Global Waypoint Driver is to move the platform given a
    class JAUS_MOBILITY_DLL GlobalWaypointDriver : public Management::Child
    {
    public:
        const static std::string Name; 
        GlobalWaypointDriver();
        virtual ~GlobalWaypointDriver();
        // Overloaded by the inheriting class to generate commands for the controlled driver.
        virtual Message* GenerateDriveCommand(const Byte status) = 0;
        // Overloaded by the inheriting class to generate default commands for the controlled driver.
        virtual Message* GenerateIdleDriveCommand(const Byte status) const = 0;
        // Returns true if target waypoint is achieved.
        virtual bool IsWaypointAchieved(const GlobalPose& currentPose,
                                        const JAUS::SetGlobalWaypoint& desiredWaypoint) const = 0;
        // Virtual method called whenever waypoint driver updates, overload to add additional behaviors/checks needed.
        virtual void WaypointDriverUpdateEvent(const unsigned int timeSinceLastUpdateMs) {}
        // Method is called when desired waypoint is achieved.
        virtual void WaypointAchieved(const JAUS::SetGlobalWaypoint& waypoint) = 0;
        // Method called whenever a Set Global Waypoint command is received by service.
        virtual bool SetGlobalWaypoint(const JAUS::SetGlobalWaypoint* command);
        // Method called whenever a Set Travel Speed command is received by service, overload for extra functionality.
        virtual bool SetDesiredTravelSpeed(const JAUS::SetTravelSpeed* command) {return true;}
        // Change the Desired Travel Speed without sending a SetTravelSpeed command
        virtual bool SetDesiredTravelSpeed(const double speed);
        // Try to take control of lower-level (e.g. primitive) driver, in order to move to waypoint.
        virtual bool SetDriverToControl(const Address& driver);
        // Gets the current GlobalWaypoint command.
        virtual JAUS::SetGlobalWaypoint GetGlobalWaypoint() const;
        // Gets the current GlobalPoseSensor report.
        virtual GlobalPose GetGlobalPose() const;
        // Gets the current velocity state report.
        virtual VelocityState GetVelocityState() const;
        // Gets the current travel speed command.
        virtual JAUS::SetTravelSpeed GetDesiredTravelSpeed() const; 
        // Gets the time when the last wrench effort command was received.
        virtual Time GetGlobalWaypointTime() const;
        // Gets the time when the last travel speed command was received.
        virtual Time GetTravelSpeedTime() const;
        // Gets the address of the lower-level (e.g. primitive) driver being controlled.
        virtual Address GetControlledDriverID() const;        
        // Generates Global Waypoint Driver related events.
        virtual bool GenerateEvent(const Events::Subscription& info) const;
        // Adds support for Report Global Waypoint events.
        virtual bool IsEventSupported(const Events::Type type,
                                      const double requestedPeriodicRate,
                                      const Message* queryMessage,
                                      double& confirmedPeriodicRate,
                                      std::string& errorMessage) const;
        // By default, the Global Waypoint Driver is discoverable to other components (overload to hide).
        virtual bool IsDiscoverable() const { return true; }
        // Method called whenever a message is received.
        virtual void Receive(const Message* message);
        // Creates messages associated with this service.
        virtual Message* CreateMessage(const UShort messageCode) const;
        // Prints information about the service.
        virtual void PrintStatus() const;
        // Method called when transitioning to a ready state.
        virtual bool Resume();
        // Method called to transition due to reset.
        virtual bool Reset();
        // Method called when transitioning to a standby state.
        virtual bool Standby();
        // Method called when transitioning to an emergency state.
        virtual bool SetEmergency();
        // Method called when leaving the emergency state.
        virtual bool ClearEmergency();
        // Method called when control is released.
        virtual bool ReleaseControl();
    private:
        // In this method, drive commands are generated.
        virtual void CheckServiceStatus(const unsigned int timeSinceLastCheckMs);
        // Creates a ReportGlobalWaypoint from QueryGlobalWaypoint.
        void CreateReportFromQuery(const QueryGlobalWaypoint* query, 
                                   ReportGlobalWaypoint& report) const;
        // Creates a ReportTravelSpeed from QueryTravelSpeed.
        void CreateReportFromQuery(const QueryTravelSpeed* query, 
                                   TravelSpeed& report) const;
        // Checks if Driver is controlled, acquire control if necessary.
        bool CheckDriver();
        // Checks if Sensor data is availabe, acquire subscriptions if necessary.
        bool CheckSensors();
        // Removes Control of driver and stops subscriptions.
        bool ReleaseResources();
        bool mWaypointAchievedFlag;                         
        Mutex mGlobalWaypointDriverMutex;                   
        Address mControlledDriverID;                        
        GlobalPoseSensor* mpGlobalPoseSensor;               
        VelocityStateSensor* mpVelocityStateSensor;         
        Time mGlobalWaypointTime;                           
        Time mTravelSpeedTime;                              
        JAUS::SetGlobalWaypoint mGlobalWaypoint;            
        JAUS::SetTravelSpeed mDesiredTravelSpeed;           
        GlobalPose mGlobalPose;                             
        VelocityState mVelocityState;                       
    };
}

#endif
/*  End of File */