SimpleEpisodicMemory.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    MemoryX::ArmarXObjects::SimpleEpisodicMemory
 * @author     fabian.peller-konrad@kit.edu ( 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 "SimpleEpisodicMemory.h"
 
#include <filesystem>
#include <fstream>
 
#include <opencv2/opencv.hpp>
 
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
namespace memoryx
{
 
    // In one of the implementation files
    const std::string SimpleEpisodicMemory::NO_EPISODE = "NO_EPISODE";
 
    const std::map<EpisodeStatus, std::string> SimpleEpisodicMemory::episode_status_descriptor = {
        {EpisodeStatus::EPISODE_STARTED, "started"},
        {EpisodeStatus::EPISODE_COMPLETED_SUCCESS, "success"},
        {EpisodeStatus::EPISODE_COMPLETED_FAILURE, "failure"},
        {EpisodeStatus::EPISODE_COMPLETED_ABORT, "abort"},
    };
 
    const std::map<ActionStatus, std::string> SimpleEpisodicMemory::action_status_descriptor = {
        {ActionStatus::ACTION_STARTED, "started"},
        {ActionStatus::ACTION_RUNNING, "running"},
        {ActionStatus::ACTION_REPEATED, "repeated"},
        {ActionStatus::ACTION_COMPLETED_SUCCESS, "success"},
        {ActionStatus::ACTION_COMPLETED_FAILURE, "failure"},
    };
 
    const std::map<ObjectPoseEventType, std::string> SimpleEpisodicMemory::object_type_descriptor =
        {
            {ObjectPoseEventType::NEW_OBJECT_RECOGNIZED, "newly detected"},
            {ObjectPoseEventType::OBJECT_POSE_UPDATE, "updated"},
    };
 
    std::string
    SimpleEpisodicMemory::getDefaultName() const
    {
        return "SimpleEpisodicMemory";
    }
 
    std::string
    SimpleEpisodicMemory::GetDefaultName()
    {
        return "SimpleEpisodicMemory";
    }
 
    void
    SimpleEpisodicMemory::onInitComponent()
    {
        m_enable_export = true; //getProperty<bool>("EnableExport");
        m_export_folder = "/tmp/EMExport"; //getProperty<std::string>("ExportFolder");
    }
 
    void
    SimpleEpisodicMemory::onConnectComponent()
    {
        clearAll();
    }
 
    void
    SimpleEpisodicMemory::onDisconnectComponent()
    {
    }
 
    void
    SimpleEpisodicMemory::onExitComponent()
    {
    }
 
    void
    SimpleEpisodicMemory::export_episode() const
    {
        ARMARX_IMPORTANT << "Exporting current episode!";
        if (m_current_episode.episodeName == NO_EPISODE)
        {
            //return;
        }
        if (m_current_episode.imageEvents.size() + m_current_episode.actionEvents.size() +
                m_current_episode.humanPoseEvents.size() + m_current_episode.speechEvents.size() +
                m_current_episode.objectPoseEvents.size() +
                m_current_episode.kinematicUnitEvents.size() +
                m_current_episode.platformUnitEvents.size() +
                m_current_episode.platformUnitTargetEvents.size() ==
            0)
        {
            ARMARX_DEBUG << "No information found. Skip export.";
            return;
        }
 
        if (std::filesystem::exists(m_export_folder))
        {
            const std::string episode_output =
                m_export_folder + "/episode_" + std::to_string(m_current_episode.startedInMs) + "/";
            if (!std::filesystem::create_directory(episode_output))
            {
                ARMARX_ERROR << "Couldn't create episode folder: " << episode_output;
                return;
            }
 
            const std::string camera_output = episode_output + "camera/";
            if (!std::filesystem::create_directory(camera_output))
            {
                ARMARX_ERROR << "Couldn't create camera folder";
                return;
            }
 
            const std::string object_output = episode_output + "objects/";
            if (!std::filesystem::create_directory(object_output))
            {
                ARMARX_ERROR << "Couldn't objects camera folder";
                return;
            }
 
            const std::string action_output = episode_output + "actions/";
            if (!std::filesystem::create_directory(action_output))
            {
                ARMARX_ERROR << "Couldn't create actions folder";
                return;
            }
 
            const std::string human_poses_output = episode_output + "poses/";
            if (!std::filesystem::create_directory(human_poses_output))
            {
                ARMARX_ERROR << "Couldn't create poses folder";
                return;
            }
 
            const std::string speech_output = episode_output + "speech/";
            if (!std::filesystem::create_directory(speech_output))
            {
                ARMARX_ERROR << "Couldn't create speech folder";
                return;
            }
 
            const std::string platformUnit_output = episode_output + "platformUnit/";
            if (!std::filesystem::create_directory(platformUnit_output))
            {
                ARMARX_ERROR << "Couldn't create platformUnit folder";
                return;
            }
 
            const std::string platformUnitTarget_output = episode_output + "platformUnitTarget/";
            if (!std::filesystem::create_directory(platformUnitTarget_output))
            {
                ARMARX_ERROR << "Couldn't create platformUnitTarget folder";
                return;
            }
 
            const std::string kinematicUnit_output = episode_output + "kinematicUnit/";
            if (!std::filesystem::create_directory(kinematicUnit_output))
            {
                ARMARX_ERROR << "Couldn't create kinematicUnit folder";
                return;
            }
 
 
            // Successfully created folders
            // Now export data
            // Start with episode information
            {
                ARMARX_DEBUG << "Export Episode info";
                std::filesystem::path path{episode_output + "episode.json"};
 
                std::ofstream e_ofs(path);
                e_ofs << "{\n";
                e_ofs << "\t\"name\": \"" + m_current_episode.episodeName + "\",\n";
                e_ofs << "\t\"status\": \"" +
                             SimpleEpisodicMemory::episode_status_descriptor.at(
                                 m_current_episode.status) +
                             "\",\n";
                e_ofs << "\t\"started\": \"" + std::to_string(m_current_episode.startedInMs) +
                             "\",\n";
                e_ofs << "\t\"ended\": \"" + std::to_string(m_current_episode.endedInMs) + "\"\n";
                e_ofs << "}";
                e_ofs.close();
            }
 
            // Export object information
            {
                ARMARX_DEBUG << "Export Object info";
                for (const auto& objectEvent : m_current_episode.objectPoseEvents)
                {
                    const double timestamp = objectEvent.receivedInMs;
 
                    std::ofstream o_ofs(object_output + "obj_" + std::to_string(timestamp) +
                                        ".json");
                    o_ofs << "{\n";
                    o_ofs << "\t\"name\": \"" + objectEvent.objectName + "\",\n";
                    o_ofs << "\t\"position\":  [" + std::to_string(objectEvent.x) + ", " +
                                 std::to_string(objectEvent.y) + ", " +
                                 std::to_string(objectEvent.z) + "],\n";
                    o_ofs << "\t\"started\": \"" + std::to_string(timestamp) + "\",\n";
                    o_ofs << "\t\"frame\": \"" + objectEvent.frame + "\",\n";
                    o_ofs << "\t\"type\": \"" +
                                 SimpleEpisodicMemory::object_type_descriptor.at(objectEvent.type) +
                                 "\"\n";
                    o_ofs << "}";
                    o_ofs.close();
                }
            }
 
            // Export action information
            {
                ARMARX_DEBUG << "Export Action info";
                for (const auto& actionEvent : m_current_episode.actionEvents)
                {
                    const double timestamp = actionEvent.receivedInMs;
 
                    std::ofstream a_ofs(action_output + "act_" + std::to_string(timestamp) +
                                        ".json");
                    a_ofs << "{\n";
                    a_ofs << "\t\"name\": \"" + actionEvent.actionName + "\",\n";
                    a_ofs << "\t\"status\":  \"" +
                                 SimpleEpisodicMemory::action_status_descriptor.at(
                                     actionEvent.status) +
                                 "\",\n";
                    a_ofs << "\t\"started\": \"" + std::to_string(timestamp) + "\"\n";
                    a_ofs << "}";
                    a_ofs.close();
                }
            }
 
            // Export human poses
            {
                ARMARX_DEBUG << "Export Human pose info";
                for (const auto& humanPose : m_current_episode.humanPoseEvents)
                {
                    const double timestamp = humanPose.receivedInMs;
 
                    std::ofstream p_ofs(human_poses_output + "pose_" + std::to_string(timestamp) +
                                        ".json");
                    p_ofs << "{\n";
 
                    for (const auto& [label, keypoint] : humanPose.keypoints)
                    {
                        p_ofs << "\t\"" + label + "\":\n";
                        p_ofs << "\t{\n";
                        p_ofs << "\t\t \"confidence\": " + std::to_string(keypoint.confidence) +
                                     ",\n";
                        p_ofs << "\t\t \"local\": [" + std::to_string(keypoint.x) + ", " +
                                     std::to_string(keypoint.y) + ", " +
                                     std::to_string(keypoint.z) + " ],\n";
                        p_ofs << "\t\t \"global\": [" + std::to_string(keypoint.globalX) + ", " +
                                     std::to_string(keypoint.globalY) + ", " +
                                     std::to_string(keypoint.globalZ) + " ]\n";
                        p_ofs << "\t},\n";
                    }
                    p_ofs << "}";
                    p_ofs.close();
                }
            }
 
            // Export speech
            {
                ARMARX_DEBUG << "Export Speech info";
                for (const auto& speech : m_current_episode.speechEvents)
                {
                    const double timestamp = speech.receivedInMs;
 
                    std::ofstream s_ofs(speech_output + "speech_" + std::to_string(timestamp) +
                                        ".json");
                    s_ofs << "{\n";
                    s_ofs << "\t \"text\": " << speech.text << "\n";
                    s_ofs << "}";
                    s_ofs.close();
                }
            }
 
            // Export kinematicUnit
            {
                ARMARX_DEBUG << "Export KinematicUnit info";
                for (const auto& kinematicUnit : m_current_episode.kinematicUnitEvents)
                {
                    const double timestamp = kinematicUnit.receivedInMs;
 
                    std::ofstream k_ofs(kinematicUnit_output + "kinematicUnit_" +
                                        std::to_string(timestamp) + ".json");
                    k_ofs << "{\n";
                    for (const auto& [key, value] : kinematicUnit.data)
                    {
                        k_ofs << "\t \"" + key + "\": {\n";
                        k_ofs << "\t\t \"jointAngle\": \"" << value.jointAngle << "\",\n";
                        k_ofs << "\t\t \"jointVelocity\": \"" << value.jointVelocity << "\",\n";
                        k_ofs << "\t\t \"jointTorque\": \"" << value.jointTorque << "\",\n";
                        k_ofs << "\t\t \"jointAcceleration\": \"" << value.jointAcceleration
                              << "\",\n";
                        k_ofs << "\t\t \"current\": \"" << value.current << "\",\n";
                        k_ofs << "\t\t \"temperature\": \"" << value.temperature << "\",\n";
                        k_ofs << "\t\t \"enabled\": \"" << value.enabled << "\"\n";
                        k_ofs << "\t },\n";
                    }
                    k_ofs << "}";
                    k_ofs.close();
                }
            }
 
            // Export platformUnit
            {
                ARMARX_DEBUG << "Export PlatformUnit info";
                for (const auto& platformUnit : m_current_episode.platformUnitEvents)
                {
                    const double timestamp = platformUnit.receivedInMs;
 
                    std::ofstream p_ofs(platformUnit_output + "platformUnit_" +
                                        std::to_string(timestamp) + ".json");
                    p_ofs << "{\n";
                    p_ofs << "\t \"x\": \"" << platformUnit.x << "\",\n";
                    p_ofs << "\t \"y\": \"" << platformUnit.y << "\",\n";
                    p_ofs << "\t \"rot\": \"" << platformUnit.rot << "\",\n";
                    p_ofs << "\t \"acc_x\": \"" << platformUnit.acc_x << "\",\n";
                    p_ofs << "\t \"acc_y\": \"" << platformUnit.acc_y << "\",\n";
                    p_ofs << "\t \"acc_rot\": \"" << platformUnit.acc_rot << "\"\n";
                    p_ofs << "}";
                    p_ofs.close();
                }
            }
 
            // Export platformUnitTarget
            {
                ARMARX_DEBUG << "Export PlatformUnitTarget info";
                for (const auto& platformUnitTarget : m_current_episode.platformUnitTargetEvents)
                {
                    const double timestamp = platformUnitTarget.receivedInMs;
 
                    std::ofstream t_ofs(platformUnitTarget_output + "platformUnitTarget_" +
                                        std::to_string(timestamp) + ".json");
                    t_ofs << "{\n";
                    t_ofs << "\t \"x\": \"" << platformUnitTarget.target_x << "\",\n";
                    t_ofs << "\t \"y\": \"" << platformUnitTarget.target_y << "\",\n";
                    t_ofs << "\t \"rot\": \"" << platformUnitTarget.target_rot << "\"\n";
                    t_ofs << "}";
                    t_ofs.close();
                }
            }
 
 
            // Export image information
            {
                ARMARX_DEBUG << "Export Image info";
                for (const auto& [imageProvider, imageEventList] : m_current_episode.imageEvents)
                {
                    if (imageEventList.size() == 0)
                    {
                        continue;
                    }
                    const std::string image_provider_output = camera_output + imageProvider + "/";
                    if (!std::filesystem::create_directory(image_provider_output))
                    {
                        ARMARX_ERROR << "Couldn't create image provider folder: " + imageProvider;
                        return;
                    }
 
                    for (const auto& imageEvent : imageEventList)
                    {
                        const double timestamp = imageEvent.receivedInMs;
 
                        auto mode = CV_8UC3;
                        if (imageEvent.colourType == memoryx::ColourSpace::GRAYSCALE)
                        {
                            mode = CV_8UC1;
                        }
                        std::vector<uchar> data(imageEvent.data);
                        ARMARX_DEBUG << "Image size is " << imageEvent.width << ", "
                                     << imageEvent.height << ", " << imageEvent.colourType << " => "
                                     << imageEvent.data.size();
                        cv::Mat cv_image =
                            cv::Mat(imageEvent.height, imageEvent.width, mode, data.data());
 
                        cv::cvtColor(cv_image, cv_image, cv::COLOR_RGB2BGR);
                        cv::imwrite(image_provider_output + "img_" + std::to_string(timestamp) +
                                        ".jpg",
                                    cv_image);
                        ARMARX_DEBUG
                            << "Exporting image to: img_" + std::to_string(timestamp) + ".jpg";
                    }
                }
            }
        }
        else
        {
            ARMARX_ERROR << "Cannot export files because folder does not exist: "
                         << m_export_folder;
        }
    }
 
    void
    SimpleEpisodicMemory::clearAll()
    {
        ARMARX_DEBUG << "Resetting episode info to default.";
        m_current_episode.episodeName = NO_EPISODE;
        m_current_episode.startedInMs = IceUtil::Time::now().toMilliSecondsDouble();
        m_current_episode.status = EpisodeStatus::EPISODE_STARTED;
    }
 
    void
    SimpleEpisodicMemory::registerEpisodeEvent(const EpisodeEvent& e, const Ice::Current& c)
    {
        std::lock_guard<std::mutex> l(episodeEventMutex);
        if (m_current_episode.episodeName != e.episodeName && e.status != EPISODE_STARTED)
        {
            ARMARX_ERROR << "Received an episode unequal to current one with non-starting status: "
                         << e.episodeName;
        }
        if (m_current_episode.episodeName != e.episodeName && m_current_episode.endedInMs != 0 &&
            e.status == EPISODE_STARTED)
        {
            ARMARX_WARNING << "Received a new starting episode without ending the last one. Last "
                              "episodes name is "
                           << m_current_episode.episodeName << ". Finishing it now.";
            EpisodeEvent abort;
            abort.episodeName = m_current_episode.episodeName;
            abort.status = EpisodeStatus::EPISODE_COMPLETED_ABORT;
            abort.receivedInMs = IceUtil::Time::now().toMilliSecondsDouble();
            registerEpisodeEvent(abort, c);
        }
 
        if (e.status == EPISODE_COMPLETED_SUCCESS || e.status == EPISODE_COMPLETED_FAILURE ||
            e.status == EPISODE_COMPLETED_ABORT)
        {
            ARMARX_DEBUG << "Received a terminating episode (" << e.episodeName << ")";
            m_current_episode.status = e.status;
            m_current_episode.endedInMs = e.receivedInMs;
        }
        else //if (e.status == EPISODE_STARTED)
        {
            ARMARX_DEBUG << "Received a starting episode (" << e.episodeName << ")";
            m_current_episode.episodeName = e.episodeName;
            m_current_episode.status = e.status;
            m_current_episode.startedInMs = e.receivedInMs;
        }
 
        std::lock_guard<std::mutex> l2(imageEventMutex);
        std::lock_guard<std::mutex> l3(humanPoseEventMutex);
        std::lock_guard<std::mutex> l4(speechEventMutex);
        std::lock_guard<std::mutex> l5(objectPoseEventMutex);
        std::lock_guard<std::mutex> l6(kinematicUnitEventMutex);
        std::lock_guard<std::mutex> l7(platformUnitEventMutex);
        std::lock_guard<std::mutex> l8(platformUnitTargetEventMutex);
        std::lock_guard<std::mutex> l9(actionEventMutex);
        if (m_enable_export && m_current_episode.endedInMs != 0)
        {
            ARMARX_DEBUG << "Exporting episode...";
            export_episode();
            clearAll();
        }
    }
 
    void
    SimpleEpisodicMemory::registerImageEvent(const ImageEvent& i, const Ice::Current& c)
    {
        std::lock_guard<std::mutex> l(imageEventMutex);
        ARMARX_DEBUG << "Received an image. Current number of images of provider " << i.providerName
                     << " in episode: "
                     << this->m_current_episode.imageEvents[i.providerName].size();
        this->m_current_episode.imageEvents[i.providerName].push_back(i);
    }
 
    void
    SimpleEpisodicMemory::registerObjectPoseEvent(const ObjectPoseEvent& o, const Ice::Current& c)
    {
        std::lock_guard<std::mutex> l(objectPoseEventMutex);
        ARMARX_DEBUG << "Received an objectPose (" << o.objectName
                     << "). Current number of objectPoses in episode: "
                     << this->m_current_episode.objectPoseEvents.size();
        this->m_current_episode.objectPoseEvents.push_back(o);
    }
 
    void
    SimpleEpisodicMemory::registerActionEvent(const ActionEvent& a, const Ice::Current& c)
    {
        std::lock_guard<std::mutex> l(actionEventMutex);
        ARMARX_DEBUG << "Received an action (" << a.actionName << " with status "
                     << SimpleEpisodicMemory::action_status_descriptor.at(a.status)
                     << ")Current number of actions in episode: "
                     << this->m_current_episode.actionEvents.size();
        this->m_current_episode.actionEvents.push_back(a);
    }
 
    void
    SimpleEpisodicMemory::registerHumanPoseEvent(const Body25HumanPoseEvent& p, const Ice::Current&)
    {
        std::lock_guard<std::mutex> l(humanPoseEventMutex);
        ARMARX_DEBUG << "Received a human pose. Current number of poses in episode: "
                     << this->m_current_episode.humanPoseEvents.size();
        this->m_current_episode.humanPoseEvents.push_back(p);
    }
 
    void
    SimpleEpisodicMemory::registerSpeechEvent(const SpeechEvent& s, const Ice::Current&)
    {
        if (s.text.find("export now") != std::string::npos)
        {
            ARMARX_IMPORTANT << "Received export token!. Terminating current episode and create an "
                                "empty new one.";
            EpisodeEvent terminate;
            terminate.episodeName = m_current_episode.episodeName;
            terminate.status = EpisodeStatus::EPISODE_COMPLETED_ABORT;
            terminate.receivedInMs = IceUtil::Time::now().toMilliSecondsDouble();
            registerEpisodeEvent(terminate);
            return;
        }
 
        std::lock_guard<std::mutex> l(speechEventMutex);
        ARMARX_DEBUG << "Received spoken text (" << s.text
                     << "). Current number of speeches in episode: "
                     << this->m_current_episode.speechEvents.size();
        this->m_current_episode.speechEvents.push_back(s);
    }
 
    void
    SimpleEpisodicMemory::registerKinematicUnitEvent(const KinematicUnitEvent& k,
                                                     const Ice::Current&)
    {
        std::lock_guard<std::mutex> l(kinematicUnitEventMutex);
        ARMARX_DEBUG << "Received a kinematicUnitEvent. Current number of events in episode: "
                     << this->m_current_episode.kinematicUnitEvents.size();
        this->m_current_episode.kinematicUnitEvents.push_back(k);
    }
 
    void
    SimpleEpisodicMemory::registerPlatformUnitEvent(const PlatformUnitEvent& p, const Ice::Current&)
    {
        std::lock_guard<std::mutex> l(platformUnitEventMutex);
        ARMARX_DEBUG << "Received a platformUnitEvent. Current number of events in episode: "
                     << this->m_current_episode.platformUnitEvents.size();
        this->m_current_episode.platformUnitEvents.push_back(p);
    }
 
    void
    SimpleEpisodicMemory::registerPlatformUnitTargetEvent(const PlatformUnitTargetEvent& t,
                                                          const Ice::Current&)
    {
        std::lock_guard<std::mutex> l(platformUnitTargetEventMutex);
        ARMARX_DEBUG << "Received a platformUnitTarget. Current number of events in episode: "
                     << this->m_current_episode.platformUnitTargetEvents.size();
        this->m_current_episode.platformUnitTargetEvents.push_back(t);
    }
 
    void
    SimpleEpisodicMemory::notifyKeyframe(const Ice::Current& c)
    {
    }
 
    armarx::PropertyDefinitionsPtr
    SimpleEpisodicMemory::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr def{
            new armarx::ComponentPropertyDefinitions{getConfigIdentifier()}};
 
        //def->bool(m_enable_export, "EnableExport");
        return def;
    }
 
} // namespace memoryx
 
ARMARX_REGISTER_COMPONENT_EXECUTABLE(::memoryx::SimpleEpisodicMemory,
                                     ::memoryx::SimpleEpisodicMemory::GetDefaultName());