ForceTorqueObserver.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::
* @author     Mirko Waechter ( mirko.waechter at kit dot edu)
* @date       2013
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#pragma once
 
#include <RobotAPI/interface/units/ForceTorqueUnit.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <ArmarXCore/observers/Observer.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
 
#include <mutex>
 
namespace armarx
{
    /**
     * \class ForceTorqueObserverPropertyDefinitions
     * \brief
     */
    class ForceTorqueObserverPropertyDefinitions:
        public ObserverPropertyDefinitions
    {
    public:
        ForceTorqueObserverPropertyDefinitions(std::string prefix):
            ObserverPropertyDefinitions(prefix)
        {
            defineRequiredProperty<std::string>("ForceTorqueTopicName", "Name of the ForceTorqueUnit Topic");
            defineOptionalProperty<bool>("VisualizeForce", true, "Visualize the force with an arrow in the debug drawer");
            defineOptionalProperty<int>("RobotUpdateFrequency", 50, "Update frequency of the local robot");
            defineOptionalProperty<int>("VisualizeForceUpdateFrequency", 30, "Frequency with which the force is visualized");
            defineOptionalProperty<float>("ForceVisualizerFactor", 0.01f, "Factor by which the forces are scaled to fit into 0..1 (only for visulization) ");
            defineOptionalProperty<float>("MaxExpectedTorqueValue", 30, "The torque visualization circle reaches the full circle at this value");
            defineOptionalProperty<float>("TorqueVisuDeadZone", 1, "Torques below this threshold are not visualized.");
            defineOptionalProperty<float>("MaxForceArrowLength", 150, "Length of the force visu arrow in mm");
            defineOptionalProperty<std::string>("RobotStateComponentName", "RobotStateComponent", "Name of the RobotStateComponent that should be used");
            defineOptionalProperty<std::string>("SensorRobotNodeMapping", "", "Triplets of sensor node name, target frame robot node name and optional channel name: Sensor values are also reported in the frame of the robot node: e. g. SensorName:RobotNodeName[:ChannelName],SensorName2:RobotNodeName2[:ChannelName2]");
 
        }
    };
 
    /**
     * \class ForceTorqueObserver
     * \ingroup RobotAPI-SensorActorUnits-observers
     * \brief Observer monitoring Force/Torque values
     *
     * The ForceTorqueObserver monitors F/T values published by ForceTorqueUnit-implementations and offers condition checks on these values.
     * Available condition checks are: *updated*, *larger*, *equals*, *smaller* and *magnitudelarger*.
     */
    class ForceTorqueObserver :
        virtual public Observer,
        virtual public ForceTorqueUnitObserverInterface
    {
    public:
        ForceTorqueObserver();
 
        void setTopicName(std::string topicName);
 
        // framework hooks
        std::string getDefaultName() const override
        {
            return "ForceTorqueUnitObserver";
        }
        void onInitObserver() override;
        void onConnectObserver() override;
        void onExitObserver() override;
 
        void visualizerFunction();
 
        void reportSensorValues(const std::string& sensorNodeName, const FramedDirectionBasePtr& forces, const FramedDirectionBasePtr& torques, const Ice::Current&) override;
 
        /**
         * @see PropertyUser::createPropertyDefinitions()
         */
        PropertyDefinitionsPtr createPropertyDefinitions() override;
 
    private:
        void updateRobot();
 
        std::mutex dataMutex;
        std::string topicName;
        RobotStateComponentInterfacePrx robot;
        VirtualRobot::RobotPtr localRobot;
        DebugDrawerInterfacePrx debugDrawer;
        PeriodicTask<ForceTorqueObserver>::pointer_type visualizerTask;
        PeriodicTask<ForceTorqueObserver>::pointer_type updateRobotTask;
        // One sensor can be reported in multiple frames => multimap
        std::multimap<std::string, std::pair<std::string, std::string> > sensorRobotNodeMapping;
 
        void offerValue(const std::string& nodeName, const std::string& type, const FramedDirectionBasePtr& value, const DataFieldIdentifierPtr& id);
 
        // ForceTorqueUnitObserverInterface interface
    public:
        DatafieldRefBasePtr createNulledDatafield(const DatafieldRefBasePtr& forceTorqueDatafieldRef, const Ice::Current&) override;
 
        DatafieldRefBasePtr getForceDatafield(const std::string& nodeName, const Ice::Current&) override;
        DatafieldRefBasePtr getTorqueDatafield(const std::string& nodeName, const Ice::Current&) override;
 
        DataFieldIdentifierPtr getForceDatafieldId(const std::string& nodeName, const std::string& frame);
        DataFieldIdentifierPtr getTorqueDatafieldId(const std::string& nodeName, const std::string& frame);
 
 
        // ManagedIceObject interface
    protected:
        void onDisconnectComponent() override;
    };
}