RigidModelObserver.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_RIGIDMODELOBSERVER_H_
#define _MIRA_RIGIDMODELOBSERVER_H_

#include <transform/Pose.h>
#include <fw/Framework.h>
#include <model/RigidModel.h>

namespace mira { namespace model {


class RigidModelObserver : public Authority
{
public:

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    RigidModelObserver(Authority& parent, RigidModelPtr rigidModel) :
        Authority(parent, parent.getName()+"_ModelObserver", Authority::INTERNAL | Authority::ANONYMOUS),
        mRigidModel(rigidModel)
    {
        start();
        typedef std::pair<std::string, JointPtr> JointPair;
        foreach(const JointPair& joint, mRigidModel->joints)
        {
            // we are only interested in joints that can be changed
            if (joint.second->type == Joint::FIXED)
                continue;
            bool mustSubscribe = false;
            // test if we have a link that matches the joints child name
            foreach(const LinkPtr& link, mRigidModel->links)
                if (link->name == joint.second->child)
                {
                    // test if that link has a collision representation
                    if (!link->collisions.empty())
                        mustSubscribe = true;
                    break;
                }
            if (mustSubscribe)
            {
                Channel<Pose3> pose = subscribe<Pose3>(joint.second->child, &RigidModelObserver::collisionChanged);
                mLastStates[joint.second->child] = pose.get();
            }
        }
    }

    void registerCollisionChangedCallback(boost::function<void ()> cb)
    {
        mCallback = cb;
    }

protected:

    void collisionChanged(ChannelRead<Pose3> data)
    {
        if (mCallback)
        {
            // only call callback if transform has changed
            if (!data->value().isApprox(mLastStates[data.getChannelID()])) {
                mLastStates[data.getChannelID()] = data->value();
                mCallback();
            }
        } else
            mLastStates[data.getChannelID()] = data->value();
    }

    std::map<std::string, Pose3, std::less<std::string>,
             Eigen::aligned_allocator<std::pair<std::string, Pose3>>> mLastStates;
    RigidModelPtr mRigidModel;
    boost::function<void ()> mCallback;
};

typedef boost::shared_ptr<RigidModelObserver> RigidModelObserverPtr;


}} // namespace

#endif