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 <string>
#include <utility>
#include <vector>
 
#include <Ice/LocalException.h>
 
#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 "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>
 
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) :
        arviz(arviz),
        locSegment(locSegment),
        graphSegment(graphSegment),
        costmapSegment(costmapSegment),
        humanSegment(humanSegment),
        roomsSegment(roomsSegment),
        visu(std::make_unique<graph::GraphVisu>())
    {
    }
 
    Visu::~Visu()
    {
    }
 
    void
    Visu::drawLocations(const objpose::ObjectPoseMap& objects,
                        const std::vector<ObjectInfo>& info,
                        viz::Layer& layer)
    {
        using namespace armem::server;
 
        std::map<armem::MemoryID, location::arondto::Location> locations;
        locSegment.doLocked(
            [&]()
            {
                locSegment.forEachEntity(
                    [&](const wm::Entity& entity)
                    {
                        if (const wm::EntityInstance* instance = entity.findLatestInstance())
                        {
                            locations[entity.id()].fromAron(instance->data());
                        }
                    });
            });
 
        for (auto& [id, location] : locations)
        {
            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,
                     bool enabled)
    {
        using namespace armem::server;
 
        std::map<armem::MemoryID, core::Graph> graphs;
        graphSegment.doLocked(
            [&]()
            {
                graphSegment.forEachEntity(
                    [&](const wm::Entity& entity)
                    {
                        core::Graph& graph = graphs[entity.id()];
                        if (enabled)
                        {
                            if (const wm::EntityInstance* instance = entity.findLatestInstance())
                            {
                                navigation::core::arondto::Graph aron;
                                aron.fromAron(instance->data());
                                fromAron(aron, graph);
                            }
                        }
                        // else: empty layer
                    });
            });
 
        for (auto& [id, graph] : graphs)
        {
            viz::Layer& layer = layers.emplace_back(arviz.layer(id.str()));
            if (enabled)
            {
                core::resolveLocations(graph, locSegment);
                visu->draw(layer, graph, {objects, info});
            }
            // else: clear layer
        }
    }
 
    void
    Visu::drawLocations(std::vector<viz::Layer>& layers, bool enabled)
    {
        viz::Layer& layer = layers.emplace_back(arviz.layer(locSegment.id().str()));
        if (enabled)
        {
            const objpose::ObjectPoseMap objects;
            const std::vector<ObjectInfo> info;
            drawLocations(objects, info, layer);
        }
    }
 
    void
    Visu::drawLocations(objpose::ObjectPoseClient& objClient,
                        std::vector<viz::Layer>& layers,
                        bool enabled)
    {
        viz::Layer& layer = layers.emplace_back(arviz.layer(locSegment.id().str()));
        if (enabled and 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, bool enabled)
    {
        const objpose::ObjectPoseMap objects;
        const std::vector<ObjectInfo> info;
        drawGraphs(objects, info, layers, enabled);
    }
 
    void
    Visu::drawGraphs(objpose::ObjectPoseClient& objClient,
                     std::vector<viz::Layer>& layers,
                     bool enabled)
    {
        if (enabled and 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, enabled);
            }
            catch (const ::Ice::NotRegisteredException& e)
            {
                ARMARX_VERBOSE << "Failed to retrieve objects from ObjectMemory: " << e.what();
            }
        }
    }
 
    namespace
    {
 
        inline void
        visualize(const 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& human : humans)
            {
                // viz::Cylinder cylinder(std::to_string(layer.size()));
                // cylinder.fromTo(conv::to3D(human.pose.translation()),
                //                 conv::to3D(human.pose.translation()) + Eigen::Vector3f{0, 0, 10});
 
 
                // cylinder.color(simox::Color::orange());
                // cylinder.radius(300);
                // layer.add(cylinder);
 
                core::Pose human3d = conv::to3D(human.pose) * human_T_mmm;
 
                viz::Robot mmm(std::to_string(layer.size()));
                mmm.file("RobotAPI", "RobotAPI/robots/MMM/mmm.xml");
                mmm.pose(human3d);
                mmm.scale(1.7); // 1.7m
                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;
                    auto arrow = viz::Arrow(std::to_string(layer.size()))
                                     .fromTo(human3d.translation(), vel3d.translation())
                                     .color(simox::Color::red());
                    layer.add(arrow);
                }
            }
        }
 
    } // namespace
 
    void
    Visu::drawCostmaps(std::vector<viz::Layer>& layers, const bool enabled, const float zOffset)
    {
        if (not enabled)
        {
            return;
        }
 
        std::map<std::string, std::vector<std::pair<std::string, algorithms::Costmap>>>
            namedProviderCostmaps;
 
        costmapSegment.doLocked(
            [&]()
            {
                using namespace armem::server;
 
                costmapSegment.forEachEntity(
                    [&](const wm::Entity& entity)
                    {
                        if (const wm::EntityInstance* instance = entity.findLatestInstance())
                        {
                            const auto it =
                                lastCostmapVisualization_.find(instance->id().getEntityID().str());
                            // only visualize this costmap if it's timestamp is newer than the last one'
                            if (it == lastCostmapVisualization_.end() ||
                                it->second < instance->id().timestamp)
                            {
                                navigation::algorithms::Costmap costmap =
                                    algorithms::fromAron(*instance);
 
                                namedProviderCostmaps[instance->id().providerSegmentName]
                                    .emplace_back(instance->id().entityName, std::move(costmap));
 
                                lastCostmapVisualization_[instance->id().getEntityID().str()] =
                                    instance->id().timestamp;
                            }
                        }
                    });
            });
 
        for (const auto& [providerName, namedCostmaps] : namedProviderCostmaps)
        {
            for (const auto& [name, costmap] : namedCostmaps)
            {
                viz::Layer& layer =
                    layers.emplace_back(arviz.layer("costmaps_" + providerName + "_" + name));
                algorithms::visualize(costmap, layer, name, zOffset);
            }
        }
    }
 
    void
    Visu::drawRooms(std::vector<viz::Layer>& layers, const bool enabled)
    {
        using namespace armem::server;
 
        std::map<armem::MemoryID, algorithms::Room> rooms;
        roomsSegment.doLocked(
            [&]()
            {
                roomsSegment.forEachEntity(
                    [&](const wm::Entity& entity)
                    {
                        algorithms::Room& room = rooms[entity.id()];
                        if (enabled)
                        {
                            if (const wm::EntityInstance* instance = entity.findLatestInstance())
                            {
                                navigation::algorithms::arondto::Room aron;
                                aron.fromAron(instance->data());
                                fromAron(aron, room);
                            }
                        }
                        // else: empty layer
                    });
            });
 
        int i = 0;
        for (auto& [id, room] : rooms)
        {
            viz::Layer& layer = layers.emplace_back(arviz.layer(id.str()));
            if (enabled)
            {
                const auto color = simox::color::GlasbeyLUT::at(i++);
 
                drawRoom(layer, room, color);
            }
            // else: clear layer
        }
    }
 
    void
    Visu::drawRoom(viz::Layer& layer, const algorithms::Room& room, const simox::Color& color) const
    {
        viz::Polygon polygon(room.name);
        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(polygon);
 
        layer.add(path);
    }
 
    void
    Visu::drawHumans(std::vector<viz::Layer>& layers,
                     const bool enabled,
                     const bool visuTransparent,
                     const Duration maxAge)
    {
        if (not enabled)
        {
            return;
        }
 
        std::map<std::string, navigation::human::Humans> namedProviderHumans;
        const DateTime timestamp = Clock::Now();
 
        humanSegment.doLocked(
            [&]()
            {
                using namespace armem::server;
 
                humanSegment.forEachEntity(
                    [&](const wm::Entity& entity)
                    {
                        namedProviderHumans[entity.id().providerSegmentName];
                        entity.getLatestSnapshot().forEachInstance(
                            [&namedProviderHumans, &timestamp, &maxAge](
                                const armarx::armem::wm::EntityInstance& instance)
                            {
                                const Duration dtToNow =
                                    timestamp - instance.metadata().referencedTime;
 
                                if (dtToNow < maxAge and dtToNow.isPositive())
                                {
                                    const auto dto = navigation::human::arondto::Human::FromAron(
                                        instance.data());
 
                                    navigation::human::Human human;
                                    fromAron(dto, human);
 
                                    namedProviderHumans[instance.id().providerSegmentName]
                                        .emplace_back(std::move(human));
                                };
                            });
                    });
            });
 
        for (const auto& [providerName, humans] : namedProviderHumans)
        {
            viz::Layer& layer = layers.emplace_back(arviz.layer("humans_" + providerName));
            visualize(humans, layer, visuTransparent);
        }
    }
 
 
} // namespace armarx::navigation::memory