Visu.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    Navigation::ArmarXObjects::NavigationMemory
 * @author     Rainer Kartmann ( rainer dot kartmann at kit dot edu )
 * @date       2021
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "Visu.h"
 
#include <map>
#include <memory>
#include <optional>
#include <set>
#include <string>
#include <utility>
#include <vector>
 
#include <Ice/LocalException.h>
 
#include <range/v3/range/conversion.hpp>
#include <range/v3/view/enumerate.hpp>
#include <range/v3/view/filter.hpp>
#include <range/v3/view/transform.hpp>
 
#include <SimoxUtility/color/Color.h>
#include <SimoxUtility/color/GlasbeyLUT.h>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/core/util/StringHelpers.h>
 
#include "RobotAPI/components/ArViz/Client/elements/Path.h"
#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/components/ArViz/Client/Elements.h>
#include <RobotAPI/components/ArViz/Client/elements/Color.h>
#include <RobotAPI/components/ArViz/Client/elements/Robot.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectInfo.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h>
#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem_locations/aron/Location.aron.generated.h>
#include <RobotAPI/libraries/aron/common/aron_conversions/framed.h>
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include <armarx/navigation/algorithms/Room.h>
#include <armarx/navigation/algorithms/aron/Room.aron.generated.h>
#include <armarx/navigation/algorithms/aron_conversions.h>
#include <armarx/navigation/algorithms/visualization.h>
#include <armarx/navigation/conversions/eigen.h>
#include <armarx/navigation/core/Graph.h>
#include <armarx/navigation/core/LocationUtils.h>
#include <armarx/navigation/core/aron/Graph.aron.generated.h>
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/graph/Visu.h>
#include <armarx/navigation/human/aron/Human.aron.generated.h>
#include <armarx/navigation/human/aron_conversions.h>
#include <armarx/navigation/human/types.h>
#include <armarx/navigation/memory/aron/LaserScannerFeatures.aron.generated.h>
#include <armarx/navigation/memory/aron_conversions.h>
#include <armarx/navigation/memory/types.h>
 
namespace armarx::navigation::memory
{
 
    Visu::Visu(viz::Client& arviz,
               const armem::server::wm::CoreSegment& locSegment,
               const armem::server::wm::CoreSegment& graphSegment,
               const armem::server::wm::CoreSegment& costmapSegment,
               const armem::server::wm::CoreSegment& humanSegment,
               const armem::server::wm::CoreSegment& roomsSegment,
               const armem::server::wm::CoreSegment& laserScannerFeaturesSegment) :
        arviz(arviz),
        locSegment(locSegment),
        graphSegment(graphSegment),
        costmapSegment(costmapSegment),
        humanSegment(humanSegment),
        roomsSegment(roomsSegment),
        laserScannerFeaturesSegment(laserScannerFeaturesSegment),
        visu(std::make_unique<graph::GraphVisu>())
    {
    }
 
    Visu::~Visu()
    {
    }
 
    void
    Visu::drawLocations(const objpose::ObjectPoseMap& objects,
                        const std::vector<ObjectInfo>& info,
                        viz::Layer& layer)
    {
        // --- critical section: convert to DTO while holding the lock ---
        std::map<armem::MemoryID, location::arondto::Location> raw;
        locSegment.doLocked(
            [&]()
            {
                using namespace armem::server;
                locSegment.forEachEntity(
                    [&](const wm::Entity& entity)
                    {
                        if (const wm::EntityInstance* instance = entity.findLatestInstance())
                        {
                            location::arondto::Location dto;
                            dto.fromAron(instance->data());
                            raw[entity.id()] = dto;
                        }
                    });
            });
 
        // --- outside lock: convert and visualize ---
        for (const auto& [id, location] : raw)
        {
 
            FramedPose framedPose;
            fromAron(location.framedPose, framedPose);
 
            const auto res = core::resolveLocation(objects, info, framedPose);
            if (res.pose.has_value())
            {
                visu->vertex->draw(layer, id.str(), res.pose.value());
            }
        }
    }
 
    void
    Visu::drawGraphs(const objpose::ObjectPoseMap& objects,
                     const std::vector<ObjectInfo>& info,
                     std::vector<viz::Layer>& layers)
    {
        // --- critical section: convert to DTO while holding the lock ---
        std::map<armem::MemoryID, navigation::core::arondto::Graph> raw;
        graphSegment.doLocked(
            [&]()
            {
                using namespace armem::server;
                graphSegment.forEachEntity(
                    [&](const wm::Entity& entity)
                    {
                        if (const wm::EntityInstance* instance = entity.findLatestInstance())
                        {
                            navigation::core::arondto::Graph dto;
                            dto.fromAron(instance->data());
                            raw[entity.id()] = dto;
                        }
                    });
            });
 
        // --- outside lock: convert and visualize ---
        for (auto& [id, dto] : raw)
        {
            viz::Layer& layer = layers.emplace_back(arviz.layer(id.str()));
            core::Graph graph;
            fromAron(dto, graph);
            core::resolveLocations(graph, locSegment);
            visu->draw(layer, graph, {.objects=objects, .info=info});
        }
    }
 
