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

Go to the documentation of this file.

#ifndef __JAUS_MOBILITY_LOCAL_WAYPOINT_DRIVER__H
#define __JAUS_MOBILITY_LOCAL_WAYPOINT_DRIVER__H

#include "jaus/core/management/management.h"
#include "jaus/mobility/drivers/globalwaypointdriver.h"
#include "jaus/mobility/drivers/settravelspeed.h"
#include "jaus/mobility/drivers/setlocalwaypoint.h"
#include "jaus/mobility/drivers/querytravelspeed.h"
#include "jaus/mobility/drivers/querylocalwaypoint.h"
#include "jaus/mobility/drivers/reporttravelspeed.h"
#include "jaus/mobility/drivers/reportlocalwaypoint.h"
#include "jaus/mobility/sensors/localposesensor.h"
#include "jaus/mobility/sensors/velocitystatesensor.h"

namespace JAUS
{
    typedef JAUS::ReportLocalWaypoint LocalWaypoint;

    //    The function of the Local Waypoint Driver is to move the platform given a
    class JAUS_MOBILITY_DLL LocalWaypointDriver : public Management::Child
    {
    public:
        const static std::string Name; 
        LocalWaypointDriver();
        virtual ~LocalWaypointDriver();
        // 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 LocalPose& currentPose,
                                        const JAUS::SetLocalWaypoint& 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::SetLocalWaypoint& waypoint) = 0;
        // Method called whenever a Set Local Waypoint command is received by service.
        virtual bool SetLocalWaypoint(const JAUS::SetLocalWaypoint* 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 LocalWaypoint command.
        virtual JAUS::SetLocalWaypoint GetLocalWaypoint() const;
        // Gets the current LocalPoseSensor report.
        virtual LocalPose GetLocalPose() 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 GetLocalWaypointTime() 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 Local Waypoint Driver related events.
        virtual bool GenerateEvent(const Events::Subscription& info) const;
        // Adds support for Report Local Waypoint events.
        virtual bool IsEventSupported(const Events::Type type,
                                      const double requestedPeriodicRate,
                                      const Message* queryMessage,
                                      double& confirmedPeriodicRate,
                                      std::string& errorMessage) const;
        // By default, the Local 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 ReportLocalWaypoint from QueryLocalWaypoint.
        void CreateReportFromQuery(const QueryLocalWaypoint* query, 
                                   LocalWaypoint& 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 mLocalWaypointDriverMutex;                    
        Address mControlledDriverID;                        
        LocalPoseSensor* mpLocalPoseSensor;                 
        VelocityStateSensor* mpVelocityStateSensor;         
        Time mLocalWaypointTime;                            
        Time mTravelSpeedTime;                              
        JAUS::SetLocalWaypoint mLocalWaypoint;              
        JAUS::SetTravelSpeed mDesiredTravelSpeed;           
        LocalPose mLocalPose;                               
        VelocityState mVelocityState;                       
    };
}

#endif
/*  End of File */