HumanMemory.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::ExampleMemory
 * @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 "HumanMemory.h"
 
#include <exception>
 
#include <qnamespace.h>
 
#include <SimoxUtility/algorithm/apply.hpp>
#include <VirtualRobot/VirtualRobot.h>
 
#include "ArmarXCore/core/time/DateTime.h"
#include <ArmarXCore/core/time/ice_conversions.h>
 
#include "RobotAPI/libraries/core/FramedPose.h"
#include <RobotAPI/interface/armem/commit.h>
#include <RobotAPI/libraries/armem/core/ice_conversions.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
#include <RobotAPI/libraries/aron/common/aron_conversions/framed.h>
 
#include "VisionX/libraries/armem_human/server/PoseSegment.h"
#include <VisionX/libraries/armem_human/aron/HumanPose.aron.generated.h>
 
namespace armarx::armem::server::human_memory
{
 
    armarx::PropertyDefinitionsPtr
    HumanMemory::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr defs =
            new ComponentPropertyDefinitions(getConfigIdentifier());
 
        setMemoryName("Human");
 
        profileSegment.defineProperties(defs, "profile.");
        personInstanceSegment.defineProperties(defs, "instance");
        poseSegment.defineProperties(defs, "pose.");
        faceRecognitionSegment.defineProperties(defs, "face.");
        identificationSegment.defineProperties(defs, "ident.");
 
