Segment.cpp

Go to the documentation of this file.

#include "Segment.h"
 
#include <filesystem>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <VirtualRobot/XML/RobotIO.h>
 
#include <ArmarXCore/core/PackagePath.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/RobotStatechartHelpers/RobotNameHelper.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/MemoryID.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/RobotDescription.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/memory_ids.h>
#include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/types.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.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 armem::robot_state::description::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;
        toAron(dto, robotDescription);
        update.instancesData = {dto.toAron()};
 
        Commit commit;
        commit.updates.push_back(update);
        iceMemory.commitLocking(commit);
    }
 
    // Helper function to handle exceptions
    // template <typename T>
    // std::optional<T> tryGet(const std::function<T()>& func) {
    //     try {
    //         return func();
    //     } catch (const std::exception& ex) {
    //         // Handle exception or log error
    //         std::cerr << "Error: " << ex.what() << std::endl;
    //         return std::nullopt; // Return an empty optional if an exception occurs
    //     }
    // }
 
    std::string
    tryGet(const std::function<std::string()>& func)
    {
        try
        {
            return func();
        }
        catch (const std::exception& ex)
        {
            return ""; // Return an empty string if an exception occurs
        }
    }
 
    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);
            const std::string package = simox::alg::split(robotFilename, "/").front();
 
            ARMARX_CHECK_NOT_EMPTY(package);
 
            ARMARX_INFO << VAROUT(package);
            ARMARX_INFO << VAROUT(robotFilename);
 
            // make sure that the relative path is without the 'package/' prefix
            const auto fixPath = [](armarx::PackagePath& pp)
            {
                auto spp = pp.serialize();
                if (simox::alg::starts_with(spp.path, spp.package))
                {
                    // remove "package" + "/"
                    const std::string fixedPath = spp.path.substr(spp.package.size() + 1, -1);
 
                    spp.path = fixedPath;
 
                    pp = armarx::PackagePath{spp};
                }
            };
 
            armarx::PackagePath robotPath{package, robotFilename};
            fixPath(robotPath);
 
            const auto fixPathDto = [](armarx::arondto::PackagePath& pp)
            {
                if (simox::alg::starts_with(pp.path, pp.package))
                {
                    // remove "package" + "/"
                    const std::string fixedPath = pp.path.substr(pp.package.size() + 1, -1);
 
                    pp.path = fixedPath;
                }
            };
 
            ARMARX_INFO << "Robot description '" << robotPath << "'";
 
            auto robotNameHelper = RobotNameHelper::Create(robotPath.toSystemPath());
 
            arondto::RobotInfo info;
            {
                for (const auto& side :
                     {RobotNameHelper::LocationLeft, RobotNameHelper::LocationRight})
                {
                    const auto arm = robotNameHelper->getArm(side);
 
                    arondto::RobotInfoElement e;
                    e.end_effector = tryGet([&]() { return arm.getEndEffector(); });
                    e.force_torque_sensor = tryGet([&]() { return arm.getForceTorqueSensor(); });
                    e.force_torque_sensor_frame =
                        tryGet([&]() { return arm.getForceTorqueSensorFrame(); });
 
                    e.full_hand_collision_model.package =
                        tryGet([&]() { return arm.getHandModelPackage(); });
                    e.full_hand_collision_model.path =
                        tryGet([&]() { return arm.getFullHandCollisionModel(); });
                    fixPathDto(e.full_hand_collision_model);
 
 
                    e.hand_controller_name = tryGet([&]() { return arm.getHandControllerName(); });
 
                    e.hand_model.package = tryGet([&]() { return arm.getHandModelPackage(); });
                    e.hand_model.path = tryGet([&]() { return arm.getHandModelPath(); });
                    fixPathDto(e.hand_model);
 
                    e.hand_root_node = tryGet([&]() { return arm.getHandRootNode(); });
                    e.kinematic_chain = tryGet([&]() { return arm.getKinematicChain(); });
 
                    e.palm_collision_model = tryGet([&]() { return arm.getPalmCollisionModel(); });
 
                    e.tcp = tryGet([&]() { return arm.getTCP(); });
                    e.torso_kinematic_chain =
                        tryGet([&]() { return arm.getTorsoKinematicChain(); });
 
                    e.hand_unit = 
                        tryGet([&]() { return arm.getHandUnitName(); });
 
 
                    info.parts.emplace(side + "Arm", e);
                }
            }
 
            arondto::RobotDescriptionVisualization visualization;
            {
                for (const auto& side :
                     {RobotNameHelper::LocationLeft, RobotNameHelper::LocationRight})
                {
                    const auto arm = robotNameHelper->getArm(side);
 
                    armarx::armem::arondto::BodyPartDescription bpd;
                    bpd.root_node_name = tryGet([&]() { return arm.getHandRootNode(); });
                    bpd.xml.package = tryGet([&]() { return arm.getHandModelPackage(); });
                    bpd.xml.path = tryGet([&]() { return arm.getHandModelPath(); });
                    fixPathDto(bpd.xml);
 
                    visualization.body_parts.emplace(side + "Hand", bpd);
                }
            }
 
            std::optional<
                std::map<std::string, std::vector<armarx::armem::arondto::ManipulationCapability>>>
                allManipulationCapabilities;
            {
                const auto robot = VirtualRobot::RobotIO::loadRobot(robotPath.toSystemPath());
                ARMARX_CHECK_NOT_NULL(robot) << robotPath.toSystemPath();
 
                for (const auto& eef : robot->getEndEffectors())
                {
                    const auto& capabilities = eef->getManipulationCapabilities();
                    if (capabilities.has_value())
                    {
                        std::vector<armarx::armem::arondto::ManipulationCapability>
                            manipulationCapabilities;
 
                        for (const auto& vrCap : capabilities->capabilities)
                        {
                            arondto::ManipulationCapability dto;
 
                            dto.affordance = vrCap.affordance;
                            dto.tcp = vrCap.tcp;
                            dto.shape = vrCap.shape;
                            dto.type = vrCap.type;
 
                            manipulationCapabilities.push_back(dto);
                        }
 
                        // initialize on demand
                        if (not allManipulationCapabilities)
                        {
                            allManipulationCapabilities.emplace();
                        }
 
                        allManipulationCapabilities->emplace(eef->getName(),
                                                             manipulationCapabilities);
                    }
                }
            }
 
            const armem::robot_state::description::RobotDescription robotDescription{
                .name = kinematicUnit->getRobotName(),
                .xml = {robotPath.serialize().package, robotPath.serialize().path},
                .visualization = visualization,
                .info = info,
                .manipulationCapabilities = allManipulationCapabilities};
 
            // 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 =
                    ::armarx::armem::robot_state::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