diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
index be19db6355d9c21e0b9a7e287e5889024a0c408c..c5f3fe36a067f6969e2c09b238ac1ed473422c2b 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
@@ -9,7 +9,6 @@
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/math/pose.h>
#include <SimoxUtility/math/rescale.h>
-
#include <VirtualRobot/XML/RobotIO.h>
#include <ArmarXCore/core/logging/Logging.h>
@@ -17,10 +16,8 @@
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/interface/core/PackagePath.h>
-#include <RobotAPI/libraries/armem/core/Time.h>
-
#include <RobotAPI/components/ArViz/Client/Elements.h>
-
+#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem_robot_state/server/description/Segment.h>
#include <RobotAPI/libraries/armem_robot_state/server/localization/Segment.h>
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h>
@@ -28,40 +25,43 @@
#include "combine.h"
-
namespace armarx::armem::server::robot_state
{
- Visu::Visu(
- const description::Segment& descriptionSegment,
- const proprioception::Segment& proprioceptionSegment,
- const localization::Segment& localizationSegment)
- : descriptionSegment(descriptionSegment),
- proprioceptionSegment(proprioceptionSegment),
- localizationSegment(localizationSegment)
+ Visu::Visu(const description::Segment& descriptionSegment,
+ const proprioception::Segment& proprioceptionSegment,
+ const localization::Segment& localizationSegment) :
+ descriptionSegment(descriptionSegment),
+ proprioceptionSegment(proprioceptionSegment),
+ localizationSegment(localizationSegment)
{
Logging::setTag("Visu");
}
-
- void Visu::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ void
+ Visu::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
- defs->optional(p.enabled, prefix + "enabled", "Enable or disable visualization of objects.");
+ defs->optional(
+ p.enabled, prefix + "enabled", "Enable or disable visualization of objects.");
defs->optional(p.frequencyHz, prefix + "frequenzyHz", "Frequency of visualization.");
- defs->optional(p.framesEnabled, prefix + "framesEnabled", "Enable or disable visualization of frames.");
- defs->optional(p.forceTorque.enabled, prefix + "forceTorque.enabled",
+ defs->optional(p.framesEnabled,
+ prefix + "framesEnabled",
+ "Enable or disable visualization of frames.");
+ defs->optional(p.forceTorque.enabled,
+ prefix + "forceTorque.enabled",
"Enable or disable visualization of force torque sensors.");
- defs->optional(p.forceTorque.forceScale, prefix + "forceTorque.forceScale",
+ defs->optional(p.forceTorque.forceScale,
+ prefix + "forceTorque.forceScale",
"Scaling of force arrows.");
}
-
- void Visu::init()
+ void
+ Visu::init()
{
}
-
- void Visu::connect(const viz::Client& arviz, DebugObserverInterfacePrx debugObserver)
+ void
+ Visu::connect(const viz::Client& arviz, DebugObserverInterfacePrx debugObserver)
{
this->arviz = arviz;
if (debugObserver)
@@ -76,15 +76,12 @@ namespace armarx::armem::server::robot_state
updateTask->join();
updateTask = nullptr;
}
- updateTask = new SimpleRunningTask<>([this]()
- {
- this->visualizeRun();
- });
+ updateTask = new SimpleRunningTask<>([this]() { this->visualizeRun(); });
updateTask->start();
}
-
- void Visu::visualizeRobots(viz::Layer& layer, const robot::Robots& robots)
+ void
+ Visu::visualizeRobots(viz::Layer& layer, const robot::Robots& robots)
{
for (const robot::Robot& robot : robots)
{
@@ -103,8 +100,8 @@ namespace armarx::armem::server::robot_state
}
}
-
- void Visu::visualizeFrames(
+ void
+ Visu::visualizeFrames(
viz::Layer& layerFrames,
const std::unordered_map<std::string, std::vector<Eigen::Affine3f>>& frames)
{
@@ -120,12 +117,14 @@ namespace armarx::armem::server::robot_state
pose = to;
- layerFrames.add(viz::Arrow(robotName + std::to_string(++i)).fromTo(from.translation(), to.translation()));
+ layerFrames.add(viz::Arrow(robotName + std::to_string(++i))
+ .fromTo(from.translation(), to.translation()));
}
}
}
- void Visu::visualizeFramesIndividual(
+ void
+ Visu::visualizeFramesIndividual(
viz::Layer& layerFrames,
const std::unordered_map<std::string, std::vector<Eigen::Affine3f>>& frames)
{
@@ -137,14 +136,14 @@ namespace armarx::armem::server::robot_state
int i = 0;
for (const auto& frame : robotFrames)
{
- layerFrames.add(viz::Pose(robotName + FRAME_NAMES.at(i++)).pose(frame.matrix()).scale(3));
+ layerFrames.add(
+ viz::Pose(robotName + FRAME_NAMES.at(i++)).pose(frame.matrix()).scale(3));
}
}
}
-
-
- void Visu::visualizeRun()
+ void
+ Visu::visualizeRun()
{
CycleUtil cycle(static_cast<int>(1000 / p.frequencyHz));
while (updateTask and not updateTask->isStopped())
@@ -158,6 +157,11 @@ namespace armarx::armem::server::robot_state
{
visualizeOnce(timestamp);
}
+ catch (const std::out_of_range& e)
+ {
+ ARMARX_WARNING << "Caught std::out_of_range while visualizing robots: \n"
+ << e.what();
+ }
catch (const std::exception& e)
{
ARMARX_WARNING << "Caught exception while visualizing robots: \n" << e.what();
@@ -176,8 +180,8 @@ namespace armarx::armem::server::robot_state
}
}
-
- void Visu::visualizeOnce(const Time& timestamp)
+ void
+ Visu::visualizeOnce(const Time& timestamp)
{
TIMING_START(tVisuTotal);
@@ -200,9 +204,9 @@ namespace armarx::armem::server::robot_state
TIMING_END_STREAM(tRobotFramePoses, ARMARX_DEBUG);
TIMING_START(tSensorValues);
- const auto sensorValues =
- proprioceptionSegment.getSensorValuesLocking(
- timestamp, debugObserver ? &*debugObserver : nullptr);
+ const proprioception::SensorValuesMap sensorValues =
+ proprioceptionSegment.getSensorValuesLocking(timestamp,
+ debugObserver ? &*debugObserver : nullptr);
TIMING_END_STREAM(tSensorValues, ARMARX_DEBUG);
TIMING_END_STREAM(tVisuGetData, ARMARX_DEBUG);
@@ -225,83 +229,51 @@ namespace armarx::armem::server::robot_state
combine(robotDescriptions, globalPoses, sensorValues, timestamp);
ARMARX_DEBUG << "Visualize " << robots.size() << " robots ...";
- viz::Layer layer = arviz.layer("Robots");
- visualizeRobots(layer, robots);
- std::vector layers{layer};
+ std::vector<viz::Layer> layers;
+
+ {
+ viz::Layer& layer = layers.emplace_back(arviz.layer("Robots"));
+ visualizeRobots(layer, robots);
+ }
ARMARX_DEBUG << "Visualize frames ...";
- if(p.framesEnabled)
+ if (p.framesEnabled)
{
- viz::Layer layerFrames = arviz.layer("Frames");
- visualizeFrames(layerFrames, frames);
- layers.push_back(layerFrames);
+ viz::Layer& layer = layers.emplace_back(arviz.layer("Localization Frames"));
+ visualizeFrames(layer, frames);
}
TIMING_END_STREAM(tVisuBuildLayers, ARMARX_DEBUG);
{
- viz::Layer layerFrames = arviz.layer("FramesIndividual");
- visualizeFramesIndividual(layerFrames, frames);
- layers.push_back(layerFrames);
+ viz::Layer& layer = layers.emplace_back(arviz.layer("Localization Frames Individual"));
+ visualizeFramesIndividual(layer, frames);
}
if (p.forceTorque.enabled)
{
- viz::Layer layerFrames = arviz.layer("ForceTorque");
+ viz::Layer& layerForceTorque = layers.emplace_back(arviz.layer("Force Torque"));
+ std::stringstream log;
+
for (const robot::Robot& robot : robots)
{
- const std::string& name = robot.description.name;
- if (robotNameHelper.find(name) == robotNameHelper.end())
- {
- const std::filesystem::path robotPath = robot.description.xml.toSystemPath();
- robotNameHelper[name] = RobotNameHelper::Create(robotPath);
- robotModels[name]
- = VirtualRobot::RobotIO::loadRobot(robotPath, VirtualRobot::RobotIO::eStructure);
- }
-
- auto model = robotModels.at(name);
- model->setJointValues(robot.config.jointMap);
- model->setGlobalPose(robot.config.globalPose.matrix());
+ const std::string& robotName = robot.description.name;
- const proprioception::ForceTorqueValuesMap& forceTorques
- = sensorValues.at(name).forceTorqueValuesMap;
-
- for (const auto& [side, ft] : forceTorques)
+ auto itSensorValues = sensorValues.find(robotName);
+ if (itSensorValues == sensorValues.end())
{
- ARMARX_CHECK(side == RobotNameHelper::LocationLeft or side == RobotNameHelper::LocationRight) << side;
-
- const std::string forceTorqueSensorName =
- robotNameHelper.at(name)->getArm(side).getForceTorqueSensor();
-
- const Eigen::Matrix4f forceTorqueSensorPose
- = model->getSensor(forceTorqueSensorName)->getGlobalPose();
-
- const std::string xyz = "XYZ";
-
- const Eigen::Vector3f from = simox::math::position(forceTorqueSensorPose);
- for (int i = 0; i < 3; ++i)
- {
- simox::Color color = simox::Color((255 * Eigen::Matrix3i::Identity().col(i)).eval());
- color.a = 128;
-
- // Force arrows.
- const float length = p.forceTorque.forceScale * ft.force(i);
- const float width = std::min(
- simox::math::rescale(std::abs(length), 0.F, 100.F, 1.F, 10.F),
- 10.F);
-
- const Eigen::Vector3f to = from + length * simox::math::orientation(forceTorqueSensorPose).col(i);
-
- std::stringstream key;
- key << side << " Force " << xyz.at(i);
- layerFrames.add(viz::Arrow(key.str()).fromTo(from, to).color(color).width(width));
-
- // Torque circle arrows.
- }
+ log << "\nNo robot '" << robotName << "' in sensor values. Available are: ";
+ log << simox::alg::join(simox::alg::get_keys(sensorValues), ", ");
+ continue;
}
+ visualizeForceTorque(layerForceTorque, robot, itSensorValues->second);
+ }
+
+ if (not log.str().empty())
+ {
+ ARMARX_INFO << "While visualizing robot force torques: " << log.str();
}
- layers.push_back(layerFrames);
}
@@ -317,15 +289,99 @@ namespace armarx::armem::server::robot_state
if (debugObserver.has_value())
{
const std::string p = "Visu | ";
- debugObserver->setDebugObserverDatafield(p + "t Total (ms)", tVisuTotal.toMilliSecondsDouble());
- debugObserver->setDebugObserverDatafield(p + "t 1 Get Data (ms)", tVisuGetData.toMilliSecondsDouble());
- debugObserver->setDebugObserverDatafield(p + "t 1.1 Descriptions (ms)", tRobotDescriptions.toMilliSecondsDouble());
- debugObserver->setDebugObserverDatafield(p + "t 1.2 Global Poses (ms)", tGlobalPoses.toMilliSecondsDouble());
- debugObserver->setDebugObserverDatafield(p + "t 1.3 Frames (ms)", tRobotFramePoses.toMilliSecondsDouble());
- debugObserver->setDebugObserverDatafield(p + "t 1.4 Sensor Values (ms)", tSensorValues.toMilliSecondsDouble());
- debugObserver->setDebugObserverDatafield(p + "t 2 Build Layers (ms)", tVisuBuildLayers.toMilliSecondsDouble());
- debugObserver->setDebugObserverDatafield(p + "t 3 Commit (ms)", tVisuCommit.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t Total (ms)",
+ tVisuTotal.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t 1 Get Data (ms)",
+ tVisuGetData.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t 1.1 Descriptions (ms)",
+ tRobotDescriptions.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t 1.2 Global Poses (ms)",
+ tGlobalPoses.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t 1.3 Frames (ms)",
+ tRobotFramePoses.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t 1.4 Sensor Values (ms)",
+ tSensorValues.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t 2 Build Layers (ms)",
+ tVisuBuildLayers.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t 3 Commit (ms)",
+ tVisuCommit.toMilliSecondsDouble());
+ }
+ }
+
+ void
+ Visu::visualizeForceTorque(viz::Layer& layer,
+ const robot::Robot& robot,
+ const proprioception::SensorValues& sensorValues)
+ {
+ const std::string& robotName = robot.description.name;
+
+ RobotModel* robotModel = nullptr;
+ if (auto it = models.find(robotName); it != models.end())
+ {
+ robotModel = &it->second;
+ }
+ else
+ {
+ const std::filesystem::path robotPath = robot.description.xml.toSystemPath();
+ ARMARX_INFO << "Loading robot model for '" << robotName << "' from " << robotPath
+ << " for visualization.";
+ auto [it2, _] = models.emplace(robotName, robotPath);
+ robotModel = &it2->second;
}
+
+ robotModel->model->setJointValues(robot.config.jointMap);
+ robotModel->model->setGlobalPose(robot.config.globalPose.matrix());
+
+ const proprioception::ForceTorqueValuesMap& forceTorques =
+ sensorValues.forceTorqueValuesMap;
+
+ for (const auto& [side, ft] : forceTorques)
+ {
+ ARMARX_CHECK(side == RobotNameHelper::LocationLeft or
+ side == RobotNameHelper::LocationRight)
+ << side;
+
+ const std::string forceTorqueSensorName =
+ robotModel->nameHelper->getArm(side).getForceTorqueSensor();
+
+ const Eigen::Matrix4f forceTorqueSensorPose =
+ robotModel->model->getSensor(forceTorqueSensorName)->getGlobalPose();
+
+ const std::string xyz = "XYZ";
+
+ const Eigen::Vector3f from = simox::math::position(forceTorqueSensorPose);
+ for (int i = 0; i < 3; ++i)
+ {
+ simox::Color color =
+ simox::Color((255 * Eigen::Matrix3i::Identity().col(i)).eval());
+ color.a = 128;
+
+ // Force arrows.
+ const float length = p.forceTorque.forceScale * ft.force(i);
+ const float width =
+ std::min(simox::math::rescale(std::abs(length), 0.F, 100.F, 1.F, 10.F), 10.F);
+
+ const Eigen::Vector3f to =
+ from + length * simox::math::orientation(forceTorqueSensorPose).col(i);
+
+ std::stringstream key;
+ ARMARX_CHECK_FITS_SIZE(i, xyz.size());
+ key << side << " Force " << xyz.at(i);
+ layer.add(viz::Arrow(key.str()).fromTo(from, to).color(color).width(width));
+
+ // TODO: Torque circle arrows.
+ }
+ }
+ }
+
+ Visu::RobotModel::RobotModel(const std::filesystem::path& robotPath) :
+ model{
+ VirtualRobot::RobotIO::loadRobot(robotPath, VirtualRobot::RobotIO::eStructure),
+ },
+ nameHelper{
+ RobotNameHelper::Create(robotPath),
+ }
+ {
}
} // namespace armarx::armem::server::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
index 265f08e2a0c10153c1b3cc8c624fd214713d57a9..2afa9a27f3396939c625050a942925db79129d64 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
@@ -28,14 +28,10 @@
#include <ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h>
#include <RobotAPI/components/ArViz/Client/Client.h>
-
+#include <RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h>
#include <RobotAPI/libraries/armem_objects/types.h>
-
#include <RobotAPI/libraries/armem_robot_state/server/forward_declarations.h>
-#include <RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h>
-
-
namespace armarx::armem::server::robot_state
{
@@ -46,41 +42,38 @@ namespace armarx::armem::server::robot_state
class Visu : public armarx::Logging
{
public:
-
Visu(const description::Segment& descriptionSegment,
const proprioception::Segment& proprioceptionSegment,
const localization::Segment& localizationSegment);
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "visu.");
+ void defineProperties(armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix = "visu.");
void init();
void connect(const viz::Client& arviz, DebugObserverInterfacePrx debugObserver = nullptr);
private:
-
void visualizeRun();
void visualizeOnce(const Time& timestamp);
- static
- void visualizeRobots(
- viz::Layer& layer,
- const robot::Robots& robots);
+ static void visualizeRobots(viz::Layer& layer, const robot::Robots& robots);
- static
- void visualizeFrames(
+ static void visualizeFrames(
viz::Layer& layerFrames,
const std::unordered_map<std::string, std::vector<Eigen::Affine3f>>& frames);
- static
- void visualizeFramesIndividual(
+ static void visualizeFramesIndividual(
viz::Layer& layerFrames,
const std::unordered_map<std::string, std::vector<Eigen::Affine3f>>& frames);
- private:
+ void visualizeForceTorque(viz::Layer& layer,
+ const robot::Robot& robot,
+ const proprioception::SensorValues& sensorValues);
+ private:
viz::Client arviz;
std::optional<DebugObserverHelper> debugObserver;
@@ -88,9 +81,15 @@ namespace armarx::armem::server::robot_state
const proprioception::Segment& proprioceptionSegment;
const localization::Segment& localizationSegment;
- std::map<std::string, RobotNameHelperPtr> robotNameHelper;
- std::map<std::string, VirtualRobot::RobotPtr> robotModels;
+ struct RobotModel
+ {
+ RobotModel(const std::filesystem::path& robotPath);
+
+ VirtualRobot::RobotPtr model;
+ RobotNameHelperPtr nameHelper;
+ };
+ std::map<std::string, RobotModel> models;
struct Properties
{
@@ -98,18 +97,17 @@ namespace armarx::armem::server::robot_state
float frequencyHz = 25.f;
bool framesEnabled = false;
-
+
struct ForceTorque
{
bool enabled = true;
float forceScale = 1.F;
};
+
ForceTorque forceTorque;
} p;
-
SimpleRunningTask<>::pointer_type updateTask;
-
};
-} // namespace armarx::armem::server::robot_state
+} // namespace armarx::armem::server::robot_state