Visu.cpp

Go to the documentation of this file.

#include "Visu.h"
 
#include <algorithm>
#include <exception>
#include <string>
 
#include <Eigen/Geometry>
 
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <SimoxUtility/math/pose.h>
#include <SimoxUtility/math/rescale.h>
#include <VirtualRobot/Nodes/Sensor.h>
#include <VirtualRobot/XML/RobotIO.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 <RobotAPI/libraries/armem_robot_state/server/proprioception/SensorValues.h>
 
#include "combine.h"
 
namespace armarx::armem::server::robot_state
{
 
    Visu::Visu(const description::Segment& descriptionSegment,
               const proprioception::Segment& proprioceptionSegment,
               const localization::Segment& localizationSegment) :
        descriptionSegment(descriptionSegment),
        proprioceptionSegment(proprioceptionSegment),
        localizationSegment(localizationSegment)
    {
        Logging::setTag("Visu");
    }
 
    void
    Visu::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
    {
        defs->optional(
            p.enabled, prefix + "enabled", "Enable or disable visualization of objects.");
        defs->optional(p.frequencyHz, prefix + "frequenzyHz", "Frequency of visualization.");
        defs->optional(p.framesEnabled,
                       prefix + "framesEnabled",
                       "Enable or disable visualization of frames.");
        defs->optional(p.forceTorque.enabled,
                       prefix + "forceTorque.enabled",
                       "Enable or disable visualization of force torque sensors.");
        defs->optional(p.forceTorque.forceScale,
                       prefix + "forceTorque.forceScale",
                       "Scaling of force arrows.");
        defs->optional(p.collisionModelEnabled,
                       prefix + "collisionModelEnabled",
                       "Enable visualization of collision model.");
    }
 
    void
    Visu::init()
    {
    }
 
    void
    Visu::connect(const viz::Client& arviz, DebugObserverInterfacePrx debugObserver)
    {
        this->arviz = arviz;
        if (debugObserver)
        {
            bool batchMode = true;
            this->debugObserver = DebugObserverHelper("RobotStateMemory", debugObserver, batchMode);
        }
 
        if (updateTask)
        {
            updateTask->stop();
            updateTask->join();
            updateTask = nullptr;
        }
        updateTask = new SimpleRunningTask<>([this]() { this->visualizeRun(); });
        updateTask->start();
    }
 
    void
    Visu::visualizeRobots(viz::Layer& layer,
                          const armem::robot_state::Robots& robots,
                          bool useColModel)
    {
        for (const armem::robot_state::Robot& robot : robots)
        {
            const armarx::data::PackagePath xmlPath = robot.description.xml.serialize();
 
            const auto* robotModel = getRobotModel(robot.description);
 
            std::map<std::string, float> jointMap = robot.config.jointMap;
 
            // we override the jointMap with the joint values from the robot model if it has not been set explicitly
            if (robotModel != nullptr)
            {
                for (const auto& [jointName, jointValue] : robotModel->model->getJointValues())
                {
                    if (jointMap.count(jointName) == 0)
                    {
                        jointMap[jointName] = jointValue;
                    }
                }
            }
 
            // clang-format off
            viz::Robot robotVisu = viz::Robot(robot.description.name)
                                   .file(xmlPath.package, xmlPath.package + "/" + xmlPath.path)
                                   .joints(jointMap)
                                   .pose(robot.config.globalPose);
 
            if (not useColModel)
            {
                robotVisu.useFullModel();
            }
            else
            {
                robotVisu.useCollisionModel();
            }
            // clang-format on
 
            layer.add(robotVisu);
        }
    }
 
    void
    Visu::visualizeFrames(
        viz::Layer& layerFrames,
        const std::unordered_map<std::string, std::vector<Eigen::Isometry3f>>& frames)
    {
        for (const auto& [robotName, robotFrames] : frames)
        {
            Eigen::Isometry3f pose = Eigen::Isometry3f::Identity();
 
            int i = 0;
            for (const auto& frame : robotFrames)
            {
                Eigen::Isometry3f from = pose;
                Eigen::Isometry3f to = pose * frame;
 
                pose = to;
 
                layerFrames.add(viz::Arrow(robotName + std::to_string(++i))
                                    .fromTo(from.translation(), to.translation()));
            }
        }
    }
 
