Segment.h

Go to the documentation of this file.

#pragma once
 
#include <filesystem>
#include <map>
#include <memory>
#include <optional>
#include <string>
#include <vector>
 
#include <SimoxUtility/caching/CacheMap.h>
#include <SimoxUtility/shapes/OrientedBox.h>
#include <VirtualRobot/VirtualRobot.h>
 
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/time/DateTime.h>
 
#include "ArmarXGui/libraries/RemoteGui/Client/Widgets.h"
 
#include "RobotAPI/libraries/armem/core/forward_declarations.h"
#include "RobotAPI/libraries/armem/server/forward_declarations.h"
#include "RobotAPI/libraries/armem/server/segment/SpecializedCoreSegment.h"
#include "RobotAPI/libraries/armem_objects/types.h"
#include "RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h"
#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/interface/core/RobotState.h>
#include <RobotAPI/interface/objectpose/ObjectPoseProvider.h>
#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
#include <RobotAPI/interface/objectpose/object_pose_types.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
#include <RobotAPI/libraries/armem/core/Prediction.h>
#include <RobotAPI/libraries/armem/server/segment/SpecializedSegment.h>
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
 
#include "ArticulatedObjectVisu.h"
#include "Decay.h"
 
namespace armarx::armem::arondto
{
    class ObjectInstance;
}
 
namespace armarx::armem::server::obj::instance
{
 
    class Segment : public segment::SpecializedCoreSegment
    {
    public:
        struct CommitStats
        {
            int numUpdated = 0;
        };
 
        using ObjectPose = objpose::ObjectPose;
        using ObjectPoseSeq = objpose::ObjectPoseSeq;
        using ObjectPoseMap = std::map<ObjectID, ObjectPose>;
 
        struct Calibration
        {
            std::string robotName = "";
            std::string robotNode = "Neck_2_Pitch";
            float offset = 0.0f;
 
            void defineProperties(armarx::PropertyDefinitionsPtr defs,
                                  const std::string& prefix = "calibration.");
        };
 
 
    public:
        Segment(server::MemoryToIceAdapter& iceMemory);
        virtual ~Segment() override;
 
 
        void defineProperties(armarx::PropertyDefinitionsPtr defs,
                              const std::string& prefix = "") override;
        void init() override;
        void connect(viz::Client arviz);
 
 
        CommitStats commitObjectPoses(const std::string& providerName,
                                      const objpose::data::ProvidedObjectPoseSeq& providedPoses,
                                      const Calibration& calibration,
                                      std::optional<Time> discardUpdatesUntil = std::nullopt);
        void commitObjectPoses(const ObjectPoseSeq& objectPoses,
                               const std::string& providerName = "");
 
 
        objpose::ObjectPoseMap getObjectPoses(const DateTime& now);
        objpose::ObjectPoseMap getObjectPosesByProvider(const std::string& providerName,
                                                        const DateTime& now);
 
        wm::Entity* findObjectEntity(const ObjectID& objectID,
                                     const std::string& providerName = "");
        std::optional<simox::OrientedBoxf> getObjectOOBB(const ObjectID& id);
 
        objpose::ProviderInfo getProviderInfo(const std::string& providerName);
 
        objpose::AttachObjectToRobotNodeOutput
        attachObjectToRobotNode(const objpose::AttachObjectToRobotNodeInput& input);
        objpose::DetachObjectFromRobotNodeOutput
        detachObjectFromRobotNode(const objpose::DetachObjectFromRobotNodeInput& input);
        objpose::DetachAllObjectsFromRobotNodesOutput
        detachAllObjectsFromRobotNodes(const objpose::DetachAllObjectsFromRobotNodesInput& input);
 
        void runUpdateObjectPoses();
 
        /**
         * @brief If the object is attached to a robot node, update it according to the current robot state.
         *
         * If there is no attachement info in `objectPose` itself, the internal data
         * structure `attachments` is queried. If an attachment is found there,
         * it is written into the given `objectPose` (thus, it is "cached" in the
         * info `objectPose`).
         *
         * @param synchronized Indicates whether the agent is already synchronized to the current time.
         */
        void updateAttachement(ObjectPose& objectPose,
                               VirtualRobot::RobotPtr agent,
                               bool& synchronized) const;
 
 
        static ObjectPoseMap getLatestObjectPoses(const wm::CoreSegment& coreSeg);
        static ObjectPoseMap getLatestObjectPoses(const wm::ProviderSegment& provSeg);
        static ObjectPose getLatestObjectPose(const wm::Entity& entity);
 
        static std::map<std::string, ObjectPoseMap>
        getLatestObjectPosesByProviders(const wm::CoreSegment& coreSeg);
        static std::map<std::string, ObjectPoseMap>
        getLatestObjectPosesByProviders(const wm::ProviderSegment& provSeg);
        static void getLatestObjectPosesByProviders(const wm::CoreSegment& coreSeg,
                                                    std::map<std::string, ObjectPoseMap>& out);
 
