Segment.h

Go to the documentation of this file.

#pragma once
 
#include <experimental/memory>
#include <map>
#include <string>
 
#include <SimoxUtility/caching/CacheMap.h>
#include <SimoxUtility/shapes/OrientedBox.h>
 
#include <ArmarXCore/core/services/tasks/TaskUtil.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/ObjectPoseStorageInterface.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 <RobotAPI/libraries/armem_objects/aron/FamiliarObjectInstance.aron.generated.h>
 
#include "Decay.h"
 
namespace armarx::armem::arondto
{
    class ObjectInstance;
}
 
namespace armarx::armem::server::obj::familiar_object_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);
 
        void commit(const std::vector<armarx::armem::arondto::FamiliarObjectInstance>&
                        familiarObjectInstances,
                    const std::string& providerName);
 
 
        std::vector<armarx::armem::arondto::FamiliarObjectInstance>
        getFamiliarObjects(const DateTime& now);
 
        std::map<std::string, std::vector<armarx::armem::arondto::FamiliarObjectInstance>>
        getFamiliarObjectsByProvider(const DateTime& now);
 
    private:
        friend struct DetachVisitor;
 
 
    public:
        /// Loaded robot models identified by the robot name.
        struct RobotsCache : public armarx::Logging
        {
            std::experimental::observer_ptr<robot_state::VirtualRobotReader> reader;
            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
        {
        };
 
        Properties p;
 
 
        static const std::string timestampPlaceholder;
    };
 
} // namespace armarx::armem::server::obj::familiar_object_instance