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 <Eigen/src/Geometry/Transform.h>
#include <SimoxUtility/math/convert/mat3f_to_rpy.h>
 
#include "PlatformUnit.h"
 
#include <ArmarXCore/observers/checks/ConditionCheckUpdated.h>
#include <ArmarXCore/observers/checks/ConditionCheckEquals.h>
#include <ArmarXCore/observers/checks/ConditionCheckInRange.h>
#include <ArmarXCore/observers/checks/ConditionCheckLarger.h>
#include <ArmarXCore/observers/checks/ConditionCheckSmaller.h>
#include <ArmarXCore/observers/checks/ConditionCheckValid.h>
#include <ArmarXCore/observers/checks/ConditionCheckEqualsWithTolerance.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");
    }
    
  
}