Segment.cpp

Go to the documentation of this file.

#include "Segment.h"
 
#include <sstream>
 
#include <sys/inotify.h>
 
#include <Eigen/Dense>
#include <Eigen/Geometry>
 
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/algorithm/string.h>
#include <SimoxUtility/json.h>
#include <SimoxUtility/math/pose/pose.h>
#include <SimoxUtility/math/regression/linear.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.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/Clock.h>
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/ice_conversions.h>
 
#include "RobotAPI/libraries/ArmarXObjects/ObjectID.h"
#include "RobotAPI/libraries/armem/server/wm/memory_definitions.h"
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
#include <RobotAPI/libraries/ArmarXObjects/Scene.h>
#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/json_conversions.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <RobotAPI/libraries/armem/util/util.h>
#include <RobotAPI/libraries/armem_objects/aron/FamiliarObjectInstance.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
#include <RobotAPI/libraries/armem_objects/client/articulated_object/aron_conversions.h>
#include <RobotAPI/libraries/armem_objects/memory_ids.h>
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
namespace armarx::armem::server::obj::familiar_object_instance
{
 
    void
    Segment::Calibration::defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix)
    {
        defs->optional(robotName,
                       prefix + "robotName",
                       "Name of robot whose note can be calibrated.\n"
                       "If not given, the 'fallbackName' is used.");
        defs->optional(robotNode, prefix + "robotNode", "Robot node which can be calibrated.");
        defs->optional(offset, prefix + "offset", "Offset for the node to be calibrated.");
    }
 
    Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
        SpecializedCoreSegment(memoryToIceAdapter,
                               objects::familiarObjectInstaceSegmentID.coreSegmentName,
                               arondto::FamiliarObjectInstance::ToAronType(),
                               64)
    {
    }
 
    Segment::~Segment()
    {
    }
 
    void
    Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
    {
        SpecializedCoreSegment::defineProperties(defs, prefix);
 
 
        defs->optional(robots.fallbackName,
                       prefix + "robots.FallbackName",
                       "Robot name to use as fallback if the robot name is not specified "
                       "in a provided object pose.");
 
 
        decay.defineProperties(defs, prefix + "decay.");
    }
 
    void
    Segment::init()
    {
        SpecializedCoreSegment::init();
 
        robots.setTag(Logging::tag);
    }
 
    void
    Segment::connect(viz::Client arviz)
    {
        (void)arviz;
        // ARMARX_INFO << "ArticulatedObjectVisu";
        // this->visu = std::make_unique<ArticulatedObjectVisu>(arviz, *this);
        // visu->init();
    }
 
    void
    Segment::commit(
        const std::vector<armarx::armem::arondto::FamiliarObjectInstance>& familiarObjectInstances,
        const std::string& providerName)
    {
        ARMARX_CHECK_NOT_EMPTY(providerName);
 
        Time now = Time::Now();
 
        ARMARX_CHECK_NOT_NULL(segmentPtr);
        const MemoryID coreSegmentID = segmentPtr->id();
 
        Commit commit;
        for (const armarx::armem::arondto::FamiliarObjectInstance& familiarObjectInstance :
             familiarObjectInstances)
        {
            EntityUpdate& update = commit.updates.emplace_back();
 
            const MemoryID providerID = coreSegmentID.withProviderSegmentName(providerName);
 
            armarx::ObjectID objectID;
            fromAron(familiarObjectInstance.objectID, objectID);
 
            update.entityID = providerID.withEntityName(objectID.str());
            update.arrivedTime = now;
            update.referencedTime = familiarObjectInstance.timestamp;
            update.confidence = familiarObjectInstance.confidence;
 
            update.instancesData.push_back(familiarObjectInstance.toAron());
        }
        // Commit non-locking.
        ARMARX_INFO << "Committing " << commit.updates.size()
                    << " familiar object instances from provider " << providerName;
        iceMemory.commit(commit);
    }
 
    std::vector<armarx::armem::arondto::FamiliarObjectInstance>
    Segment::getFamiliarObjects(const DateTime& now)
    {
        std::vector<armarx::armem::arondto::FamiliarObjectInstance> familiarObjects;
 
        segmentPtr->forEachEntity(
            [&familiarObjects, &now](const wm::Entity& entity)
            {
                entity.getLatestSnapshot().forEachInstance(
                    [&familiarObjects, &now](const wm::EntityInstance& instance)
                    {
                        armarx::armem::arondto::FamiliarObjectInstance dto;
                        dto.fromAron(instance.data());
 
                        if (dto.timestamp <= now)
                        {
                            familiarObjects.push_back(dto);
                        }
                    });
            });
 
        return familiarObjects;
    }
 
    std::map<std::string, std::vector<armarx::armem::arondto::FamiliarObjectInstance>>
    Segment::getFamiliarObjectsByProvider(const DateTime& now)
    {
        std::map<std::string, std::vector<armarx::armem::arondto::FamiliarObjectInstance>>
            familiarObjectsByProvider;
 
        segmentPtr->forEachProviderSegment(
 
            [&now, &familiarObjectsByProvider](const wm::ProviderSegment& ps)
            {
                auto& familiarObjects = familiarObjectsByProvider[ps.name()];
                ps.forEachEntity(
                    [&familiarObjects, &now](const wm::Entity& entity)
                    {
                        entity.getLatestSnapshot().forEachInstance(
                            [&familiarObjects, &now](const wm::EntityInstance& instance)
                            {
                                armarx::armem::arondto::FamiliarObjectInstance dto;
                                dto.fromAron(instance.data());
 
                                if (dto.timestamp <= now)
                                {
                                    familiarObjects.push_back(dto);
                                }
                            });
                    });
            }
 
        );
 
        return familiarObjectsByProvider;
    }
 
    VirtualRobot::RobotPtr
    Segment::RobotsCache::get(const std::string& _robotName, const std::string& providerName)
    {
        std::string robotName = _robotName;
 
        if (robotName.empty())
        {
            auto antiSpam = deactivateSpam(10 * 60); // 10 minutes.
 
            std::stringstream ss;
            if (providerName.empty())
            {
                ss << "The provided object pose";
            }
            else
            {
                ss << "The object pose provided by '" << providerName << "'";
            }
            ss << " does not specify a robot name";
 
            if (fallbackName.empty())
            {
                ss << ", and no fallback robot name was configured (e.g. via the properties).\n"
                   << "For these object poses, the object instance segment is not able "
                   << "to provide transformed object poses (global and in robot root frame).";
                ARMARX_INFO << antiSpam << ss.str();
 
                return nullptr;
            }
            else
            {
                ss << ". The object instance segment is falling back to '" << fallbackName << "'.";
                ARMARX_INFO << antiSpam << ss.str();
 
                robotName = fallbackName;
            }
        }
        ARMARX_CHECK_NOT_EMPTY(robotName);
 
        // Lookup in cache.
        if (auto it = loaded.find(robotName); it != loaded.end())
        {
            ARMARX_CHECK_NOT_NULL(it->second);
            return it->second;
        }
        else
        {
            // Try to fetch the robot.
            ARMARX_CHECK_NOT_NULL(reader);
            VirtualRobot::RobotPtr robot = reader->getRobot(
                robotName, Clock::Now(), VirtualRobot::RobotIO::RobotDescription::eStructure);
 
            if (robot)
            {
                bool synchronized = reader->synchronizeRobot(*robot, Clock::Now());
                if (not synchronized)
                {
                    ARMARX_INFO << "The robot '" << robotName << "' could be loaded, but not"
                                << " synchronized successfully (e.g., the global localization "
                                   "could be missing). "
                                << "Make sure to synchronize it before use if necessary.";
                }
                // Store robot if valid.
                loaded.emplace(robotName, robot);
            }
            return robot; // valid or nullptr
        }
    }
 
 
} // namespace armarx::armem::server::obj::familiar_object_instance