PlatformUnitObserver.cpp

Go to the documentation of this file.

/*
* This file is part of ArmarX.
*
* ArmarX is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* ArmarX is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* @package    ArmarX::Core
* @author     Kai Welke (welke _at_ kit _dot_ edu)
* @date       2011 Kai Welke
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#include "PlatformUnitObserver.h"
 
#include <SimoxUtility/math/convert/mat3f_to_rpy.h>
 
#include <ArmarXCore/observers/checks/ConditionCheckEquals.h>
#include <ArmarXCore/observers/checks/ConditionCheckEqualsWithTolerance.h>
#include <ArmarXCore/observers/checks/ConditionCheckInRange.h>
#include <ArmarXCore/observers/checks/ConditionCheckLarger.h>
#include <ArmarXCore/observers/checks/ConditionCheckSmaller.h>
#include <ArmarXCore/observers/checks/ConditionCheckUpdated.h>
#include <ArmarXCore/observers/checks/ConditionCheckValid.h>
 
#include "PlatformUnit.h"
 
namespace armarx
{
    // ********************************************************************
    // observer framework hooks
    // ********************************************************************
    void
    PlatformUnitObserver::onInitObserver()
    {
        // TODO: check if platformNodeName exists?
        platformNodeName = getProperty<std::string>("PlatformName").getValue();
 
 
        // register all checks
        offerConditionCheck("valid", new ConditionCheckValid());
        offerConditionCheck("updated", new ConditionCheckUpdated());
        offerConditionCheck("equals", new ConditionCheckEquals());
        offerConditionCheck("inrange", new ConditionCheckInRange());
        offerConditionCheck("approx", new ConditionCheckEqualsWithTolerance());
        offerConditionCheck("larger", new ConditionCheckLarger());
        offerConditionCheck("smaller", new ConditionCheckSmaller());
 
        usingTopic(platformNodeName + "State");
        usingTopic("GlobalRobotPoseLocalization");
    }
 
    void
    PlatformUnitObserver::onConnectObserver()
    {
        // register all channels
        offerChannel("platformPose", "Current Position of " + platformNodeName);
        offerChannel("platformVelocity", "Current velocity of " + platformNodeName);
        offerChannel("platformOdometryPose", "Current Odometry Position of " + platformNodeName);
 
        // register all data fields
        offerDataField("platformPose",
                       "positionX",
                       VariantType::Float,
                       "Current X position of " + platformNodeName + " in mm");
        offerDataField("platformPose",
                       "positionY",
                       VariantType::Float,
                       "Current Y position of " + platformNodeName + " in mm");
        offerDataField("platformPose",
                       "rotation",
                       VariantType::Float,
                       "Current Rotation of " + platformNodeName + " in radian");
 
        offerDataField("platformVelocity",
                       "velocityX",
                       VariantType::Float,
                       "Current X velocity of " + platformNodeName + " in mm/s");
        offerDataField("platformVelocity",
                       "velocityY",
                       VariantType::Float,
                       "Current Y velocity of " + platformNodeName + " in mm/s");
        offerDataField("platformVelocity",
                       "velocityRotation",
                       VariantType::Float,
                       "Current Rotation velocity of " + platformNodeName + " in radian/s");
 
        offerDataField("platformOdometryPose",
                       "positionX",
                       VariantType::Float,
                       "Current Odometry X position of " + platformNodeName + " in mm");
        offerDataField("platformOdometryPose",
                       "positionY",
                       VariantType::Float,
                       "Current Odometry Y position of " + platformNodeName + " in mm");
        offerDataField("platformOdometryPose",
                       "rotation",
                       VariantType::Float,
                       "Current Odometry Rotation of " + platformNodeName + " in radian");
 
        // odometry pose is always zero in the beginning - set it  so that it can be queried
        reportPlatformOdometryPose(0, 0, 0, Ice::emptyCurrent);
    }
 
    void
    PlatformUnitObserver::reportGlobalRobotPose(const ::armarx::TransformStamped& transformStamped,
                                                const ::Ice::Current&)
    {
        const Eigen::Isometry3f global_T_robot(transformStamped.transform);
 
        const float x = global_T_robot.translation().x();
        const float y = global_T_robot.translation().y();
        const float rotationAroundZ = simox::math::mat3f_to_rpy(global_T_robot.linear()).z();
 
        nameValueMapToDataFields("platformPose", x, y, rotationAroundZ);
        updateChannel("platformPose");
    }
 
    void
    PlatformUnitObserver::reportPlatformVelocity(::Ice::Float currentPlatformVelocityX,
                                                 ::Ice::Float currentPlatformVelocityY,
                                                 ::Ice::Float currentPlatformVelocityRotation,
                                                 const Ice::Current& c)
    {
        setDataField("platformVelocity", "velocityX", currentPlatformVelocityX);
        setDataField("platformVelocity", "velocityY", currentPlatformVelocityY);
        setDataField("platformVelocity", "velocityRotation", currentPlatformVelocityRotation);
        updateChannel("platformVelocity");
    }
 
    // ********************************************************************
    // private methods
    // ********************************************************************
    void
    PlatformUnitObserver::nameValueMapToDataFields(std::string channelName,
                                                   ::Ice::Float platformPositionX,
                                                   ::Ice::Float platformPositionY,
                                                   ::Ice::Float platformRotation)
    {
        setDataField(channelName, "positionX", platformPositionX);
        setDataField(channelName, "positionY", platformPositionY);
        setDataField(channelName, "rotation", platformRotation);
    }
 
    PropertyDefinitionsPtr
    PlatformUnitObserver::createPropertyDefinitions()
    {
        return PropertyDefinitionsPtr(
            new PlatformUnitObserverPropertyDefinitions(getConfigIdentifier()));
    }
 
    void
    armarx::PlatformUnitObserver::reportPlatformOdometryPose(Ice::Float x,
                                                             Ice::Float y,
                                                             Ice::Float angle,
                                                             const Ice::Current&)
    {
        nameValueMapToDataFields("platformOdometryPose", x, y, angle);
        updateChannel("platformOdometryPose");
    }
 
 
} // namespace armarx