    void
    Visu::drawLocations(std::vector<viz::Layer>& layers)
    {
        viz::Layer& layer = layers.emplace_back(arviz.layer(locSegment.id().str()));
        const objpose::ObjectPoseMap objects;
        const std::vector<ObjectInfo> info;
        drawLocations(objects, info, layer);
    }
 
    void
    Visu::drawLocations(objpose::ObjectPoseClient& objClient, std::vector<viz::Layer>& layers)
    {
        viz::Layer& layer = layers.emplace_back(arviz.layer(locSegment.id().str()));
        if (objClient.isConnected())
        {
            try
            {
                // this call might fail => "no object with id 'ObjectMemory' registered."
                const objpose::ObjectPoseMap objects = objClient.fetchObjectPosesAsMap();
 
                auto objectFinder = objClient.getObjectFinder();
                objectFinder.setLogObjectDiscoveryError(false);
                const std::vector<ObjectInfo> info = objectFinder.findAllObjects();
 
                drawLocations(objects, info, layer);
            }
            catch (const ::Ice::NotRegisteredException& e)
            {
                ARMARX_VERBOSE << "Failed to retrieve objects from ObjectMemory: " << e.what();
            }
        }
    }
 
    void
    Visu::drawGraphs(std::vector<viz::Layer>& layers)
    {
        const objpose::ObjectPoseMap objects;
        const std::vector<ObjectInfo> info;
        drawGraphs(objects, info, layers);
    }
 
    void
    Visu::drawGraphs(objpose::ObjectPoseClient& objClient, std::vector<viz::Layer>& layers)
    {
        if (objClient.isConnected())
        {
            try
            {
                // this call might fail => "no object with id 'ObjectMemory' registered."
                const objpose::ObjectPoseMap objects = objClient.fetchObjectPosesAsMap();
 
                auto objectFinder = objClient.getObjectFinder();
                objectFinder.setLogObjectDiscoveryError(false);
                const std::vector<ObjectInfo> info = objectFinder.findAllObjects();
 
                drawGraphs(objects, info, layers);
            }
            catch (const ::Ice::NotRegisteredException& e)
            {
                ARMARX_VERBOSE << "Failed to retrieve objects from ObjectMemory: " << e.what();
            }
        }
    }
 
    void
    Visu::drawCostmaps(std::vector<viz::Layer>& layers, const float zOffset)
    {
        // --- critical section: convert to Costmap while holding the lock ---
        struct RawCostmap
        {
            armem::MemoryID id;
            algorithms::Costmap costmap;
        };
 
        std::vector<RawCostmap> raw;
 
        costmapSegment.doLocked(
            [&]()
            {
                using namespace armem::server;
                costmapSegment.forEachEntity(
                    [&](const wm::Entity& entity)
                    {
                        if (const wm::EntityInstance* instance = entity.findLatestInstance())
                        {
                            const std::string entityIdStr = instance->id().getEntityID().str();
                            const auto it = lastCostmapVisualization_.find(entityIdStr);
                            if (it == lastCostmapVisualization_.end() ||
                                it->second < instance->id().timestamp)
                            {
                                raw.push_back(
                                    {instance->id(),
                                     algorithms::costmapFromAron(instance->data())});
                                lastCostmapVisualization_[entityIdStr] = instance->id().timestamp;
                            }
                        }
                    });
            });
 
        // --- outside lock: visualize ---
        for (auto& [id, costmap] : raw)
        {
 
            viz::Layer& layer = layers.emplace_back(
                arviz.layer("costmaps_" + id.providerSegmentName + "_" + id.entityName));
            algorithms::visualize(costmap, layer, id.entityName, zOffset);
        }
    }
 