        static void getLatestObjectPoses(const wm::CoreSegment& coreSeg, ObjectPoseMap& out);
        static void getLatestObjectPoses(const wm::ProviderSegment& provSeg, ObjectPoseMap& out);
        static void getLatestObjectPose(const wm::Entity& entity, ObjectPose& out);
 
 
        static arondto::ObjectInstance getLatestInstanceData(const wm::Entity& entity);
 
        ::armarx::armem::articulated_object::ArticulatedObjects getArticulatedObjects();
 
        static std::map<DateTime, ObjectPose>
        getObjectPosesInRange(const wm::Entity& entity, const DateTime& start, const DateTime& end);
 
        std::map<std::string, ObjectPoseMap> getLatestObjectPosesByProviders();
 
    private:
        ObjectPoseMap getLatestObjectPoses() const;
 
 
        void updateObjectPoses(ObjectPoseMap& objectPoses, const DateTime& now);
        void updateObjectPoses(ObjectPoseMap& objectPoses,
                               const DateTime& now,
                               VirtualRobot::RobotPtr agent,
                               bool& agentSynchronized) const;
        void updateObjectPose(ObjectPose& objectPose,
                              const DateTime& now,
                              VirtualRobot::RobotPtr agent,
                              bool& agentSynchronized) const;
 
 
        ObjectPoseMap filterObjectPoses(const ObjectPoseMap& objectPoses) const;
 
 
        void storeDetachedSnapshot(wm::Entity& entity,
                                   const arondto::ObjectInstance& data,
                                   Time now,
                                   bool commitAttachedPose,
                                   bool forgetObject);
 
 
        std::optional<wm::EntityInstance> findClassInstance(const ObjectID& objectID) const;
 
        friend struct DetachVisitor;
 
 
    private:
        void storeScene(const std::string& filename, const armarx::objects::Scene& scene);
        std::optional<armarx::objects::Scene> loadScene(const std::string& filename);
        std::optional<armarx::objects::Scene> loadScene(const std::filesystem::path& path);
        std::optional<std::filesystem::path> resolveSceneFilepath(const std::string& filename);
 
        armarx::objects::Scene getSceneSnapshot() const;
        void commitSceneSnapshot(const armarx::objects::Scene& scene, const std::string& sceneName);
        void commitSceneSnapshotFromFilename(const std::string& filename, bool lockMemory);
 
 
    public:
        /// Loaded robot models identified by the robot name.
        struct RobotsCache : public armarx::Logging
        {
            robot_state::VirtualRobotReader* reader = nullptr;
            std::string fallbackName;
 
            std::map<std::string, VirtualRobot::RobotPtr> loaded;
 
            VirtualRobot::RobotPtr get(const std::string& robotName,
                                       const std::string& providerName = "");
        };
 
        RobotsCache robots;
 
 
        objpose::ProviderInfoMap providers;
 
 
        ObjectFinder objectFinder;
 
        /// Decay model.
        Decay decay;
 
 
    private:
        struct Properties
        {
            bool discardSnapshotsWhileAttached = true;
 
            bool separateProviderForAttachedObjects = false;
 
            bool automaticallyDetachObjectsOnHandOpening = false;
            float automaticallyDetachPositionThreshold = 0.2;
 
            /// Package containing the scene snapshots
            std::string sceneSnapshotsPackage = armarx::ObjectFinder::DefaultObjectsPackageName;
            std::string sceneSnapshotsDirectory = "scenes";
            std::string sceneSnapshotsToLoad = "";
 
            bool autoReloadSceneSnapshotsOnFileChange = false;
 
            std::vector<std::string> getSceneSnapshotsToLoad() const;
 
            std::string attachedProviderName = "attached";
        };
 
        Properties p;
 
 
        /// Caches results of attempts to retrieve the OOBB from ArmarXObjects.
        simox::caching::CacheMap<ObjectID, std::optional<simox::OrientedBoxf>> oobbCache;
 
        /// Class name -> dataset name.
        simox::caching::CacheMap<std::string, std::string> classNameToDatasetCache;
 
        std::unique_ptr<CMakePackageFinder> finder;
 
        static const std::string timestampPlaceholder;
 
    public:
        struct RemoteGui
        {
            armarx::RemoteGui::Client::GroupBox group;
 
            armarx::RemoteGui::Client::LineEdit storeLoadLine;
            armarx::RemoteGui::Client::Button storeButton;
            armarx::RemoteGui::Client::Button loadButton;
 
            armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
            armarx::RemoteGui::Client::CheckBox infiniteHistory;
            armarx::RemoteGui::Client::CheckBox discardSnapshotsWhileAttached;
 
            armarx::RemoteGui::Client::Button detachAllObjectsButton;
            armarx::RemoteGui::Client::CheckBox detachAllObjectsCommitAttachedPoseCheckBox;
 
            void setup(const Segment& data);
            void update(Segment& data);
        };
 
 
    private:
        std::unique_ptr<ArticulatedObjectVisu> visu;
 
        SimpleRunningTask<>::pointer_type fileWatcherTask;
    };
 
} // namespace armarx::armem::server::obj::instance