VirtualRobotReader.cpp

Go to the documentation of this file.

#include "VirtualRobotReader.h"
 
#include <filesystem>
#include <optional>
#include <string>
#include <thread>
 
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/XML/BaseIO.h>
#include <VirtualRobot/XML/RobotIO.h>
 
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.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>
 
#include "RobotAPI/libraries/armem/core/forward_declarations.h"
 
namespace armarx::armem::robot_state
{
 
    bool
    VirtualRobotReader::synchronizeRobot(VirtualRobot::Robot& robot,
                                         const armem::Time& timestamp) const
    {
        const auto robotState = queryState(robot.getName(), 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;
    }
 
    bool
    VirtualRobotReader::synchronizeRobotJoints(VirtualRobot::Robot& robot,
                                               const armem::Time& timestamp) const
    {
        const auto robotState = queryJointState(robot.getName(), 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);
 
        return true;
    }
 
    bool
    armarx::armem::robot_state::VirtualRobotReader::synchronizeRobotPose(
        VirtualRobot::Robot& robot,
        const Time& timestamp) const
    {
        const auto robotState = queryState(robot.getName(), timestamp);
        if (not robotState)
        {
            ARMARX_VERBOSE << deactivateSpam(5) << "Querying robot state failed for robot `"
                           << robot.getName() << "` " << "(type `" << robot.getType() << "`)!";
            return false;
        }
 
        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::getRobotWaiting(const std::string& name,
                                        const armem::Time& timestamp,
                                        const VirtualRobot::RobotIO::RobotDescription& loadMode)
    {
        ARMARX_INFO << "Trying to get robot `" << name << "` ...";
 
        VirtualRobot::RobotPtr robot = nullptr;
 
        // while (robot == nullptr)
        {
            // might be nullptr if query fails
            robot = getRobot(name, timestamp, loadMode);
 
            if (robot == nullptr)
            {
                ARMARX_WARNING << deactivateSpam(1) << "Failed to query robot `" << name
                               << "`. At the moment, a blocking behavior is not possible in "
                                  "onConnectComponent()."
                               << "Therefore, a nullptr will be returned. This will likely cause "
                                  "problems downstream.";
                //    << "`. Will try again ...";
            }
            else
            {
                ARMARX_INFO << "Got robot `" << name << "`.";
            }
            // Clock::WaitFor(Duration::MilliSeconds(200));
        }
 
 
        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)
    {
        // Get robot model
        VirtualRobot::RobotPtr robot = nullptr;
 
        while (true)
        {
            const auto ts = timestamp.isInvalid() ? armarx::Clock::Now() : timestamp;
 
            robot = getRobot(name, ts, loadMode);
            if (robot) // we successfully got the robot
            {
                break;
            }
 
            // If we are in non-blocking mode, we are done -> failure
            if (not blocking)
            {
                ARMARX_VERBOSE << deactivateSpam(5) << "Failed to get robot `" << name << "`";
                return nullptr;
            }
 
            // If we are in blocking mode, we will retry
            ARMARX_VERBOSE << deactivateSpam(5) << "Retrying to query robot after failure";
            Clock::WaitFor(sleepAfterFailure);
        };
 
        // Pedantic check. Should not happen, but just in case.
        if (robot == nullptr)
        {
            ARMARX_VERBOSE << deactivateSpam(5) << "Failed to get robot `" << name << "`";
            return nullptr;
        }
 
        ARMARX_DEBUG << "Got robot model";
 
        while (true)
        {
            const auto ts = timestamp.isInvalid() ? armarx::Clock::Now() : timestamp;
 
            // Synchronize robot model.
            // If we can synchronize the robot, we are done -> success
            if (synchronizeRobot(*robot, ts))
            {
                return robot;
            }
 
            // If we are in non-blocking mode, we are done -> failure
            if (not blocking)
            {
                ARMARX_VERBOSE << deactivateSpam(5) << "Failed to get synchronized robot `" << name
                               << "`";
                return nullptr;
            }
 
            // If we are in non-blocking mode, we can try again.
            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;
    }
 
 
} // namespace armarx::armem::robot_state