KinematicUnitObserver.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     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 <mutex>
#include <string>
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/application/properties/Properties.h>
#include <ArmarXCore/core/system/ImportExport.h>
#include <ArmarXCore/interface/observers/VariantBase.h>
#include <ArmarXCore/observers/ConditionCheck.h>
#include <ArmarXCore/observers/Observer.h>
 
#include <RobotAPI/interface/observers/KinematicUnitObserverInterface.h>
#include <RobotAPI/interface/units/KinematicUnitInterface.h>
 
namespace armarx
{
    ARMARX_CREATE_CHECK(KinematicUnitObserver, equals)
    ARMARX_CREATE_CHECK(KinematicUnitObserver, larger)
 
    class KinematicUnitObserverPropertyDefinitions : public ObserverPropertyDefinitions
    {
    public:
        KinematicUnitObserverPropertyDefinitions(std::string prefix) :
            ObserverPropertyDefinitions(prefix)
        {
            defineRequiredProperty<std::string>(
                "RobotNodeSetName",
                "Robot node set name as defined in robot xml file, e.g. 'LeftArm'");
            defineRequiredProperty<std::string>("RobotFileName",
                                                "Robot file name, e.g. robot_model.xml");
            defineOptionalProperty<std::string>("RobotFileNameProject",
                                                "",
                                                "Project in which the robot filename is located "
                                                "(if robot is loaded from an external project)");
            defineOptionalProperty<std::string>(
                "TopicPrefix", "", "Prefix for the sensor value topic name.");
        }
    };
 
    /**
     * \class KinematicUnitObserver
     * \ingroup RobotAPI-SensorActorUnits-observers
     * \brief Observer monitoring kinematic sensor and actor values
     *
     * The KinematicUnitObserver allows to install conditions on all channel reported by the KinematicUnit.
     * These include joint angles, velocities, torques and motor temperatures
     *
     * The KinematicUnitObserver retrieves its configuration from a VirtualRobot robot model. Within the model, the joints
     * which are observer by the unit are define by a robotnodeset.
     *
     * Available condition checks are: *valid*, *updated*, *equals*, *inrange*, *approx*, *larger* and *smaller*.
     */
    class ARMARXCORE_IMPORT_EXPORT KinematicUnitObserver :
        virtual public Observer,
        virtual public KinematicUnitObserverInterface
    {
    public:
        // framework hooks
        void onInitObserver() override;
        void onConnectObserver() override;
 
 
        // slice interface implementation
        void reportControlModeChanged(const NameControlModeMap& jointModes,
                                      Ice::Long timestamp,
                                      bool aValueChanged,
                                      const Ice::Current& c = Ice::emptyCurrent) override;
        void reportJointAngles(const NameValueMap& jointAngles,
                               Ice::Long timestamp,
                               bool aValueChanged,
                               const Ice::Current& c = Ice::emptyCurrent) override;
        void reportJointVelocities(const NameValueMap& jointVelocities,
                                   Ice::Long timestamp,
                                   bool aValueChanged,
                                   const Ice::Current& c = Ice::emptyCurrent) override;
        void reportJointTorques(const NameValueMap& jointTorques,
                                Ice::Long timestamp,
                                bool aValueChanged,
                                const Ice::Current& c = Ice::emptyCurrent) override;
        void reportJointAccelerations(const NameValueMap& jointAccelerations,
                                      Ice::Long timestamp,
                                      bool aValueChanged,
                                      const Ice::Current& c) override;
        void reportJointCurrents(const NameValueMap& jointCurrents,
                                 Ice::Long timestamp,
                                 bool aValueChanged,
                                 const Ice::Current& c = Ice::emptyCurrent) override;
        void reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures,
                                          Ice::Long timestamp,
                                          bool aValueChanged,
                                          const Ice::Current& c = Ice::emptyCurrent) override;
        void reportJointStatuses(const NameStatusMap& jointStatuses,
                                 Ice::Long timestamp,
                                 bool aValueChanged,
                                 const Ice::Current& c = Ice::emptyCurrent) override;
 
        std::string
        getDefaultName() const override
        {
            return "KinematicUnitObserver";
        }
 
        /**
         * @see PropertyUser::createPropertyDefinitions()
         */
        PropertyDefinitionsPtr createPropertyDefinitions() override;
 
        static std::string
        ControlModeToString(ControlMode mode)
        {
            switch (mode)
            {
                case eDisabled:
                    return "Disabled";
 
                case ePositionControl:
                    return "PositionControl";
 
                case eVelocityControl:
                    return "VelocityControl";
 
                case eTorqueControl:
                    return "TorqueControl";
 
                case ePositionVelocityControl:
                    return "PositionVelocityControl";
 
                case eUnknown:
                default:
                    return "Unknown";
            }
        }
 
        static ControlMode
        StringToControlMode(const std::string& mode)
        {
            if (mode == "Disabled")
            {
                return eDisabled;
            }
 
            if (mode == "PositionControl")
            {
                return ePositionControl;
            }
 
            if (mode == "VelocityControl")
            {
                return eVelocityControl;
            }
 
            if (mode == "TorqueControl")
            {
                return eTorqueControl;
            }
 
            if (mode == "PositionVelocityControl")
            {
                return ePositionVelocityControl;
            }
 
            return eUnknown;
        }
 
    protected:
        void nameValueMapToDataFields(const std::string& channelName,
                                      const NameValueMap& nameValueMap,
                                      Ice::Long timestamp,
                                      bool aValueChanged);
        std::set<std::string> initializedChannels;
        std::mutex initializedChannelsMutex;
 
    private:
        std::string robotNodeSetName;
        std::set<std::string> robotNodes;
    };
 
    /**
          @class KinematicUnitDatafieldCreator
          @brief
          @ingroup RobotAPI-SensorActorUnits-util
         */
    class KinematicUnitDatafieldCreator
    {
    public:
        KinematicUnitDatafieldCreator(const std::string& channelName) : _channelName(channelName)
        {
        }
 
        /**
             * @brief Function to create a Channel Representation for a KinematicUnitObserver
             * @param kinematicUnitOberserverName Name of the KinematicUnitObserver
             * @param jointName Name of a joint of the robot like it is specified
             * in the simox-robot-xml-file
             * @return returns a ChannelRepresentationPtr
             */
        DataFieldIdentifier
        getDatafield(const std::string& kinematicUnitObserverName,
                     const std::string& jointName) const
        {
            if (kinematicUnitObserverName.empty())
            {
                throw LocalException("kinematicUnitObserverName must not be empty!");
            }
 
            if (jointName.empty())
            {
                throw LocalException("jointName must not be empty!");
            }
 
            return DataFieldIdentifier(kinematicUnitObserverName, _channelName, jointName);
        }
 
    private:
        std::string _channelName;
    };
} // namespace armarx
 
namespace armarx::channels::KinematicUnitObserver
{
    const KinematicUnitDatafieldCreator jointAngles("jointAngles");
    const KinematicUnitDatafieldCreator jointVelocities("jointVelocities");
    const KinematicUnitDatafieldCreator jointTorques("jointTorques");
    const KinematicUnitDatafieldCreator jointCurrents("jointCurrents");
    const KinematicUnitDatafieldCreator jointTemperatures("jointTemperatures");
} // namespace armarx::channels::KinematicUnitObserver