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/Core>
#include <Eigen/Geometry>
 
#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 <RobotAPI/libraries/armem/core/Time.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/Person.aron.generated.h>
#include <VisionX/libraries/armem_human/aron/PersonInstance.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());
        def->defineRequiredProperty<std::string>("robotName");
        return def;
    }
 
    void
    PersonMemoryDebugger::onInitComponent()
    {
    }
 
    void
    PersonMemoryDebugger::onConnectComponent()
    {
        idCounter = 0;
        running = true;
        properties.robotName = this->getProperty<std::string>("robotName");
        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 [firstName lastName]' to commit a profile";
        ARMARX_INFO << "Use 'read [firstName lastName]' to read a profile";
        ARMARX_INFO << "Use 'readPreferences [firstName lastName]' to read a profiles preferences";
        ARMARX_INFO << "Use 'writePreferences [firstName lastName]' to write debug values to a "
                       "profiles preferences";
        ARMARX_INFO << "Use 'setCustomAttribute [firstName lastName]' to write a debug attribute "
                       "to a profile";
        ARMARX_INFO << "Use 'readDrinkPreference [firstName lastName]' to read the drink "
                       "preferences from a profile";
        ARMARX_INFO << "Use 'setDrinkPreference [firstName lastName drink]' to set a drink as "
                       "someones favorite drink";
        ARMARX_INFO << "Use 'createPersonInstance [firstName lastName]' to create a new "
                       "PersonInstance and commit it to memory";
        ARMARX_INFO << "Use 'exit' to exit";
        std::string command;
        //std::getline(std::cin, 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 if (command == "read")
        {
            readProfileCommand();
        }
        else if (command == "readPreferences")
        {
            readPreferencesCommand();
        }
        else if (command == "writePreferences")
        {
            writeNewPreferencesCommand();
        }
        else if (command == "setCustomAttribute")
        {
            setCustomAttributeCommand();
        }
        else if (command == "readDrinkPreference")
        {
            readDrinkPreferenceCommand();
        }
        else if (command == "setDrinkPreference")
        {
            setDrinkPreferenceCommand();
        }
        else if (command == "createPersonInstance")
        {
            createPersonInstanceCommand();
        }
        else
        {
            ARMARX_WARNING << "Unrecognized command. Ignoring... "
                           << "\n"
                           << command;
        }
 
        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 firstName;
        std::cin >> firstName;
        std::string lastName;
        std::cin >> lastName;
        armarx::armem::human::PersonID id = armarx::armem::human::PersonID();
        id.firstName = firstName;
        id.lastName = lastName;
        commitProfile(id);
    }
 
    void
    PersonMemoryDebugger::readProfileCommand()
    {
        std::string firstName;
        std::cin >> firstName;
        std::string lastName;
        std::cin >> lastName;
        this->readProfile(firstName, lastName);
    }
 
    void
    PersonMemoryDebugger::readPreferencesCommand()
    {
        std::string firstName;
        std::cin >> firstName;
        std::string lastName;
        std::cin >> lastName;
        this->readPreferences(firstName, lastName);
    }
 
    void
    PersonMemoryDebugger::readDrinkPreferenceCommand()
    {
        std::string firstName;
        std::cin >> firstName;
        std::string lastName;
        std::cin >> lastName;
        this->readDrinkPreference(firstName, lastName);
    }
 
    void
    PersonMemoryDebugger::setDrinkPreferenceCommand()
    {
        std::string firstName;
        std::cin >> firstName;
        std::string lastName;
        std::cin >> lastName;
        std::string drink;
        std::cin >> drink;
        this->setDrinkPreference(firstName, lastName, drink);
    }
 
    void
    PersonMemoryDebugger::writeNewPreferencesCommand()
    {
        std::string firstName;
        std::cin >> firstName;
        std::string lastName;
        std::cin >> lastName;
        this->writeNewPreferences(firstName, lastName);
    }
 
    void
    PersonMemoryDebugger::setCustomAttributeCommand()
    {
        std::string firstName;
        std::cin >> firstName;
        std::string lastName;
        std::cin >> lastName;
        this->setCustomAttribute(firstName, lastName);
    }
 
    void
    PersonMemoryDebugger::createPersonInstanceCommand()
    {
        std::string firstName;
        std::cin >> firstName;
        std::string lastName;
        std::cin >> lastName;
        this->createPersonInstance(firstName, lastName);
    }
 
    void
    PersonMemoryDebugger::commitProfile(armarx::armem::human::PersonID& personID)
    {
        armarx::armem::human::Person debugProfile;
        debugProfile.id = personID;
        debugProfile.physiologicalBiometrics.gender = armarx::armem::human::Gender::FEMALE;
        debugProfile.physiologicalBiometrics.handedness = armarx::armem::human::Handedness::LEFT;
        debugProfile.physiologicalBiometrics.heightInMillimeters = 160000;
 
        this->profileWriter.connect(memoryNameSystem());
 
        ARMARX_INFO << "Committing profile with name: " << debugProfile.id.firstName << " "
                    << debugProfile.id.lastName;
 
        profileWriter.commitHumanProfile(
            debugProfile, "person_memory_debugger", armarx::armem::Time::Now());
    }
 
    std::optional<armarx::armem::MemoryID>
    PersonMemoryDebugger::findProfileId(std::string& entityId)
    {
        armarx::armem::client::Reader profileReader =
            memoryNameSystem().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 =
            memoryNameSystem().getWriter(armarx::human::FaceRecognitionCoreSegmentID);
 
        // get the id counter
        std::stringstream ss;
        ss << idCounter++;
 
        armarx::armem::human::FaceRecognition bo;
        bo.framedPosition3D =
            armarx::FramedPosition(facePosition, armarx::GlobalFrame, properties.robotName);
        // 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);
        bo.extents2D = Eigen::Vector2i::Zero();
 
        armarx::human::arondto::FaceRecognition dto;
        armarx::armem::human::toAron(dto, bo);
 
        armarx::armem::MemoryID entityId =
            armarx::human::FaceRecognitionCoreSegmentID.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 = memoryNameSystem().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 = armarx::GlobalFrame;
        pose.timestamp = armarx::DateTime::Now();
        armarx::FramedOrientation defaultOrientation(
            Eigen::Quaternionf::UnitRandom(), armarx::GlobalFrame, properties.robotName);
 
        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(facePosition, "camera", properties.robotName),
                .orientationCamera = armarx::FramedOrientation(
                    Eigen::Quaternionf::UnitRandom(), "camera", properties.robotName),
                .positionRobot =
                    armarx::FramedPosition(facePosition, "robot", properties.robotName),
                .orientationRobot = armarx::FramedOrientation(
                    Eigen::Quaternionf::UnitRandom(), "robot", properties.robotName),
                .positionGlobal =
                    armarx::FramedPosition(facePosition, armarx::GlobalFrame, properties.robotName),
                .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 PersonID '" << humanId << "'.";
        poseWriter.commit(update);
    }
 
    void
    PersonMemoryDebugger::readProfile(const std::string& firstName, const std::string& lastName)
    {
        ARMARX_INFO << "Reading profile for " << firstName << " " << lastName;
        this->profileReader.connect(memoryNameSystem());
        armarx::armem::human::Person profile = profileReader.queryLatestProfileByName(
            firstName, lastName, PersonMemoryDebugger::profile_provider_name);
        ARMARX_INFO << profile.id.firstName << " " << profile.id.lastName;
        ARMARX_INFO << profile.physiologicalBiometrics.heightInMillimeters.value_or(0);
        if (profile.physiologicalBiometrics.gender == armarx::armem::human::Gender::DIVERSE)
        {
            ARMARX_INFO << "Gender: diverse";
        }
        else if (profile.physiologicalBiometrics.gender == armarx::armem::human::Gender::FEMALE)
        {
            ARMARX_INFO << "Gender: female";
        }
        else
        {
            ARMARX_INFO << "Other gender";
        }
    }
 
    void
    PersonMemoryDebugger::readPreferences(const std::string& firstName, const std::string& lastName)
    {
        ARMARX_INFO << "Reading preferences for " << firstName << " " << lastName;
        this->profileReader.connect(memoryNameSystem());
        const armarx::core::time::Duration maxAge =
            armarx::core::time::Duration::Minutes(10); // 10 minutes
        const armarx::armem::human::Preferences preferences = profileReader.queryPreferences(
            firstName, lastName, maxAge, PersonMemoryDebugger::profile_provider_name);
 
        if (!preferences.food.has_value())
        {
            ARMARX_INFO << "No preferences for food set";
        }
        ARMARX_INFO << "Food preferences (preferred): " << preferences.food.value().preferred;
        ARMARX_INFO << "Food preferences (disliked): " << preferences.food.value().disliked;
    }
 
    void
    PersonMemoryDebugger::readDrinkPreference(const std::string& firstName,
                                              const std::string& lastName)
    {
        ARMARX_INFO << "Reading preferred drinks for " << firstName << " " << lastName;
        this->profileReader.connect(memoryNameSystem());
        const std::vector<std::string> drinks = profileReader.getPreferredDrinks(
            firstName, lastName, PersonMemoryDebugger::profile_provider_name);
 
        if (drinks.empty())
        {
            ARMARX_INFO << "No drinks set";
            return;
        }
        ARMARX_INFO << "Preferred drinks: " << drinks;
    }
 
    void
    PersonMemoryDebugger::setDrinkPreference(const std::string& firstName,
                                             const std::string& lastName,
                                             const std::string& drink)
    {
        this->profileWriter.connect(memoryNameSystem());
 
        armarx::armem::human::PersonID personID;
        personID.firstName = firstName;
        personID.lastName = lastName;
 
        profileWriter.setPreferredDrink(personID,
                                        PersonMemoryDebugger::profile_provider_name,
                                        armarx::armem::Time::Now(),
                                        drink);
 
        ARMARX_INFO << "Set drink '" << drink << "' as favorite drink for " << personID.firstName << " " << personID.lastName;
    }
 
    void
    PersonMemoryDebugger::writeNewPreferences(const std::string& firstName,
                                              const std::string& lastName)
    {
        this->profileWriter.connect(memoryNameSystem());
 
        armarx::armem::human::Preferences preferences;
        preferences.food = std::optional<armarx::armem::human::Preference>(
            {.preferred = {"pizza", "chocolate"}, .disliked = {"vegetables"}});
 
        armarx::armem::human::PersonID personID;
        personID.firstName = firstName;
        personID.lastName = lastName;
        profileWriter.commitPreferencesToProfile(personID,
                                                 PersonMemoryDebugger::profile_provider_name,
                                                 armarx::armem::Time::Now(),
                                                 preferences);
        ARMARX_INFO << "Committed preferences to " << personID.firstName << " " << personID.lastName;
    }
 
    void
    PersonMemoryDebugger::setCustomAttribute(const std::string& firstName,
                                             const std::string& lastName)
    {
        this->profileWriter.connect(memoryNameSystem());
        std::vector<armarx::armem::human::Preference> preferences;
 
        armarx::armem::human::PersonID personID;
        personID.firstName = firstName;
        personID.lastName = lastName;
 
        profileWriter.setCustomAttributeInProfile(personID,
                                                  PersonMemoryDebugger::profile_provider_name,
                                                  armarx::armem::Time::Now(),
                                                  "favouriteNumber",
                                                  "42");
 
        ARMARX_INFO << "Set custom attribute for " << personID.firstName << " " << personID.lastName;
    }
 
    void
    PersonMemoryDebugger::createPersonInstance(const std::string& firstName,
                                               const std::string& lastName)
    {
        ARMARX_INFO << "Creating PersonInstance for " << firstName << " " << lastName;
 
        // First, find or create the profile
        std::string profileEntityName = firstName + "-" + lastName;
        auto profileId = findProfileId(profileEntityName);
 
        if (!profileId.has_value())
        {
            ARMARX_INFO << "Profile not found. Creating profile first...";
            armarx::armem::human::PersonID personID;
            personID.firstName = firstName;
            personID.lastName = lastName;
            commitProfile(personID);
 
            // After committing, find the profile ID again
            profileId = findProfileId(profileEntityName);
            if (!profileId.has_value())
            {
                ARMARX_WARNING << "Failed to create or find profile. Cannot create PersonInstance.";
                return;
            }
        }
 
        // Create dummy pose and face recognition if needed
        // For debugging, we'll use default/empty memory IDs or create minimal data
        Eigen::Vector3f defaultPosition(0.0f, 0.0f, 1.5f); // 1.5m height as default
 
        // Create the PersonInstance
        armarx::armem::human::PersonInstance personInstance;
        personInstance.profileID = profileId.value();
 
        // Set empty/invalid IDs for face and pose (they can be updated later)
        personInstance.faceRecognitionID = armarx::armem::MemoryID();
        personInstance.poseID = armarx::armem::MemoryID();
 
        // Set the global pose (identity pose at default position)
        personInstance.globalPose = Eigen::Isometry3f::Identity();
        personInstance.globalPose.translation() = defaultPosition;
 
        // Convert to DTO for serialization
        armarx::human::arondto::PersonInstance dto;
        armarx::armem::human::toAron(dto, personInstance);
 
        // Create the entity update
        armarx::armem::MemoryID entityId =
            armarx::human::PersonInstanceCoreSegmentID
                .withProviderSegmentName("person_memory_debugger")
                .withEntityName(profileEntityName);
 
        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(),
        };
 
        // Get writer for PersonInstance segment
        armarx::armem::client::Writer personInstanceWriter =
            memoryNameSystem().getWriter(armarx::human::PersonInstanceCoreSegmentID);
 
        // Commit the PersonInstance
        personInstanceWriter.commit(update);
 
        ARMARX_INFO << "Successfully created PersonInstance for " << firstName << " " << lastName
                    << " with entity name '" << profileEntityName << "'";
    }
 
    std::string
    PersonMemoryDebugger::getDefaultName() const
    {
        return "PersonMemoryDebugger";
    }
 
    //    ARMARX_REGISTER_COMPONENT_EXECUTABLE(PersonMemoryDebugger,
    //                                         PersonMemoryDebugger::GetDefaultName());
 
} // namespace visionx::components::person_memory_debugger