    void
    Visu::visualizeFramesIndividual(
        viz::Layer& layerFrames,
        const std::unordered_map<std::string, std::vector<Eigen::Isometry3f>>& frames)
    {
 
        std::vector<std::string> FRAME_NAMES{/*world*/ "map", "odom", "robot"};
 
        for (const auto& [robotName, robotFrames] : frames)
        {
            int i = 0;
            for (const auto& frame : robotFrames)
            {
                layerFrames.add(
                    viz::Pose(robotName + FRAME_NAMES.at(i++)).pose(frame.matrix()).scale(3));
            }
        }
    }
 
    void
    Visu::visualizeRun()
    {
        CycleUtil cycle(static_cast<int>(1000 / 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::out_of_range& e)
                {
                    ARMARX_WARNING << "Caught std::out_of_range while visualizing robots: \n"
                                   << e.what();
                }
                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();
                }
            }
            cycle.waitForCycleDuration();
        }
    }
 
    void
    Visu::visualizeOnce(const Time& timestamp)
    {
        TIMING_START(tVisuTotal);
 
        // TODO(fabian.reister): use timestamp
 
        // Get data.
        TIMING_START(tVisuGetData);
 
        TIMING_START(tRobotDescriptions);
        const description::RobotDescriptionMap robotDescriptions =
            descriptionSegment.getRobotDescriptionsLocking(timestamp);
        TIMING_END_STREAM(tRobotDescriptions, ARMARX_DEBUG);
 
        TIMING_START(tGlobalPoses);
        const auto globalPoses = localizationSegment.getRobotGlobalPosesLocking(timestamp);
        TIMING_END_STREAM(tGlobalPoses, ARMARX_DEBUG);
 
        TIMING_START(tRobotFramePoses);
        const auto frames = localizationSegment.getRobotFramePosesLocking(timestamp);
        TIMING_END_STREAM(tRobotFramePoses, ARMARX_DEBUG);
 
        TIMING_START(tSensorValues);
        const proprioception::SensorValuesMap sensorValues =
            proprioceptionSegment.getSensorValuesLocking(timestamp,
                                                         debugObserver ? &*debugObserver : nullptr);
        TIMING_END_STREAM(tSensorValues, ARMARX_DEBUG);
 
        TIMING_END_STREAM(tVisuGetData, ARMARX_DEBUG);
 
 
        // Build layers.
        TIMING_START(tVisuBuildLayers);
 
        // We need all 3 information:
        // - robot description
        // - global pose
        // - joint positions
        // => this is nothing but an armem::Robot
        ARMARX_DEBUG << "Combining robot ..."
                     << "\n- " << robotDescriptions.size() << " descriptions"
                     << "\n- " << globalPoses.size() << " global poses"
                     << "\n- " << sensorValues.size() << " joint positions";
 
        const armem::robot_state::Robots robots =
            combine(robotDescriptions, globalPoses, sensorValues, timestamp);
 
        ARMARX_DEBUG << "Visualize " << robots.size() << " robots ...";
        std::vector<viz::Layer> layers;
 
        {
            viz::Layer& layer = layers.emplace_back(arviz.layer("Robots"));
            visualizeRobots(layer, robots, false);
        }
 
        if (p.collisionModelEnabled)
        {
            viz::Layer& layer = layers.emplace_back(arviz.layer("RobotsCol"));
            visualizeRobots(layer, robots, true);
        }
 
        ARMARX_DEBUG << "Visualize frames ...";
        if (p.framesEnabled)
        {
            viz::Layer& layer = layers.emplace_back(arviz.layer("Localization Frames"));
            visualizeFrames(layer, frames);
        }
 
        TIMING_END_STREAM(tVisuBuildLayers, ARMARX_DEBUG);
 
        {
            viz::Layer& layer = layers.emplace_back(arviz.layer("Localization Frames Individual"));
            visualizeFramesIndividual(layer, frames);
        }
 
        if (p.forceTorque.enabled)
        {
            viz::Layer& layerForceTorque = layers.emplace_back(arviz.layer("Force Torque"));
            std::stringstream log;
 
            for (const armem::robot_state::Robot& robot : robots)
            {
                const std::string& robotName = robot.description.name;
 
                auto itSensorValues = sensorValues.find(robotName);
                if (itSensorValues == sensorValues.end())
                {
                    log << "\nNo robot '" << robotName << "' in sensor values. Available are: ";
                    log << simox::alg::join(simox::alg::get_keys(sensorValues), ", ");
                    continue;
                }
 
                visualizeForceTorque(layerForceTorque, robot, itSensorValues->second);
            }
 
            if (not log.str().empty())
            {
                ARMARX_INFO << "While visualizing robot force torques: " << log.str();
            }
        }
 
 
        // Commit layers.
 
        ARMARX_DEBUG << "Commit visualization ...";
        TIMING_START(tVisuCommit);
        arviz.commit(layers);
        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 1.2 Global Poses (ms)",
                                                     tGlobalPoses.toMilliSecondsDouble());
            debugObserver->setDebugObserverDatafield(p + "t 1.3 Frames (ms)",
                                                     tRobotFramePoses.toMilliSecondsDouble());
            debugObserver->setDebugObserverDatafield(p + "t 1.4 Sensor Values (ms)",
                                                     tSensorValues.toMilliSecondsDouble());
            debugObserver->setDebugObserverDatafield(p + "t 2 Build Layers (ms)",
                                                     tVisuBuildLayers.toMilliSecondsDouble());
            debugObserver->setDebugObserverDatafield(p + "t 3 Commit (ms)",
                                                     tVisuCommit.toMilliSecondsDouble());
        }
    }
 
    Visu::RobotModel*
    Visu::getRobotModel(const armem::robot_state::description::RobotDescription& robotDescription)
    {
        const std::string& robotName = robotDescription.name;
 
        RobotModel* robotModel = nullptr;
        if (auto it = models.find(robotName); it != models.end())
        {
            robotModel = &it->second;
        }
        else
        {
            const std::filesystem::path robotPath = robotDescription.xml.toSystemPath();
            ARMARX_INFO << "Loading robot model for '" << robotName << "' from " << robotPath
                        << " for visualization.";
            auto [it2, _] = models.emplace(robotName, robotPath);
            robotModel = &it2->second;
        }
        return robotModel;
    }
 
    void
    Visu::visualizeForceTorque(viz::Layer& layer,
                               const armem::robot_state::Robot& robot,
                               const proprioception::SensorValues& sensorValues)
    {
 
        Visu::RobotModel* robotModel = getRobotModel(robot.description);
 
        if (robotModel == nullptr)
        {
            // ARMARX_WARNING << "Robot model is null";
            return;
        }
 
        robotModel->model->setJointValues(robot.config.jointMap);
        robotModel->model->setGlobalPose(robot.config.globalPose.matrix());
 
        const proprioception::ForceTorqueValuesMap& forceTorques =
            sensorValues.forceTorqueValuesMap;
 
        for (const auto& [side, ft] : forceTorques)
        {
            ARMARX_CHECK(side == RobotNameHelper::LocationLeft or
                         side == RobotNameHelper::LocationRight)
                << side;
 
            const std::string forceTorqueSensorName =
                robotModel->nameHelper->getArm(side).getForceTorqueSensor();
 
            const Eigen::Matrix4f forceTorqueSensorPose =
                robotModel->model->getSensor(forceTorqueSensorName)->getGlobalPose();
 
            const std::string xyz = "XYZ";
 
            const Eigen::Vector3f from = simox::math::position(forceTorqueSensorPose);
            for (int i = 0; i < 3; ++i)
            {
                simox::Color color =
                    simox::Color((255 * Eigen::Matrix3i::Identity().col(i)).eval());
                color.a = 128;
 
                // Force arrows.
                const float length = p.forceTorque.forceScale * ft.force(i);
                const float width =
                    std::min(simox::math::rescale(std::abs(length), 0.F, 100.F, 1.F, 10.F), 10.F);
 
                const Eigen::Vector3f to =
                    from + length * simox::math::orientation(forceTorqueSensorPose).col(i);
 
                std::stringstream key;
                ARMARX_CHECK_FITS_SIZE(i, xyz.size());
                key << side << " Force " << xyz.at(i);
                layer.add(viz::Arrow(key.str()).fromTo(from, to).color(color).width(width));
 
                // TODO: Torque circle arrows.
            }
        }
    }
 
    Visu::RobotModel::RobotModel(const std::filesystem::path& robotPath) :
        model{
            VirtualRobot::RobotIO::loadRobot(robotPath, VirtualRobot::RobotIO::eStructure),
        },
        nameHelper{
            RobotNameHelper::Create(robotPath),
        }
    {
    }
 
} // namespace armarx::armem::server::robot_state