PersonMemoryDebugger.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    VisionX::ArmarXObjects::person_memory_debugger
 * @author     Peter Albrecht ( usnlf at student dot kit dot edu )
 * @date       2024
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#include "PersonMemoryDebugger.h"
#include <optional>
#include <utility>
#include <Eigen/src/Geometry/AngleAxis.h>
#include <Eigen/src/Geometry/Quaternion.h>
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/algorithm/string/string_tools.h>
 
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
#include "RobotAPI/libraries/armem/core/Commit.h"
#include "RobotAPI/libraries/armem/core/MemoryID.h"
#include "RobotAPI/libraries/armem/core/forward_declarations.h"
#include "RobotAPI/libraries/armem/core/wm/memory_definitions.h"
#include "RobotAPI/libraries/core/FramedPose.h"
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
 
#include "VisionX/libraries/human/pose/model/k4a_bt_body_32.h"
#include <VisionX/libraries/armem_human/aron/FaceRecognition.aron.generated.h>
#include <VisionX/libraries/armem_human/aron/HumanPose.aron.generated.h>
#include <VisionX/libraries/armem_human/aron/Profile.aron.generated.h>
#include <VisionX/libraries/armem_human/aron_conversions.h>
#include <VisionX/libraries/armem_human/memory_ids.h>
#include <VisionX/libraries/armem_human/server/PoseSegment.h>
#include <VisionX/libraries/armem_human/server/face_recognition/Segment.h>
#include <VisionX/libraries/armem_human/server/profile/Segment.h>
#include <VisionX/libraries/armem_human/types.h>
 
namespace VisionX::components::person_memory_debugger
{
 
    const std::string PersonMemoryDebugger::pose_provider_name = "person_memory_debugger";
    const std::string PersonMemoryDebugger::profile_provider_name = "person_memory_debugger";
    const std::string PersonMemoryDebugger::recognition_provider_name = "person_memory_debugger";
 
