From b932b87c1ef8e74a9cb510afa73d707cf7b5a3ae Mon Sep 17 00:00:00 2001
From: Patrick Hegemann <335495-patrickhegemann@users.noreply.gitlab.com>
Date: Tue, 8 Feb 2022 18:32:42 +0100
Subject: [PATCH] Add getGlobalRobotPose to RobotStateMemory
---
.../server/RobotStateMemory/RobotStateMemory.cpp | 16 ++++++++++++++++
.../server/RobotStateMemory/RobotStateMemory.h | 9 ++++++++-
.../interface/core/RobotLocalization.ice | 6 +++++-
.../armem_robot_state/server/common/Visu.cpp | 3 ++-
4 files changed, 31 insertions(+), 3 deletions(-)
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
index 0aef2635f..5de9bc3fb 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
@@ -22,6 +22,8 @@
#include "RobotStateMemory.h"
+#include <RobotAPI/interface/core/PoseBase.h>
+#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/server/common/Visu.h>
@@ -31,6 +33,7 @@
#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/algorithm/string.h>
@@ -182,6 +185,19 @@ namespace armarx::armem::server::robot_state
}
+ armarx::PoseBasePtr RobotStateMemory::getGlobalRobotPose(Ice::Long timestamp, const std::string& robotName, const ::Ice::Current& /*unused*/)
+ {
+ auto poseMap = localizationSegment.getRobotGlobalPoses(armem::Time::now());
+
+ bool robotNameFound = !(poseMap.find(robotName) == poseMap.end());
+ ARMARX_CHECK(robotNameFound)
+ << "Robot with name " << robotName << " does not exist. "
+ << "Available robots are: " << simox::alg::get_keys(poseMap);
+
+ return new Pose(poseMap[robotName].matrix());
+ }
+
+
/*************************************************************/
// RobotUnit Streaming functions
/*************************************************************/
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
index 863c9b571..bd7eceda9 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
@@ -39,6 +39,8 @@
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h>
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.h>
+#include <RobotAPI/interface/core/RobotLocalization.h>
+
namespace armarx::plugins
{
@@ -62,7 +64,8 @@ namespace armarx::armem::server::robot_state
class RobotStateMemory :
virtual public armarx::Component,
virtual public armem::server::ReadWritePluginUser,
- virtual public armarx::ArVizComponentPluginUser
+ virtual public armarx::ArVizComponentPluginUser,
+ virtual public armarx::GlobalRobotPoseProvider
{
public:
@@ -73,6 +76,10 @@ namespace armarx::armem::server::robot_state
std::string getDefaultName() const override;
+ // GlobalRobotPoseProvider interface
+ virtual armarx::PoseBasePtr getGlobalRobotPose(Ice::Long timestamp, const std::string& robotName, const ::Ice::Current&) override;
+
+
protected:
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
diff --git a/source/RobotAPI/interface/core/RobotLocalization.ice b/source/RobotAPI/interface/core/RobotLocalization.ice
index dbaee0f7a..19b0ebae4 100644
--- a/source/RobotAPI/interface/core/RobotLocalization.ice
+++ b/source/RobotAPI/interface/core/RobotLocalization.ice
@@ -24,10 +24,14 @@
#pragma once
+#include <RobotAPI/interface/armem/server/MemoryInterface.ice>
#include <RobotAPI/interface/core/GeometryBase.ice>
+#include <RobotAPI/interface/core/PoseBase.ice>
module armarx{
-
+ interface GlobalRobotPoseProvider extends armem::server::MemoryInterface {
+ PoseBase getGlobalRobotPose(long timestamp, string robotName);
+ }
interface GlobalRobotPoseLocalizationListener{
void reportGlobalRobotPose(TransformStamped currentPose);
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 051ab03e7..67d537d19 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
@@ -12,6 +12,7 @@
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/interface/core/PackagePath.h>
#include <RobotAPI/libraries/armem/core/Time.h>
@@ -79,7 +80,7 @@ namespace armarx::armem::server::robot_state
{
for (const robot::Robot& robot : robots)
{
- const data::PackagePath xmlPath = robot.description.xml.serialize();
+ const armarx::data::PackagePath xmlPath = robot.description.xml.serialize();
// clang-format off
viz::Robot robotVisu = viz::Robot(robot.description.name)
--
GitLab