SelfLocalization.cpp

Go to the documentation of this file.

#include "SelfLocalization.h"
 
#include <cmath>
#include <optional>
 
#include <SimoxUtility/math/convert/mat3f_to_rpy.h>
 
#include <ArmarXCore/core/IceGridAdmin.h>
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem_robot_state/client/common/constants.h>
#include <RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h>
#include <RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/libraries/helpers/EarlyVisionHelpers/Gaussian.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
#include <MemoryX/libraries/memorytypes/entity/SimpleEntity.h>
 
namespace armarx
{
 
    SelfLocalization::SelfLocalization() :
        skipper(0.)
    {
        addPlugin(transformReaderPlugin);
        addPlugin(transformWriterPlugin);
    }
 
    SelfLocalization::~SelfLocalization() = default;
 
    PropertyDefinitionsPtr
    SelfLocalization::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr def =
            new ComponentPropertyDefinitions(getConfigIdentifier());
 
        def->topic(reportTopic);
        def->topic(globalRobotPoseTopic);
        def->topic(gobalRobotPoseCorrectionTopic);
 
        // old working memory
        def->optional(properties.updateWorkingMemory,
                      "working_memory.update",
                      "If enabled, updates the global pose in the working memory");
        def->optional(properties.workingMemoryUpdateFrequency, "working_memory.update_frequency");
 
        // old long-term memory
        def->optional(properties.longtermMemoryName,
                      "longterm_memory",
                      "Which legacy long-term memory to use if longterm_memory.update"
                      "or longterm_memory.retrieve_initial_pose are set");
        def->optional(properties.updateLongTermMemory,
                      "longterm_memory.update",
                      "If enabled, updates the global pose in the longterm memory");
        def->optional(properties.longtermMemoryUpdateFrequency, "longterm_memory.update_frequency");
        def->optional(properties.initialPoseFromLTM, "longterm_memory.retrieve_initial_pose");
 
 
        // armem
        def->optional(properties.updateArMem, "armem.update", "If enabled, updates ArMem");
        def->optional(properties.armMemUpdateFrequency, "armem.update_frequency");
 
        // other components
        def->component(robotStateComponent,
                       "RobotStateComponent",
                       "robot_state_component",
                       "The name of the RobotStateComponent. Used to get local transformation of "
                       "laser scanners");
 
        // initialize properties: localization unit
        {
            const std::string fullTypeName =
                ::IceProxy::armarx::LocalizationUnitInterface::ice_staticId();
            properties.localizationUnitName =
                PropertyDefinitionContainer_ice_class_name(fullTypeName);
        }
 
        def->optional(properties.useLocalizationUnit,
                      "localizationUnit.use",
                      "If enabled, connects to the localization unit");
        def->optional(properties.localizationUnitName,
                      "localizationUnit.proxyName",
                      "If enabled, connects to the localization unit");
 
