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 529aeba908795871d1690ef937dbecf0736b1a8f..515a11d940a3ef3ec78502ebe535ba65000626aa 100644 --- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp +++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp @@ -24,8 +24,7 @@ namespace armarx::armem::robot_state if (not robotState) { ARMARX_VERBOSE << deactivateSpam(5) << "Querying robot state failed for robot `" - << robot.getName() << "` " - << "(type `" << robot.getType() << "`)!"; + << robot.getName() << "` " << "(type `" << robot.getType() << "`)!"; return false; } @@ -43,8 +42,7 @@ namespace armarx::armem::robot_state if (not robotState) { ARMARX_VERBOSE << deactivateSpam(5) << "Querying robot state failed for robot `" - << robot.getName() << "` " - << "(type `" << robot.getType() << "`)!"; + << robot.getName() << "` " << "(type `" << robot.getType() << "`)!"; return false; } @@ -84,6 +82,29 @@ namespace armarx::armem::robot_state return robot; } + VirtualRobot::RobotPtr + VirtualRobotReader::getRobotWaiting(const std::string& name, + const armem::Time& timestamp, + const VirtualRobot::RobotIO::RobotDescription& loadMode) + { + + 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 + << "`. Will try again ..."; + } + Clock::WaitFor(Duration::MilliSeconds(200)); + } + + return robot; + } + VirtualRobot::RobotPtr VirtualRobotReader::getSynchronizedRobot(const std::string& name, const VirtualRobot::BaseIO::RobotDescription& loadMode, @@ -128,4 +149,4 @@ namespace armarx::armem::robot_state } -} // namespace armarx::armem::robot_state::client::common +} // namespace armarx::armem::robot_state 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 0c0eb05aa6cd8f96bec6acdeb90c0261aca43c80..571838aa3104abbbc89f1834d50f7f8c2a73ca41 100644 --- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h +++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h @@ -69,6 +69,15 @@ namespace armarx::armem::robot_state VirtualRobot::RobotIO::RobotDescription::eStructure, bool blocking = true); + /** + @brief In contrast to getRobot(), this function will retry to query the robot until it exists. + */ + [[nodiscard]] VirtualRobot::RobotPtr + getRobotWaiting(const std::string& name, + const armem::Time& timestamp = armem::Time::Invalid(), + const VirtualRobot::RobotIO::RobotDescription& loadMode = + VirtualRobot::RobotIO::RobotDescription::eStructure); + private: [[nodiscard]] VirtualRobot::RobotPtr _getSynchronizedRobot(const std::string& name,