ros_timing.h

// Utilities for timing events and actions using the ROS time
// File: ros_timing.h
// Author: Philipp Allgeuer <pallgeuer@ais.uni-bonn.de>

// Ensure header is only included once
#ifndef ROS_TIMING_H
#define ROS_TIMING_H

// Includes
#include <cstddef>
#include <ros/time.h>
#include <ros/service_client.h>

// Nimbro utilities namespace
namespace nimbro_utils
{
    class RosTimeMarker
    {
    public:
        // Constructors
        RosTimeMarker() : markerTime(0), iHaveMarker(false) {} 

        // Timing functions
        void unsetMarker() { iHaveMarker = false; } 
        void setMarker()
        {
            // Record the current ROS time
            markerTime = ros::Time::now();
            iHaveMarker = true;
        } 
        bool haveMarker() const { return iHaveMarker; } 
        double getElapsed() const { return (iHaveMarker ? (ros::Time::now() - markerTime).toSec() : -1.0); } 
        bool hasElapsed(double duration) const { return !iHaveMarker || (iHaveMarker && ((ros::Time::now() - markerTime).toSec() >= duration)); } 

    private:
        // Internal variables
        ros::Time markerTime;
        bool iHaveMarker;
    };

    class RosTimeTracker
    {
    public:
        explicit RosTimeTracker(std::size_t N) : N(N)
        {
            // Make sure we have room for at least one marker
            if(N < 1) N = 1;

            // Allocate memory for the marker and flag arrays
            markerTime = new ros::Time[N];
            iHaveMarker = new bool[N];

            // Initialise the arrays explicitly
            for(std::size_t i = 0;i < N;i++)
            {
                markerTime[i].fromSec(0);
                iHaveMarker[i] = false;
            }
        }

        virtual ~RosTimeTracker() 
        {
            // Delete the allocated memory
            delete[] markerTime;
            delete[] iHaveMarker;
        }

        // Timing functions (in all these functions m is the index of the marker, valid values are 0 -> N-1)
        void unsetMarker(std::size_t m)
        {
            // Forget the required marker
            if(m < N) iHaveMarker[m] = false;
        }
        void setMarker(std::size_t m)
        {
            // Record the current ROS time in the appropriate marker
            if(m < N)
            {
                markerTime[m] = ros::Time::now();
                iHaveMarker[m] = true;
            }
        }
        bool haveMarker(std::size_t m) const { return (m < N ? iHaveMarker[m] : false); }
        double getElapsed(std::size_t m) const
        {
            // Return the elapsed time since marker m was set
            if(m < N) return (iHaveMarker[m] ? (ros::Time::now() - markerTime[m]).toSec() : -1.0);
            else return -1.0;
        }
        bool hasElapsed(std::size_t m, double duration) const
        {
            // Return whether the given duration has elapsed since marker m was set
            if(m < N) return !iHaveMarker[m] || (iHaveMarker[m] && ((ros::Time::now() - markerTime[m]).toSec() >= duration));
            else return false;
        }

    private:
        // Internal variables
        ros::Time* markerTime;
        bool* iHaveMarker;
        std::size_t N;
    };

    template <class T>
    class RosServiceCaller
    {
    public:
        RosServiceCaller(double reissueDelay, double failRetryDelay)
            : failRetryDelay(failRetryDelay)
            , reissueDelay(reissueDelay)
        {}

        // Service client set function
        void setServiceClient(const ros::ServiceClient& SC) { m_srv = SC; } 

        // Service call functions
        bool callService() { return callService(data); }
        bool callService(T& srv_data)
        {
            // Declare variables
            bool ret = false;

            // Decide whether to call the service or not (don't want to spam the service by calling it every time!)
            if(lastGoodCall.hasElapsed(reissueDelay) && lastBadCall.hasElapsed(failRetryDelay))
            {
                ret = m_srv.call<T>(srv_data); // If setServiceClient() hasn't been called (i.e. m_srv is invalid) then this returns false
                if(ret) lastGoodCall.setMarker();
                else lastBadCall.setMarker();
            }

            // Return whether the service call was carried out AND was successful
            return ret;
        }

    public:
        // Variables
        T data; 

    private:
        // Internal variables
        const double failRetryDelay;
        const double reissueDelay;
        ros::ServiceClient m_srv;
        RosTimeMarker lastGoodCall;
        RosTimeMarker lastBadCall;
    };
}

#endif /* ROS_TIMING_H */
// EOF