    void
    Visu::drawHumans(std::vector<viz::Layer>& layers,
                     const bool visuTransparent,
                     const Duration maxAge)
    {
        struct RawHuman
        {
            std::string providerName;
            DateTime referencedTime;
            navigation::human::arondto::Human dto;
        };
 
        std::set<std::string> providerNames;
        std::vector<RawHuman> raw;
 
        const DateTime timestamp = Clock::Now();
 
        // --- critical section: convert to DTO while holding the lock ---
        humanSegment.doLocked(
            [&]()
            {
                using namespace armem::server;
                humanSegment.forEachEntity(
                    [&](const wm::Entity& entity)
                    {
                        providerNames.insert(entity.id().providerSegmentName);
                        entity.getLatestSnapshot().forEachInstance(
                            [&](const armarx::armem::wm::EntityInstance& instance)
                            {
                                raw.push_back(
                                    {instance.id().providerSegmentName,
                                     instance.metadata().referencedTime,
                                     navigation::human::arondto::Human::FromAron(
                                         instance.data())});
                            });
                    });
            });
 
        // --- outside lock: filter by age, convert, and visualize ---
        std::map<std::string, navigation::human::Humans> namedProviderHumans;
        for (const auto& providerName : providerNames)
        {
            namedProviderHumans[providerName]; // ensure layer is cleared even if no humans remain
        }
 
        for (const auto& entry : raw)
        {
            const Duration dtToNow = timestamp - entry.referencedTime;
            if (dtToNow < maxAge and dtToNow.isPositive())
            {
                navigation::human::Human human;
                fromAron(entry.dto, human);
                namedProviderHumans[entry.providerName].emplace_back(std::move(human));
            }
        }
 
        for (const auto& [providerName, humans] : namedProviderHumans)
        {
            viz::Layer& layer = layers.emplace_back(arviz.layer("humans_" + providerName));
            drawHumansInLayer(humans, layer, visuTransparent);
        }
    }
 
    void
    Visu::drawRooms(std::vector<viz::Layer>& layers)
    {
        // --- critical section: convert to DTO while holding the lock ---
        std::map<armem::MemoryID, std::optional<navigation::algorithms::arondto::Room>> raw;
        roomsSegment.doLocked(
            [&]()
            {
                using namespace armem::server;
                roomsSegment.forEachEntity(
                    [&](const wm::Entity& entity)
                    {
                        std::optional<navigation::algorithms::arondto::Room> dto;
                        if (const wm::EntityInstance* instance = entity.findLatestInstance())
                        {
                            auto& d = dto.emplace();
                            d.fromAron(instance->data());
                        }
                        raw[entity.id()] = dto;
                    });
            });
 
        // --- outside lock: convert and visualize ---
        int i = 0;
        for (const auto& [id, dto] : raw)
        {
            viz::Layer& layer = layers.emplace_back(arviz.layer(id.str()));
            if (dto.has_value())
            {
                algorithms::Room room;
                fromAron(dto.value(), room);
                drawRoom(layer, room, simox::color::GlasbeyLUT::at(i));
            }
            // else: empty layer clears previous visualization
            ++i;
        }
    }
 
