From de1ac12519492eb72f554f1e729285f1b97539a3 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Tue, 17 Aug 2021 12:19:45 +0200
Subject: [PATCH] Update / fix visualization in RobotStateMemory
---
.../armem_robot_state/CMakeLists.txt | 2 +
.../common/localization/TransformHelper.cpp | 25 +-
.../armem_robot_state/server/common/Visu.cpp | 275 ++++++++----------
.../armem_robot_state/server/common/Visu.h | 16 +-
.../server/common/combine.cpp | 64 ++++
.../armem_robot_state/server/common/combine.h | 38 +++
.../proprioception/RobotStateWriter.cpp | 115 ++++----
.../server/proprioception/RobotStateWriter.h | 11 +-
.../server/proprioception/Segment.cpp | 22 +-
9 files changed, 334 insertions(+), 234 deletions(-)
create mode 100644 source/RobotAPI/libraries/armem_robot_state/server/common/combine.cpp
create mode 100644 source/RobotAPI/libraries/armem_robot_state/server/common/combine.h
diff --git a/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
index 2d1842b46..c86ed671e 100644
--- a/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
@@ -39,6 +39,7 @@ armarx_add_library(
server/forward_declarations.h
server/common/Visu.h
+ server/common/combine.h
server/localization/Segment.h
@@ -68,6 +69,7 @@ armarx_add_library(
client/localization/TransformWriter.cpp
server/common/Visu.cpp
+ server/common/combine.cpp
server/localization/Segment.cpp
diff --git a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
index 18b84d7c6..f20f68cf8 100644
--- a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
@@ -125,8 +125,7 @@ namespace armarx::armem::common::robot_state::localization
std::vector<std::string> chain;
- const auto& agentProviderSegment =
- localizationCoreSegment.getProviderSegment(query.header.agent);
+ const auto& agentProviderSegment = localizationCoreSegment.getProviderSegment(query.header.agent);
auto addToChain = [&](const std::string & parentFrame, const std::string & frame)
{
@@ -138,14 +137,14 @@ namespace armarx::armem::common::robot_state::localization
}
else
{
- ARMARX_WARNING << deactivateSpam(10) << "Cannot perform tf lookup '" << parentFrame << " -> " << frame
- << "'";
+ ARMARX_WARNING << deactivateSpam(10) << "Cannot perform tf lookup '" << parentFrame << " -> " << frame << "'";
}
};
std::array<std::string, 3> knownChain
{
- GlobalFrame, MapFrame, OdometryFrame}; // Robot comes next
+ GlobalFrame, MapFrame, OdometryFrame
+ }; // Robot comes next
auto* frameBeginIt =
std::find(knownChain.begin(), knownChain.end(), query.header.parentFrame);
@@ -238,10 +237,10 @@ namespace armarx::armem::common::robot_state::localization
const armem::base::ProviderSegmentBase<Args...>& agentProviderSegment,
const armem::Time& timestamp)
{
- ARMARX_DEBUG << "getEntity:" + entityName;
+ // ARMARX_DEBUG << "getEntity:" + entityName;
const auto& entity = agentProviderSegment.getEntity(entityName);
- ARMARX_DEBUG << "History (size: " << entity.size() << "): " << entity.getTimestamps();
+ // ARMARX_DEBUG << "History (size: " << entity.size() << "): " << entity.getTimestamps();
// if (entity.history.empty())
// {
@@ -253,27 +252,27 @@ namespace armarx::armem::common::robot_state::localization
std::vector<::armarx::armem::robot_state::Transform> transforms;
transforms.push_back(_convertEntityToTransform(entity.getLatestSnapshot().getInstance(0)));
- ARMARX_DEBUG << "obtaining transform";
+ // ARMARX_DEBUG << "obtaining transform";
if (transforms.size() > 1)
{
// TODO(fabian.reister): remove
return transforms.front().transform;
- ARMARX_DEBUG << "More than one snapshots received: " << transforms.size();
+ // ARMARX_DEBUG << "More than one snapshots received: " << transforms.size();
const auto p = _interpolateTransform(transforms, timestamp);
- ARMARX_DEBUG << "Done interpolating transform";
+ // ARMARX_DEBUG << "Done interpolating transform";
return p;
}
// accept this to fail (will raise armem::error::MissingEntry)
if (transforms.empty())
{
- ARMARX_DEBUG << "empty transform";
+ // ARMARX_DEBUG << "empty transform";
throw armem::error::MissingEntry("foo", "bar", "foo2", "bar2", 0);
}
- ARMARX_DEBUG << "single transform";
+ // ARMARX_DEBUG << "single transform";
return transforms.front().transform;
}
@@ -310,8 +309,6 @@ namespace armarx::armem::common::robot_state::localization
const auto poseNextIt = std::find_if(transforms.begin(), transforms.end(), timestampBeyond);
- ARMARX_WARNING << "Fooasdfasdjfh";
-
ARMARX_CHECK(it == poseNextIt);
return poseNextIt;
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 0bcccee6c..051ab03e7 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
@@ -21,16 +21,22 @@
#include <RobotAPI/libraries/armem_robot_state/server/localization/Segment.h>
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h>
+#include "combine.h"
namespace armarx::armem::server::robot_state
{
- Visu::Visu(const description::Segment& descriptionSegment, const proprioception::Segment& proprioceptionSegment, const localization::Segment& 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)
@@ -55,6 +61,12 @@ namespace armarx::armem::server::robot_state
this->debugObserver = DebugObserverHelper("RobotStateMemory", debugObserver, batchMode);
}
+ if (updateTask)
+ {
+ updateTask->stop();
+ updateTask->join();
+ updateTask = nullptr;
+ }
updateTask = new SimpleRunningTask<>([this]()
{
this->visualizeRun();
@@ -65,24 +77,21 @@ namespace armarx::armem::server::robot_state
void Visu::visualizeRobots(viz::Layer& layer, const robot::Robots& robots)
{
- const auto visualizeRobot = [&](const robot::Robot & robot)
+ for (const robot::Robot& robot : robots)
{
- const auto xmlPath = robot.description.xml.serialize();
+ const data::PackagePath xmlPath = robot.description.xml.serialize();
// clang-format off
- auto robotVisu = viz::Robot(robot.description.name)
- // .file(xmlPath.package, xmlPath.path)
- .file(xmlPath.package, xmlPath.path)
- .joints(robot.config.jointMap)
- .pose(robot.config.globalPose);
+ viz::Robot robotVisu = viz::Robot(robot.description.name)
+ .file(xmlPath.package, xmlPath.path)
+ .joints(robot.config.jointMap)
+ .pose(robot.config.globalPose);
robotVisu.useFullModel();
// clang-format on
layer.add(robotVisu);
- };
-
- std::for_each(robots.begin(), robots.end(), visualizeRobot);
+ }
}
@@ -106,161 +115,117 @@ namespace armarx::armem::server::robot_state
}
- robot::Robots combine(
- const std::unordered_map<std::string, robot::RobotDescription>& robotDescriptions,
- const std::unordered_map<std::string, Eigen::Affine3f>& globalRobotPoseMap,
- const std::unordered_map<std::string, std::map<std::string, float>>& robotJointPositionMap)
+ void Visu::visualizeRun()
{
-
- robot::Robots robots;
-
- for (const auto &[robotName, robotDescription] : robotDescriptions)
+ CycleUtil cycle(static_cast<int>(1000 / p.frequencyHz));
+ while (updateTask and not updateTask->isStopped())
{
- const auto& globalPose = globalRobotPoseMap.find(robotName);
- if (globalPose == globalRobotPoseMap.end())
+ if (p.enabled)
{
- ARMARX_WARNING << deactivateSpam(10) << "No global pose for robot '" << robotName
- << "'";
- continue;
- }
+ const Time timestamp = Time::now();
+ ARMARX_DEBUG << "Visu task at " << armem::toStringMilliSeconds(timestamp);
- const auto& jointMap = robotJointPositionMap.find(robotName);
- if (jointMap == robotJointPositionMap.end())
- {
- ARMARX_WARNING << deactivateSpam(10) << "No joint positions for robot '"
- << robotName << "'";
- continue;
+ try
+ {
+ visualizeOnce(timestamp);
+ }
+ catch (const std::exception& e)
+ {
+ ARMARX_WARNING << "Caught exception while visualizing robots: \n" << e.what();
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << "Caught unknown exception while visualizing robots.";
+ }
+
+ if (debugObserver.has_value())
+ {
+ debugObserver->sendDebugObserverBatch();
+ }
}
+ cycle.waitForCycleDuration();
+ }
+ }
- ARMARX_DEBUG << "Found the following joints for robot " << robotName << ": "
- << simox::alg::get_keys(jointMap->second);
- // TODO(fabian.reister): based on data
- const armem::Time timestamp = IceUtil::Time::now();
+ void Visu::visualizeOnce(const Time& timestamp)
+ {
+ TIMING_START(tVisuTotal);
- robots.emplace_back(robot::Robot
- {
- .description = robotDescription,
- .instance = "", // TODO(fabian.reister): set this properly
- .config = {
- .timestamp = timestamp,
- .globalPose = globalPose->second,
- .jointMap = jointMap->second
- },
- .timestamp = timestamp,
- });
- }
+ // TODO(fabian.reister): use timestamp
- return robots;
- }
+ // Get data.
+ TIMING_START(tVisuGetData);
+ TIMING_START(tRobotDescriptions);
+ const description::RobotDescriptionMap robotDescriptions =
+ descriptionSegment.getRobotDescriptionsLocking(timestamp);
+ TIMING_END_STREAM(tRobotDescriptions, ARMARX_DEBUG);
- void Visu::visualizeRun()
- {
- CycleUtil cycle(static_cast<int>(1000 / p.frequencyHz));
- while (updateTask && not updateTask->isStopped())
+ TIMING_START(tGlobalPoses);
+ const auto globalPoses = localizationSegment.getRobotGlobalPosesLocking(timestamp);
+ TIMING_END_STREAM(tGlobalPoses, ARMARX_DEBUG);
+
+ TIMING_START(tRobotFramePoses);
+ const auto frames = localizationSegment.getRobotFramePosesLocking(timestamp);
+ TIMING_END_STREAM(tRobotFramePoses, ARMARX_DEBUG);
+
+ TIMING_START(tJointPositions);
+ const auto jointPositions =
+ proprioceptionSegment.getRobotJointPositionsLocking(
+ timestamp, debugObserver ? &*debugObserver : nullptr);
+ TIMING_END_STREAM(tJointPositions, ARMARX_DEBUG);
+
+ TIMING_END_STREAM(tVisuGetData, ARMARX_DEBUG);
+
+
+ // Build layers.
+ TIMING_START(tVisuBuildLayers);
+
+ // We need all 3 information:
+ // - robot description
+ // - global pose
+ // - joint positions
+ // => this is nothing but an armem::Robot
+ ARMARX_DEBUG << "Combining robot ..."
+ << "\n- " << robotDescriptions.size() << " descriptions"
+ << "\n- " << globalPoses.size() << " global poses"
+ << "\n- " << jointPositions.size() << " joint positions";
+
+ const robot::Robots robots =
+ combine(robotDescriptions, globalPoses, jointPositions, timestamp);
+
+ ARMARX_DEBUG << "Visualize " << robots.size() << " robots ...";
+ viz::Layer layer = arviz.layer("Robots");
+ visualizeRobots(layer, robots);
+
+ ARMARX_DEBUG << "Visualize frames ...";
+ viz::Layer layerFrames = arviz.layer("Frames");
+ visualizeFrames(layerFrames, frames);
+
+ TIMING_END_STREAM(tVisuBuildLayers, ARMARX_DEBUG);
+
+
+ // Commit layers.
+
+ 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())
{
- {
- // std::scoped_lock lock(visuMutex);
- ARMARX_DEBUG << "Update task";
- if (p.enabled)
- {
- TIMING_START(tVisuTotal);
-
- // TODO(fabian.reister): use timestamp
- 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_DEBUG);
-
- TIMING_START(tGlobalRobotPoseMap);
- const auto globalRobotPoseMap =
- localizationSegment.getRobotGlobalPosesLocking(timestamp);
- 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, debugObserver ? &*debugObserver : nullptr);
- TIMING_END_STREAM(tRobotJointPositionMap, ARMARX_DEBUG);
-
- TIMING_END_STREAM(tVisuGetData, ARMARX_DEBUG);
-
-
- // Build layers.
- TIMING_START(tVisuBuildLayers);
-
- // we need all 3 informations:
- // - robot description
- // - global pose
- // - joint positions
- // => this is nothing but a armem::Robot
-
- const robot::Robots robots =
- combine(robotDescriptions, globalRobotPoseMap, robotJointPositionMap);
-
- ARMARX_DEBUG << "Visualize robots ...";
- viz::Layer layer = arviz.layer("Robots");
- visualizeRobots(layer, robots);
-
- ARMARX_DEBUG << "Visualize frames ...";
- viz::Layer layerFrames = arviz.layer("Frames");
- visualizeFrames(layerFrames, frames);
-
- TIMING_END_STREAM(tVisuBuildLayers, ARMARX_DEBUG);
-
-
- // Commit layers.
-
- 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 << "Caught exception while visualizing robots: \n" << ex.what();
- continue;
- }
- catch (...)
- {
- ARMARX_WARNING << "Caught unknown exception while visualizing robots.";
- continue;
- }
-
- if (debugObserver.has_value())
- {
- debugObserver->sendDebugObserverBatch();
- }
- }
- }
- cycle.waitForCycleDuration();
+ 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 Joint Positions (ms)", tJointPositions.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t 2 Build Layers (ms)", tVisuBuildLayers.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t 3 Commit (ms)", tVisuCommit.toMilliSecondsDouble());
}
}
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 e407abc0d..ce69772c1 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
@@ -55,14 +55,19 @@ namespace armarx::armem::server::robot_state
void connect(const viz::Client& arviz, DebugObserverInterfacePrx debugObserver = nullptr);
- protected:
+ private:
+
+ void visualizeRun();
+ void visualizeOnce(const Time& timestamp);
- static void visualizeRobots(
+
+ static
+ void visualizeRobots(
viz::Layer& layer,
- const robot::Robots& robots
- );
+ const robot::Robots& robots);
- static void visualizeFrames(
+ static
+ void visualizeFrames(
viz::Layer& layerFrames,
const std::unordered_map<std::string, std::vector<Eigen::Affine3f>>& frames);
@@ -84,7 +89,6 @@ namespace armarx::armem::server::robot_state
SimpleRunningTask<>::pointer_type updateTask;
- void visualizeRun();
};
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/combine.cpp b/source/RobotAPI/libraries/armem_robot_state/server/common/combine.cpp
new file mode 100644
index 000000000..abe3f0bbb
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/combine.cpp
@@ -0,0 +1,64 @@
+#include "combine.h"
+
+#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <sstream>
+
+
+namespace armarx::armem::server
+{
+
+ robot::Robots
+ robot_state::combine(
+ const description::RobotDescriptionMap& robotDescriptions,
+ const localization::RobotPoseMap& globalPoses,
+ const proprioception::RobotJointPositionMap& jointPositions,
+ const armem::Time& timestamp)
+ {
+ std::stringstream logs;
+
+ robot::Robots robots;
+ for (const auto& [robotName, robotDescription] : robotDescriptions)
+ {
+ // Handle missing values gracefully instead of skipping the robot altogether.
+
+ robot::Robot& robot = robots.emplace_back();
+
+ robot.description = robotDescription;
+ robot.instance = ""; // TODO(fabian.reister): set this properly
+ robot.config.timestamp = timestamp;
+ robot.config.globalPose = Eigen::Affine3f::Identity();
+ robot.config.jointMap = {};
+ robot.timestamp = timestamp;
+
+ if (auto it = globalPoses.find(robotName); it != globalPoses.end())
+ {
+ robot.config.globalPose = it->second;
+ }
+ else
+ {
+ logs << "\nNo global pose for robot '" << robotName << "'.";
+ }
+ if (auto it = jointPositions.find(robotName); it != jointPositions.end())
+ {
+ robot.config.jointMap = it->second;
+ }
+ else
+ {
+ logs << "\nNo joint positions for robot '" << robotName << "'.";
+ }
+ }
+
+ if (not logs.str().empty())
+ {
+ // These are handled, so they are no warnings.
+ ARMARX_VERBOSE << deactivateSpam(60) << logs.str();
+ }
+
+ return robots;
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/combine.h b/source/RobotAPI/libraries/armem_robot_state/server/common/combine.h
new file mode 100644
index 000000000..811599b69
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/combine.h
@@ -0,0 +1,38 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/libraries/armem/core/forward_declarations.h>
+#include <RobotAPI/libraries/armem_robot_state/server/forward_declarations.h>
+
+
+namespace armarx::armem::server::robot_state
+{
+
+ robot::Robots
+ combine(
+ const description::RobotDescriptionMap& robotDescriptions,
+ const localization::RobotPoseMap& globalPoses,
+ const proprioception::RobotJointPositionMap& jointPositions,
+ const armem::Time& timestamp);
+
+} // namespace armarx::armem::server::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
index fd8ace277..c2ed636a9 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
@@ -89,32 +89,40 @@ namespace armarx::armem::server::robot_state::proprioception
}
if (not batch.empty())
{
- Update update = buildUpdate(batch);
-
auto start = std::chrono::high_resolution_clock::now();
- auto endProprioception = start, endLocalization = start;
+ auto endBuildUpdate = start, endProprioception = start, endLocalization = start;
- armem::CommitResult result;
- {
- // Commits lock the core segments.
+ Update update = buildUpdate(batch);
+ endBuildUpdate = std::chrono::high_resolution_clock::now();
+
+ // Commits lock the core segments.
- result = memory.commitLocking(update.proprioception);
- endProprioception = std::chrono::high_resolution_clock::now();
+ // Proprioception
+ armem::CommitResult result = memory.commitLocking(update.proprioception);
+ endProprioception = std::chrono::high_resolution_clock::now();
- localizationSegment.commitTransformLocking(update.localization);
- endLocalization = std::chrono::high_resolution_clock::now();
+ // Localization
+ for (const armem::robot_state::Transform& transform : update.localization)
+ {
+ localizationSegment.doLocked([&localizationSegment, &transform]()
+ {
+ localizationSegment.commitTransform(transform);
+ });
}
+ endLocalization = std::chrono::high_resolution_clock::now();
+
if (not result.allSuccess())
{
- ARMARX_WARNING << "Could not add data to memory. Error message: " << result.allErrorMessages();
+ ARMARX_WARNING << "Could not commit data to memory. Error message: " << result.allErrorMessages();
}
if (debugObserver)
{
auto end = std::chrono::high_resolution_clock::now();
- debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit [ms]", toDurationMs(start, end));
- debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit Proprioception [ms]", toDurationMs(start, endProprioception));
- debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit Localization [ms]", toDurationMs(endProprioception, endLocalization));
+ debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit (ms)", toDurationMs(start, end));
+ debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit 1. Build Update (ms)", toDurationMs(start, endBuildUpdate));
+ debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit 2. Proprioception (ms)", toDurationMs(endBuildUpdate, endProprioception));
+ debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit 3. Localization (ms)", toDurationMs(endProprioception, endLocalization));
}
}
@@ -130,9 +138,7 @@ namespace armarx::armem::server::robot_state::proprioception
RobotStateWriter::Update RobotStateWriter::buildUpdate(std::queue<RobotUnitData>& dataQueue)
{
ARMARX_CHECK_GREATER(dataQueue.size(), 0);
-
ARMARX_DEBUG << "RobotStateWriter: Commit batch of " << dataQueue.size() << " timesteps to memory...";
- auto start = std::chrono::high_resolution_clock::now();
// Send batch to memory
Update update;
@@ -141,52 +147,61 @@ namespace armarx::armem::server::robot_state::proprioception
{
const RobotUnitData& data = dataQueue.front();
- armem::EntityUpdate& up = update.proprioception.add();
- up.entityID = properties.robotUnitProviderID.withEntityName(properties.robotUnitProviderID.providerSegmentName);
- up.timeCreated = data.timestamp;
- up.instancesData = { data.proprioception };
+ {
+ armem::EntityUpdate& up = update.proprioception.add();
+ up.entityID = properties.robotUnitProviderID.withEntityName(properties.robotUnitProviderID.providerSegmentName);
+ up.timeCreated = data.timestamp;
+ up.instancesData = { data.proprioception };
+ }
- // odometry pose -> localization segment
- if (data.proprioception->hasElement("platform"))
+ // Extract odometry data.
+ const std::string platformKey = "platform";
+ if (data.proprioception->hasElement(platformKey))
{
ARMARX_DEBUG << "Found odometry data.";
- prop::arondto::Platform platform;
- platform.fromAron(aron::datanavigator::DictNavigator::DynamicCastAndCheck(data.proprioception->getElement("platform")));
- const Eigen::Vector3f& relPose = platform.relativePosition;
-
- Eigen::Affine3f odometryPose = Eigen::Affine3f::Identity();
- odometryPose.translation() << relPose.x(), relPose.y(), 0; // TODO set height
- odometryPose.linear() = simox::math::rpy_to_mat3f(0.F, 0.F, relPose.z());
-
- armem::robot_state::Transform& transform = update.localization;
- transform.header.parentFrame = armarx::OdometryFrame;
- transform.header.frame = "root"; // TODO: robot root node
- transform.header.agent = properties.robotUnitProviderID.providerSegmentName;
- transform.header.timestamp = data.timestamp;
- transform.transform = odometryPose;
+ auto platformData = aron::datanavigator::DictNavigator::DynamicCastAndCheck(data.proprioception->getElement(platformKey));
+ update.localization.emplace_back(getTransform(platformData, data.timestamp));
}
- // ToDo: Detect that now odometry data was send
- /*
- else if (!noOdometryDataLogged)
+ else
{
- ARMARX_INFO << "No odometry data! (No group with name '" << properties.platformGroupName << "'.). \n"
- << "If you are using a mobile platform this should not have happened.";
+ ARMARX_INFO << "No odometry data! "
+ << "(No element '" << platformKey << "' in proprioception data.)"
+ << "\nIf you are using a mobile platform this should not have happened."
+ << "\nThis error is only logged once."
+ << "\nThese keys exist: " << data.proprioception->getAllKeys()
+ ;
noOdometryDataLogged = true;
}
- */
dataQueue.pop();
}
- auto stop = std::chrono::high_resolution_clock::now();
- auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
- ARMARX_DEBUG << "RobotUnitData: The total runtime of sending a batch to the memory is: " << duration;
- if (debugObserver)
- {
- debugObserver->setDebugObserverDatafield("RobotStateWriter | t Build Update [ms]", duration.count() / 1000.f);
- }
-
return update;
}
+
+ armem::robot_state::Transform
+ RobotStateWriter::getTransform(
+ const aron::datanavigator::DictNavigatorPtr& platformData,
+ const Time& timestamp) const
+ {
+ prop::arondto::Platform platform;
+ platform.fromAron(platformData);
+
+ const Eigen::Vector3f& relPose = platform.relativePosition;
+
+ Eigen::Affine3f odometryPose = Eigen::Affine3f::Identity();
+ odometryPose.translation() << relPose.x(), relPose.y(), 0; // TODO set height
+ odometryPose.linear() = simox::math::rpy_to_mat3f(0.f, 0.f, relPose.z());
+
+ armem::robot_state::Transform transform;
+ transform.header.parentFrame = armarx::OdometryFrame;
+ transform.header.frame = "root"; // TODO: robot root node
+ transform.header.agent = properties.robotUnitProviderID.providerSegmentName;
+ transform.header.timestamp = timestamp;
+ transform.transform = odometryPose;
+
+ return transform;
+ }
+
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
index 3f8d33ef8..10c5a8b9e 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
@@ -67,18 +67,25 @@ namespace armarx::armem::server::robot_state::proprioception
struct Update
{
armem::Commit proprioception;
- armem::robot_state::Transform localization;
+ std::vector<armem::robot_state::Transform> localization;
};
Update buildUpdate(std::queue<RobotUnitData>& dataQueue);
+ private:
+
+ armem::robot_state::Transform
+ getTransform(
+ const aron::datanavigator::DictNavigatorPtr& platformData,
+ const Time& timestamp) const;
+
+
public:
struct Properties
{
unsigned int memoryBatchSize = 50;
armem::MemoryID robotUnitProviderID;
- std::string platformGroupName = "sens.Platform";
};
Properties properties;
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 3f6f24f45..02a98ef64 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
@@ -82,14 +82,23 @@ namespace armarx::armem::server::robot_state::proprioception
segment->forEachEntity([&](const wm::Entity & entity)
{
adn::DictNavigatorPtr data;
- TIMING_START(_tFindData)
- if (const wm::EntitySnapshot* snapshot = entity.findFirstSnapshotAfterOrAt(timestamp))
{
- data = snapshot->findInstanceData();
- }
- TIMING_END_COMMENT_STREAM(_tFindData, "tFindData " + std::to_string(i), ARMARX_DEBUG)
- tFindData += _tFindData;
+ TIMING_START(_tFindData)
+
+ const wm::EntitySnapshot* snapshot = entity.findLatestSnapshotBeforeOrAt(timestamp);
+ if (not snapshot)
+ {
+ // Got no snapshot <= timestamp. Take latest instead (if present).
+ snapshot = entity.findLatestSnapshot();
+ }
+ if (snapshot)
+ {
+ data = snapshot->findInstanceData();
+ }
+ TIMING_END_COMMENT_STREAM(_tFindData, "tFindData " + std::to_string(i), ARMARX_DEBUG)
+ tFindData += _tFindData;
+ }
if (data)
{
TIMING_START(_tReadJointPositions)
@@ -99,7 +108,6 @@ namespace armarx::armem::server::robot_state::proprioception
TIMING_END_COMMENT_STREAM(_tReadJointPositions, "tReadJointPositions " + std::to_string(i), ARMARX_DEBUG)
tReadJointPositions += _tReadJointPositions;
}
-
++i;
});
TIMING_END_STREAM(tProcessEntities, ARMARX_DEBUG)
--
GitLab