LegacyRobotStateMemoryAdapter.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    RobotAPI::ArmarXObjects::MemoryNameSystem
 * @author     Rainer Kartmann ( rainer dot kartmann at kit dot edu )
 * @date       2020
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "LegacyRobotStateMemoryAdapter.h"
 
#include <SimoxUtility/math/convert/mat3f_to_rpy.h>
 
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/RobotDescription.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
 
namespace armarx::armem
{
 
    armarx::PropertyDefinitionsPtr
    LegacyRobotStateMemoryAdapter::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr defs =
            new ComponentPropertyDefinitions(getConfigIdentifier());
 
        std::string prefix = "mem.";
        defs->optional(properties.frequency,
                       prefix + "updateFrequency",
                       "The frequency in Hz to check for updates and send them to the memory.");
        defs->optional(properties.memoryNameSystemName,
                       prefix + "memoryNameSystemName",
                       "The name of the MemoryNameSystem.");
        defs->optional(properties.robotStateMemoryName,
                       prefix + "memoryName",
                       "The name of the RobotStateMemory.");
 
        prefix = "listener.";
        defs->topic<KinematicUnitListener>("RealRobotState", prefix + "KinematicUnitName");
        defs->topic<PlatformUnitListener>("Armar6PlatformUnit", prefix + "PlatformUnitName");
        return defs;
    }
 
    std::string
    LegacyRobotStateMemoryAdapter::getDefaultName() const
    {
        return "LegacyRobotStateMemoryAdapter";
    }
 
    void
    LegacyRobotStateMemoryAdapter::checkUpdateAndSendToMemory()
    {
        std::lock_guard l(updateMutex);
        if (updateChanged)
        {
            // If the update is not changed it has probably been committed already.
            ARMARX_INFO << deactivateSpam()
                        << "Try to send data to robotStateMemory but data has not changed.";
            return;
        }
 
        // convert the update into a commit and send to memory
        arondto::Proprioception prop;
        for (const auto& [k, v] : update.jointAngles)
        {
            prop.joints.position[k] = v;
        }
        for (const auto& [k, v] : update.jointAccelerations)
        {
            prop.joints.acceleration[k] = v;
        }
        for (const auto& [k, v] : update.jointCurrents)
        {
            prop.joints.motorCurrent[k] = v;
        }
        for (const auto& [k, v] : update.jointTorques)
        {
            prop.joints.torque[k] = v;
        }
        for (const auto& [k, v] : update.jointVelocities)
        {
            prop.joints.velocity[k] = v;
        }
 
        // is this corect??
        prop.platform.acceleration = Eigen::Vector3f::Zero();
        prop.platform.relativePosition =
            Eigen::Vector3f(update.platformPose.x, // this should be globasl AFAIK
                            update.platformPose.y,
                            update.platformPose.rotationAroundZ);
        prop.platform.velocity = Eigen::Vector3f(std::get<0>(update.platformVelocity),
                                                 std::get<1>(update.platformVelocity),
                                                 std::get<2>(update.platformVelocity));
 
        armem::EntityUpdate entityUpdate;
        entityUpdate.entityID = propEntityID;
        entityUpdate.referencedTime = Time(Duration::MicroSeconds(
            _timestampUpdateFirstModifiedInUs)); // we take the oldest timestamp
 
        entityUpdate.instancesData = {prop.toAron()};
 
        ARMARX_DEBUG << "Committing " << entityUpdate;
        armem::EntityUpdateResult updateResult = memoryWriter.commit(entityUpdate);
        ARMARX_DEBUG << updateResult;
        if (!updateResult.success)
        {
            ARMARX_ERROR << updateResult.errorMessage;
        }
 
        // store odometry pose in localization segment
        {
            armem::arondto::Transform transform;
            transform.header.agent = "Armar3";
            transform.header.parentFrame = OdometryFrame;
            transform.header.frame = armarx::armem::robot_state::constants::robotRootNodeName;
            transform.header.timestamp =
                armem::Time(armem::Duration::MicroSeconds(_timestampUpdateFirstModifiedInUs));
 
            Eigen::Isometry3f tf = Eigen::Isometry3f::Identity();
            tf.translation().x() = (update.platformPose.x);
            tf.translation().y() = (update.platformPose.y);
            tf.linear() =
                Eigen::AngleAxisf((update.platformPose.rotationAroundZ), Eigen::Vector3f::UnitZ())
                    .toRotationMatrix();
 
            transform.transform = tf.matrix();
 
            armem::EntityUpdate locUpdate;
            locUpdate.entityID = locEntityID;
            locUpdate.referencedTime =
                Time(Duration::MicroSeconds(_timestampUpdateFirstModifiedInUs));
            locUpdate.instancesData = {transform.toAron()};
 
            ARMARX_DEBUG << "Committing " << entityUpdate;
            armem::EntityUpdateResult updateResult = memoryWriter.commit(locUpdate);
            ARMARX_DEBUG << updateResult;
            if (!updateResult.success)
            {
                ARMARX_ERROR << updateResult.errorMessage;
            }
        }
 
 
        // reset update
        updateChanged = true;
    }
 
    void
    LegacyRobotStateMemoryAdapter::updateTimestamps(long ts)
    {
        if (updateChanged)
        {
            _timestampUpdateFirstModifiedInUs = IceUtil::Time::now().toMicroSeconds();
        }
        updateChanged = false;
    }
 
    void
    LegacyRobotStateMemoryAdapter::reportControlModeChanged(const NameControlModeMap&,
                                                            Ice::Long,
                                                            bool,
                                                            const Ice::Current&)
    {
    }
 
    void
    LegacyRobotStateMemoryAdapter::reportJointAngles(const NameValueMap& m,
                                                     Ice::Long ts,
                                                     bool,
                                                     const Ice::Current&)
    {
        ARMARX_DEBUG << "Got an update for joint angles";
        std::lock_guard l(updateMutex);
        update.jointAngles = m;
        updateTimestamps(ts);
    }
 
    void
    LegacyRobotStateMemoryAdapter::reportJointVelocities(const NameValueMap& m,
                                                         Ice::Long ts,
                                                         bool,
                                                         const Ice::Current&)
    {
        ARMARX_DEBUG << "Got an update for joint vels";
        std::lock_guard l(updateMutex);
        update.jointVelocities = m;
        updateTimestamps(ts);
    }
 
    void
    LegacyRobotStateMemoryAdapter::reportJointTorques(const NameValueMap& m,
                                                      Ice::Long ts,
                                                      bool,
                                                      const Ice::Current&)
    {
        ARMARX_DEBUG << "Got an update for joint torques";
        std::lock_guard l(updateMutex);
        update.jointTorques = m;
        updateTimestamps(ts);
    }
 
    void
    LegacyRobotStateMemoryAdapter::reportJointAccelerations(const NameValueMap& m,
                                                            Ice::Long ts,
                                                            bool,
                                                            const Ice::Current&)
    {
        ARMARX_DEBUG << "Got an update for joint accels";
        std::lock_guard l(updateMutex);
        update.jointAccelerations = m;
        updateTimestamps(ts);
    }
 
    void
    LegacyRobotStateMemoryAdapter::reportJointCurrents(const NameValueMap& m,
                                                       Ice::Long ts,
                                                       bool,
                                                       const Ice::Current&)
    {
        ARMARX_DEBUG << "Got an update for joint currents";
        std::lock_guard l(updateMutex);
        update.jointCurrents = m;
        updateTimestamps(ts);
    }
 
    void
    LegacyRobotStateMemoryAdapter::reportJointMotorTemperatures(const NameValueMap&,
                                                                Ice::Long,
                                                                bool,
                                                                const Ice::Current&)
    {
    }
 
    void
    LegacyRobotStateMemoryAdapter::reportJointStatuses(const NameStatusMap&,
                                                       Ice::Long,
                                                       bool,
                                                       const Ice::Current&)
    {
    }
 
    // void LegacyRobotStateMemoryAdapter::reportPlatformPose(const PlatformPose& p, const Ice::Current &)
    // {
    //     ARMARX_DEBUG << "Got an update for platform pose";
    //     std::lock_guard l(updateMutex);
    //     update.platformPose = p;
    //     updateTimestamps(p.timestampInMicroSeconds);
    // }
 
    void
    LegacyRobotStateMemoryAdapter::reportGlobalRobotPose(
        const ::armarx::TransformStamped& transformStamped,
        const ::Ice::Current&)
    {
 
        ARMARX_DEBUG << "Got an update for platform pose";
 
        const Eigen::Isometry3f global_T_robot(transformStamped.transform);
        const float yaw = simox::math::mat3f_to_rpy(global_T_robot.linear()).z();
 
        armarx::PlatformPose p;
        p.x = global_T_robot.translation().x();
        p.y = global_T_robot.translation().y();
        p.rotationAroundZ = yaw;
        p.timestampInMicroSeconds = transformStamped.header.timestampInMicroSeconds;
 
        {
            std::lock_guard l(updateMutex);
            update.platformPose = p;
            updateTimestamps(p.timestampInMicroSeconds);
        }
    }
 
    void
    LegacyRobotStateMemoryAdapter::reportPlatformVelocity(Ice::Float f1,
                                                          Ice::Float f2,
                                                          Ice::Float f3,
                                                          const Ice::Current&)
    {
        ARMARX_DEBUG << "Got an update for platform vels";
        auto now = IceUtil::Time::now().toMicroSeconds();
 
        std::lock_guard l(updateMutex);
        update.platformVelocity = {f1, f2, f3};
        updateTimestamps(now);
    }
 
    void
    LegacyRobotStateMemoryAdapter::reportPlatformOdometryPose(Ice::Float f1,
                                                              Ice::Float f2,
                                                              Ice::Float f3,
                                                              const Ice::Current&)
    {
        ARMARX_DEBUG << "Got an update for platform odom pose";
        auto now = IceUtil::Time::now().toMicroSeconds();
 
        std::lock_guard l(updateMutex);
        update.platformOdometryPose = {f1, f2, f3};
        updateTimestamps(now);
    }
 
    void
    LegacyRobotStateMemoryAdapter::commitArmar3RobotDescription()
    {
        ARMARX_IMPORTANT << "Commiting Armar3 to descriptions";
        armem::arondto::RobotDescription desc;
        desc.name = "Armar3";
        desc.timestamp = armem::Time::Now();
        desc.xml.package = "RobotAPI";
        desc.xml.path = "robots/Armar3/ArmarIII.xml";
 
        armem::Commit c;
        auto& entityUpdate = c.add();
 
        entityUpdate.confidence = 1.0;
        entityUpdate.entityID.memoryName = armarx::armem::robot_state::constants::memoryName;
        entityUpdate.entityID.coreSegmentName =
            armarx::armem::robot_state::constants::descriptionCoreSegment;
        entityUpdate.entityID.providerSegmentName = "Armar3";
        entityUpdate.entityID.entityName = "Armar3";
 
        entityUpdate.instancesData = {desc.toAron()};
        entityUpdate.referencedTime = armem::Time::Now();
        auto res = memoryWriter.commit(c);
        if (!res.allSuccess())
        {
            ARMARX_WARNING << "Could not send data to memory." << res.allErrorMessages();
        }
    }
 
    void
    LegacyRobotStateMemoryAdapter::onInitComponent()
    {
        usingProxy(properties.memoryNameSystemName);
 
        const int minFrequency = 1;
        const int maxFrequency = 100;
        properties.frequency = std::clamp(properties.frequency, minFrequency, maxFrequency);
        const int fInMS = (1000 / properties.frequency);
 
        // create running task and run method checkUpdateAndSendToMemory in a loop
        runningTask = new PeriodicTask<LegacyRobotStateMemoryAdapter>(
            this, &LegacyRobotStateMemoryAdapter::checkUpdateAndSendToMemory, fInMS);
    }
 
    void
    LegacyRobotStateMemoryAdapter::onConnectComponent()
    {
        auto mns = client::MemoryNameSystem(
            getProxy<mns::MemoryNameSystemInterfacePrx>(properties.memoryNameSystemName), this);
 
        // Wait for the memory to become available and add it as dependency.
        ARMARX_IMPORTANT << "Waiting for memory '" << properties.robotStateMemoryName << "' ...";
        propEntityID.memoryName = properties.robotStateMemoryName;
        try
        {
            memoryWriter = mns.useWriter(properties.robotStateMemoryName);
        }
        catch (const armem::error::ArMemError& e)
        {
            ARMARX_ERROR << e.what();
            return;
        }
 
        // update RobotDescription (since there will be no robot unit, we have to submit the description by ourselves)
        commitArmar3RobotDescription();
 
        runningTask->start();
    }
 
    void
    LegacyRobotStateMemoryAdapter::onDisconnectComponent()
    {
        runningTask->stop();
    }
 
    void
    LegacyRobotStateMemoryAdapter::onExitComponent()
    {
        runningTask->stop();
    }
} // namespace armarx::armem