SimpleEpisodicMemoryKinematicUnitConnector.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    RobotComponents::ArmarXObjects::SimpleEpisodicMemoryKinematicUnitConnector
 * @author     Fabian PK ( fabian dot peller-konrad at kit dot edu )
 * @date       2020
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "SimpleEpisodicMemoryKinematicUnitConnector.h"
 
namespace armarx
{
 
    armarx::PropertyDefinitionsPtr SimpleEpisodicMemoryKinematicUnitConnector::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr def{new armarx::ComponentPropertyDefinitions{getConfigIdentifier()}};
 
        def->topic<KinematicUnitListener>("RealRobotState", "KinematicUnitName");
        def->optional(frequency, "UpdateFrequency", "Frequency of updates in Hz");
        return def;
    }
 
    std::string SimpleEpisodicMemoryKinematicUnitConnector::getDefaultName() const
    {
        return "SimpleEpisodicMemoryKinematicUnitConnector";
    }
 
    SimpleEpisodicMemoryKinematicUnitConnector::SimpleEpisodicMemoryKinematicUnitConnector() :
        frequency(10),
        updated(false),
        timestampLastUpdateInMs(0)
    {
 
    }
 
 
    void SimpleEpisodicMemoryKinematicUnitConnector::onInitComponent()
    {
        usingProxy(m_simple_episodic_memory_proxy_name);
    }
 
 
    void SimpleEpisodicMemoryKinematicUnitConnector::onConnectComponent()
    {
        getProxy(m_simple_episodic_memory, m_simple_episodic_memory_proxy_name);
        periodic_task = new PeriodicTask<SimpleEpisodicMemoryKinematicUnitConnector>(this,
                &SimpleEpisodicMemoryKinematicUnitConnector::checkAndSendToMemory, (1.0f / frequency * 1000));
        periodic_task->start();
    }
 
 
    void SimpleEpisodicMemoryKinematicUnitConnector::onDisconnectComponent()
    {
        periodic_task->stop();
    }
 
 
    void SimpleEpisodicMemoryKinematicUnitConnector::onExitComponent()
    {
 
    }
 
    void SimpleEpisodicMemoryKinematicUnitConnector::checkAndSendToMemory()
    {
        std::lock_guard l(jointAngle_mutex);
        std::lock_guard ll(jointVelocity_mutex);
        std::lock_guard lll(jointTorque_mutex);
        std::lock_guard llll(jointAcceleration_mutex);
        std::lock_guard lllll(jointCurrent_mutex);
        std::lock_guard llllll(jointTemperature_mutex);
        std::lock_guard lllllll(jointEnabled_mutex);
 
        std::lock_guard u(updatedMutex);
        if (!updated || timestampLastUpdateInMs == 0)
        {
            return;
        }
 
        memoryx::KinematicUnitEvent event;
        event.receivedInMs = timestampLastUpdateInMs;
 
        for (const auto& [key, value] : jointAngleMap)
        {
            event.data[key].jointAngle = value;
        }
 
        for (const auto& [key, value] : jointVelocityMap)
        {
            event.data[key].jointVelocity = value;
        }
 
        for (const auto& [key, value] : jointTorqueMap)
        {
            event.data[key].jointTorque = value;
        }
 
        for (const auto& [key, value] : jointAccelerationMap)
        {
            event.data[key].jointAcceleration = value;
        }
 
        for (const auto& [key, value] : jointCurrentMap)
        {
            event.data[key].current = value;
        }
 
        for (const auto& [key, value] : jointTemperatureMap)
        {
            event.data[key].temperature = value;
        }
 
        for (const auto& [key, value] : jointEnabledMap)
        {
            event.data[key].enabled = value;
        }
 
        m_simple_episodic_memory->registerKinematicUnitEvent(event);
 
        updated = false;
        timestampLastUpdateInMs = 0;
 
    }
 
    void SimpleEpisodicMemoryKinematicUnitConnector::reportControlModeChanged(const NameControlModeMap&, Ice::Long, bool updated, const Ice::Current&)
    {
        //ARMARX_DEBUG << "Received reportControlModeChanged";
    }
    void SimpleEpisodicMemoryKinematicUnitConnector::reportJointAngles(const NameValueMap& data, Ice::Long ts, bool updated, const Ice::Current&)
    {
        std::lock_guard l(jointAngle_mutex);
        double received = IceUtil::Time::now().toMilliSecondsDouble();
        for (const auto& [key, value] : data)
        {
            jointAngleMap[key] = value;
        }
        if (updated)
        {
            std::lock_guard u(updatedMutex);
            this->updated = true;
            this->timestampLastUpdateInMs = received;
        }
    }
    void SimpleEpisodicMemoryKinematicUnitConnector::reportJointVelocities(const NameValueMap& data, Ice::Long, bool updated, const Ice::Current&)
    {
        std::lock_guard l(jointVelocity_mutex);
        double received = IceUtil::Time::now().toMilliSecondsDouble();
        for (const auto& [key, value] : data)
        {
            jointVelocityMap[key] = value;
        }
        if (updated)
        {
            std::lock_guard u(updatedMutex);
            this->updated = true;
            this->timestampLastUpdateInMs = received;
        }
    }
    void SimpleEpisodicMemoryKinematicUnitConnector::reportJointTorques(const NameValueMap& data, Ice::Long, bool updated, const Ice::Current&)
    {
        std::lock_guard l(jointTorque_mutex);
        double received = IceUtil::Time::now().toMilliSecondsDouble();
        for (const auto& [key, value] : data)
        {
            jointTorqueMap[key] = value;
        }
        if (updated)
        {
            std::lock_guard u(updatedMutex);
            this->updated = true;
            this->timestampLastUpdateInMs = received;
        }
    }
    void SimpleEpisodicMemoryKinematicUnitConnector::reportJointAccelerations(const NameValueMap& data, Ice::Long updated, bool, const Ice::Current&)
    {
        std::lock_guard l(jointAcceleration_mutex);
        double received = IceUtil::Time::now().toMilliSecondsDouble();
        for (const auto& [key, value] : data)
        {
            jointTorqueMap[key] = value;
        }
        if (updated)
        {
            std::lock_guard u(updatedMutex);
            this->updated = true;
            this->timestampLastUpdateInMs = received;
        }
    }
    void SimpleEpisodicMemoryKinematicUnitConnector::reportJointCurrents(const NameValueMap& data, Ice::Long, bool updated, const Ice::Current&)
    {
        std::lock_guard l(jointCurrent_mutex);
        double received = IceUtil::Time::now().toMilliSecondsDouble();
        for (const auto& [key, value] : data)
        {
            jointCurrentMap[key] = value;
        }
        if (updated)
        {
            std::lock_guard u(updatedMutex);
            this->updated = true;
            this->timestampLastUpdateInMs = received;
        }
    }
    void SimpleEpisodicMemoryKinematicUnitConnector::reportJointMotorTemperatures(const NameValueMap& data, Ice::Long, bool updated, const Ice::Current&)
    {
        std::lock_guard l(jointTemperature_mutex);
        double received = IceUtil::Time::now().toMilliSecondsDouble();
        for (const auto& [key, value] : data)
        {
            jointTemperatureMap[key] = value;
        }
        if (updated)
        {
            std::lock_guard u(updatedMutex);
            this->updated = true;
            this->timestampLastUpdateInMs = received;
        }
    }
    void SimpleEpisodicMemoryKinematicUnitConnector::reportJointStatuses(const NameStatusMap& data, Ice::Long, bool updated, const Ice::Current&)
    {
        std::lock_guard l(jointEnabled_mutex);
        double received = IceUtil::Time::now().toMilliSecondsDouble();
        for (const auto& [key, value] : data)
        {
            jointEnabledMap[key] = value.enabled;
        }
        if (updated)
        {
            std::lock_guard u(updatedMutex);
            this->updated = true;
            this->timestampLastUpdateInMs = received;
        }
    }
}