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;
        }
    }
} // namespace armarx