VirtualRobotReader.cpp

Go to the documentation of this file.

#include "VirtualRobotReader.h"
 
#include <optional>
#include <thread>
 
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/XML/RobotIO.h>
 
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/time/Clock.h>
 
namespace armarx::armem::robot_state
{
    // TODO(fabian.reister): register property defs
    void
    VirtualRobotReader::registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def)
    {
        RobotReader::registerPropertyDefinitions(def);
    }
 
    bool
    VirtualRobotReader::synchronizeRobot(VirtualRobot::Robot& robot,
                                         const armem::Time& timestamp) const
    {
        // const static auto packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
        // const auto package = armarx::ArmarXDataPath::getProject(packages, robot.getFilename());
 
        const robot::RobotDescription robotDescription{.name = robot.getName(),
                                                       .xml = PackagePath{"", ""}};
 
        const auto robotState = queryState(robotDescription, timestamp);
        if (not robotState)
        {
            ARMARX_VERBOSE << deactivateSpam(5) << "Querying robot state failed for robot `"
                           << robot.getName() << "` "
                           << "(type `" << robot.getType() << "`)!";
            return false;
        }
 
        robot.setJointValues(robotState->jointMap);
        robot.setGlobalPose(robotState->globalPose.matrix());
 
        return true;
    }
 
    VirtualRobot::RobotPtr
    VirtualRobotReader::getRobot(const std::string& name,
                                 const armem::Time& timestamp,
                                 const VirtualRobot::RobotIO::RobotDescription& loadMode)
    {
        ARMARX_VERBOSE << deactivateSpam(60) << "Querying robot description for robot '" << name
                       << "'";
        const auto description = queryDescription(name, timestamp);
 
        if (not description)
        {
            ARMARX_VERBOSE << deactivateSpam(5) << "The description of robot `" << name
                           << "` is not a available!";
 
            return nullptr;
        }
 
        const std::string xmlFilename = description->xml.toSystemPath();
        ARMARX_CHECK(std::filesystem::exists(xmlFilename)) << xmlFilename;
 
        ARMARX_VERBOSE << deactivateSpam(5) << "Loading (virtual) robot '" << description->name
                       << "' from XML file '" << xmlFilename << "'";
 
        auto robot = VirtualRobot::RobotIO::loadRobot(xmlFilename, loadMode);
        ARMARX_CHECK_NOT_NULL(robot) << "Could not load robot from file `" << xmlFilename << "`";
 
        robot->setName(name);
 
        return robot;
    }
 
    VirtualRobot::RobotPtr
    VirtualRobotReader::getSynchronizedRobot(const std::string& name,
                                             const VirtualRobot::BaseIO::RobotDescription& loadMode,
                                             bool blocking)
    {
        return _getSynchronizedRobot(name, armem::Time::Invalid(), loadMode, blocking);
    }
 
    VirtualRobot::RobotPtr
    VirtualRobotReader::getSynchronizedRobot(
        const std::string& name,
        const armem::Time& timestamp,
        const VirtualRobot::RobotIO::RobotDescription& loadMode,
        const bool blocking)
    {
        return _getSynchronizedRobot(name, timestamp, loadMode, blocking);
    }
 
    VirtualRobot::RobotPtr
    VirtualRobotReader::_getSynchronizedRobot(
        const std::string& name,
        const Time& timestamp,
        const VirtualRobot::BaseIO::RobotDescription& loadMode,
        bool blocking)
    {
        while (blocking)
        {
            const auto ts = timestamp.isInvalid() ? armarx::Clock::Now() : timestamp;
 
            VirtualRobot::RobotPtr robot = getRobot(name, ts, loadMode);
            if (robot and synchronizeRobot(*robot, ts))
            {
                return robot;
            }
 
            ARMARX_VERBOSE << deactivateSpam(5) << "Retrying to query robot after failure";
            Clock::WaitFor(sleepAfterFailure);
        }
 
        ARMARX_VERBOSE << deactivateSpam(5) << "Failed to get synchronized robot `" << name << "`";
        return nullptr;
    }
 
    std::optional<std::map<RobotReader::Hand, robot::ForceTorque>>
    VirtualRobotReader::queryForceTorque(const std::string& name, const Time& timestamp)
    {
        const auto description = queryDescription(name, timestamp);
        if (not description.has_value())
        {
            return std::nullopt;
        }
        return RobotReader::queryForceTorque(description.value(), timestamp);
    }
 
 
} // namespace armarx::armem::robot_state