Transformer.hpp

Go to the documentation of this file.

#ifndef TRANSFORMER_H
#define TRANSFORMER_H

#include <Eigen/Geometry>
#include <string>
#include <base/Time.hpp>
#include <aggregator/StreamAligner.hpp>
#include <map>
#include <boost/bind.hpp>
#include <base/samples/RigidBodyState.hpp>
#include "TransformationStatus.hpp"

namespace transformer {
 
typedef base::samples::RigidBodyState TransformationType;
class TransformationElement;

class Transformation
{
    friend class Transformer;
    private:
        Transformation(const std::string &sourceFrame, const std::string &targetFrame)
            : valid(false)
            , sourceFrame(sourceFrame)
            , targetFrame(targetFrame)
            , generatedTransformations(0)
            , failedNoChain(0)
            , failedNoSample(0)
            , failedInterpolationImpossible(0) {};
        bool valid;

        std::string sourceFrame;
        std::string targetFrame;
        std::string sourceFrameMapped;
        std::string targetFrameMapped;
        std::vector<TransformationElement *> transformationChain;

        mutable base::Time lastGeneratedValue;
        mutable uint64_t generatedTransformations;
        mutable uint64_t failedNoChain;
        mutable uint64_t failedNoSample;
        mutable uint64_t failedInterpolationImpossible;
        boost::function<void (const base::Time &ts)> transformationChangedCallback;
        
        void setFrameMapping(const std::string &frameName, const std::string &newName);
        
        void setTransformationChain(const std::vector<TransformationElement *> &chain);
        
        Transformation(const Transformation &other)
        {
        }
        
    public:
        TransformationStatus getStatus() const;

        void updateStatus(TransformationStatus& status) const;

        const std::string &getSourceFrame() const
        {
            if(sourceFrameMapped.empty())
                return sourceFrame;
            
            return sourceFrameMapped;
        }
        
        const std::string &getTargetFrame() const
        {
            if(targetFrameMapped.empty())
                return targetFrame;
            
            return targetFrameMapped;
        }       

        void reset()
        {
            valid = false;
            transformationChain.clear();
            lastGeneratedValue = base::Time();
            generatedTransformations = 0;
            failedNoChain = 0;
            failedNoSample = 0;
            failedInterpolationImpossible = 0;
        }

        void registerUpdateCallback(boost::function<void (const base::Time &ts)> callback);
        
        bool get(const base::Time& atTime, transformer::TransformationType& result, bool interpolate = false) const;
        bool getChain(const base::Time& atTime, std::vector<TransformationType>& result, bool interpolate = false) const;

        template <class T>
        bool get(const base::Time& atTime, T& result, bool interpolate = false) const;
        bool getChain(const base::Time& atTime, std::vector<Eigen::Affine3d>& result, bool interpolate = false) const;
};

class TransformationElement {
    public:
        TransformationElement(const std::string &sourceFrame, const std::string &targetFrame): sourceFrame(sourceFrame), targetFrame(targetFrame) {};
        virtual ~TransformationElement() {};
        
        virtual bool getTransformation(const base::Time &atTime, bool doInterpolation, TransformationType &tr) = 0;

        virtual void addTransformationChangedCallback(boost::function<void (const base::Time &ts)> callback)
        {
            elementChangedCallbacks.push_back(callback);
        };
        
        virtual void clearTransformationChangedCallbacks()
        {
            elementChangedCallbacks.clear();
        }
        
        const std::string &getSourceFrame() const
        {
            return sourceFrame;
        }
        
        const std::string &getTargetFrame() const
        {
            return targetFrame;
        }

    protected:
        std::vector<boost::function<void (const base::Time &ts)> > elementChangedCallbacks;
    private:
        std::string sourceFrame;
        std::string targetFrame;
};

class StaticTransformationElement : public TransformationElement {
    public:
        StaticTransformationElement(const std::string &sourceFrame, const std::string &targetFrame, const TransformationType &transform) : TransformationElement(sourceFrame, targetFrame), staticTransform(transform) {};
        virtual ~StaticTransformationElement() {};
        
        virtual bool getTransformation(const base::Time& atTime, bool doInterpolation, TransformationType& tr)
        {
            tr = staticTransform;
            tr.time = atTime;
            return true;
        };
    private:
        TransformationType staticTransform;
};

class DynamicTransformationElement : public TransformationElement {
    public:
        DynamicTransformationElement(const std::string& sourceFrame, const std::string& targetFrame, aggregator::StreamAligner& aggregator, int priority = -10);
        virtual ~DynamicTransformationElement();
        
        virtual bool getTransformation(const base::Time& atTime, bool doInterpolation, TransformationType& result);
        
        int getStreamIdx() const
        {
            return streamIdx;
        }
        
    private:
        
        void aggregatorCallback(const base::Time &ts, const TransformationType &value); 

        aggregator::StreamAligner &aggregator;
        base::Time lastTransformTime;
        TransformationType lastTransform;
        bool gotTransform;
        int streamIdx;
};

class InverseTransformationElement : public TransformationElement {
    public:
        InverseTransformationElement(TransformationElement *source): TransformationElement(source->getTargetFrame(), source->getSourceFrame()), nonInverseElement(source) {};
        virtual ~InverseTransformationElement() {};
        virtual bool getTransformation(const base::Time& atTime, bool doInterpolation, TransformationType& tr);

