diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp index 409b3cddae23e3013fa47a887427cd7610c7171f..b3dee3440967cf79ae9feee73403116345a23cbf 100644 --- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp +++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp @@ -1,7 +1,9 @@ #include "RobotReader.h" +#include <chrono> #include <mutex> #include <optional> +#include <thread> #include <ArmarXCore/core/PackagePath.h> #include <ArmarXCore/core/exceptions/LocalException.h> @@ -91,19 +93,48 @@ namespace armarx::armem::robot_state return robot; } + void + RobotReader::setSyncTimeout(const armem::Time& duration) + { + syncTimeout = duration; + } + + void + RobotReader::setSleepAfterSyncFailure(const armem::Time& duration) + { + ARMARX_CHECK_NONNEGATIVE(duration.toMicroSeconds()); + sleepAfterFailure = duration; + } + bool RobotReader::synchronize(robot::Robot& obj, const armem::Time& timestamp) { - auto state = queryState(obj.description, timestamp); + const auto tsStartFunctionInvokation = armem::Time::now(); - if (not state) /* c++20 [[unlikely]] */ + while (true) { - ARMARX_WARNING << "Could not synchronize object " << obj.description.name; - return false; - } + auto state = queryState(obj.description, timestamp); - obj.config = std::move(*state); - return true; + if (not state) /* c++20 [[unlikely]] */ + { + ARMARX_VERBOSE << "Could not synchronize object " << obj.description.name; + + // if the syncTime is non-positive there will be no retry + const auto elapsedTime = armem::Time::now() - tsStartFunctionInvokation; + if (elapsedTime > syncTimeout) + { + ARMARX_WARNING << "Could not synchronize object " << obj.description.name; + return false; + } + + ARMARX_INFO << "Retrying to query robot state after failure"; + std::this_thread::sleep_for( + std::chrono::microseconds(sleepAfterFailure.toMicroSeconds())); + } + + obj.config = std::move(*state); + return true; + } } std::optional<robot::RobotDescription> @@ -322,8 +353,8 @@ namespace armarx::armem::robot_state std::optional<AronClass> tryCast(const wm::EntityInstance& item) { - static_assert( - std::is_base_of<armarx::aron::codegenerator::cpp::AronGeneratedClass, AronClass>::value); + static_assert(std::is_base_of<armarx::aron::codegenerator::cpp::AronGeneratedClass, + AronClass>::value); try { diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h index 20cafbd2225160c7b9abacc2d73b981f60260e34..46b0924fc2cb06ebee34ff8b61174746939dd7dc 100644 --- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h +++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h @@ -24,6 +24,7 @@ #include <mutex> #include <optional> +#include "RobotAPI/libraries/armem/core/Time.h" #include <RobotAPI/libraries/armem/client/MemoryNameSystem.h> #include <RobotAPI/libraries/armem/client/Reader.h> #include <RobotAPI/libraries/armem_robot/client/interfaces.h> @@ -67,10 +68,10 @@ namespace armarx::armem::robot_state const armem::Time& timestamp) const; using JointTrajectory = std::map<armem::Time, robot::RobotState::JointMap>; - - JointTrajectory - queryJointStates(const robot::RobotDescription& description, - const armem::Time& begin, const armem::Time& end) const; + + JointTrajectory queryJointStates(const robot::RobotDescription& description, + const armem::Time& begin, + const armem::Time& end) const; std::optional<robot::RobotState::Pose> queryGlobalPose(const robot::RobotDescription& description, @@ -80,6 +81,8 @@ namespace armarx::armem::robot_state queryPlatformState(const robot::RobotDescription& description, const armem::Time& timestamp) const; + void setSyncTimeout(const armem::Time& duration); + void setSleepAfterSyncFailure(const armem::Time& duration); enum class Hand { @@ -96,6 +99,11 @@ namespace armarx::armem::robot_state const armem::Time& start, const armem::Time& end) const; + protected: + // by default, no timeout mechanism + armem::Time syncTimeout = armem::Time::microSeconds(0); + armem::Time sleepAfterFailure = armem::Time::microSeconds(0); + private: std::optional<robot::RobotState> getRobotState(const armarx::armem::wm::Memory& memory, const std::string& name) const; @@ -106,9 +114,8 @@ namespace armarx::armem::robot_state std::optional<robot::RobotState::JointMap> getRobotJointState(const armarx::armem::wm::Memory& memory, const std::string& name) const; - JointTrajectory - getRobotJointStates(const armarx::armem::wm::Memory& memory, - const std::string& name) const; + JointTrajectory getRobotJointStates(const armarx::armem::wm::Memory& memory, + const std::string& name) const; std::optional<robot::PlatformState> getRobotPlatformState(const armarx::armem::wm::Memory& memory, @@ -117,7 +124,7 @@ namespace armarx::armem::robot_state std::map<RobotReader::Hand, robot::ForceTorque> getForceTorque(const armarx::armem::wm::Memory& memory, const std::string& name) const; - + std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>> getForceTorques(const armarx::armem::wm::Memory& memory, const std::string& name) const; diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp index 4dea491c8e0a2eec9b8c04677433977263ffdda9..5ef7564f8a0e374060114d0391bfaeebf1df6198 100644 --- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp +++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp @@ -1,8 +1,10 @@ #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> @@ -19,30 +21,33 @@ namespace armarx::armem::robot_state { } - void VirtualRobotReader::connect() + void + VirtualRobotReader::connect() { RobotReader::connect(); } // TODO(fabian.reister): register property defs - void VirtualRobotReader::registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def) + void + VirtualRobotReader::registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def) { RobotReader::registerPropertyDefinitions(def); } - bool VirtualRobotReader::synchronizeRobot(VirtualRobot::Robot& robot, - const armem::Time& timestamp) + bool + VirtualRobotReader::synchronizeRobot(VirtualRobot::Robot& robot, const armem::Time& timestamp) { const auto packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages(); const auto package = armarx::ArmarXDataPath::getProject(packages, robot.getFilename()); - const robot::RobotDescription robotDescription{.name = robot.getName(), - .xml = PackagePath{package, robot.getFilename()}}; + const robot::RobotDescription robotDescription{ + .name = robot.getName(), .xml = PackagePath{package, robot.getFilename()}}; const auto robotState = queryState(robotDescription, timestamp); if (not robotState) { - ARMARX_WARNING << "Querying robot state failed for robot `" << robot.getName() << " / " << robot.getType() << "`!"; + ARMARX_WARNING << "Querying robot state failed for robot `" << robot.getName() << " / " + << robot.getType() << "`!"; return false; } @@ -52,9 +57,10 @@ namespace armarx::armem::robot_state return true; } - VirtualRobot::RobotPtr VirtualRobotReader::getRobot(const std::string& name, - const armem::Time& timestamp, - const VirtualRobot::RobotIO::RobotDescription& loadMode) + VirtualRobot::RobotPtr + VirtualRobotReader::getRobot(const std::string& name, + const armem::Time& timestamp, + const VirtualRobot::RobotIO::RobotDescription& loadMode) { ARMARX_INFO << "Querying robot description for robot '" << name << "'"; const auto description = queryDescription(name, timestamp); @@ -64,8 +70,10 @@ namespace armarx::armem::robot_state return nullptr; } - const std::string xmlFilename = ArmarXDataPath::resolvePath(description->xml.serialize().path); - ARMARX_INFO << "Loading (virtual) robot '" << description->name << "' from XML file '" << xmlFilename << "'"; + const std::string xmlFilename = + ArmarXDataPath::resolvePath(description->xml.serialize().path); + ARMARX_INFO << "Loading (virtual) robot '" << description->name << "' from XML file '" + << xmlFilename << "'"; auto robot = VirtualRobot::RobotIO::loadRobot(xmlFilename, loadMode); robot->setName(name); @@ -76,15 +84,28 @@ namespace armarx::armem::robot_state } - VirtualRobot::RobotPtr VirtualRobotReader::getSynchronizedRobot(const std::string& name, - const armem::Time& timestamp, - const VirtualRobot::RobotIO::RobotDescription& loadMode) + VirtualRobot::RobotPtr + VirtualRobotReader::getSynchronizedRobot( + const std::string& name, + const armem::Time& timestamp, + const VirtualRobot::RobotIO::RobotDescription& loadMode, + const bool blocking) { - auto robot = getRobot(name, timestamp, loadMode); - - synchronizeRobot(*robot, timestamp); + while (blocking) + { + VirtualRobot::RobotPtr robot = getRobot(name, timestamp, loadMode); + if (robot and synchronizeRobot(*robot, timestamp)) + { + return robot; + } + + ARMARX_INFO << "Retrying to query robot after failure"; + std::this_thread::sleep_for( + std::chrono::microseconds(sleepAfterFailure.toMicroSeconds())); + } - return robot; + ARMARX_WARNING << "Failed to get synchronized robot `" << name << "`"; + return nullptr; } diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h index 68c0d5e7df911f23b06fcc582108bbf9f205e49c..c14e60aee78a490bd1ffb0937ca008dc56895499 100644 --- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h +++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h @@ -21,12 +21,12 @@ #pragma once -#include "RobotReader.h" - #include <VirtualRobot/Robot.h> #include <VirtualRobot/VirtualRobot.h> #include <VirtualRobot/XML/RobotIO.h> +#include "RobotReader.h" + namespace armarx::armem::robot_state { @@ -42,7 +42,7 @@ namespace armarx::armem::robot_state { public: VirtualRobotReader(armem::client::MemoryNameSystem& memoryNameSystem); - virtual ~VirtualRobotReader() = default; + ~VirtualRobotReader() override = default; void connect(); void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def); @@ -59,7 +59,8 @@ namespace armarx::armem::robot_state getSynchronizedRobot(const std::string& name, const armem::Time& timestamp, const VirtualRobot::RobotIO::RobotDescription& loadMode = - VirtualRobot::RobotIO::RobotDescription::eStructure); + VirtualRobot::RobotIO::RobotDescription::eStructure, + bool blocking = true); }; } // namespace armarx::armem::robot_state