RobotStateObserver.h

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     Stefan Ulbrich <stefan dot ulbrich at kit dot edu>, 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
*/
 
#pragma once
 
#include <ArmarXCore/core/system/ImportExport.h>
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/interface/observers/VariantBase.h>
#include <RobotAPI/interface/units/KinematicUnitInterface.h>
#include <RobotAPI/interface/core/RobotStateObserverInterface.h>
#include <RobotAPI/interface/core/RobotState.h>
#include <ArmarXCore/observers/ConditionCheck.h>
#include <ArmarXCore/observers/Observer.h>
 
#include <VirtualRobot/VirtualRobot.h>
 
#include <string>
 
namespace armarx
{
 
    /**
     * RobotStatePropertyDefinition Property Definitions
     */
    class RobotStateObserverPropertyDefinitions:
        public ObserverPropertyDefinitions
    {
    public:
        RobotStateObserverPropertyDefinitions(std::string prefix):
            ObserverPropertyDefinitions(prefix)
        {
            defineOptionalProperty<std::string>("TCPsToReport", "", "comma seperated list of nodesets' endeffectors, which poses and velocities that should be reported. * for all, empty for none");
        }
    };
 
    using FramedPoseBaseMap = ::std::map< ::std::string, ::armarx::FramedPoseBasePtr>;
 
    /**
     * ArmarX RobotStateObserver.
     *
     * The RobotStateObserver allows to install conditions on all channel reported by the KinematicUnit.
     * These include joint angles, velocities, torques and motor temperatures
     *
     * The RobotStateObserver retrieves its configuration from a VirtualRobot robot model. Within the model, the joints
     * which are observer by the unit are define by a robotnodeset
     */
    class ARMARXCORE_IMPORT_EXPORT RobotStateObserver :
        virtual public Observer,
        virtual public RobotStateObserverInterface
    {
    public:
        RobotStateObserver();
        // framework hooks
        void onInitObserver() override;
        void onConnectObserver() override;
 
        PropertyDefinitionsPtr createPropertyDefinitions() override;
 
        void setRobot(VirtualRobot::RobotPtr robot);
 
        std::string getDefaultName() const override
        {
            return "RobotStateObserver";
        }
 
        void updatePoses();
        void updateNodeVelocities(const NameValueMap& jointVel, long timestampMicroSeconds);
    protected:
 
        void updateVelocityDatafields(const FramedDirectionMap& tcpTranslationVelocities, const FramedDirectionMap& tcpOrientationVelocities);
 
        void udpatePoseDatafields(const FramedPoseBaseMap& poseMap);
    private:
        std::string robotNodeSetName;
        RobotStateComponentInterfacePrx server;
        VirtualRobot::RobotPtr  robot, velocityReportRobot;
        std::vector<std::pair<VirtualRobot::RobotNodePtr, std::string> > nodesToReport;
        std::recursive_mutex dataMutex;
        IceUtil::Time lastVelocityUpdate;
 
        // RobotStateObserverInterface interface
    public:
        void getPoseDatafield_async(
            const AMD_RobotStateObserverInterface_getPoseDatafieldPtr& amd,
            const std::string& nodeName,
            const Ice::Current&) const override;
    };
 
    using RobotStateObserverPtr = IceInternal::Handle<RobotStateObserver>;
}