        virtual void addTransformationChangedCallback(boost::function<void (const base::Time &ts)> callback) 
        {
            nonInverseElement->addTransformationChangedCallback(callback);
        };
        
        TransformationElement* getElement();
        TransformationElement const* getElement() const;
    private:
        TransformationElement *nonInverseElement;
};


class  TransformationNode;

class TransformationTree
{
    public:
        TransformationTree() : maxSeekDepth(20) {};

        std::pair<int, int> getElementsCount() const;
        
        void addTransformation(TransformationElement *element);
        
        bool getTransformationChain(std::string from, std::string to, std::vector<TransformationElement *> &result);
        
        std::vector<TransformationElement *> &getAvailableElements()
        {
            return availableElements;
        }
        
        ~TransformationTree();  
        
        void clear();

        void dumpTree() const;
        
    private:
        const int maxSeekDepth;

        void addMatchingTransforms(std::string from, transformer::TransformationNode* node);
        
        std::vector< TransformationNode* >::const_iterator checkForMatchingChildFrame(const std::string &to, const TransformationNode &node);
        
        std::vector<TransformationElement *> availableElements;
};

class Transformer
{
    protected:
        aggregator::StreamAligner aggregator;
        std::map<std::pair<std::string, std::string>, int> transformToStreamIndex;
        std::vector<Transformation *> transformations;
        TransformationTree transformationTree;
        int priority;
        TransformerStatus transformerStatus;

        void recomputeAvailableTransformations();
        
    public:
        
        Transformer( int priority = -10 ) 
            : priority( priority ) {};
        
        virtual void clear();
        
        void addTransformationChain(std::string from, std::string to, const std::vector<TransformationElement *> &chain);
        
        Transformation &registerTransformation(std::string sourceFrame, std::string targetFrame);

        const std::vector<Transformation *> &getRegisteredTransformations()
        {
            return transformations;
        }

        void unregisterTransformation(Transformation *transformation);
        
        void registerTransformCallback(Transformation &transform , boost::function<void (const base::Time &ts, const Transformation &t)> callback) 
        {
            transform.registerUpdateCallback(boost::bind( callback, _1, boost::ref(transform) ));
        }

        template <class T> int registerDataStream(base::Time dataPeriod, boost::function<void (const base::Time &ts, const T &value)> callback, int priority = -1, const std::string &name = std::string())
        {
            return aggregator.registerStream<T>(boost::bind( callback, _1, _2), 0, dataPeriod, priority, name);
        };

        template <class T> int registerDataStreamWithTransform(base::Time dataPeriod, Transformation &transformation, boost::function<void (const base::Time &ts, const T &value, const Transformation &t)> callback, int priority = - 1, const std::string &name = std::string())
        {
            return aggregator.registerStream<T>(boost::bind( callback, _1, _2, boost::ref(transformation) ), 0, dataPeriod, priority, name);
        };

        void unregisterDataStream(int idx)
        {
            return aggregator.unregisterStream(idx);
        };

        void disableStream( int idx )
        {
            return aggregator.disableStream( idx );
        }

        void enableStream( int idx )
        {
            return aggregator.enableStream( idx );
        }

        bool isStreamActive( int idx ) const 
        {
            return aggregator.isStreamActive( idx );
        }
        
        void requestTransformationAtTime(int idx, base::Time ts)
        {
            aggregator.push(idx, ts, false);
        };
        
        template <class T> void pushData( int idx,const base::Time &ts, const T& data )
        {
            aggregator.push(idx, ts, data);
        };
        
        int step() {
            return aggregator.step();
        }
        
        const aggregator::StreamAlignerStatus &getStreamAlignerStatus()
        {
            return aggregator.getStatus();
        }
        
        void setTimeout(const base::Time &t )
        {
            aggregator.setTimeout(t);
        }
        
        virtual void pushDynamicTransformation(const TransformationType &tr);
        
        void pushStaticTransformation(const TransformationType &tr);

        
        void setFrameMapping(const std::string &frameName, const std::string &newName);

        const aggregator::StreamAlignerStatus& getStatus()
        {
            return aggregator.getStatus();
        }
        
        const TransformerStatus& getTransformerStatus();
        
        virtual ~Transformer();
};

template<class T>
bool Transformation::get(const base::Time& atTime, T& result, bool interpolate) const
{
    result = T::Identity();
    if (!valid)
    {
        failedNoChain++;
        return false;
    }

    for(std::vector<TransformationElement *>::const_iterator it = transformationChain.begin(); it != transformationChain.end(); it++)
    {
        TransformationType tr;
        if(!(*it)->getTransformation(atTime, interpolate, tr))
        {
            if (interpolate)
                failedInterpolationImpossible++;
            else
                failedNoSample++;

            //no sample available, return
            return false;
        }
        
        //TODO, this might be a costly operation
        T trans( tr );
        
        //apply transformation
        result = result * trans;
    }
    lastGeneratedValue = atTime;
    generatedTransformations++;
    return true;
}

}
#endif // TRANSFORMER_H