Segment.cpp

Go to the documentation of this file.

#include "Segment.h"
#include <filesystem>
#include <SimoxUtility/algorithm/string/string_tools.h>
 
#include <ArmarXCore/core/application/properties/PluginAll.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/time/TimeUtil.h>
 
#include <RobotAPI/libraries/armem_robot/types.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
 
#include <RobotAPI/libraries/armem/core/MemoryID.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/server/MemoryToIceAdapter.h>
 
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/memory_ids.h>
 
 
namespace armarx::armem::server::robot_state::description
{
 
    Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
        Base(memoryToIceAdapter, armem::robot_state::descriptionSegmentID.coreSegmentName, arondto::RobotDescription::ToAronType())
    {
    }
 
    Segment::~Segment() = default;
 
 
    void Segment::onConnect(const RobotUnitInterfacePrx& robotUnitPrx)
    {
        robotUnit = robotUnitPrx;
 
        // store the robot description linked to the robot unit in this segment
        updateRobotDescription();
    }
 
 
    void Segment::commitRobotDescription(const robot::RobotDescription& robotDescription)
    {
        const Time now = Time::Now();
 
        const MemoryID providerID = segmentPtr->id().withProviderSegmentName(robotDescription.name);
        if (not segmentPtr->hasProviderSegment(providerID.providerSegmentName))
        {
            segmentPtr->addProviderSegment(providerID.providerSegmentName, arondto::RobotDescription::ToAronType());
        }
 
 
        EntityUpdate update;
        update.entityID = providerID.withEntityName("description");
        update.arrivedTime = update.referencedTime = update.sentTime = now;
 
        arondto::RobotDescription dto;
        robot::toAron(dto, robotDescription);
        update.instancesData =
        {
            dto.toAron()
        };
 
        Commit commit;
        commit.updates.push_back(update);
        iceMemory.commitLocking(commit);
    }
 
 
    void Segment::updateRobotDescription()
    {
        ARMARX_CHECK_NOT_NULL(robotUnit);
        KinematicUnitInterfacePrx kinematicUnit = robotUnit->getKinematicUnit();
        if (kinematicUnit)
        {
            const std::string robotName = kinematicUnit->getRobotName();
            const std::string robotFilename = kinematicUnit->getRobotFilename();
 
            const std::vector<std::string> packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
            const std::string package = armarx::ArmarXDataPath::getProject(packages, robotFilename);
 
            // make sure that the relative path is without the 'package/' prefix
            const std::string robotFileRelPath = [&robotFilename, &package]()-> std::string {
 
                if(simox::alg::starts_with(robotFilename, package))
                {
                    // remove "package" + "/"
                    const std::string fixedRobotFilename = robotFilename.substr(package.size() + 1, -1);
                    return fixedRobotFilename;
                }
                
                return robotFilename;
            }();
 
            ARMARX_INFO << "Robot description '" << VAROUT(robotFileRelPath) << "' found in package " << package;
 
            const robot::RobotDescription robotDescription
            {
                .name = kinematicUnit->getRobotName(),
                .xml  = {package, robotFileRelPath}
            };
 
            // make sure that the package path is valid
            ARMARX_CHECK(std::filesystem::exists(robotDescription.xml.toSystemPath()));
 
            commitRobotDescription(robotDescription);
        }
        else
        {
            ARMARX_WARNING << "Robot unit '" << robotUnit->ice_getIdentity().name << "' does not have a kinematic unit."
                           << "\n Cannot commit robot description.";
        }
    }
 
 
    RobotDescriptionMap
    Segment::getRobotDescriptionsLocking(const armem::Time& timestamp) const
    {
        return doLocked([this, &timestamp]()
        {
            return getRobotDescriptions(timestamp);
        });
    }
 
 
    RobotDescriptionMap
    Segment::getRobotDescriptions(const armem::Time& timestamp) const
    {
        ARMARX_CHECK_NOT_NULL(segmentPtr);
        (void) timestamp;  // Unused
 
        RobotDescriptionMap robotDescriptions;
        segmentPtr->forEachEntity([this, &robotDescriptions](const wm::Entity & entity)
        {
            const wm::EntityInstance& entityInstance = entity.getLatestSnapshot().getInstance(0);
            const auto description     = robot::convertRobotDescription(entityInstance);
            if (description)
            {
                ARMARX_DEBUG << "Key is " << armem::MemoryID(entity.id());
                robotDescriptions.emplace(description->name, *description);
            }
            else
            {
                ARMARX_WARNING << "Could not convert entity instance to 'RobotDescription'";
            }
        });
 
        ARMARX_VERBOSE << deactivateSpam(60) <<  "Number of known robot descriptions: " << robotDescriptions.size();
        return robotDescriptions;
    }
 
} // namespace armarx::armem::server::robot_state::description