Visualization.cpp

Go to the documentation of this file.

#include "Visualization.h"
 
#include <algorithm>
#include <exception>
#include <string>
 
#include <Eigen/Geometry>
 
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/math/pose.h>
 
#include "ArmarXCore/core/time/Metronome.h"
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/interface/core/PackagePath.h>
 
#include <RobotAPI/components/ArViz/Client/Elements.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem_robot_state/server/description/Segment.h>
#include <RobotAPI/libraries/armem_robot_state/server/localization/Segment.h>
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h>
 
#include <VisionX/libraries/ArViz/HumanPose.h>
 
 
namespace armarx::armem::server::human
{
 
    Visu::Visu(const PoseSegment& poseSegment) : poseSegment(poseSegment)
    {
        Logging::setTag("HumanVisualization");
    }
 
 
    void
    Visu::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
    {
        defs->optional(
            p.enabled, prefix + "enabled", "Enable or disable visualization of humans.");
        defs->optional(p.frequencyHz, prefix + "frequenzyHz", "Frequency of visualization.");
    }
 
 
    void
    Visu::init()
    {
        ;
    }
 
 
    void
    Visu::connect(const viz::Client& arviz, DebugObserverInterfacePrx debugObserver)
    {
        this->arviz = arviz;
        if (debugObserver)
        {
            bool batchMode = true;
            this->debugObserver = DebugObserverHelper("HumanMemory", debugObserver, batchMode);
        }
 
        if (updateTask)
        {
            updateTask->stop();
            updateTask->join();
            updateTask = nullptr;
        }
        updateTask = new SimpleRunningTask<>([this]() { this->visualizeRun(); });
        updateTask->start();
    }
 
 
    void
    Visu::visualizeHumanPoses(armarx::viz::HumanPoseLayers &layers,
                              const std::vector<armarx::armem::human::HumanPose>& humanPoses)
    {
        std::string providerName = "azure_kinect"; // TODO: Get actual value.
 
        std::map<std::string, armarx::armem::human::HumanPose> poseMap;
        int unknown = 0;
        for (const armarx::armem::human::HumanPose& humanPose : humanPoses)
        {
            if (humanPose.humanTrackingId.has_value())
            {
                poseMap[humanPose.humanTrackingId.value()] = humanPose;
            }
            else
            {
                // ARMARX_WARNING << "Found unknown human id...";
                poseMap["unknown_human_" + std::to_string(unknown++)] = humanPose;
            }
        }
 
        armarx::viz::addPosesToLayer(poseMap, layers, providerName);
    }
 
 
    void
    Visu::visualizeRun()
    {
        armarx::Metronome metronome{armarx::Frequency::Hertz(p.frequencyHz)};
        while (updateTask and not updateTask->isStopped())
        {
            if (p.enabled)
            {
                const Time timestamp = Time::Now();
                ARMARX_DEBUG << "Visu task at " << armem::toStringMilliSeconds(timestamp);
 
                try
                {
                    visualizeOnce(timestamp);
                }
                catch (const std::exception& e)
                {
                    ARMARX_WARNING << "Caught exception while visualizing robots: \n" << e.what();
                }
                catch (...)
                {
                    ARMARX_WARNING << "Caught unknown exception while visualizing robots.";
                }
 
                if (debugObserver.has_value())
                {
                    debugObserver->sendDebugObserverBatch();
                }
            }
            metronome.waitForNextTick();
        }
    }
 
 
    void
    Visu::visualizeOnce(const Time& timestamp)
    {
        TIMING_START(tVisuTotal);
 
        // TODO(fabian.reister): use timestamp
 
        const Duration maxAge = Duration::MilliSeconds(1000);
 
        // Get data.
        TIMING_START(tVisuGetData);
 
        TIMING_START(tRobotDescriptions);
        const std::map<std::string, std::vector<armarx::armem::human::HumanPose>>
            humanPosesPerProvider = poseSegment.getHumanPoseEntities(maxAge);
        TIMING_END_STREAM(tRobotDescriptions, ARMARX_DEBUG);
 
        TIMING_END_STREAM(tVisuGetData, ARMARX_DEBUG);
 
        // Build layers.
        TIMING_START(tVisuBuildLayers);
 
        std::vector<viz::Layer> providerLayers;
        for (const auto& [providerName, humanPoses] : humanPosesPerProvider)
        {
            viz::Layer layerSkeleton = arviz.layer("HumanPoses_" + providerName);
            viz::Layer layerFrames = arviz.layer("HumanPoses_" + providerName + "_Frames");
            armarx::viz::HumanPoseLayers layers {.skeleton = layerSkeleton, .frames = layerFrames};
 
            visualizeHumanPoses(layers, humanPoses);
            providerLayers.push_back(layerSkeleton);
            providerLayers.push_back(layerFrames);
        }
 
        TIMING_END_STREAM(tVisuBuildLayers, ARMARX_DEBUG);
 
 
        // Commit layers.
 
        ARMARX_DEBUG << "Commit visualization ...";
        TIMING_START(tVisuCommit);
        arviz.commit(providerLayers);
        TIMING_END_STREAM(tVisuCommit, ARMARX_DEBUG);
 
        TIMING_END_STREAM(tVisuTotal, ARMARX_DEBUG);
 
        if (debugObserver.has_value())
        {
            const std::string p = "Visu | ";
            debugObserver->setDebugObserverDatafield(p + "t Total (ms)",
                                                     tVisuTotal.toMilliSecondsDouble());
            debugObserver->setDebugObserverDatafield(p + "t 1 Get Data (ms)",
                                                     tVisuGetData.toMilliSecondsDouble());
            debugObserver->setDebugObserverDatafield(p + "t 1.1 Descriptions (ms)",
                                                     tRobotDescriptions.toMilliSecondsDouble());
 
            debugObserver->setDebugObserverDatafield(p + "t 2 Build Layers (ms)",
                                                     tVisuBuildLayers.toMilliSecondsDouble());
            debugObserver->setDebugObserverDatafield(p + "t 3 Commit (ms)",
                                                     tVisuCommit.toMilliSecondsDouble());
        }
    }
 
} // namespace armarx::armem::server::human