        return def;
    }
 
    void
    SelfLocalization::publishSelfLocalization(const PoseStamped& map_T_robot)
    {
        ARMARX_TRACE;
 
        if (not connected.load())
        {
            ARMARX_WARNING
                << "Will not publish self localization as some dependencies are not available.";
            return;
        }
 
        ARMARX_DEBUG << "publishSelfLocalizationOnTopic";
        publishSelfLocalizationOnTopic(map_T_robot);
 
        ARMARX_DEBUG << "publishSelfLocalizationToArMem";
        publishSelfLocalizationToArMem(map_T_robot);
 
        // legacy
        ARMARX_DEBUG << "publishSelfLocalizationToWorkingMemory";
        publishSelfLocalizationToWorkingMemory(map_T_robot);
 
        ARMARX_DEBUG << "publishSelfLocalizationToLongtermMemory";
        publishSelfLocalizationToLongtermMemory(map_T_robot);
    }
 
    void
    SelfLocalization::onInitComponent()
    {
        // Legacy memory.
        if (properties.updateWorkingMemory)
        {
            usingProxy("WorkingMemory");
        }
        if (properties.updateLongTermMemory or properties.initialPoseFromLTM)
        {
            usingProxy(properties.longtermMemoryName);
        }
 
        if (properties.useLocalizationUnit)
        {
            ARMARX_INFO << "Localization unit is enabled.";
            usingProxy(properties.localizationUnitName);
        }
    }
 
    void
    SelfLocalization::onDisconnectComponent()
    {
        connected.store(false);
    }
 
    void
    SelfLocalization::publishSelfLocalizationOnTopic(const PoseStamped& map_T_robot)
    {
        ARMARX_TRACE;
 
        const Eigen::Affine3f globalPose = global_T_map * map_T_robot.pose;
 
        const float yaw = simox::math::mat3f_to_rpy(globalPose.linear()).z();
 
        PlatformPose platformPose;
        platformPose.x = globalPose.translation().x();
        platformPose.y = globalPose.translation().y();
        platformPose.rotationAroundZ = yaw;
        platformPose.timestampInMicroSeconds =
            map_T_robot.timestamp.toDurationSinceEpoch().toMicroSeconds();
 
        const PoseStamped globalPoseStamped{.pose = globalPose, .timestamp = map_T_robot.timestamp};
 
        globalRobotPoseTopic->reportGlobalRobotPose(toTransformStamped(globalPoseStamped));
 
 
        {
            const armem::client::robot_state::localization::TransformQuery odomQuery{
                .header = {.parentFrame = OdometryFrame,
                           .frame = robotRootFrame,
                           .agent = agentName,
                           .timestamp = map_T_robot.timestamp}};
 
            try
            {
 
                const auto odomLookupResult = transformReaderPlugin->get().lookupTransform(odomQuery);
 
                if (odomLookupResult.status ==
                    armem::client::robot_state::localization::TransformResult::Status::Success)
                {
                    const Eigen::Affine3f robot_T_odom =
                        odomLookupResult.transform.transform.inverse();
                    const Eigen::Affine3f map_T_odom = map_T_robot.pose * robot_T_odom;
                    const Eigen::Affine3f global_T_odom = global_T_map * map_T_odom;
                    const PoseStamped globalOdomPoseStamped{.pose = global_T_odom,
                                                            .timestamp = map_T_robot.timestamp};
 
                    gobalRobotPoseCorrectionTopic->reportGlobalRobotPoseCorrection(
                        toTransformStamped(globalOdomPoseStamped));
 
                    if (properties.useLocalizationUnit)
                    {
                        ARMARX_CHECK_NOT_NULL(localizationUnitPrx);
                        localizationUnitPrx->reportGlobalRobotPoseCorrection(
                            toTransformStamped(globalOdomPoseStamped));
                    }
                }
                else
                {
                    ARMARX_WARNING << deactivateSpam(60) << odomLookupResult.errorMessage;
                }
            }
            catch (...)
            {
                ARMARX_WARNING << deactivateSpam(1)
                               << "Failed to publish global pose on topics as the following "
                                  "exception occured: "
                               << GetHandledExceptionString();
                return;
            }
        }
 
        // To my future me: publish 3D stamped pose
        reportTopic->reportCorrectedPose(platformPose.x, platformPose.y, yaw);
    }
 
    void
    SelfLocalization::publishSelfLocalizationToWorkingMemory(const PoseStamped& map_T_robot)
    {
        ARMARX_TRACE;
 
        if (not properties.updateWorkingMemory or not workingMemory)
        {
            return;
        }
 
        ARMARX_CHECK_NOT_NULL(wmAgentsMemory) << "Agents memory not set";
        ARMARX_CHECK_NOT_NULL(wmAgentInstance) << "Agent instance not set";
 
        // check if enough time has passed since last update
        if (not skipper.checkFrequency("workingMemoryUpdate",
                                       properties.workingMemoryUpdateFrequency))
        {
            return;
        }
 
        const Eigen::Affine3f globalPose = global_T_map * map_T_robot.pose;
 
        const FramedPosePtr reportPose =
            new FramedPose(globalPose.linear(), globalPose.translation(), GlobalFrame, agentName);
 
        try
        {
            // This seems to be the way to update the current pose
            // Since in simulation another component (SelfLocalizationDynamicSimulation) updates this memory location
            // it is a bad idea to enable the working memory update in simulation
            wmAgentInstance->setPose(reportPose);
            const std::string robotAgentId =
                wmAgentsMemory->upsertEntityByName(wmAgentInstance->getName(), wmAgentInstance);
            wmAgentInstance->setId(robotAgentId);
        }
        catch (IceUtil::Exception const& ex)
        {
            ARMARX_WARNING << "Caught exception while updating the working memory:\n" << ex;
        }
    }
 
    void
    SelfLocalization::publishSelfLocalizationToLongtermMemory(const PoseStamped& map_T_robot)
    {
        ARMARX_TRACE;
 
        if (not properties.updateLongTermMemory or not longtermMemory)
        {
            return;
        }
 
        ARMARX_CHECK_NOT_NULL(ltmSelfLocalisationSegment)
            << "Self localisation memory segment is not set";
 
        // check if enough time has passed since last update
        if (not skipper.checkFrequency("longtermMemoryUpdate",
                                       properties.longtermMemoryUpdateFrequency))
        {
            return;
        }
 
        if (not ltmPoseEntityId)
        {
            ARMARX_WARNING << "Pose entity id not available";
        }
 
        const Eigen::Affine3f globalPose = global_T_map * map_T_robot.pose;
 
        const FramedPosePtr reportPose =
            new FramedPose(globalPose.linear(), globalPose.translation(), GlobalFrame, agentName);
 
        try
        {
            memoryx::SimpleEntityPtr poseEntity = new memoryx::SimpleEntity();
            poseEntity->setName(agentName);
            poseEntity->putAttribute("pose", reportPose);
            poseEntity->setId(*ltmPoseEntityId);
            // We just update the 'pose' attribute since ID and name have been set by the inital insert
            ltmSelfLocalisationSegment->upsertEntity(*ltmPoseEntityId, poseEntity);
        }
        catch (IceUtil::Exception const& ex)
        {
            ARMARX_WARNING << "Caught exception while updating the longterm memory:\n" << ex;
        }
    }
 
    armem::robot_state::Transform
    SelfLocalization::toTransform(const PoseStamped& poseStamped)
    {
        return {.header{.parentFrame = MapFrame,
                        .frame = OdometryFrame,
                        .agent = agentName,
                        .timestamp = poseStamped.timestamp},
                .transform = poseStamped.pose};
    }
 
    void
    SelfLocalization::publishSelfLocalizationToArMem(const PoseStamped& map_T_robot)
    {
        ARMARX_TRACE;
 
        if (not properties.updateArMem)
        {
            return;
        }
 
        // check if enough time has passed since last update
        if (not skipper.checkFrequency("armMemUpdateFrequency", properties.armMemUpdateFrequency))
        {
            return;
        }
 
        const armem::client::robot_state::localization::TransformQuery odomQuery{
            .header = {.parentFrame = OdometryFrame,
                       .frame = robotRootFrame,
                       .agent = agentName,
                       .timestamp = map_T_robot.timestamp}};
 
        const auto odomLookupResult = transformReaderPlugin->get().lookupTransform(odomQuery);
 
        if (odomLookupResult.status !=
            armem::client::robot_state::localization::TransformResult::Status::Success)
        {
            ARMARX_WARNING << deactivateSpam(60) << odomLookupResult.errorMessage;
            return;
        }
 
        const Eigen::Affine3f robot_T_odom = odomLookupResult.transform.transform.inverse();
        const Eigen::Affine3f map_T_odom   = map_T_robot.pose * robot_T_odom;
 
        const armem::robot_state::Transform mapToOdomTransform{
            .header = {.parentFrame = MapFrame,
                       .frame = OdometryFrame,
                       .agent = agentName,
                       .timestamp = map_T_robot.timestamp},
            .transform = map_T_odom};
 
        const auto status = transformWriterPlugin->get().commitTransform(mapToOdomTransform);
 
        if (not status)
        {
            ARMARX_WARNING << "Failed to commit transform";
        }
    }
 
    TransformStamped
    SelfLocalization::toTransformStamped(const PoseStamped& poseStamped)
    {
        FrameHeader header;
        header.parentFrame = GlobalFrame;
        header.frame = robotRootFrame;
        header.agent = agentName;
        header.timestampInMicroSeconds = poseStamped.timestamp.toMicroSecondsSinceEpoch();
 
        TransformStamped transform;
        transform.header = header;
        transform.transform = poseStamped.pose.matrix();
 
        return transform;
    }
 
    void
    SelfLocalization::setupArMem()
    {
        ARMARX_TRACE;
 
        if (not properties.updateArMem)
        {
            return;
        }
 
        ARMARX_INFO << "setup ArMem ...";
 
        // transformReader = std::make_unique<armem::client::robot_state::localization::TransformReader>(*this);
        // transformWriter = std::make_unique<armem::client::robot_state::localization::TransformWriter>(*this);
 
        ARMARX_INFO << "Publishing world to map transform";
        // Publish World to Map transform (static transform)
        const armem::robot_state::Transform worldToMapTransform{
            .header = {.parentFrame = GlobalFrame,
                       .frame = MapFrame,
                       .agent = getAgentName(),
                       .timestamp = Clock::Now()},
            .transform = global_T_map,
        };
 
        if (not transformWriterPlugin->get().commitTransform(worldToMapTransform))
        {
            ARMARX_WARNING
                << "Something went wrong during setupArMem. Updating ArMem will be deactivated";
            properties.updateArMem = false;
        }
    }
 
    void
    SelfLocalization::setupWorkingMemory()
    {
        ARMARX_TRACE;
 
        if (not properties.updateWorkingMemory or not workingMemory)
        {
            return;
        }
 
        ARMARX_INFO << "setup WorkingMemory ...";
 
        wmAgentsMemory = workingMemory->getAgentInstancesSegment();
 
        wmAgentInstance = new memoryx::AgentInstance();
        wmAgentInstance->setSharedRobot(robotStateComponent->getSynchronizedRobot());
        wmAgentInstance->setAgentFilePath(robotStateComponent->getRobotFilename());
        wmAgentInstance->setName(agentName);
    }
 
    void
    SelfLocalization::setupLongTermMemory()
    {
        ARMARX_TRACE;
 
        if (not longtermMemory)
        {
            return;
        }
 
        // Always initialize this segment variable. It is used in globalPoseFromLongTermMemory() as well.
        ltmSelfLocalisationSegment = longtermMemory->getSelfLocalisationSegment();
 
        if (not properties.updateLongTermMemory)
        {
            return;
        }
 
        ARMARX_INFO << "setup LongTermMemory ...";
 
        const auto getOrCreatePoseEntityID = [&]() -> std::string
        {
            const memoryx::EntityBasePtr existingPoseEntity =
                ltmSelfLocalisationSegment->getEntityByName(agentName);
 
            if (existingPoseEntity)
            {
                return existingPoseEntity->getId();
            }
 
            // if poseEntity does not exist, create it ....
            ARMARX_WARNING << "No pose in longterm memory found for agent: " << agentName;
            ARMARX_WARNING << "Self localisation will start at origin";
 
            FramedPosePtr pose =
                new FramedPose(Eigen::Matrix4f::Identity(), GlobalFrame, agentName);
 
            memoryx::SimpleEntityPtr poseEntity = new memoryx::SimpleEntity();
            poseEntity->setName(agentName);
            poseEntity->putAttribute("pose", pose);
 
            return ltmSelfLocalisationSegment->addEntity(poseEntity);
        };
 
        try
        {
            ltmPoseEntityId = getOrCreatePoseEntityID();
            ARMARX_INFO << "Entity ID: " << *ltmPoseEntityId;
        }
        catch (...)
        {
            ARMARX_WARNING
                << "Could not setup LongTermMemory. Updating LongTermMemory will be deactivated.";
            properties.updateLongTermMemory = false;
        }
    }
 
    void
    SelfLocalization::onConnectComponent()
    {
        ARMARX_TRACE;
 
        // const bool reconnecting = robot == nullptr;
        if (properties.useLocalizationUnit)
        {
            getProxy(localizationUnitPrx, properties.localizationUnitName);
        }
 
        if (onConnectCalled) // reconnecting
        {
            onReconnectComponent();
            return;
        }
 
 
        ARMARX_INFO << "connecting ...";
        global_T_map = worldToMapTransform();
 
        robot = RemoteRobot::createLocalClone(robotStateComponent);
        RemoteRobot::synchronizeLocalClone(robot, robotStateComponent);
 
        agentName = robotStateComponent->getRobotName();
        robotRootFrame = armarx::armem::robot_state::constants::
            robotRootNodeName; // robot->getRootNode()->getName();
 
        setupArMem();
 
        // legacy
        if (properties.updateWorkingMemory)
        {
            getProxy(workingMemory, "WorkingMemory");
            ARMARX_CHECK_NOT_NULL(workingMemory) << "Working memory updates are enabled, "
                                                 << "but working memory is not available.";
            setupWorkingMemory();
        }
        if (properties.updateLongTermMemory or properties.initialPoseFromLTM)
        {
            getProxy(longtermMemory, properties.longtermMemoryName);
            ARMARX_CHECK_NOT_NULL(longtermMemory)
                << "Long-term memory updates or pose retrieval are enabled, "
                << "but long-term memory is not available.";
 
            setupLongTermMemory();
        }
 
        (void)globalPoseFromLongTermMemory();
 
        connected.store(true);
        onConnectCalled = true;
        ARMARX_INFO << "... done.";
    }
 
    void
    SelfLocalization::onReconnectComponent()
    {
        ARMARX_TRACE;
 
        ARMARX_INFO << "SelfLocalization::onReconnectComponent";
        setupArMem();
 
        // legacy
        setupWorkingMemory();
        setupLongTermMemory();
 
        connected.store(true);
 
        ARMARX_INFO << "... done.";
    }
 
    std::optional<Eigen::Affine3f>
    SelfLocalization::globalPoseFromLongTermMemory() const
    {
        ARMARX_TRACE;
 
        if (not properties.initialPoseFromLTM or not longtermMemory)
        {
            return std::nullopt;
        }
 
        ARMARX_INFO << "Querying initial pose from long term memory";
 
        if (not ltmSelfLocalisationSegment)
        {
            ARMARX_FATAL << "Could not retrieve global pose from long-term memory."
                         << "Reason: Self-localization segment not available.";
            return std::nullopt;
        }
 
        try
        {
            ARMARX_CHECK_NOT_NULL(ltmSelfLocalisationSegment);
 
            // Load estimatedPose from longterm memory
            const memoryx::EntityBasePtr poseEntity =
                ltmSelfLocalisationSegment->getEntityByName(agentName);
            if (not poseEntity)
            {
                ARMARX_FATAL << "Could not retrieve global pose from long-term memory."
                             << "Reason: Pose entity not available.";
                return std::nullopt;
            }
 
            const auto poseEntityId = poseEntity->getId();
            ARMARX_DEBUG << "Using pose entity from LTM with ID: " << poseEntityId;
            const memoryx::EntityAttributeBasePtr poseAttribute = poseEntity->getAttribute("pose");
 
            if (not poseAttribute)
            {
                ARMARX_FATAL << "Could not retrieve global pose from long-term memory."
                             << "Reason: Pose attribute not available.";
                return std::nullopt;
            }
 
            // So this is how you access a typed attribute, fancy
            const FramedPosePtr framedPose =
                armarx::VariantPtr::dynamicCast(poseAttribute->getValueAt(0))
                    ->getClass<armarx::FramedPose>();
 
            if (not framedPose)
            {
                ARMARX_FATAL << "Could not retrieve global pose from long-term memory."
                             << "Reason: Casting to framed pose failed.";
                return std::nullopt;
            }
 
            const Eigen::Affine3f pose(framedPose->toGlobalEigen(robot));
            ARMARX_INFO << "Long-term memory global pose prior: " << pose.matrix();
 
            return pose;
        }
        catch (...)
        {
            ARMARX_FATAL << "Could not retrieve global pose from long-term memory."
                         << "Reason: unknown.";
        }
 
        return std::nullopt;
    }
 
    const std::string&
    SelfLocalization::getAgentName() const noexcept
    {
        return agentName;
    }
 
    const std::string&
    SelfLocalization::getRobotRootFrame() const noexcept
    {
        return robotRootFrame;
    }
 
    const VirtualRobot::RobotPtr&
    SelfLocalization::getRobot() const noexcept
    {
        return robot;
    }
 
    armem::client::robot_state::localization::TransformWriterInterface&
    SelfLocalization::getTransformWriter() const
    {
        return transformWriterPlugin->get();
    }
 
    const armem::client::robot_state::localization::TransformReaderInterface&
    SelfLocalization::getTransformReader() const
    {
        return transformReaderPlugin->get();
    }
 
} // namespace armarx