        return defs;
    }
 
    HumanMemory::HumanMemory() :
        profileSegment(iceAdapter()),
        personInstanceSegment(iceAdapter()),
        poseSegment(iceAdapter()),
        faceRecognitionSegment(iceAdapter()),
        identificationSegment(iceAdapter()),
        visu(poseSegment, faceRecognitionSegment, personInstanceSegment)
    {
        addPlugin(virtualRobotReaderPlugin);
    }
 
    std::string
    HumanMemory::getDefaultName() const
    {
        return "HumanMemory";
    }
 
    void
    HumanMemory::onInitComponent()
    {
        profileSegment.init();
        personInstanceSegment.init();
        poseSegment.init();
        faceRecognitionSegment.init();
        identificationSegment.init();
        visu.init();
 
        workingMemory().addCoreSegment("RecognizedAction");
    }
 
    void
    HumanMemory::onConnectComponent()
    {
        visu.connect(arviz, getDebugObserver());
    }
 
    void
    HumanMemory::onDisconnectComponent()
    {
    }
 
    void
    HumanMemory::onExitComponent()
    {
    }
 
    VirtualRobot::RobotPtr
    HumanMemory::getSynchronizedRobot(const std::string& robotName,
                                      const armarx::DateTime& timestamp)
    {
        // seach and retrieve robot
        auto& robot = [&]() -> VirtualRobot::RobotPtr&
        {
            // retrieve robot from memory if it is not in cache
            if (robots.count(robotName) == 0)
            {
                auto newRobot = virtualRobotReaderPlugin->get().getRobot(robotName);
                ARMARX_CHECK_NOT_NULL(newRobot)
                    << "Failed to obtain robot with name `" << robotName << "`.";
 
                robots[robotName] = newRobot;
            }
 
            return robots.at(robotName);
        }();
 
        // synchronize robot
        bool const success = virtualRobotReaderPlugin->get().synchronizeRobot(*robot, timestamp);
 
        if (not success)
        {
            //            throw armarx::LocalException(
            //                "Failed synchronizing the robot to calculate the global human pose from a reported "
            //                "human pose in camera coordinate system. Query timestamp is '")
            //                << timestamp << "'.";
            return nullptr;
        }
 
        return robot;
    }
 
    void
    HumanMemory::commitHumanPosesInCameraFrame(
        const ::armarx::aron::data::dto::AronDictSeq& dictSeq,
        const ::std::string& providerName,
        const ::armarx::core::time::dto::DateTime& iceTimestamp,
        const ::Ice::Current& /*c*/)
    {
        if (dictSeq.empty())
        {
            return;
        }
 
        // convert from ice
        armarx::DateTime timestamp;
        fromIce(iceTimestamp, timestamp);
 
        // convert to our aron type
        const auto toHumanPose = [](const ::armarx::aron::data::dto::DictPtr& dict)
        { return armarx::human::arondto::HumanPose::FromAron(dict); };
 
        std::vector<armarx::human::arondto::HumanPose> humanPoses =
            simox::alg::apply(dictSeq, toHumanPose);
 
        // synchronize robot
        const std::string robotName =
            humanPoses.front().keypoints.begin()->second.positionCamera.header.agent;
 
        const auto robot = getSynchronizedRobot(robotName, timestamp);
 
        if (not robot)
        {
            ARMARX_INFO << deactivateSpam(1) << "Failed to obtain robot `" << robotName
                        << "`. Won't commit poses";
            return;
        }
 
        ARMARX_CHECK_NOT_NULL(robot) << "Failed to obtain robot with name `" << robotName << "`.";
 
        // fill global pose info
        {
            for (armarx::human::arondto::HumanPose& humanPose : humanPoses)
            {
                for (auto& [_, keyPoint] : humanPose.keypoints)
                {
                    const auto& cameraFrame = keyPoint.positionCamera.header.frame;
                    const auto& agent = keyPoint.positionCamera.header.agent;
 
                    const FramedPosition keypoint(
                        keyPoint.positionCamera.position, cameraFrame, agent);
                    // ARMARX_IMPORTANT << VAROUT(keyPoint.positionCamera.position.transpose());
 
                    // global pose
                    {
                        keyPoint.positionGlobal = armarx::arondto::FramedPosition();
                        armarx::toAron(keyPoint.positionGlobal.value(),
                                       armarx::FramedPosition(keypoint.toGlobalEigen(robot),
                                                              armarx::GlobalFrame,
                                                              agent));
                    }
 
                    // pose in root frame
                    {
                        keyPoint.positionRobot = armarx::arondto::FramedPosition();
                        armarx::toAron(keyPoint.positionRobot.value(),
                                       armarx::FramedPosition(keypoint.toRootEigen(robot),
                                                              robot->getRootNode()->getName(),
                                                              agent));
                    }
 
                    // Orientation information available.
                    if (keyPoint.orientationCamera.has_value())
                    {
                        ARMARX_CHECK_EQUAL(keyPoint.orientationCamera->header.frame,
                                           keyPoint.positionCamera.header.frame);
 
                        const FramedPose keypointPose(
                            keyPoint.orientationCamera->orientation.toRotationMatrix(),
                            keyPoint.positionCamera.position,
                            cameraFrame,
                            agent);
 
                        // global pose
                        {
                            const Eigen::Isometry3f global_T_human(
                                keypointPose.toGlobalEigen(robot));
 
                            keyPoint.orientationGlobal = armarx::arondto::FramedOrientation();
 
                            armarx::toAron(
                                keyPoint.orientationGlobal.value(),
                                armarx::FramedOrientation(Eigen::Matrix3f{global_T_human.linear()},
                                                          armarx::GlobalFrame,
                                                          agent));
                        }
 
                        // pose in root frame
                        {
                            const Eigen::Isometry3f robot_root_T_human(
                                keypointPose.toRootEigen(robot));
 
 
                            keyPoint.orientationRobot = armarx::arondto::FramedOrientation();
 
                            armarx::toAron(keyPoint.orientationRobot.value(),
                                           armarx::FramedOrientation(
                                               Eigen::Matrix3f{robot_root_T_human.linear()},
                                               robot->getRootNode()->getName(),
                                               agent));
                        }
                    }
                }
            }
        }
 
        // commit human poses
        commitHumanPoses(humanPoses, providerName, timestamp);
    }
 
    void
    HumanMemory::commitHumanPoses(
        const std::vector<::armarx::human::arondto::HumanPose>& humanPoses,
        const std::string& providerName,
        const armarx::DateTime& timestamp)
    {
        armem::Commit commit;
 
        const auto providerId = armem::MemoryID(
            workingMemory().name(), human::PoseSegment::CORE_SEGMENT_NAME, providerName);
        const auto entityID = providerId.withEntityName("human_poses").withTimestamp(timestamp);
 
        armem::EntityUpdate update;
        update.entityID = entityID;
        update.sentTime = timestamp; // TODO this is not fully correct
        update.referencedTime = timestamp;
 
        std::transform(
            humanPoses.begin(),
            humanPoses.end(),
            std::back_inserter(update.instancesData),
            [](const ::armarx::human::arondto::HumanPose& humanPose) -> armarx::aron::data::DictPtr
            { return humanPose.toAron(); });
 
        commit.add(update);
 
        ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
 
        iceAdapter().commitLocking(commit);
    }
} // namespace armarx::armem::server::human_memory