SimpleEpisodicMemoryPlatformUnitConnector.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::SimpleEpisodicMemoryPlatformUnitConnector
 * @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 "SimpleEpisodicMemoryPlatformUnitConnector.h"
 
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
namespace armarx
{
 
    armarx::PropertyDefinitionsPtr
    SimpleEpisodicMemoryPlatformUnitConnector::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr def{
            new armarx::ComponentPropertyDefinitions{getConfigIdentifier()}};
 
        def->topic<PlatformUnitListener>("Armar6PlatformUnit", "PlatformUnitName");
        def->optional(frequency, "UpdateFrequency", "Frequency of updates in Hz");
        return def;
    }
 
    std::string
    SimpleEpisodicMemoryPlatformUnitConnector::getDefaultName() const
    {
        return "SimpleEpisodicMemoryPlatformUnitConnector";
    }
 
    SimpleEpisodicMemoryPlatformUnitConnector::SimpleEpisodicMemoryPlatformUnitConnector() :
        frequency(10),
        updated_pose(false),
        timestampLastUpdateInMs_pose(0),
        updated_target(false),
        timestampLastUpdateInMs_target(0)
    {
    }
 
    void
    SimpleEpisodicMemoryPlatformUnitConnector::onInitComponent()
    {
        usingProxy(m_simple_episodic_memory_proxy_name);
    }
 
    void
    SimpleEpisodicMemoryPlatformUnitConnector::onConnectComponent()
    {
        getProxy(m_simple_episodic_memory, m_simple_episodic_memory_proxy_name);
        periodic_task = new PeriodicTask<SimpleEpisodicMemoryPlatformUnitConnector>(
            this,
            &SimpleEpisodicMemoryPlatformUnitConnector::checkAndSendToMemory,
            (1.0f / frequency * 1000));
        periodic_task->start();
    }
 
    void
    SimpleEpisodicMemoryPlatformUnitConnector::onDisconnectComponent()
    {
        periodic_task->stop();
    }
 
    void
    SimpleEpisodicMemoryPlatformUnitConnector::onExitComponent()
    {
    }
 
    void
    SimpleEpisodicMemoryPlatformUnitConnector::checkAndSendToMemory()
    {
        std::lock_guard l(platformPose_mutex);
        std::lock_guard ll(platformTarget_mutex);
        std::lock_guard lll(platformAcceleration_mutex);
 
        {
            std::lock_guard u(updatedMutex_pose);
            if (updated_pose && timestampLastUpdateInMs_pose != 0)
            {
                memoryx::PlatformUnitEvent event;
                event.receivedInMs = timestampLastUpdateInMs_pose;
 
                event.x = x;
                event.y = y;
                event.rot = rot;
                event.acc_x = acc_x;
                event.acc_y = acc_y;
                event.acc_rot = acc_rot;
 
                m_simple_episodic_memory->registerPlatformUnitEvent(event);
 
                updated_pose = false;
                timestampLastUpdateInMs_pose = 0;
            }
        }
 
        {
            std::lock_guard u(updatedMutex_target);
            if (updated_target && timestampLastUpdateInMs_target != 0)
            {
                memoryx::PlatformUnitTargetEvent event;
                event.receivedInMs = timestampLastUpdateInMs_target;
 
                event.target_x = goal_x;
                event.target_y = goal_y;
                event.target_rot = goal_rot;
 
                m_simple_episodic_memory->registerPlatformUnitTargetEvent(event);
 
                updated_target = false;
                timestampLastUpdateInMs_target = 0;
            }
        }
    }
 
    void
    SimpleEpisodicMemoryPlatformUnitConnector::reportPlatformPose(const PlatformPose& pose,
                                                                  const Ice::Current&)
    {
        std::lock_guard l(platformPose_mutex);
        std::lock_guard u(updatedMutex_pose);
        double received = IceUtil::Time::now().toMilliSecondsDouble();
        x = pose.x;
        y = pose.y;
        rot = pose.rotationAroundZ;
        updated_pose = true;
        timestampLastUpdateInMs_pose = received;
    }
 
    void
    SimpleEpisodicMemoryPlatformUnitConnector::reportNewTargetPose(Ice::Float t1,
                                                                   Ice::Float t2,
                                                                   Ice::Float t3,
                                                                   const Ice::Current&)
    {
        std::lock_guard l(platformTarget_mutex);
        std::lock_guard u(updatedMutex_target);
        double received = IceUtil::Time::now().toMilliSecondsDouble();
        goal_x = t1;
        goal_y = t2;
        goal_rot = t3;
        updated_target = true;
        timestampLastUpdateInMs_target = received;
    }
 
    void
    SimpleEpisodicMemoryPlatformUnitConnector::reportPlatformVelocity(Ice::Float v1,
                                                                      Ice::Float v2,
                                                                      Ice::Float v3,
                                                                      const Ice::Current&)
    {
        std::lock_guard l(platformAcceleration_mutex);
        std::lock_guard u(updatedMutex_pose);
        double received = IceUtil::Time::now().toMilliSecondsDouble();
        acc_x = v1;
        acc_y = v2;
        acc_rot = v3;
        updated_pose = true;
        timestampLastUpdateInMs_pose = received;
    }
 
    void
    SimpleEpisodicMemoryPlatformUnitConnector::reportPlatformOdometryPose(Ice::Float,
                                                                          Ice::Float,
                                                                          Ice::Float,
                                                                          const Ice::Current&)
    {
    }
 
    ARMARX_REGISTER_COMPONENT_EXECUTABLE(SimpleEpisodicMemoryPlatformUnitConnector, "SimpleEpisodicMemoryPlatformUnitConnector");
} // namespace armarx