FrameworkTransformer.h

Go to the documentation of this file.

/*
 * Copyright (C) 2012 by
 *   MetraLabs GmbH (MLAB), GERMANY
 * and
 *   Neuroinformatics and Cognitive Robotics Labs (NICR) at TU Ilmenau, GERMANY
 * All rights reserved.
 *
 * Contact: info@mira-project.org
 *
 * Commercial Usage:
 *   Licensees holding valid commercial licenses may use this file in
 *   accordance with the commercial license agreement provided with the
 *   software or, alternatively, in accordance with the terms contained in
 *   a written agreement between you and MLAB or NICR.
 *
 * GNU General Public License Usage:
 *   Alternatively, this file may be used under the terms of the GNU
 *   General Public License version 3.0 as published by the Free Software
 *   Foundation and appearing in the file LICENSE.GPL3 included in the
 *   packaging of this file. Please review the following information to
 *   ensure the GNU General Public License version 3.0 requirements will be
 *   met: http://www.gnu.org/copyleft/gpl.html.
 *   Alternatively you may (at your option) use any later version of the GNU
 *   General Public License if such license has been publicly approved by
 *   MLAB and NICR (or its successors, if any).
 *
 * IN NO EVENT SHALL "MLAB" OR "NICR" BE LIABLE TO ANY PARTY FOR DIRECT,
 * INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT OF
 * THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF "MLAB" OR
 * "NICR" HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * "MLAB" AND "NICR" SPECIFICALLY DISCLAIM ANY WARRANTIES, INCLUDING,
 * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
 * FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS
 * ON AN "AS IS" BASIS, AND "MLAB" AND "NICR" HAVE NO OBLIGATION TO
 * PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS OR MODIFICATIONS.
 */

#ifndef _MIRA_FRAMEWORKTRANSFORMER_H_
#define _MIRA_FRAMEWORKTRANSFORMER_H_

#include <transform/Pose.h>
#include <transform/Transformer.h>
#include <transform/TransformCast.h>
#include <math/NearestNeighborInterpolator.h>
#include <math/LinearInterpolator.h>

#include <factory/TypeId.h>

#include <fw/Channel.h>

namespace mira {


class FrameworkTransformer;
class Authority;


class FrameworkTransformerNode : public AbstractTransformerNode
{
    typedef AbstractTransformerNode Base;

public:
    enum Type {
        UNSPECIFIED = 0,
        MOVING,
        FIXED
    };

protected:
    friend class FrameworkTransformer;

    FrameworkTransformerNode(const std::string& id, Authority& authority) :
        Base(id), mType(UNSPECIFIED), mAuthority(authority) {}

public:

    virtual bool isEmpty() const;

    template<typename Transform, typename Filter>
    Transform getTransform(const Time& time, Filter&& filter);

    Type getType() const;
    void setType(Type type);

private:

    template<typename ChannelTransformType, typename Transform, typename Filter>
    bool getTransformData(const Time& time, Transform& oTransform, Filter&& filter);

private:

    Type mType;

    Authority& mAuthority;
    Channel<void> mChannel;
};

template<>
inline std::string toString<FrameworkTransformerNode::Type>(const FrameworkTransformerNode::Type& value,
                                                            int precision)
{
    switch (value) {
        case FrameworkTransformerNode::UNSPECIFIED: return "UNSPECIFIED";
        case FrameworkTransformerNode::MOVING:      return "MOVING";
        case FrameworkTransformerNode::FIXED:       return "FIXED";
        default: return "-INVALID VALUE-";
    }
}


template <typename Derived>
class IFrameworkTransformer
{
public:

