Skip to content
Snippets Groups Projects
Commit 074d70d1 authored by Fabian Reister's avatar Fabian Reister
Browse files

(Virtual) robot reader: waiting until data is available

parent 7d7a3907
No related branches found
No related tags found
1 merge request!220(Virtual) robot reader: waiting until data is available
Hide whitespace changes
Inline Side-by-side
source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+ 40
− 9
View file @ 074d70d1
#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
{
......
source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
+ 15
− 8
View file @ 074d70d1
......@@ -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;
......
source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
+ 40
− 19
View file @ 074d70d1
#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;
}
......
source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
+ 5
− 4
View file @ 074d70d1
......@@ -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
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment