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 "KinematicUnit.h"
#include <ArmarXCore/observers/checks/ConditionCheckValid.h>
#include <ArmarXCore/observers/checks/ConditionCheckUpdated.h>
#include <ArmarXCore/observers/checks/ConditionCheckEquals.h>
#include <ArmarXCore/observers/checks/ConditionCheckInRange.h>
#include <ArmarXCore/observers/checks/ConditionCheckLarger.h>
#include <ArmarXCore/observers/checks/ConditionCheckSmaller.h>
#include <ArmarXCore/observers/variant/ChannelRef.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
 
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <ArmarXCore/observers/checks/ConditionCheckEqualsWithTolerance.h>
 
#include <SimoxUtility/algorithm/string/string_tools.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()));
    }
}