    template <typename Reflector>
    void reflect(Reflector& r)
    {
        Derived* This = static_cast<Derived*>(this);

        r.interface("IFrameworkTransformer");
        r.method("addLink",
                 (void (Derived::*)(const std::string&, const std::string&))&Derived::addLink,
                 This,
                 "add a link between the two specified nodes",
                 "child", "child node frame ID", "/robot/ArmFrame",
                 "parent", "parent node frame ID", "/robot/RobotBaseFrame");

        r.method("getTransform2", &Derived::getTransform2, This,
                 "obtains the 2D transform between the two specified nodes",
                 "target", "target frame ID", "/robot/RobotFrame",
                 "source", "source frame ID", "/GlobalFrame");

        r.method("getTransformCov2", &Derived::getTransformCov2, This,
                 "obtains the 2D transform with covariance between the two specified nodes",
                 "target", "target frame ID", "/robot/RobotFrame",
                 "source", "source frame ID", "/GlobalFrame");

        r.method("getTransform3", &Derived::getTransform3, This,
                 "obtains the 3D transform between the two specified nodes",
                 "target", "target frame ID", "/robot/RobotFrame",
                 "source", "source frame ID", "/GlobalFrame");

        r.method("getTransformCov3", &Derived::getTransformCov3, This,
                 "obtains the 3D transform with covariance between the two specified nodes",
                 "target", "target frame ID", "/robot/RobotFrame",
                 "source", "source frame ID", "/GlobalFrame");

        r.method("publishTransform2", &Derived::publishTransform2, This,
                 "publishes the 2D transform of the specified node",
                 "nodeID", "ID of node to change", "/robot/HandFrame",
                 "transform", "transformation for the node", Pose2());

        r.method("publishTransformCov2", &Derived::publishTransformCov2, This,
                 "publishes the 2D transform with covariance of the specified node",
                 "nodeID", "ID of node to change", "/robot/HandFrame",
                 "transform", "transformation for the node", PoseCov2());

        r.method("publishTransform3", &Derived::publishTransform3, This,
                 "publishes the 3D transform of the specified node",
                 "nodeID", "ID of node to change", "/robot/HandFrame",
                 "transform", "transformation for the node", Pose3());

        r.method("publishTransformCov3", &Derived::publishTransformCov3, This,
                 "publishes the 3D transform with covariance of the specified node",
                 "nodeID", "ID of node to change", "/robot/HandFrame",
                 "transform", "transformation for the node", PoseCov3());
    }
};

template <typename Derived>
class IFrameworkTransformerLinkType : public IFrameworkTransformer<Derived>
{
public:

    typedef IFrameworkTransformer<Derived> Base;