    void
    Visu::drawLaserScannerFeatures(std::vector<viz::Layer>& layers)
    {
        static const std::string globalEntityName = "global";
 
        viz::Layer& convexHullLayer =
            layers.emplace_back(arviz.layer("laser_scanner_features_convex_hulls"));
        viz::Layer& chainLayer = layers.emplace_back(arviz.layer("laser_scanner_features_chains"));
 
        struct RawLaserScannerFeatures
        {
            armem::MemoryID id;
            memory::arondto::LaserScannerFeatures dto;
        };
 
        // --- critical section: convert to aron DTO while holding the lock ---
        std::vector<RawLaserScannerFeatures> raw;
        laserScannerFeaturesSegment.doLocked(
            [&]()
            {
                using namespace armem::server;
                laserScannerFeaturesSegment.forEachEntity(
                    [&](const wm::Entity& entity)
                    {
                        if (const wm::EntityInstance* instance = entity.findLatestInstance())
                        {
                            auto& entry = raw.emplace_back();
                            entry.id = instance->id();
                            entry.dto.fromAron(instance->data());
                        }
                    });
            });
 
        if (raw.empty())
        {
            return;
        }
 
 
        // --- outside lock: convert to business objects and visualize ---
        static constexpr float zOffset = 20.F;
 
        // The segment holds the features of several providers (e.g. laser_scanner_feature_extraction
        // and dynamic_scene_provider). Draw each provider in its own color and 1 cm above the
        // previous one, so that it is visible which component contributed which feature.
        static constexpr float providerZSpacing = 10.F;
 
        namespace rv = ranges::views;
        const auto globalFeatures =
            raw |
            rv::transform(
                [](const RawLaserScannerFeatures& r)
                    -> std::pair<armem::MemoryID, memory::LaserScannerFeatures>
                {
                    memory::LaserScannerFeatures features;
                    fromAron(r.dto, features);
                    return {r.id, features};
                }) |
            rv::filter([](const auto& p) noexcept
                       { return p.second.frame == armarx::GlobalFrame; }) |
            ranges::to_vector;
 
        // Assign the color/height per provider in alphabetical order, so that a provider keeps its
        // color across runs regardless of the order the segment is iterated in.
        std::map<std::string, std::size_t> providerIndices;
        for (const auto& [id, features] : globalFeatures)
        {
            providerIndices.emplace(id.providerSegmentName, 0);
        }
        {
            std::size_t nextProviderIndex = 0;
            for (auto& [providerSegmentName, index] : providerIndices)
            {
                index = nextProviderIndex++;
            }
        }
 
        for (const auto& [id, features] : globalFeatures)
        {
            const std::string idStr = id.str();
 
            const std::size_t provider = providerIndices.at(id.providerSegmentName);
            const float z = zOffset + static_cast<float>(provider) * providerZSpacing;
            const simox::Color providerColor = simox::color::GlasbeyLUT::at(provider);
 
            ARMARX_VERBOSE << deactivateSpam(id.providerSegmentName, 5)
                           << "Drawing laser scanner features of provider "
                           << QUOTED(id.providerSegmentName) << " at z " << z << " mm";
 
            for (const auto& [index, feature] : ranges::views::enumerate(features.features))
            {
 
                if (not feature.convexHull.empty())
                {
                    viz::Polygon polygon("convex_hull_" + idStr + "_" + std::to_string(index));
                    for (const Eigen::Vector2f& pt : feature.convexHull)
                    {
                        polygon.addPoint(Eigen::Vector3f(pt.x(), pt.y(), z));
                    }
                    polygon.color(providerColor.with_alpha(80));
                    polygon.lineColor(providerColor);
                    polygon.lineWidth(3.F);
                    convexHullLayer.add(polygon);
                }
 
                if (feature.chain.size() >= 2)
                {
                    std::vector<Eigen::Vector3f> pts3d;
                    pts3d.reserve(feature.chain.size());
                    for (const Eigen::Vector2f& pt : feature.chain)
                    {
                        pts3d.emplace_back(pt.x(), pt.y(), z);
                    }
                    chainLayer.add(viz::Path("chain_" + idStr + "_" + std::to_string(index))
                                       .points(pts3d)
                                       .width(5.F)
                                       .color(providerColor));
                }
            }
        }
    }
 
    void
    Visu::drawRoom(viz::Layer& layer, const algorithms::Room& room, const simox::Color& color) const
    {
        viz::Polygon polygon(room.name + "_polygon");
        viz::Path path(room.name + "_path");
 
        for (const auto& point : room.polygon)
        {
            polygon.addPoint(conv::to3D(point));
            path.addPoint(conv::to3D(point));
        }
 
        // close polygon
        path.addPoint(conv::to3D(room.polygon.front()));
 
        polygon.color(color.with_alpha(50));
        polygon.lineColor(simox::Color::black());
        polygon.lineWidth(0);
 
        layer.add(viz::Text(room.name + "_text")
                      .text(room.name)
                      .position(conv::to3D(room.center()))
                      .scale(5));
        layer.add(polygon);
        layer.add(path);
    }
 
    void
    Visu::drawHumansInLayer(const navigation::human::Humans& humans,
                            viz::Layer& layer,
                            const bool visuTransparent)
    {
        const Eigen::Translation3f human_T_mmm(Eigen::Vector3f{0, 0, 1000});
 
        ARMARX_VERBOSE << deactivateSpam(1) << "Visualizing " << humans.size() << " humans";
        for (const auto& [i, human] : ranges::views::enumerate(humans))
        {
            const std::string idx = std::to_string(i);
            core::Pose human3d = conv::to3D(human.pose) * human_T_mmm;
 
            viz::Robot mmm("human_" + idx);
            mmm.file("RobotAPI", "RobotAPI/robots/MMM/mmm.xml");
            mmm.pose(human3d);
            mmm.scale(1.7);
            mmm.overrideColor(viz::Color::orange(255, visuTransparent ? 100 : 255));
            layer.add(mmm);
 
            if (human.linearVelocity != Eigen::Vector2f::Zero())
            {
                core::Pose vel3d = human3d;
                vel3d.translation().head<2>() += human.linearVelocity * 2;
                layer.add(viz::Arrow("human_vel_" + idx)
                              .fromTo(human3d.translation(), vel3d.translation())
                              .color(simox::Color::red()));
            }
        }
    }
 
} // namespace armarx::navigation::memory