    armarx::PropertyDefinitionsPtr
    PersonMemoryDebugger::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr def =
            new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
        return def;
    }
 
    void
    PersonMemoryDebugger::onInitComponent()
    {
    }
 
    void
    PersonMemoryDebugger::onConnectComponent()
    {
        idCounter = 0;
        running = true;
        mns = memoryNameSystem();
        while (running)
        {
            this->run();
        }
    }
 
    void
    PersonMemoryDebugger::onDisconnectComponent()
    {
    }
 
    void
    PersonMemoryDebugger::onExitComponent()
    {
    }
 
    void
    PersonMemoryDebugger::run()
    {
        ARMARX_INFO << "Use 'pose [id] [x] [y] [z]' to commit a pose at location";
        ARMARX_INFO << "Use 'face [profile entityName] [x] [y] [z]' to commit a face at location";
        ARMARX_INFO << "Use 'profile [profile entityName]' to commit a profile";
        ARMARX_INFO << "Use 'exit' to exit";
        std::string command;
        std::cin >> command;
        if (command == "pose")
        {
            poseCommand();
        }
        else if (command == "face")
        {
            faceRecognitionCommand();
        }
        else if (command == "profile")
        {
            profileCommand();
        }
        else if (command == "exit")
        {
            running = false;
        }
        else 
        {
            ARMARX_WARNING << "Unrecognized command. Ignoring";
        }
 
        std::cin.clear();
    }
 
    void 
    PersonMemoryDebugger::poseCommand()
    {
        std::string id, x_s, y_s, z_s;
        std::cin >> id >> x_s >> y_s >> z_s;
        float x = std::stof(x_s);
        float y = std::stof(y_s);
        float z = std::stof(z_s);
        Eigen::Vector3f pos(x, y, z);
        commitPose(pos, id);
    }
 
    void 
    PersonMemoryDebugger::faceRecognitionCommand()
    {
        std::string id, x_s, y_s, z_s;
        std::cin >> id >> x_s >> y_s >> z_s;
 
        auto profileId = findProfileId(id);
        if (not profileId.has_value())
        {
            ARMARX_WARNING << "Could not find profile. Continuing..";
        }
 
        float x = std::stof(x_s);
        float y = std::stof(y_s);
        float z = std::stof(z_s);
 
        Eigen::Vector3f pos(x, y, z);
        commitFaceRecognition(pos, profileId);
    }
    
    void
    PersonMemoryDebugger::profileCommand()
    {
        std::string name;
        std::cin >> name;
        commitProfile(name);
    }
 
    void
    PersonMemoryDebugger::commitProfile(std::string& name)
    {
        armarx::armem::client::Writer poseWriter =
            mns.getWriter(armarx::human::ProfileCoreSegmentID);
 
        // we only set the firstName. FaceImages are not required, 
        // as the user manually sets the profile in the face recognition.
        // therefore, this will not work with a real implementation of face recognition
        // TODO: implement option to add face images
        armarx::human::arondto::Profile dto;
        dto.firstName = name;
 
        armarx::armem::MemoryID entityId = armarx::human::ProfileCoreSegmentID
            .withProviderSegmentName(pose_provider_name)
            .withEntityName(name);
 
        armarx::armem::EntityUpdate update = {
            .entityID = entityId,
            .instancesData = {dto.toAron()},
            .referencedTime = armarx::armem::Time::Now(),
            .confidence = 1.0F,
            .sentTime = armarx::armem::Time::Now(),
            .arrivedTime = armarx::armem::Time::Now(),
        };
 
        ARMARX_INFO << "Committing profile with name: " << name;
        poseWriter.commit(update);
    }
 
    std::optional<armarx::armem::MemoryID> 
    PersonMemoryDebugger::findProfileId(std::string& entityId)
    {
        armarx::armem::client::Reader profileReader =
            mns.getReader(armarx::human::ProfileCoreSegmentID);
 
        armarx::armem::client::QueryResult queryResult =
            profileReader.getLatestSnapshotsIn(armarx::human::ProfileCoreSegmentID);
        std::optional<armarx::armem::MemoryID> foundProfileId = std::nullopt;
 
        // memory fetch failed
        if (not queryResult.success)
        {
            return std::nullopt;
        }
 
        // find the first matching profile
        queryResult.memory.forEachInstance(
            [&entityId, &foundProfileId](armarx::armem::wm::EntityInstance& instance)
            {
                if (foundProfileId.has_value())
                {
                    // we want the first match, and ignore all further matches here.
                    return;
                }
 
                if (instance.id().entityName == entityId)
                {
                    // match found
                    foundProfileId = instance.id();
                }
            }
        );
 
        return foundProfileId;
    }
 
    void
    PersonMemoryDebugger::commitFaceRecognition(Eigen::Vector3f& facePosition, std::optional<armarx::armem::MemoryID> profileId)
    {
        armarx::armem::client::Writer poseWriter =
            mns.getWriter(armarx::human::FaceRecognitionCoreSegmentID);
 
        // get the id counter
        std::stringstream ss;
        ss << idCounter++;
 
        armarx::armem::human::FaceRecognition bo;
        bo.framedPosition3D = armarx::FramedPosition(facePosition, "global", "Armar7");
        // doesnt make sense, but this isn't set in prod anyways
        bo.position2D = Eigen::Vector2i::Identity();
        bo.position3DGlobal = facePosition;
        // profile id may be nullopt, but this case is communicated to the user
        bo.profileID = std::move(profileId);
 
        armarx::human::arondto::FaceRecognition dto;
        armarx::armem::human::toAron(dto, bo);
 
        armarx::armem::MemoryID entityId = armarx::human::PoseCoreSegmentID
            .withProviderSegmentName(pose_provider_name)
            .withEntityName(ss.str());
 
        armarx::armem::EntityUpdate update = {
            .entityID = entityId,
            .instancesData = {dto.toAron()},
            .referencedTime = armarx::armem::Time::Now(),
            .confidence = 1.0F,
            .sentTime = armarx::armem::Time::Now(),
            .arrivedTime = armarx::armem::Time::Now(),
        };
 
        // hack
        ARMARX_INFO << "Committing face recognition for profile: " << profileId.value_or(armarx::armem::MemoryID().withEntityName("INVALID")).entityName;
        poseWriter.commit(update);
        
    }
 
    void
    PersonMemoryDebugger::commitPose(Eigen::Vector3f& facePosition, std::string& humanId)
    {
        armarx::armem::client::Writer poseWriter =
            mns.getWriter(armarx::human::PoseCoreSegmentID);
 
        armarx::armem::human::HumanPose pose;
        pose.humanTrackingId = humanId;
        pose.poseModelId = armarx::human::pose::model::k4a_bt_body_32::ModelId;
        pose.cameraFrameName = "global";
        pose.timestamp = armarx::DateTime::Now();
        armarx::FramedOrientation defaultOrientation(Eigen::Quaternionf::UnitRandom(), "global", "Armar7");
        
        using namespace armarx::human::pose::model::k4a_bt_body_32;
 
        // iterate over all joints, and set their global positions to the face pos.
        // this is the only value required to be valid by the PersonInstanceUpdater
        for (auto& joint : JointNames.names())
        {
            armarx::armem::human::PoseKeypoint key = {
                .label = joint,
                .confidence = 1.0F,
                .positionCamera = armarx::FramedPosition(),
                .orientationCamera = armarx::FramedOrientation(),
                .positionRobot = armarx::FramedPosition(),
                .orientationRobot = armarx::FramedOrientation(),
                .positionGlobal = armarx::FramedPosition(facePosition, "global", "Armar7"),
                .orientationGlobal = defaultOrientation,
            };
            // set every joint to face position
            pose.keypoints[joint] = key;
        }
 
        armarx::armem::MemoryID entityId = armarx::human::PoseCoreSegmentID
            .withProviderSegmentName(pose_provider_name)
            .withEntityName(humanId);
 
        armarx::human::arondto::HumanPose dto;
        armarx::armem::human::toAron(dto, pose);
 
        armarx::armem::EntityUpdate update = {
            .entityID = entityId,
            .instancesData = {dto.toAron()},
            .referencedTime = armarx::armem::Time::Now(),
            .confidence = 1.0F,
            .sentTime = armarx::armem::Time::Now(),
            .arrivedTime = armarx::armem::Time::Now(),
        };
 
        ARMARX_INFO << "Committing pose with HumanID '" << humanId << "'.";
        poseWriter.commit(update);
        
    }
 
    std::string
    PersonMemoryDebugger::getDefaultName() const
    {
        return "PersonMemoryDebugger";
    }
 
    //    ARMARX_REGISTER_COMPONENT_EXECUTABLE(PersonMemoryDebugger,
    //                                         PersonMemoryDebugger::GetDefaultName());
 
} // namespace VisionX::components::person_memory_debugger