KinematicUnitObserver.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 "KinematicUnitObserver.h"
 
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/XML/RobotIO.h>
 
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/observers/checks/ConditionCheckEquals.h>
#include <ArmarXCore/observers/checks/ConditionCheckEqualsWithTolerance.h>
#include <ArmarXCore/observers/checks/ConditionCheckInRange.h>
#include <ArmarXCore/observers/checks/ConditionCheckLarger.h>
#include <ArmarXCore/observers/checks/ConditionCheckSmaller.h>
#include <ArmarXCore/observers/checks/ConditionCheckUpdated.h>
#include <ArmarXCore/observers/checks/ConditionCheckValid.h>
#include <ArmarXCore/observers/variant/ChannelRef.h>
 
#include "KinematicUnit.h"
 
namespace armarx
{
    // ********************************************************************
    // observer framework hooks
    // ********************************************************************
    void
    KinematicUnitObserver::onInitObserver()
    {
        robotNodeSetName = getProperty<std::string>("RobotNodeSetName").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(getProperty<std::string>("TopicPrefix").getValue() + robotNodeSetName + "State");
    }
 
    void
    KinematicUnitObserver::onConnectObserver()
    {
        // read names of kinematic chain elements belonging to this unit from XML and setup a map of all joints
        // the kinematic chain elements belonging to this unit are defined in a RobotNodeSet
        std::string robotFile = getProperty<std::string>("RobotFileName").getValue();
        std::string project = getProperty<std::string>("RobotFileNameProject").getValue();
        Ice::StringSeq includePaths;
 
        if (!project.empty())
        {
            CMakePackageFinder finder(project);
 
            auto pathsString = finder.getDataDir();
            Ice::StringSeq projectIncludePaths = simox::alg::split(pathsString, ";,");
            includePaths.insert(
                includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
        }
 
        if (!ArmarXDataPath::getAbsolutePath(robotFile, robotFile, includePaths))
        {
            throw UserException("Could not find robot file " + robotFile);
        }
 
        VirtualRobot::RobotPtr robot =
            VirtualRobot::RobotIO::loadRobot(robotFile, VirtualRobot::RobotIO::eStructure);
 
        if (robotNodeSetName == "")
        {
            throw UserException("RobotNodeSet not defined");
        }
 
        auto robotNodeSetPtr = robot->getRobotNodeSet(robotNodeSetName);
 
        std::vector<VirtualRobot::RobotNodePtr> robotNodes;
        robotNodes = robotNodeSetPtr->getAllRobotNodes();
        auto robotNodeNames = robotNodeSetPtr->getNodeNames();
        this->robotNodes = std::set<std::string>(robotNodeNames.begin(), robotNodeNames.end());
        // register all channels
        offerChannel("jointangles", "Joint values of the " + robotNodeSetName + " kinematic chain");
        offerChannel("jointvelocities",
                     "Joint velocities of the " + robotNodeSetName + " kinematic chain");
        offerChannel("jointaccelerations",
                     "Joint accelerations of the " + robotNodeSetName + " kinematic chain");
        offerChannel("jointtorques",
                     "Joint torques of the" + robotNodeSetName + " kinematic chain");
        offerChannel("jointcurrents",
                     "Joint currents of the " + robotNodeSetName + " kinematic chain");
        offerChannel("jointmotortemperatures",
                     "Joint motor temperatures of the " + robotNodeSetName + " kinematic chain");
        offerChannel("jointcontrolmodes",
                     "Joint motor temperatures of the " + robotNodeSetName + " kinematic chain");
 
 
        // register all data fields
        for (std::vector<VirtualRobot::RobotNodePtr>::iterator it = robotNodes.begin();
             it != robotNodes.end();
             it++)
        {
            std::string jointName = (*it)->getName();
            ARMARX_VERBOSE << "* " << jointName << std::endl;
            offerDataFieldWithDefault("jointcontrolmodes",
                                      jointName,
                                      ControlModeToString(eUnknown),
                                      "Controlmode of the " + jointName + " joint");
            offerDataField("jointangles",
                           jointName,
                           VariantType::Float,
                           "Joint angle of the " + jointName + " joint in radians");
            offerDataField("jointvelocities",
                           jointName,
                           VariantType::Float,
                           "Joint velocity of the " + jointName + " joint");
            offerDataField("jointaccelerations",
                           jointName,
                           VariantType::Float,
                           "Joint acceleration of the " + jointName + " joint");
            offerDataField("jointtorques",
                           jointName,
                           VariantType::Float,
                           "Joint torque of the " + jointName + " joint");
            offerDataField("jointcurrents",
                           jointName,
                           VariantType::Float,
                           "Joint current of the " + jointName + " joint");
            offerDataField("jointmotortemperatures",
                           jointName,
                           VariantType::Float,
                           "Joint motor temperature of the " + jointName + " joint");
        }
 
        updateChannel("jointcontrolmodes");
    }
 
    // ********************************************************************
    // KinematicUnitListener interface implementation
    // ********************************************************************
    void
    KinematicUnitObserver::reportControlModeChanged(const NameControlModeMap& jointModes,
                                                    Ice::Long timestamp,
                                                    bool aValueChanged,
                                                    const Ice::Current& c)
    {
        try
        {
            if (jointModes.size() == 0)
            {
                return;
            }
 
            for (auto elem : jointModes)
            {
                setDataFieldFlatCopy(
                    "jointcontrolmodes", elem.first, new Variant(ControlModeToString(elem.second)));
            }
 
            updateChannel("jointcontrolmodes");
        }
        catch (...)
        {
            handleExceptions();
        }
    }
 
    void
    KinematicUnitObserver::reportJointAngles(const NameValueMap& jointAngles,
                                             Ice::Long timestamp,
                                             bool aValueChanged,
                                             const Ice::Current& c)
    {
        try
        {
 
 
            if (jointAngles.size() == 0)
            {
                return;
            }
 
            nameValueMapToDataFields("jointangles", jointAngles, timestamp, aValueChanged);
 
 
            updateChannel("jointangles");
        }
        catch (...)
        {
            handleExceptions();
        }
    }
 
    void
    KinematicUnitObserver::reportJointVelocities(const NameValueMap& jointVelocities,
                                                 Ice::Long timestamp,
                                                 bool aValueChanged,
                                                 const Ice::Current& c)
    {
        try
        {
            if (jointVelocities.size() == 0)
            {
                return;
            }
 
 
            nameValueMapToDataFields("jointvelocities", jointVelocities, timestamp, aValueChanged);
 
            updateChannel("jointvelocities");
        }
        catch (...)
        {
            handleExceptions();
        }
    }
 
    void
    KinematicUnitObserver::reportJointTorques(const NameValueMap& jointTorques,
                                              Ice::Long timestamp,
                                              bool aValueChanged,
                                              const Ice::Current& c)
    {
        try
        {
            if (jointTorques.size() == 0)
            {
                return;
            }
 
 
            nameValueMapToDataFields("jointtorques", jointTorques, timestamp, aValueChanged);
 
            updateChannel("jointtorques");
        }
        catch (...)
        {
            handleExceptions();
        }
    }
 
    void
    KinematicUnitObserver::reportJointAccelerations(const NameValueMap& jointAccelerations,
                                                    Ice::Long timestamp,
                                                    bool aValueChanged,
                                                    const Ice::Current& c)
    {
        try
        {
            if (jointAccelerations.size() == 0)
            {
                return;
            }
 
 
            nameValueMapToDataFields(
                "jointaccelerations", jointAccelerations, timestamp, aValueChanged);
 
            updateChannel("jointaccelerations");
        }
        catch (...)
        {
            handleExceptions();
        }
    }
 
    void
    KinematicUnitObserver::reportJointCurrents(const NameValueMap& jointCurrents,
                                               Ice::Long timestamp,
                                               bool aValueChanged,
                                               const Ice::Current& c)
    {
        try
        {
            if (jointCurrents.size() == 0)
            {
                return;
            }
 
 
            nameValueMapToDataFields("jointcurrents", jointCurrents, timestamp, aValueChanged);
 
            updateChannel("jointcurrents");
        }
        catch (...)
        {
            handleExceptions();
        }
    }
 
    void
    KinematicUnitObserver::reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures,
                                                        Ice::Long timestamp,
                                                        bool aValueChanged,
                                                        const Ice::Current& c)
    {
        try
        {
            if (jointMotorTemperatures.size() == 0)
            {
                return;
            }
 
 
            nameValueMapToDataFields(
                "jointmotortemperatures", jointMotorTemperatures, timestamp, aValueChanged);
 
            updateChannel("jointmotortemperatures");
        }
        catch (...)
        {
            handleExceptions();
        }
    }
 
