SceneProvider.cpp

Go to the documentation of this file.

#include "SceneProvider.h"
 
#include <optional>
#include <utility>
#include <vector>
 
#include <VirtualRobot/SceneObjectSet.h> // IWYU pragma: keep
#include <VirtualRobot/XML/BaseIO.h>
#include <VirtualRobot/XML/ObjectIO.h>
#include <VirtualRobot/XML/RobotIO.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.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/time/Frequency.h>
#include <ArmarXCore/core/time/Metronome.h>
#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <RobotAPI/libraries/ArmarXObjects/ObjectInfo.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
#include <RobotAPI/libraries/ArmarXObjects/util.h>
#include <RobotAPI/libraries/armem_robot_state/types.h>
 
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/orientation_aware/Costmap3D.h>
#include <armarx/navigation/core/DynamicScene.h>
#include <armarx/navigation/core/StaticScene.h>
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/core/types.h>
#include <armarx/navigation/memory/client/costmap/Reader.h>
#include <armarx/navigation/memory/client/costmap3d/Reader.h>
#include <armarx/navigation/memory/client/graph/Reader.h>
#include <armarx/navigation/memory/client/human/Reader.h>
#include <armarx/navigation/memory/client/laser_scanner_features/Reader.h>
 
namespace armarx::navigation::server::scene_provider
{
 
    SceneProvider::SceneProvider(const InjectedServices& srv, const Config& config) :
        srv(srv), config(config)
    {
    }
 
    const core::Scene&
    SceneProvider::scene() const
    {
        return scn;
    }
 
    bool
    SceneProvider::initialize(const DateTime& timestamp)
    {
        scn.timestamp = timestamp;
 
        // Only load structure. This includes the primitive models (no mesh files though).
        scn.robot = srv.virtualRobotReader->getRobotWaiting(
            config.robotName, timestamp, VirtualRobot::RobotIO::RobotDescription::eStructure);
        scn.robot->setPrimitiveApproximationModel(config.primitiveApproximationModels);
 
        ARMARX_CHECK_NOT_NULL(scn.robot);
 
        scn.staticScene.emplace(getStaticScene(timestamp));
        scn.dynamicScene.emplace(getDynamicScene(timestamp));
        scn.graph = getSceneGraph(timestamp);
 
        return true; // TODO(fabian.reister): return false if sync fails
    }
 
    bool
    SceneProvider::synchronize(const DateTime& timestamp, const bool fullUpdate)
    {
        scn.timestamp = timestamp;
 
        ARMARX_CHECK_NOT_NULL(srv.virtualRobotReader);
        ARMARX_CHECK(srv.virtualRobotReader->synchronizeRobotPose(*scn.robot, timestamp));
 
        if (fullUpdate)
        {
            scn.staticScene.emplace(getStaticScene(timestamp));
        }
 
 
        scn.dynamicScene.emplace(getDynamicScene(timestamp));
 
        const auto updateAttachedObjects = [this](const std::vector<objpose::ObjectPose*>& objPoses)
        {
            for (auto* objPose : objPoses)
            {
                objPose->updateAttached(scn.robot);
            }
        };
 
        updateAttachedObjects(scn.dynamicScene->attachedObjects);
 
        // scn.graph = getSceneGraph(timestamp);
 
        const auto platformState =
            srv.virtualRobotReader->queryPlatformState(scn.robot->getName(), timestamp);
        ARMARX_CHECK(platformState.has_value())
            << "Could not get a platform state for ts: " << VAROUT(timestamp);
 
        scn.platformVelocity = core::Twist{.linear = platformState->twist.linear,
                                           .angular = platformState->twist.angular};
 
        return true; // TODO(fabian.reister): return false if sync fails
    }
 
