SceneProvider.cpp

Go to the documentation of this file.

#include "SceneProvider.h"
 
#include <utility>
#include <vector>
 
#include <VirtualRobot/SceneObjectSet.h> // IWYU pragma: keep
#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/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/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/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->synchronizeRobot(*scn.robot, timestamp));
 
        if (fullUpdate)
        {
            scn.staticScene.emplace(getStaticScene(timestamp));
        }
        scn.dynamicScene.emplace(getDynamicScene(timestamp));
        // 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_INFO << "Static objects:";
        for (const auto& objectPose : objectPosesStatic)
        {
            ARMARX_INFO << "- " << objectPose.objectID;
        }
 
        const auto objects = armarx::objpose::util::asSceneObjects(objectPosesStatic);
 
        ARMARX_CHECK_NOT_NULL(objects);
        ARMARX_INFO << objects->getSize() << " objects in the scene";
 
        ARMARX_INFO << "Retrieving costmap in memory";
 
        ARMARX_TRACE;
 
        const algorithms::Costmap costmap = [&]()
        {
            // 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_INFO << "Static costmap available.";
                    return costmap.costmap.value();
                }
 
                ARMARX_INFO << deactivateSpam(5) << "Static costmap `" << query.name
                            << "` from provider " << query.providerName << " not available yet.";
            }
        }();
 
        const auto locations = srv.graphReader->locations();
 
        ARMARX_INFO << "Retrieved static costmap";
 
        return {.objects = objects,
                .objectMap = std::move(objectMap),
                .objectInfo = std::move(objectInfo),
                .distanceToObstaclesCostmap = costmap,
                .locations = std::move(locations)};
    }
 
    core::DynamicScene
    SceneProvider::getDynamicScene(const DateTime& timestamp) const
    {
        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 = {},
            .timestamp = timestamp};
 
        return {.humans = srv.humanReader->queryHumans(queryHumans).humans,
                .laserScannerFeatures =
                    srv.laserScannerFeaturesReader->queryData(queryFeatures).features};
    }
 
    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