Skip to content
Snippets Groups Projects
Commit de1ac125 authored by Rainer Kartmann's avatar Rainer Kartmann
Browse files

Update / fix visualization in RobotStateMemory

parent 8e4a9adb
No related branches found
No related tags found
1 merge request!188ArMem Updates
Hide whitespace changes
Inline Side-by-side
Showing
with 334 additions and 234 deletions
source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
+ 2
− 0
View file @ de1ac125
...@@ -39,6 +39,7 @@ armarx_add_library( ...@@ -39,6 +39,7 @@ armarx_add_library(
server/forward_declarations.h server/forward_declarations.h
server/common/Visu.h server/common/Visu.h
server/common/combine.h
server/localization/Segment.h server/localization/Segment.h
...@@ -68,6 +69,7 @@ armarx_add_library( ...@@ -68,6 +69,7 @@ armarx_add_library(
client/localization/TransformWriter.cpp client/localization/TransformWriter.cpp
server/common/Visu.cpp server/common/Visu.cpp
server/common/combine.cpp
server/localization/Segment.cpp server/localization/Segment.cpp
......
source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
+ 11
− 14
View file @ de1ac125
...@@ -125,8 +125,7 @@ namespace armarx::armem::common::robot_state::localization ...@@ -125,8 +125,7 @@ namespace armarx::armem::common::robot_state::localization
std::vector<std::string> chain; std::vector<std::string> chain;
const auto& agentProviderSegment = const auto& agentProviderSegment = localizationCoreSegment.getProviderSegment(query.header.agent);
localizationCoreSegment.getProviderSegment(query.header.agent);
auto addToChain = [&](const std::string & parentFrame, const std::string & frame) auto addToChain = [&](const std::string & parentFrame, const std::string & frame)
{ {
...@@ -138,14 +137,14 @@ namespace armarx::armem::common::robot_state::localization ...@@ -138,14 +137,14 @@ namespace armarx::armem::common::robot_state::localization
} }
else 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 std::array<std::string, 3> knownChain
{ {
GlobalFrame, MapFrame, OdometryFrame}; // Robot comes next GlobalFrame, MapFrame, OdometryFrame
}; // Robot comes next
auto* frameBeginIt = auto* frameBeginIt =
std::find(knownChain.begin(), knownChain.end(), query.header.parentFrame); std::find(knownChain.begin(), knownChain.end(), query.header.parentFrame);
...@@ -238,10 +237,10 @@ namespace armarx::armem::common::robot_state::localization ...@@ -238,10 +237,10 @@ namespace armarx::armem::common::robot_state::localization
const armem::base::ProviderSegmentBase<Args...>& agentProviderSegment, const armem::base::ProviderSegmentBase<Args...>& agentProviderSegment,
const armem::Time& timestamp) const armem::Time& timestamp)
{ {
ARMARX_DEBUG << "getEntity:" + entityName; // ARMARX_DEBUG << "getEntity:" + entityName;
const auto& entity = agentProviderSegment.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()) // if (entity.history.empty())
// { // {
...@@ -253,27 +252,27 @@ namespace armarx::armem::common::robot_state::localization ...@@ -253,27 +252,27 @@ namespace armarx::armem::common::robot_state::localization
std::vector<::armarx::armem::robot_state::Transform> transforms; std::vector<::armarx::armem::robot_state::Transform> transforms;
transforms.push_back(_convertEntityToTransform(entity.getLatestSnapshot().getInstance(0))); transforms.push_back(_convertEntityToTransform(entity.getLatestSnapshot().getInstance(0)));
ARMARX_DEBUG << "obtaining transform"; // ARMARX_DEBUG << "obtaining transform";
if (transforms.size() > 1) if (transforms.size() > 1)
{ {
// TODO(fabian.reister): remove // TODO(fabian.reister): remove
return transforms.front().transform; 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); const auto p = _interpolateTransform(transforms, timestamp);
ARMARX_DEBUG << "Done interpolating transform"; // ARMARX_DEBUG << "Done interpolating transform";
return p; return p;
} }
// accept this to fail (will raise armem::error::MissingEntry) // accept this to fail (will raise armem::error::MissingEntry)
if (transforms.empty()) if (transforms.empty())
{ {
ARMARX_DEBUG << "empty transform"; // ARMARX_DEBUG << "empty transform";
throw armem::error::MissingEntry("foo", "bar", "foo2", "bar2", 0); throw armem::error::MissingEntry("foo", "bar", "foo2", "bar2", 0);
} }
ARMARX_DEBUG << "single transform"; // ARMARX_DEBUG << "single transform";
return transforms.front().transform; return transforms.front().transform;
} }
...@@ -310,8 +309,6 @@ namespace armarx::armem::common::robot_state::localization ...@@ -310,8 +309,6 @@ namespace armarx::armem::common::robot_state::localization
const auto poseNextIt = std::find_if(transforms.begin(), transforms.end(), timestampBeyond); const auto poseNextIt = std::find_if(transforms.begin(), transforms.end(), timestampBeyond);
ARMARX_WARNING << "Fooasdfasdjfh";
ARMARX_CHECK(it == poseNextIt); ARMARX_CHECK(it == poseNextIt);
return poseNextIt; return poseNextIt;
......
source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
+ 120
− 155
View file @ de1ac125
...@@ -21,16 +21,22 @@ ...@@ -21,16 +21,22 @@
#include <RobotAPI/libraries/armem_robot_state/server/localization/Segment.h> #include <RobotAPI/libraries/armem_robot_state/server/localization/Segment.h>
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h> #include <RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h>
#include "combine.h"
namespace armarx::armem::server::robot_state 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), : descriptionSegment(descriptionSegment),
proprioceptionSegment(proprioceptionSegment), proprioceptionSegment(proprioceptionSegment),
localizationSegment(localizationSegment) 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)
...@@ -55,6 +61,12 @@ namespace armarx::armem::server::robot_state ...@@ -55,6 +61,12 @@ namespace armarx::armem::server::robot_state
this->debugObserver = DebugObserverHelper("RobotStateMemory", debugObserver, batchMode); this->debugObserver = DebugObserverHelper("RobotStateMemory", debugObserver, batchMode);
} }
if (updateTask)
{
updateTask->stop();
updateTask->join();
updateTask = nullptr;
}
updateTask = new SimpleRunningTask<>([this]() updateTask = new SimpleRunningTask<>([this]()
{ {
this->visualizeRun(); this->visualizeRun();
...@@ -65,24 +77,21 @@ namespace armarx::armem::server::robot_state ...@@ -65,24 +77,21 @@ namespace armarx::armem::server::robot_state
void Visu::visualizeRobots(viz::Layer& layer, const robot::Robots& robots) 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 // clang-format off
auto robotVisu = viz::Robot(robot.description.name) viz::Robot robotVisu = viz::Robot(robot.description.name)
// .file(xmlPath.package, xmlPath.path) .file(xmlPath.package, xmlPath.path)
.file(xmlPath.package, xmlPath.path) .joints(robot.config.jointMap)
.joints(robot.config.jointMap) .pose(robot.config.globalPose);
.pose(robot.config.globalPose);
robotVisu.useFullModel(); robotVisu.useFullModel();
// clang-format on // clang-format on
layer.add(robotVisu); layer.add(robotVisu);
}; }
std::for_each(robots.begin(), robots.end(), visualizeRobot);
} }
...@@ -106,161 +115,117 @@ namespace armarx::armem::server::robot_state ...@@ -106,161 +115,117 @@ namespace armarx::armem::server::robot_state
} }
robot::Robots combine( void Visu::visualizeRun()
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)
{ {
CycleUtil cycle(static_cast<int>(1000 / p.frequencyHz));
robot::Robots robots; while (updateTask and not updateTask->isStopped())
for (const auto &[robotName, robotDescription] : robotDescriptions)
{ {
const auto& globalPose = globalRobotPoseMap.find(robotName); if (p.enabled)
if (globalPose == globalRobotPoseMap.end())
{ {
ARMARX_WARNING << deactivateSpam(10) << "No global pose for robot '" << robotName const Time timestamp = Time::now();
<< "'"; ARMARX_DEBUG << "Visu task at " << armem::toStringMilliSeconds(timestamp);
continue;
}
const auto& jointMap = robotJointPositionMap.find(robotName); try
if (jointMap == robotJointPositionMap.end()) {
{ visualizeOnce(timestamp);
ARMARX_WARNING << deactivateSpam(10) << "No joint positions for robot '" }
<< robotName << "'"; catch (const std::exception& e)
continue; {
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 void Visu::visualizeOnce(const Time& timestamp)
const armem::Time timestamp = IceUtil::Time::now(); {
TIMING_START(tVisuTotal);
robots.emplace_back(robot::Robot // TODO(fabian.reister): use timestamp
{
.description = robotDescription,
.instance = "", // TODO(fabian.reister): set this properly
.config = {
.timestamp = timestamp,
.globalPose = globalPose->second,
.jointMap = jointMap->second
},
.timestamp = 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() TIMING_START(tGlobalPoses);
{ const auto globalPoses = localizationSegment.getRobotGlobalPosesLocking(timestamp);
CycleUtil cycle(static_cast<int>(1000 / p.frequencyHz)); TIMING_END_STREAM(tGlobalPoses, ARMARX_DEBUG);
while (updateTask && not updateTask->isStopped())
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())
{ {
{ const std::string p = "Visu | ";
// std::scoped_lock lock(visuMutex); debugObserver->setDebugObserverDatafield(p + "t Total (ms)", tVisuTotal.toMilliSecondsDouble());
ARMARX_DEBUG << "Update task"; debugObserver->setDebugObserverDatafield(p + "t 1 Get Data (ms)", tVisuGetData.toMilliSecondsDouble());
if (p.enabled) debugObserver->setDebugObserverDatafield(p + "t 1.1 Descriptions (ms)", tRobotDescriptions.toMilliSecondsDouble());
{ debugObserver->setDebugObserverDatafield(p + "t 1.2 Global Poses (ms)", tGlobalPoses.toMilliSecondsDouble());
TIMING_START(tVisuTotal); debugObserver->setDebugObserverDatafield(p + "t 1.3 Frames (ms)", tRobotFramePoses.toMilliSecondsDouble());
debugObserver->setDebugObserverDatafield(p + "t 1.4 Joint Positions (ms)", tJointPositions.toMilliSecondsDouble());
// TODO(fabian.reister): use timestamp debugObserver->setDebugObserverDatafield(p + "t 2 Build Layers (ms)", tVisuBuildLayers.toMilliSecondsDouble());
const Time timestamp = Time::now(); debugObserver->setDebugObserverDatafield(p + "t 3 Commit (ms)", tVisuCommit.toMilliSecondsDouble());
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();
} }
} }
......
source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
+ 10
− 6
View file @ de1ac125
...@@ -55,14 +55,19 @@ namespace armarx::armem::server::robot_state ...@@ -55,14 +55,19 @@ namespace armarx::armem::server::robot_state
void connect(const viz::Client& arviz, DebugObserverInterfacePrx debugObserver = nullptr); 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, viz::Layer& layer,
const robot::Robots& robots const robot::Robots& robots);
);
static void visualizeFrames( static
void visualizeFrames(
viz::Layer& layerFrames, viz::Layer& layerFrames,
const std::unordered_map<std::string, std::vector<Eigen::Affine3f>>& frames); const std::unordered_map<std::string, std::vector<Eigen::Affine3f>>& frames);
...@@ -84,7 +89,6 @@ namespace armarx::armem::server::robot_state ...@@ -84,7 +89,6 @@ namespace armarx::armem::server::robot_state
SimpleRunningTask<>::pointer_type updateTask; SimpleRunningTask<>::pointer_type updateTask;
void visualizeRun();
}; };
......
source/RobotAPI/libraries/armem_robot_state/server/common/combine.cpp 0 → 100644
+ 64
− 0
View file @ de1ac125
#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;
}
}
source/RobotAPI/libraries/armem_robot_state/server/common/combine.h 0 → 100644
+ 38
− 0
View file @ de1ac125
/*
* 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
source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
+ 65
− 50
View file @ de1ac125
...@@ -89,32 +89,40 @@ namespace armarx::armem::server::robot_state::proprioception ...@@ -89,32 +89,40 @@ namespace armarx::armem::server::robot_state::proprioception
} }
if (not batch.empty()) if (not batch.empty())
{ {
Update update = buildUpdate(batch);
auto start = std::chrono::high_resolution_clock::now(); auto start = std::chrono::high_resolution_clock::now();
auto endProprioception = start, endLocalization = start; auto endBuildUpdate = start, endProprioception = start, endLocalization = start;
armem::CommitResult result; Update update = buildUpdate(batch);
{ endBuildUpdate = std::chrono::high_resolution_clock::now();
// Commits lock the core segments.
// Commits lock the core segments.
result = memory.commitLocking(update.proprioception); // Proprioception
endProprioception = std::chrono::high_resolution_clock::now(); armem::CommitResult result = memory.commitLocking(update.proprioception);
endProprioception = std::chrono::high_resolution_clock::now();
localizationSegment.commitTransformLocking(update.localization); // Localization
endLocalization = std::chrono::high_resolution_clock::now(); 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()) 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) if (debugObserver)
{ {
auto end = std::chrono::high_resolution_clock::now(); auto end = std::chrono::high_resolution_clock::now();
debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit [ms]", toDurationMs(start, end)); debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit (ms)", toDurationMs(start, end));
debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit Proprioception [ms]", toDurationMs(start, endProprioception)); debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit 1. Build Update (ms)", toDurationMs(start, endBuildUpdate));
debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit Localization [ms]", toDurationMs(endProprioception, endLocalization)); 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 ...@@ -130,9 +138,7 @@ namespace armarx::armem::server::robot_state::proprioception
RobotStateWriter::Update RobotStateWriter::buildUpdate(std::queue<RobotUnitData>& dataQueue) RobotStateWriter::Update RobotStateWriter::buildUpdate(std::queue<RobotUnitData>& dataQueue)
{ {
ARMARX_CHECK_GREATER(dataQueue.size(), 0); ARMARX_CHECK_GREATER(dataQueue.size(), 0);
ARMARX_DEBUG << "RobotStateWriter: Commit batch of " << dataQueue.size() << " timesteps to memory..."; ARMARX_DEBUG << "RobotStateWriter: Commit batch of " << dataQueue.size() << " timesteps to memory...";
auto start = std::chrono::high_resolution_clock::now();
// Send batch to memory // Send batch to memory
Update update; Update update;
...@@ -141,52 +147,61 @@ namespace armarx::armem::server::robot_state::proprioception ...@@ -141,52 +147,61 @@ namespace armarx::armem::server::robot_state::proprioception
{ {
const RobotUnitData& data = dataQueue.front(); const RobotUnitData& data = dataQueue.front();
armem::EntityUpdate& up = update.proprioception.add(); {
up.entityID = properties.robotUnitProviderID.withEntityName(properties.robotUnitProviderID.providerSegmentName); armem::EntityUpdate& up = update.proprioception.add();
up.timeCreated = data.timestamp; up.entityID = properties.robotUnitProviderID.withEntityName(properties.robotUnitProviderID.providerSegmentName);
up.instancesData = { data.proprioception }; up.timeCreated = data.timestamp;
up.instancesData = { data.proprioception };
}
// odometry pose -> localization segment // Extract odometry data.
if (data.proprioception->hasElement("platform")) const std::string platformKey = "platform";
if (data.proprioception->hasElement(platformKey))
{ {
ARMARX_DEBUG << "Found odometry data."; ARMARX_DEBUG << "Found odometry data.";
prop::arondto::Platform platform; auto platformData = aron::datanavigator::DictNavigator::DynamicCastAndCheck(data.proprioception->getElement(platformKey));
platform.fromAron(aron::datanavigator::DictNavigator::DynamicCastAndCheck(data.proprioception->getElement("platform"))); update.localization.emplace_back(getTransform(platformData, data.timestamp));
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;
} }
// ToDo: Detect that now odometry data was send else
/*
else if (!noOdometryDataLogged)
{ {
ARMARX_INFO << "No odometry data! (No group with name '" << properties.platformGroupName << "'.). \n" ARMARX_INFO << "No odometry data! "
<< "If you are using a mobile platform this should not have happened."; << "(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; noOdometryDataLogged = true;
} }
*/
dataQueue.pop(); 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; 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;
}
} }
source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
+ 9
− 2
View file @ de1ac125
...@@ -67,18 +67,25 @@ namespace armarx::armem::server::robot_state::proprioception ...@@ -67,18 +67,25 @@ namespace armarx::armem::server::robot_state::proprioception
struct Update struct Update
{ {
armem::Commit proprioception; armem::Commit proprioception;
armem::robot_state::Transform localization; std::vector<armem::robot_state::Transform> localization;
}; };
Update buildUpdate(std::queue<RobotUnitData>& dataQueue); Update buildUpdate(std::queue<RobotUnitData>& dataQueue);
private:
armem::robot_state::Transform
getTransform(
const aron::datanavigator::DictNavigatorPtr& platformData,
const Time& timestamp) const;
public: public:
struct Properties struct Properties
{ {
unsigned int memoryBatchSize = 50; unsigned int memoryBatchSize = 50;
armem::MemoryID robotUnitProviderID; armem::MemoryID robotUnitProviderID;
std::string platformGroupName = "sens.Platform";
}; };
Properties properties; Properties properties;
......
source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
+ 15
− 7
View file @ de1ac125
...@@ -82,14 +82,23 @@ namespace armarx::armem::server::robot_state::proprioception ...@@ -82,14 +82,23 @@ namespace armarx::armem::server::robot_state::proprioception
segment->forEachEntity([&](const wm::Entity & entity) segment->forEachEntity([&](const wm::Entity & entity)
{ {
adn::DictNavigatorPtr data; adn::DictNavigatorPtr data;
TIMING_START(_tFindData)
if (const wm::EntitySnapshot* snapshot = entity.findFirstSnapshotAfterOrAt(timestamp))
{ {
data = snapshot->findInstanceData(); TIMING_START(_tFindData)
}
TIMING_END_COMMENT_STREAM(_tFindData, "tFindData " + std::to_string(i), ARMARX_DEBUG) const wm::EntitySnapshot* snapshot = entity.findLatestSnapshotBeforeOrAt(timestamp);
tFindData += _tFindData; 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) if (data)
{ {
TIMING_START(_tReadJointPositions) TIMING_START(_tReadJointPositions)
...@@ -99,7 +108,6 @@ namespace armarx::armem::server::robot_state::proprioception ...@@ -99,7 +108,6 @@ namespace armarx::armem::server::robot_state::proprioception
TIMING_END_COMMENT_STREAM(_tReadJointPositions, "tReadJointPositions " + std::to_string(i), ARMARX_DEBUG) TIMING_END_COMMENT_STREAM(_tReadJointPositions, "tReadJointPositions " + std::to_string(i), ARMARX_DEBUG)
tReadJointPositions += _tReadJointPositions; tReadJointPositions += _tReadJointPositions;
} }
++i; ++i;
}); });
TIMING_END_STREAM(tProcessEntities, ARMARX_DEBUG) TIMING_END_STREAM(tProcessEntities, ARMARX_DEBUG)
......
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