    void
    KinematicUnitObserver::reportJointStatuses(const NameStatusMap& jointStatuses,
                                               Ice::Long timestamp,
                                               bool aValueChanged,
                                               const Ice::Current& c)
    {
    }
 
    // ********************************************************************
    // private methods
    // ********************************************************************
    void
    KinematicUnitObserver::nameValueMapToDataFields(const std::string& channelName,
                                                    const NameValueMap& nameValueMap,
                                                    Ice::Long timestamp,
                                                    bool aValueChanged)
    {
        //    ARMARX_INFO << deactivateSpam(10) << " timestamp is " << (IceUtil::Time::now() - IceUtil::Time::microSeconds(timestamp)).toMicroSecondsDouble() << " µs old";
        bool newChannel;
        {
            std::unique_lock lock(initializedChannelsMutex);
            newChannel = initializedChannels.count(channelName) == 0;
            initializedChannels.insert(channelName);
        }
        if (aValueChanged || newChannel)
        {
 
            std::unordered_map<::std::string, ::armarx::VariantBasePtr> map;
            if (timestamp < 0)
            {
                for (const auto& it : nameValueMap)
                {
                    if (robotNodes.count(it.first))
                    {
                        map[it.first] = new Variant(it.second);
                    }
                }
            }
            else
            {
                for (const auto& it : nameValueMap)
                {
                    if (robotNodes.count(it.first))
                    {
                        map[it.first] = new TimedVariant(new Variant(it.second),
                                                         IceUtil::Time::microSeconds(timestamp));
                    }
                }
            }
            setDataFieldsFlatCopy(channelName, map);
        }
        else
        {
            updateDatafieldTimestamps(channelName, timestamp);
        }
    }
 
    PropertyDefinitionsPtr
    KinematicUnitObserver::createPropertyDefinitions()
    {
        return PropertyDefinitionsPtr(
            new KinematicUnitObserverPropertyDefinitions(getConfigIdentifier()));
    }
} // namespace armarx