    core::StaticScene
    SceneProvider::getStaticScene(const DateTime& timestamp) const
    {
        ARMARX_TRACE;
 
        ARMARX_VERBOSE << "Getting static scene info";
 
        const objpose::ObjectPoseSeq objectPoses = srv.objectPoseClient.fetchObjectPoses();
 
        objpose::ObjectPoseMap objectMap;
        for (const objpose::ObjectPose& object : objectPoses)
        {
            objectMap.emplace(object.objectID, object);
        }
 
        std::vector<ObjectInfo> objectInfo = srv.objectPoseClient.objectFinder.findAllObjects();
 
        // remove those objects that belong to an object dataset. the manipulation object / distance computation is broken
        auto objectPosesStatic = armarx::objpose::util::filterObjects(
            objectPoses, {"KIT", "HOPE", "MDB", "YCB", "Kitchen", "Maintenance"});
 
        ARMARX_VERBOSE << "Static objects:";
        for (const auto& objectPose : objectPosesStatic)
        {
            ARMARX_VERBOSE << "- " << objectPose.objectID;
        }
 
        // we maintain a local cache of the objects to avoid having to reload them every iteration
        objects = armarx::objpose::util::asSceneObjects(
            objects, objectPosesStatic, VirtualRobot::ObjectIO::eCollisionModel);
 
        ARMARX_CHECK_NOT_NULL(objects);
        ARMARX_VERBOSE << objects->getSize() << " objects in the scene";
 
        ARMARX_VERBOSE << "Retrieving costmap in memory";
 
        ARMARX_TRACE;
 
        const algorithms::Costmap costmap = [&]()
        {
            Metronome metronome(Frequency::Hertz(10));
            // waiting for static costmap to become available
            while (true)
            {
                const memory::client::costmap::Reader::Query query{
                    .providerName = config.staticCostmapProviderName,
                    .name = config.staticCostmapName,
                    .timestamp = armarx::Clock::Now()};
 
 
                if (const memory::client::costmap::Reader::Result costmap =
                        srv.costmapReader->query(query))
                {
                    ARMARX_CHECK(costmap.costmap.has_value());
                    ARMARX_VERBOSE << "Static costmap available.";
                    return costmap.costmap.value();
                }
 
                ARMARX_INFO << deactivateSpam(5) << "Static costmap `" << query.name
                            << "` from provider " << query.providerName << " not available yet.";
 
                metronome.waitForNextTick();
            }
        }();
 
        const auto costmap3d = [&]() -> std::optional<algorithms::orientation_aware::Costmap3D>
        {
            const memory::client::costmap3d::Reader::Query query{.providerName =
                                                                     config.costmap3dProviderName,
                                                                 .name = config.costmap3dName,
                                                                 .timestamp = armarx::Clock::Now()};
 
 
            if (const memory::client::costmap3d::Reader::Result costmap =
                    srv.costmap3dReader->query(query))
            {
                ARMARX_CHECK(costmap.costmap.has_value());
                ARMARX_VERBOSE << "Static costmap3d available.";
                return costmap.costmap;
            }
 
            ARMARX_INFO << deactivateSpam(5) << "Static costmap3d `" << query.name
                        << "` from provider " << query.providerName << " not available yet.";
            return std::nullopt;
        }();
 
        const auto locations = srv.graphReader->locations();
 
        ARMARX_INFO  << deactivateSpam(5) << "Retrieved static costmap";
 
        return {.objects = objects,
                .objectMap = std::move(objectMap),
                .objectInfo = std::move(objectInfo),
                .distanceToObstaclesCostmap = costmap,
                .orientationAwareCostmap = costmap3d,
                .locations = std::move(locations)};
    }
 
    core::DynamicScene
    SceneProvider::getDynamicScene(const DateTime& timestamp)
    {
        const memory::client::human::Reader::Query queryHumans{
            .providerName = config.humanProviderName,
            .timestamp = timestamp,
            .maxAge = Duration::MilliSeconds(500)};
 
        const memory::client::laser_scanner_features::Reader::Query queryFeatures{
            .providerName = config.laserScannerFeaturesProviderName,
            .name = "global",
            .timestamp = timestamp};
 
        // add all object that are attached to the root node of the robot
        std::vector<objpose::ObjectPose*> attachedObjects;
        const std::string& rootNode = scn.robot->getRootNode()->getName();
        for (auto& [_, obj] : scn.staticScene->objectMap)
        {
            if (obj.attachment.has_value())
            {
                if (obj.attachment->frameName == rootNode)
                {
                    attachedObjects.emplace_back(&obj);
                }
            }
        }
 
        return {.humans = srv.humanReader->queryHumans(queryHumans).humans,
                .laserScannerFeatures =
                    srv.laserScannerFeaturesReader->queryData(queryFeatures).features,
                .attachedObjects = std::move(attachedObjects)};
    }
 
    core::SceneGraph
    SceneProvider::getSceneGraph(const DateTime& /*timestamp*/) const
    {
        ARMARX_CHECK_NOT_NULL(srv.graphReader);
        return {.subgraphs = srv.graphReader->graphs()};
    }
 
} // namespace armarx::navigation::server::scene_provider