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 <mutex>
 
#include <ArmarXCore/observers/Observer.h>
 
#include <RobotAPI/interface/units/ForceTorqueUnit.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <RobotAPI/libraries/core/FramedPose.h>
 
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;
    };
} // namespace armarx