diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
index 2d548e7c1f8932b15f526c07d0e87b6475114cc7..3155f7a354152e836a10b9f6ae04d56107189f26 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
@@ -140,8 +140,6 @@ namespace armarx::armem::server::robot_state
robotUnit.plugin->waitUntilRobotUnitIsRunning();
}
RobotUnitInterfacePrx robotUnitPrx = robotUnit.plugin->getRobotUnit();
- ARMARX_CHECK_NOT_NULL(robotUnitPrx->getKinematicUnit())
- << "Robot unit '" << robotUnit.plugin->getRobotUnitName() << "' must have a kinematic unit.";
descriptionSegment.connect(robotUnitPrx);
@@ -149,7 +147,7 @@ namespace armarx::armem::server::robot_state
localizationSegment.connect();
- commonVisu.connect(getArvizClient());
+ commonVisu.connect(getArvizClient(), debugObserver->getDebugObserver());
robotUnit.reader.connect(*robotUnit.plugin, *debugObserver);
robotUnit.writer.connect(*debugObserver);
diff --git a/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
index 498a422fb201bf435cfb8ddbfdc43b71ba1944a0..79de71c71a7f4ca0cf911cede288254e0c904d85 100644
--- a/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
@@ -11,6 +11,8 @@ armarx_add_library(
# ArmarX
ArmarXCore
ArmarXCoreInterfaces
+ DebugObserverHelper
+ # ArmarXGui
ArmarXGuiComponentPlugins
# This package
RobotAPICore
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 97f47e93bf620bdf42157c2523675a4535ea6701..f84c223c292d85adbb7e7db190e471b7210e58d8 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
@@ -36,12 +36,19 @@ namespace armarx::armem::server::robot_state
}
- void Visu::init() {}
+ void Visu::init()
+ {
+ }
- void Visu::connect(const viz::Client& arviz)
+ void Visu::connect(const viz::Client& arviz, DebugObserverInterfacePrx debugObserver)
{
this->arviz = arviz;
+ if (debugObserver)
+ {
+ bool batchMode = true;
+ this->debugObserver = DebugObserverHelper("RobotStateMemory", debugObserver, batchMode);
+ }
updateTask = new SimpleRunningTask<>([this]()
{
@@ -53,7 +60,6 @@ namespace armarx::armem::server::robot_state
void Visu::visualizeRobots(viz::Layer& layer, const robot::Robots& robots)
{
-
const auto visualizeRobot = [&](const robot::Robot & robot)
{
const auto xmlPath = robot.description.xml.serialize();
@@ -87,71 +93,14 @@ namespace armarx::armem::server::robot_state
for (const auto& frame : robotFrames)
{
Eigen::Affine3f from = pose;
+ Eigen::Affine3f to = pose * frame;
- pose = pose * frame; // to
-
- Eigen::Affine3f to = pose;
-
- const auto arr = viz::Arrow(robotName + std::to_string(++i)).fromTo(from.translation(), to.translation());
- layerFrames.add(arr);
+ layerFrames.add(viz::Arrow(robotName + std::to_string(++i)).fromTo(from.translation(), to.translation()));
}
}
}
-
-
- // void Visu::RemoteGui::setup(const Visu& visu)
- // {
- // using namespace armarx::RemoteGui::Client;
-
- // enabled.setValue(visu.enabled);
- // inGlobalFrame.setValue(visu.inGlobalFrame);
- // alpha.setRange(0, 1.0);
- // alpha.setValue(visu.alpha);
- // alphaByConfidence.setValue(visu.alphaByConfidence);
- // oobbs.setValue(visu.oobbs);
- // objectFrames.setValue(visu.objectFrames);
- // {
- // float max = 10000;
- // objectFramesScale.setRange(0, max);
- // objectFramesScale.setDecimals(2);
- // objectFramesScale.setSteps(int(10 * max));
- // objectFramesScale.setValue(visu.objectFramesScale);
- // }
-
- // GridLayout grid;
- // int row = 0;
- // grid.add(Label("Enabled"), {row, 0}).add(enabled, {row, 1});
- // row++;
- // grid.add(Label("Global Frame"), {row, 0}).add(inGlobalFrame, {row, 1});
- // row++;
- // grid.add(Label("Alpha"), {row, 0}).add(alpha, {row, 1}, {1, 3});
- // row++;
- // grid.add(Label("Alpha by Confidence"), {row, 0}).add(alphaByConfidence, {row, 1});
- // row++;
- // grid.add(Label("OOBB"), {row, 0}).add(oobbs, {row, 1});
- // row++;
- // grid.add(Label("Object Frames"), {row, 0}).add(objectFrames, {row, 1});
- // grid.add(Label("Scale:"), {row, 2}).add(objectFramesScale, {row, 3});
- // row++;
-
- // group.setLabel("Visualization");
- // group.addChild(grid);
- // }
-
- // void Visu::RemoteGui::update(Visu& visu)
- // {
- // visu.enabled = enabled.getValue();
- // visu.inGlobalFrame = inGlobalFrame.getValue();
- // visu.alpha = alpha.getValue();
- // visu.alphaByConfidence = alphaByConfidence.getValue();
- // visu.oobbs = oobbs.getValue();
- // visu.objectFrames = objectFrames.getValue();
- // visu.objectFramesScale = objectFramesScale.getValue();
- // }
-
-
robot::Robots combine(
const std::unordered_map<std::string, robot::RobotDescription>& robotDescriptions,
const std::unordered_map<std::string, Eigen::Affine3f>& globalRobotPoseMap,
@@ -209,32 +158,43 @@ namespace armarx::armem::server::robot_state
{
// std::scoped_lock lock(visuMutex);
ARMARX_DEBUG << "Update task";
-
if (p.enabled)
{
- TIMING_START(Visu);
+ TIMING_START(tVisuTotal);
// TODO(fabian.reister): use timestamp
- const auto timestamp = IceUtil::Time::now();
+ const Time timestamp = Time::now();
try
{
+ // Get data.
+ TIMING_START(tVisuGetData);
+
TIMING_START(tRobotDescriptions);
const description::Segment::RobotDescriptionMap robotDescriptions =
descriptionSegment.getRobotDescriptionsLocking(timestamp);
- TIMING_END_STREAM(tRobotDescriptions, ARMARX_VERBOSE);
+ TIMING_END_STREAM(tRobotDescriptions, ARMARX_DEBUG);
TIMING_START(tGlobalRobotPoseMap);
const auto globalRobotPoseMap =
localizationSegment.getRobotGlobalPosesLocking(timestamp);
- TIMING_END_STREAM(tGlobalRobotPoseMap, ARMARX_VERBOSE);
+ TIMING_END_STREAM(tGlobalRobotPoseMap, ARMARX_DEBUG);
+
+ TIMING_START(tRobotFramePoses);
+ const auto frames = localizationSegment.getRobotFramePosesLocking(timestamp);
+ TIMING_END_STREAM(tRobotFramePoses, ARMARX_DEBUG);
TIMING_START(tRobotJointPositionMap);
const auto robotJointPositionMap =
- proprioceptionSegment.getRobotJointPositionsLocking(timestamp);
- TIMING_END_STREAM(tRobotJointPositionMap, ARMARX_VERBOSE);
+ proprioceptionSegment.getRobotJointPositionsLocking(
+ timestamp, debugObserver ? &*debugObserver : nullptr);
+ TIMING_END_STREAM(tRobotJointPositionMap, ARMARX_DEBUG);
+
+ TIMING_END_STREAM(tVisuGetData, ARMARX_DEBUG);
- const auto frames = localizationSegment.getRobotFramePoses(timestamp);
+
+ // Build layers.
+ TIMING_START(tVisuBuildLayers);
// we need all 3 informations:
// - robot description
@@ -244,44 +204,55 @@ namespace armarx::armem::server::robot_state
const robot::Robots robots =
combine(robotDescriptions, globalRobotPoseMap, robotJointPositionMap);
- viz::Layer layer = arviz.layer("Robots");
- ARMARX_DEBUG << "visualizing robots";
+ ARMARX_DEBUG << "Visualize robots ...";
+ viz::Layer layer = arviz.layer("Robots");
visualizeRobots(layer, robots);
- ARMARX_DEBUG << "Committing robots";
-
+ ARMARX_DEBUG << "Visualize frames ...";
viz::Layer layerFrames = arviz.layer("Frames");
-
- ARMARX_DEBUG << "visualizing frames";
visualizeFrames(layerFrames, frames);
- ARMARX_DEBUG << "Committing frames";
+ TIMING_END_STREAM(tVisuBuildLayers, ARMARX_DEBUG);
- arviz.commit({layer, layerFrames});
-
- ARMARX_DEBUG << "Done committing";
- TIMING_END_STREAM(Visu, ARMARX_VERBOSE);
+ // Commit layers.
- // if (debugObserver)
- // {
- // debugObserver->setDebugChannel(getName(),
- // {
- // { "t Visualize [ms]", new Variant(Visu.toMilliSecondsDouble()) },
- // });
- // }
+ ARMARX_DEBUG << "Commit visualization ...";
+ TIMING_START(tVisuCommit);
+ arviz.commit({layer, layerFrames});
+ TIMING_END_STREAM(tVisuCommit, ARMARX_DEBUG);
+
+ TIMING_END_STREAM(tVisuTotal, ARMARX_DEBUG);
+
+ 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 Pose (ms)", tGlobalRobotPoseMap.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t 1.3 Frames (ms)", tRobotFramePoses.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t 1.4 Joint Positions (ms)", tRobotJointPositionMap.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t 2 Build Layers (ms)", tVisuBuildLayers.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t 3 Commit (ms)", tVisuCommit.toMilliSecondsDouble());
+ }
}
catch (const std::exception& ex)
{
- ARMARX_WARNING << ex.what();
+ ARMARX_WARNING << "Caught exception while visualizing robots: \n" << ex.what();
continue;
}
catch (...)
{
- ARMARX_WARNING << "Unknown exception";
+ ARMARX_WARNING << "Caught unknown exception while visualizing robots.";
continue;
}
+
+ if (debugObserver.has_value())
+ {
+ debugObserver->sendDebugObserverBatch();
+ }
}
}
cycle.waitForCycleDuration();
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 e659981fb0c0763397cf78ad9c98bc1b1f1ee1c8..5af7ee0ac9bb01fa394430d9a07ec70da4d25b50 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
@@ -21,10 +21,11 @@
#pragma once
+#include <optional>
+
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
-
-// #include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
+#include <ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h>
#include <RobotAPI/components/ArViz/Client/Client.h>
@@ -42,12 +43,10 @@ namespace armarx::armem::server::robot_state
{
class Segment;
}
-
namespace proprioception
{
class Segment;
}
-
namespace description
{
class Segment;
@@ -73,7 +72,7 @@ namespace armarx::armem::server::robot_state
void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "visu.");
void init();
- void connect(const viz::Client& arviz);
+ void connect(const viz::Client& arviz, DebugObserverInterfacePrx debugObserver = nullptr);
protected:
@@ -89,7 +88,9 @@ namespace armarx::armem::server::robot_state
private:
+
viz::Client arviz;
+ std::optional<DebugObserverHelper> debugObserver;
const description::Segment& descriptionSegment;
const proprioception::Segment& proprioceptionSegment;
@@ -105,23 +106,6 @@ namespace armarx::armem::server::robot_state
SimpleRunningTask<>::pointer_type updateTask;
void visualizeRun();
- // struct RemoteGui
- // {
- // armarx::RemoteGui::Client::GroupBox group;
-
- // armarx::RemoteGui::Client::CheckBox enabled;
-
- // armarx::RemoteGui::Client::CheckBox inGlobalFrame;
- // armarx::RemoteGui::Client::FloatSlider alpha;
- // armarx::RemoteGui::Client::CheckBox alphaByConfidence;
- // armarx::RemoteGui::Client::CheckBox oobbs;
- // armarx::RemoteGui::Client::CheckBox objectFrames;
- // armarx::RemoteGui::Client::FloatSpinBox objectFramesScale;
-
- // // void setup(const Visu& visu);
- // // void update(Visu& visu);
- // };
-
};
} // namespace armarx::armem::server::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
index f43d8709e24ae1cf778fd1c678e882b7cd212187..8c5a777bde8007cbe8dffe0d09dc50d7095b6c68 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
@@ -83,77 +83,104 @@ namespace armarx::armem::server::robot_state::proprioception
}
- Segment::RobotJointPositionMap Segment::getRobotJointPositionsLocking(const armem::Time& timestamp) const
+ Segment::RobotJointPositionMap Segment::getRobotJointPositionsLocking(
+ const armem::Time& timestamp,
+ DebugObserverHelper* debugObserver) const
{
TIMING_START(tRobotJointPositionsLock);
std::scoped_lock lock(mutex());
- TIMING_END_STREAM(tRobotJointPositionsLock, ARMARX_VERBOSE);
- return getRobotJointPositions(timestamp);
+ TIMING_END_STREAM(tRobotJointPositionsLock, ARMARX_DEBUG);
+ if (debugObserver)
+ {
+ debugObserver->setDebugObserverDatafield(dp + "t 0 Lock Core Segment (ms)", tRobotJointPositionsLock.toMilliSecondsDouble());
+ }
+ return getRobotJointPositions(timestamp, debugObserver);
}
- Segment::RobotJointPositionMap Segment::getRobotJointPositions(const armem::Time& timestamp) const
+ Segment::RobotJointPositionMap Segment::getRobotJointPositions(
+ const armem::Time& timestamp,
+ DebugObserverHelper* debugObserver) const
{
namespace adn = aron::datanavigator;
ARMARX_CHECK_NOT_NULL(coreSegment);
RobotJointPositionMap jointMap;
- TIMING_START(tProviders);
+ TIMING_START(tProcessProviders);
for (const auto& [robotName, provSeg] : *coreSegment)
{
- TIMING_START(tEntities);
+ TIMING_START(tProcessEntities);
int i = 0;
for (const auto& [name, entity] : provSeg.entities())
{
- TIMING_START(tTryCatch);
- try
+ TIMING_START(tProcessEntity);
+
+ TIMING_START(tGetLatestInstance);
+ if (entity.empty())
{
- TIMING_START(tGetLatestInstance);
- const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
- TIMING_END_COMMENT_STREAM(tGetLatestInstance, "tGetLatestInstance " + std::to_string(i), ARMARX_VERBOSE);
-
- TIMING_START(tRobotJointPositionsTryCast);
- // tryCast is too slow.
- // const auto jointState = tryCast<armarx::armem::arondto::JointState>(entityInstance);
- aron::datanavigator::DictNavigatorPtr data = entityInstance.data();
- // Avoid throwing an exception.
- if (!(data->hasElement("name") && data->hasElement("position")))
- {
- continue;
- }
- armarx::armem::arondto::JointState jointState;
- jointState.name = adn::StringNavigator::DynamicCastAndCheck(data->getElement("name"))->getValue();
- jointState.position = adn::FloatNavigator::DynamicCastAndCheck(data->getElement("position"))->getValue();
- TIMING_END_COMMENT_STREAM(tRobotJointPositionsTryCast, "tRobotJointPositionsTryCast " + std::to_string(i), ARMARX_VERBOSE);
-
- TIMING_START(tEmplace);
- if (jointState.name.size() > 0)
- {
- jointMap[robotName].emplace(jointState.name, jointState.position);
- }
- else
- {
- // This may happen, just ignore it.
- // ARMARX_WARNING << "Could not convert entity instance to 'JointState'";
- }
- TIMING_END_COMMENT_STREAM(tEmplace, "tEmplace " + std::to_string(i), ARMARX_VERBOSE);
+ continue;
}
- catch (const armem::error::EntityHistoryEmpty&)
+ const wm::EntitySnapshot& snapshot = entity.getLatestSnapshot();
+ if (snapshot.empty())
{
+ continue;
}
- catch (const armem::error::MissingEntry&)
+ const wm::EntityInstance& entityInstance = snapshot.getInstance(0);
+ TIMING_END_COMMENT_STREAM(tGetLatestInstance, "tGetLatestInstance " + std::to_string(i), ARMARX_DEBUG);
+
+ TIMING_START(tCastJointState);
+ // tryCast is too slow.
+ // const auto jointState = tryCast<armarx::armem::arondto::JointState>(entityInstance);
+ aron::datanavigator::DictNavigatorPtr data = entityInstance.data();
+ // Avoid throwing an exception.
+ if (!(data->hasElement("name") && data->hasElement("position")))
{
+ continue;
+ }
+ armarx::armem::arondto::JointState jointState;
+ jointState.name = adn::StringNavigator::DynamicCastAndCheck(data->getElement("name"))->getValue();
+ jointState.position = adn::FloatNavigator::DynamicCastAndCheck(data->getElement("position"))->getValue();
+ TIMING_END_COMMENT_STREAM(tCastJointState, "tCastJointState " + std::to_string(i), ARMARX_DEBUG);
+
+ TIMING_START(tEmplace);
+ if (jointState.name.size() > 0)
+ {
+ jointMap[robotName].emplace(jointState.name, jointState.position);
+ }
+ else
+ {
+ // This may happen, just ignore it.
+ // ARMARX_WARNING << "Could not convert entity instance to 'JointState'";
+ }
+ TIMING_END_COMMENT_STREAM(tEmplace, "tEmplace " + std::to_string(i), ARMARX_DEBUG);
+
+ TIMING_END_COMMENT_STREAM(tProcessEntity, "tProcessEntity " + std::to_string(i), ARMARX_DEBUG);
+
+ if (debugObserver)
+ {
+ debugObserver->setDebugObserverDatafield(dp + "t 1.1.1.1 GetLatestInstance (ms)", tGetLatestInstance.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(dp + "t 1.1.1.2 CastJointState (ms)", tCastJointState.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(dp + "t 1.1.1.3 Emplace (ms)", tEmplace.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(dp + "t 1.1.1.T Process Entity (ms)", tProcessEntity.toMilliSecondsDouble());
}
- TIMING_END_STREAM(tTryCatch, ARMARX_VERBOSE);
++i;
}
- TIMING_END_STREAM(tEntities, ARMARX_VERBOSE);
+ TIMING_END_STREAM(tProcessEntities, ARMARX_DEBUG);
+ if (debugObserver)
+ {
+ debugObserver->setDebugObserverDatafield(dp + "t 1.1 Process Entities (ms)", tProcessEntities.toMilliSecondsDouble());
+ }
}
- TIMING_END_STREAM(tProviders, ARMARX_VERBOSE);
+ TIMING_END_STREAM(tProcessProviders, ARMARX_DEBUG);
ARMARX_INFO << deactivateSpam(60) << "Number of known robot joint maps: " << jointMap.size();
+ if (debugObserver)
+ {
+ debugObserver->setDebugObserverDatafield(dp + "t 1 Process Providers (ms)", tProcessProviders.toMilliSecondsDouble());
+ }
+
return jointMap;
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
index abc827c90d236c57e76624de5b16b71a8e48f8b0..f6c49f55e28226d90110f610c990f0b7208bdff7 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
@@ -35,6 +35,10 @@
#include <RobotAPI/libraries/armem_objects/types.h>
+namespace armarx
+{
+ class DebugObserverHelper;
+}
namespace armarx::armem
{
namespace server
@@ -69,8 +73,10 @@ namespace armarx::armem::server::robot_state::proprioception
std::mutex& mutex() const;
- RobotJointPositionMap getRobotJointPositions(const armem::Time& timestamp) const;
- RobotJointPositionMap getRobotJointPositionsLocking(const armem::Time& timestamp) const;
+ RobotJointPositionMap getRobotJointPositions(
+ const armem::Time& timestamp, DebugObserverHelper* debugObserver = nullptr) const;
+ RobotJointPositionMap getRobotJointPositionsLocking(
+ const armem::Time& timestamp, DebugObserverHelper* debugObserver = nullptr) const;
const armem::MemoryID& getRobotUnitProviderID() const;
@@ -90,6 +96,8 @@ namespace armarx::armem::server::robot_state::proprioception
};
Properties p;
+ // Debug Observer prefix
+ const std::string dp = "Proprioception::getRobotJointPositions() | ";
};
} // namespace armarx::armem::server::robot_state::proprioception