    template <typename Reflector>
    void reflect(Reflector& r)
    {
        MIRA_REFLECT_BASE(r, Base);

        Derived* This = static_cast<Derived*>(this);

        r.interface("IFrameworkTransformerLinkType");
        r.method("addLink",
                 (void (Derived::*)(const std::string&, const std::string&,
                                    FrameworkTransformerNode::Type))&Derived::addLink,
                 This,
                 "add a link between the two specified nodes, with a specified type",
                 "child", "child node frame ID", "/robot/ArmFrame",
                 "parent", "parent node frame ID", "/robot/RobotBaseFrame",
                 "type", "Type of the node", FrameworkTransformerNode::MOVING);

        r.method("addTypedLink",
                 (void (Derived::*)(const std::string&, const std::string&,
                                    FrameworkTransformerNode::Type))&Derived::addLink,
                 This,
                 "alias for addLink(child, parent, type), provided for backward compatibility",
                 "child", "child node frame ID", "/robot/ArmFrame",
                 "parent", "parent node frame ID", "/robot/RobotBaseFrame",
                 "type", "Type of the node", FrameworkTransformerNode::MOVING);
    }
};


class FrameworkTransformer :
    public GenericTransformer<FrameworkTransformerNode>,
    public IFrameworkTransformerLinkType<FrameworkTransformer>
{

    typedef GenericTransformer<FrameworkTransformerNode> Base;

public:

    FrameworkTransformer(Authority& authority);

    ~FrameworkTransformer();

public:
    void addLink(const std::string& childID, const std::string& parentID);

    void addLink(const std::string& childID, const std::string& parentID,
                 FrameworkTransformerNode::Type type);

    template<typename Transform>
    void publishTransform(const std::string& nodeID, const Transform& transform,
                          const Time& time = Time::now() );

    template<typename Transform, typename Filter = NearestNeighborInterpolator>
    void publishTransformIndirect(const std::string& nodeID,
                                  const std::string& targetID,
                                  const std::string& sourceID,
                                  const Transform& transform,
                                  const Time& time = Time::now(),
                                  Filter&& filter = NearestNeighborInterpolator());

    void storeTransformTree(const Path& filename);

protected:

    void registerInterface(const std::string& interface,
                           const std::string& otherTransformer);

    bool createLink(const std::string& childID, const std::string& parentID,
                    FrameworkTransformerNode::Type type);

    void notifyTransformerOfLink(const std::string& otherTransformer,
                                 const std::string& childID, const std::string& parentID,
                                 FrameworkTransformerNode::Type type);

    void notifyTransformerOfLink(const std::string& otherTransformer,
                                 bool implementsLinkType,
                                 const std::string& childID, const std::string& parentID,
                                 FrameworkTransformerNode::Type type);

private:

    FrameworkTransformer::NodePtr createNode(const std::string& ID);

    void setNodeType(NodePtr node, FrameworkTransformerNode::Type type);

private:

    // for rpc calls only
    friend class IFrameworkTransformer<FrameworkTransformer>;
    RigidTransform2f getTransform2(const std::string& target,
                                   const std::string& source);
    RigidTransformCov2f getTransformCov2(const std::string& target,
                                         const std::string& source);
    RigidTransform3f getTransform3(const std::string& target,
                                   const std::string& source);
    RigidTransformCov3f getTransformCov3(const std::string& target,
                                         const std::string& source);
    void publishTransform2(const std::string& nodeID, const RigidTransform2f& transform);
    void publishTransformCov2(const std::string& nodeID, const RigidTransformCov2f& transform);
    void publishTransform3(const std::string& nodeID, const RigidTransform3f& transform);
    void publishTransformCov3(const std::string& nodeID, const RigidTransformCov3f& transform);

private:

    Authority& mAuthority;

    // lock node existence check + creation
    boost::mutex mMutex;
};


inline bool FrameworkTransformerNode::isEmpty() const
{
    return mChannel.isEmpty();
}

inline FrameworkTransformerNode::Type FrameworkTransformerNode::getType() const
{
    return mType;
}

inline void FrameworkTransformerNode::setType(FrameworkTransformerNode::Type type)
{
    mType = type;
}


// the general version
template <typename ChannelTransformType, typename Filter>
struct FrameworkTransformReadAccessor {
    static void get(Channel<void>& channel, const Time& time, ChannelTransformType& oTransform, Filter& filter) {
        Channel<ChannelTransformType> ch = channel_cast<ChannelTransformType>(channel);
        oTransform = ch.get(time, filter);
    }
};

// special version for NearestNeighborInterpolator (defined in C file for compile speed)
template <typename ChannelTransformType>
struct FrameworkTransformReadAccessor<ChannelTransformType, NearestNeighborInterpolator> {
    static void get(Channel<void>& channel, const Time& time, ChannelTransformType& oTransform, NearestNeighborInterpolator&);
};

// special version for LinearInterpolator (defined in C file for compile speed)
template <typename ChannelTransformType>
struct FrameworkTransformReadAccessor<ChannelTransformType, LinearInterpolator> {
    static void get(Channel<void>& channel, const Time& time, ChannelTransformType& oTransform, LinearInterpolator&);
};

// special version for LinearInterpolatorNoExtrapolation (defined in C file for compile speed)
template <typename ChannelTransformType>
struct FrameworkTransformReadAccessor<ChannelTransformType, LinearInterpolatorNoExtrapolation> {
    static void get(Channel<void>& channel, const Time& time, ChannelTransformType& oTransform, LinearInterpolatorNoExtrapolation&);
};

// special version for LinearInterpolatorNearestNeighbourExtrapolator (defined in C file for compile speed)
template <typename ChannelTransformType>
struct FrameworkTransformReadAccessor<ChannelTransformType, LinearInterpolatorNearestNeighbourExtrapolator> {
    static void get(Channel<void>& channel, const Time& time, ChannelTransformType& oTransform, LinearInterpolatorNearestNeighbourExtrapolator&);
};

// special version for LinearInterpolatorExtrapolationLimit (defined in C file for compile speed)
template <typename ChannelTransformType>
struct FrameworkTransformReadAccessor<ChannelTransformType, LinearInterpolatorExtrapolationLimit> {
    static void get(Channel<void>& channel, const Time& time, ChannelTransformType& oTransform, LinearInterpolatorExtrapolationLimit&);
};

// defined in C file for compile speed
template <typename Transform>
struct FrameworkTransformWriteAccessor {
    static void set(Authority& authority, const std::string& nodeID,
                    const Transform& transform, const Time& time);
};


template<typename Transform, typename Filter>
inline Transform FrameworkTransformerNode::getTransform(const Time& time,
                                                        Filter&& filter)
{
    Transform transform;

    // probe the type of the channel and perform a transform cast
    // from that type to our desired transform type. If none of the supported
    // transform types matches, throw an exception
    // TODO can we do this better somehow??
    if( !getTransformData<RigidTransform2f>(time,transform,filter) &&
        !getTransformData<RigidTransform3f>(time,transform,filter) &&
        !getTransformData<RigidTransformCov2f>(time,transform,filter) &&
        !getTransformData<RigidTransformCov3f>(time,transform,filter))
        MIRA_THROW(XRuntime, "Unknown transform type in channel '"
                   << mChannel.getID() << "', typename: " 
                   << mChannel.getTypename() << ", typeid: " << mChannel.getTypeId());
    return transform;
}

template<typename ChannelTransformType, typename Transform, typename Filter>
inline bool FrameworkTransformerNode::getTransformData(const Time& time,
                                                       Transform& oTransform,
                                                       Filter&& filter)
{
    int type = mChannel.getTypeId();
    if(type>=0) {
        // we have a type id, so use it for fast lookup
        if(type!=typeId<ChannelTransformType>())
            return false; // types do not match
        // else types match
    } else {
        // if we trap here, the channel is yet untyped, so we need to
        // use the typename of the channel to obtain the type
        if(mChannel.getTypename()!=typeName<ChannelTransformType>())
            return false; // typenames do not match
        // else typenames match
    }

    // if we reach here, the types match, so get the filtered transform
    ChannelTransformType transform;
    typedef typename boost::remove_reference<Filter>::type FilterType;
    FrameworkTransformReadAccessor<ChannelTransformType,FilterType>::get(mChannel, time, transform, filter);
    // ... and cast it to the desired type
    oTransform = transform_cast<Transform>(transform);

    return true;
}


template<typename Transform>
inline void FrameworkTransformer::publishTransform(const std::string& nodeID,
                                                   const Transform& transform,
                                                   const Time& time)
{
    FrameworkTransformWriteAccessor<Transform>::set(mAuthority,nodeID,transform,time);
}

template<typename Transform, typename Filter>
inline void FrameworkTransformer::publishTransformIndirect(const std::string& nodeID,
                                                           const std::string& targetID,
                                                           const std::string& sourceID,
                                                           const Transform& transform,
                                                           const Time& time,
                                                           Filter&& filter)
{
    NodePtr node   = this->getNode(nodeID);
    NodePtr target = this->getNode(targetID);
    NodePtr source = this->getNode(sourceID);
    if (!node)
        MIRA_THROW(XRuntime, "Node " << nodeID << " does not exist");
    if (!target)
        MIRA_THROW(XRuntime, "Target node " << targetID << " does not exist");
    if (!source)
        MIRA_THROW(XRuntime, "Source node " << sourceID << " does not exist");
    Transform inferredTransform = this->template inferTransform<Transform>(node,target,source,transform,time,filter);
    publishTransform(nodeID,inferredTransform, time);
}


} // namespace

#endif /* _MIRA_FRAMEWORKTRANSFORMER_H_ */