diff --git a/source/RobotAPI/components/ArViz/Client/Elements.cpp b/source/RobotAPI/components/ArViz/Client/Elements.cpp
index f479f4ef5456fc7a88d0f3a0fa87d2e77e9aab66..d97fea3b03d55ddb82a379628197a5f04ffc3d74 100644
--- a/source/RobotAPI/components/ArViz/Client/Elements.cpp
+++ b/source/RobotAPI/components/ArViz/Client/Elements.cpp
@@ -107,9 +107,9 @@ namespace armarx::viz
float angle = std::acos(naturalDir.dot(dir));
if (cross.squaredNorm() < 1.0e-12f)
{
- // Directions are almost colinear ==> Do no rotation
+ // Directions are almost colinear ==> Angle is either 0 or 180 deg
cross = Eigen::Vector3f::UnitX();
- angle = 0.0f;
+ // Keep angle
}
Eigen::Vector3f axis = cross.normalized();
Eigen::Quaternionf ori(Eigen::AngleAxisf(angle, axis));
diff --git a/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp b/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
index 2c93faf7fa4a464909bd6dea214c74022571474f..e52609fa085feac2dc4ef8fc93f68127aef8b040 100644
--- a/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
+++ b/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
@@ -275,13 +275,18 @@ namespace armarx
void fillExampleLayer(viz::Layer& layer, double timeInSeconds)
{
+ for (int i = 0; i < 6; ++i)
{
- Eigen::Vector3f pos = Eigen::Vector3f::Zero();
- pos.z() = +300.0f;
+ Eigen::Vector3f pos = Eigen::Vector3f(-200.0, 200.0, 300);
+ pos.x() += -300 * i;
+
+ Eigen::Vector3f normal = Eigen::Vector3f::Zero();
+ normal(i / 2) = (i % 2 == 0 ? 1.0 : -1.0);
- viz::ArrowCircle circle = viz::ArrowCircle("circle")
+ viz::ArrowCircle circle = viz::ArrowCircle("circle " + std::to_string(i))
.position(pos)
.radius(100.0f)
+ .normal(normal)
.width(10.0f)
.color(viz::Color::fromRGBA(255, 0, 255));
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
index eab8bf5e6dd721395647519d3e04fecc62ffbb7f..f77e01607b0036a2f9525b88dc919f0f1882608b 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
@@ -47,6 +47,7 @@ namespace armarx::armem::server::robot_state
RobotStateMemory::RobotStateMemory() :
descriptionSegment(iceAdapter()),
proprioceptionSegment(iceAdapter()),
+ exteroceptionSegment(iceAdapter()),
localizationSegment(iceAdapter()),
commonVisu(descriptionSegment, proprioceptionSegment, localizationSegment)
{
@@ -86,6 +87,7 @@ namespace armarx::armem::server::robot_state
setMemoryName(armem::robot_state::memoryID.memoryName);
descriptionSegment.defineProperties(defs, prefix + "desc.");
+ exteroceptionSegment.defineProperties(defs, prefix + "ext.");
proprioceptionSegment.defineProperties(defs, prefix + "prop.");
localizationSegment.defineProperties(defs, prefix + "loc.");
commonVisu.defineProperties(defs, prefix + "visu.");
@@ -104,6 +106,7 @@ namespace armarx::armem::server::robot_state
{
descriptionSegment.init();
proprioceptionSegment.init();
+ exteroceptionSegment.init();
localizationSegment.init();
commonVisu.init();
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
index aed5fec8081906601372f4851afeb44d25f82507..70513a0cf56ff0cd6d82603cc3266fb405e5af3d 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
@@ -38,6 +38,7 @@
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h>
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.h>
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h>
+#include <RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.h>
namespace armarx::plugins
@@ -105,6 +106,9 @@ namespace armarx::armem::server::robot_state
proprioception::Segment proprioceptionSegment;
armem::data::MemoryID robotUnitProviderID;
+ // - exteroception
+ exteroception::Segment exteroceptionSegment;
+
// - localization
localization::Segment localizationSegment;
diff --git a/source/RobotAPI/components/units/HandUnit.h b/source/RobotAPI/components/units/HandUnit.h
index 3816605266f1bc44dce40a752bfe94b14af8177b..0241caff84c93709619c0bcabae2736d443e5c1b 100644
--- a/source/RobotAPI/components/units/HandUnit.h
+++ b/source/RobotAPI/components/units/HandUnit.h
@@ -172,7 +172,7 @@ namespace armarx
// HandUnitInterface interface
public:
- std::string getHandName(const Ice::Current&) override;
+ std::string getHandName(const Ice::Current& = Ice::emptyCurrent) override;
void setJointForces(const NameValueMap& targetJointForces, const Ice::Current&) override;
void sendJointCommands(const NameCommandMap& targetJointCommands, const Ice::Current&) override;
};
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index 5d9d3c04b278f8032125f98cf82d63aaa8253155..9e290598fee8047f853044e54ea3775b62ab49f7 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -108,6 +108,7 @@ set(SLICE_FILES
aron.ice
aron/Aron.ice
+ aron/test/AronConversionTestInterface.ice
armem.ice
diff --git a/source/RobotAPI/interface/aron/test/AronConversionTestInterface.ice b/source/RobotAPI/interface/aron/test/AronConversionTestInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..924f40c6d8664123c822eb9f7eb338d8d50b52d5
--- /dev/null
+++ b/source/RobotAPI/interface/aron/test/AronConversionTestInterface.ice
@@ -0,0 +1,61 @@
+/*
+ * 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/>.
+ *
+ * package SpeechX::aron_cpp_to_python_conv_test
+ * author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * date 2023
+ * copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#pragma once
+
+#include <RobotAPI/interface/aron/Aron.ice>
+
+
+module armarx { module aron { module test
+{
+
+module dto
+{
+
+ struct TestAronConversionRequest
+ {
+ string aronClassName;
+ ::armarx::aron::data::dto::Dict probe;
+ };
+ struct TestAronConversionResponse
+ {
+ bool success;
+ string errorMessage;
+
+ ::armarx::aron::data::dto::Dict probe;
+ };
+
+};
+
+
+module dti
+{
+
+ interface AronConversionTestInterface
+ {
+ dto::TestAronConversionResponse testAronConversion(dto::TestAronConversionRequest req);
+ };
+
+};
+
+};};};
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
index 4b61a29ecf41a7af19336ccdc81d30c7dc32189a..f5516353eecf459e4d2cde12db3737b396908972 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
@@ -104,6 +104,7 @@ namespace armarx
{
return findObject(id.dataset(), id.className());
}
+
std::optional<ObjectInfo> ObjectFinder::findObject(const objpose::ObjectPose& obj) const
{
return findObject(obj.objectID);
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
index fea3677bd5aa3d0d2b762beb32e2c7ff8d6cc9ea..6d20564f660aee832698ca9d1eda297b56de9f3c 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
@@ -59,12 +59,17 @@ namespace armarx
return _id.str();
}
- ObjectInfo::path ObjectInfo::objectDirectory() const
+ ObjectInfo::path ObjectInfo::objectDirectory(const bool fixDataPath) const
{
+ if(fixDataPath)
+ {
+ return _relObjectsPath / _id.dataset() / _id.className();
+ }
+
return path(_packageName) / _relObjectsPath / _id.dataset() / _id.className();
}
- PackageFileLocation ObjectInfo::file(const std::string& _extension, const std::string& suffix) const
+ PackageFileLocation ObjectInfo::file(const std::string& _extension, const std::string& suffix, const bool fixDataPath) const
{
std::string extension = _extension;
if (extension.at(0) != '.')
@@ -75,8 +80,8 @@ namespace armarx
PackageFileLocation loc;
loc.package = _packageName;
- loc.relativePath = objectDirectory() / filename;
- loc.absolutePath = _absPackageDataDir / loc.relativePath;
+ loc.relativePath = objectDirectory(fixDataPath) / filename;
+ loc.absolutePath = _absPackageDataDir / objectDirectory(false) / filename;
return loc;
}
@@ -86,14 +91,49 @@ namespace armarx
return file(".xml");
}
+ PackageFileLocation ObjectInfo::urdf() const
+ {
+ return file(".urdf");
+ }
+
+ PackageFileLocation ObjectInfo::sdf() const
+ {
+ return file(".sdf");
+ }
+
+ std::optional<PackageFileLocation> ObjectInfo::getModel() const
+ {
+ if (fs::is_regular_file(simoxXML().absolutePath))
+ {
+ return simoxXML();
+ }
+ else if (fs::is_regular_file(urdf().absolutePath))
+ {
+ return urdf();
+ }
+ else if (fs::is_regular_file(sdf().absolutePath))
+ {
+ return sdf();
+ }
+ else
+ {
+ return std::nullopt;
+ }
+ }
+
PackageFileLocation ObjectInfo::articulatedSimoxXML() const
{
- return file(".xml", "_articulated");
+ return file(".xml", "_articulated", true);
}
PackageFileLocation ObjectInfo::articulatedUrdf() const
{
- return file(".urdf", "_articulated");
+ return file(".urdf", "_articulated", true);
+ }
+
+ PackageFileLocation ObjectInfo::articulatedSdf() const
+ {
+ return file(".sdf", "_articulated");
}
std::optional<PackageFileLocation> ObjectInfo::getArticulatedModel() const
@@ -106,6 +146,10 @@ namespace armarx
{
return articulatedUrdf();
}
+ else if (fs::is_regular_file(articulatedSdf().absolutePath))
+ {
+ return articulatedSdf();
+ }
else
{
return std::nullopt;
@@ -293,5 +337,3 @@ std::ostream& armarx::operator<<(std::ostream& os, const ObjectInfo& rhs)
{
return os << rhs.id();
}
-
-
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
index 70fd46b47fb7a8a3c992dfe5d6c3d4676915aed7..ccbd4eb1be32116d8fc7ced05d8148f027fefccf 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
@@ -76,14 +76,19 @@ namespace armarx
std::string idStr() const;
- PackageFileLocation file(const std::string& extension, const std::string& suffix = "") const;
+ PackageFileLocation file(const std::string& extension, const std::string& suffix = "", bool fixDataPath = false) const;
PackageFileLocation simoxXML() const;
+ PackageFileLocation urdf() const;
+ PackageFileLocation sdf() const;
+ /// Return the Simox XML, URDF or SDF, if one exists.
+ std::optional<PackageFileLocation> getModel() const;
PackageFileLocation articulatedSimoxXML() const;
PackageFileLocation articulatedUrdf() const;
- /// Return the articulated Simox XML or URDF, if one exists.
+ PackageFileLocation articulatedSdf() const;
+ /// Return the articulated Simox XML, URDF or SDF, if one exists.
std::optional<PackageFileLocation> getArticulatedModel() const;
@@ -131,7 +136,7 @@ namespace armarx
private:
- path objectDirectory() const;
+ path objectDirectory(bool fixDataPath) const;
std::optional<std::vector<std::string>> loadNames(const std::string& jsonKey) const;
diff --git a/source/RobotAPI/libraries/ArmarXObjects/json_conversions.cpp b/source/RobotAPI/libraries/ArmarXObjects/json_conversions.cpp
index cf42e169e67f0f18aef427c65fd24a73160983bf..446b625e4e91359fadd8dd43b4dacdc460a9d513 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/json_conversions.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/json_conversions.cpp
@@ -83,7 +83,7 @@ void armarx::objects::to_json(simox::json::json& j, const SceneObject& rhs)
j["instanceName"] = rhs.instanceName;
j["collection"] = rhs.collection;
j["position"] = rhs.position;
- j["orientation"] = rhs.orientation;
+ j["orientation"] = rhs.orientation.normalized();
if (rhs.isStatic.has_value())
{
j["isStatic"] = rhs.isStatic.value();
@@ -106,6 +106,7 @@ void armarx::objects::from_json(const simox::json::json& j, SceneObject& rhs)
j.at("collection").get_to(rhs.collection);
j.at("position").get_to(rhs.position);
j.at("orientation").get_to(rhs.orientation);
+ rhs.orientation.normalize();
if (j.count("isStatic"))
{
diff --git a/source/RobotAPI/libraries/armem/client/util/MemoryListener.h b/source/RobotAPI/libraries/armem/client/util/MemoryListener.h
index ceb765e60db13404fbe170900342cff554b3affe..521d9ce6981e9503d90515b0fc10c518eedaae5a 100644
--- a/source/RobotAPI/libraries/armem/client/util/MemoryListener.h
+++ b/source/RobotAPI/libraries/armem/client/util/MemoryListener.h
@@ -63,12 +63,16 @@ namespace armarx::armem::client::util
};
subscribe(subscriptionID, cb);
}
+
template <class CalleeT>
void subscribe(const MemoryID& subscriptionID, CalleeT* callee, MemberCallbackUpdatedOnly<CalleeT> callback)
{
auto cb = [callee, callback](const MemoryID&, const std::vector<MemoryID>& updatedSnapshotIDs)
{
- (callee->*callback)(updatedSnapshotIDs);
+ if(callee)
+ {
+ (callee->*callback)(updatedSnapshotIDs);
+ }
};
subscribe(subscriptionID, cb);
}
diff --git a/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.cpp b/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.cpp
index ec17f944c60747160a3306032fa89c13899d6b86..e7ceabba97898778af1c13d9ad9c805357f79ac2 100644
--- a/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.cpp
@@ -6,11 +6,10 @@
#include <map>
#include <optional>
#include <ostream>
+#include <type_traits>
#include <utility>
#include <vector>
-#include <type_traits>
-
// ICE
#include <IceUtil/Handle.h>
#include <IceUtil/Time.h>
@@ -34,15 +33,13 @@
#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
// RobotAPI Armem
-#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
-#include <RobotAPI/libraries/armem/util/util.h>
-
#include <RobotAPI/libraries/armem/client/Query.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/selectors.h>
-
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/util/util.h>
#include <RobotAPI/libraries/armem_laser_scans/aron/LaserScan.aron.generated.h>
#include <RobotAPI/libraries/armem_laser_scans/aron_conversions.h>
#include <RobotAPI/libraries/armem_laser_scans/types.h>
@@ -62,21 +59,22 @@ namespace armarx::armem::laser_scans::client
Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
ARMARX_DEBUG << "TransformReader: registerPropertyDefinitions";
- registerPropertyDefinitions(def);
const std::string prefix = propertyPrefix;
-
}
- void Reader::connect()
+ void
+ Reader::connect()
{
// Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "MappingDataReader: Waiting for memory '"
- << constants::memoryName << "' ...";
+ ARMARX_IMPORTANT << "MappingDataReader: Waiting for memory '" << constants::memoryName
+ << "' ...";
try
{
- memoryReader = memoryNameSystem.useReader(MemoryID().withMemoryName(constants::memoryName));
- ARMARX_IMPORTANT << "MappingDataReader: Connected to memory '" << constants::memoryName << "'";
+ memoryReader =
+ memoryNameSystem.useReader(MemoryID().withMemoryName(constants::memoryName));
+ ARMARX_IMPORTANT << "MappingDataReader: Connected to memory '" << constants::memoryName
+ << "'";
}
catch (const armem::error::CouldNotResolveMemoryServer& e)
{
@@ -91,27 +89,43 @@ namespace armarx::armem::laser_scans::client
armarx::armem::client::query::Builder qb;
ARMARX_VERBOSE << "Query for agent: " << query.agent
- << " memory name: " << constants::memoryName;
+ << " memory name: " << constants::memoryName;
if (query.sensorList.empty()) // all sensors
{
// clang-format off
- qb
+ auto& snapshots = qb
.coreSegments().withName(constants::memoryName)
.providerSegments().withName(query.agent)
.entities().all()
- .snapshots().timeRange(query.timeRange.min, query.timeRange.max);
+ .snapshots();
// clang-format on
+ if (query.timeRange.has_value())
+ {
+ snapshots.timeRange(query.timeRange->min, query.timeRange->max);
+ }
+ else
+ {
+ snapshots.latest();
+ }
}
else
{
// clang-format off
- qb
+ auto& snapshots = qb
.coreSegments().withName(constants::memoryName)
.providerSegments().withName(query.agent)
.entities().withNames(query.sensorList)
- .snapshots().timeRange(query.timeRange.min, query.timeRange.max);
+ .snapshots();
// clang-format on
+ if (query.timeRange.has_value())
+ {
+ snapshots.timeRange(query.timeRange->min, query.timeRange->max);
+ }
+ else
+ {
+ snapshots.latest();
+ }
}
return qb;
@@ -126,16 +140,14 @@ namespace armarx::armem::laser_scans::client
ARMARX_WARNING << "No entities!";
}
- const auto convert =
- [](const auto& aronLaserScanStamped,
- const wm::EntityInstance & ei) -> LaserScanStamped
+ const auto convert = [](const auto& aronLaserScanStamped,
+ const wm::EntityInstance& ei) -> LaserScanStamped
{
LaserScanStamped laserScanStamped;
fromAron(aronLaserScanStamped, laserScanStamped);
const auto ndArrayNavigator =
- aron::data::NDArray::DynamicCast(
- ei.data()->getElement("scan"));
+ aron::data::NDArray::DynamicCast(ei.data()->getElement("scan"));
ARMARX_CHECK_NOT_NULL(ndArrayNavigator);
@@ -145,47 +157,48 @@ namespace armarx::armem::laser_scans::client
};
// loop over all entities and their snapshots
- providerSegment.forEachEntity([&outV, &convert](const wm::Entity & entity)
- {
- // If we don't need this warning, we could directly iterate over the snapshots.
- if (entity.empty())
+ providerSegment.forEachEntity(
+ [&outV, &convert](const wm::Entity& entity)
{
- ARMARX_WARNING << "Empty history for " << entity.id();
- }
- ARMARX_DEBUG << "History size: " << entity.size();
-
- entity.forEachInstance([&outV, &convert](const wm::EntityInstance & entityInstance)
- {
- if (const auto o = tryCast<arondto::LaserScanStamped>(entityInstance))
+ // If we don't need this warning, we could directly iterate over the snapshots.
+ if (entity.empty())
{
- outV.push_back(convert(*o, entityInstance));
+ ARMARX_WARNING << "Empty history for " << entity.id();
}
+ ARMARX_DEBUG << "History size: " << entity.size();
+
+ entity.forEachInstance(
+ [&outV, &convert](const wm::EntityInstance& entityInstance)
+ {
+ if (const auto o = tryCast<arondto::LaserScanStamped>(entityInstance))
+ {
+ outV.push_back(convert(*o, entityInstance));
+ }
+ return true;
+ });
return true;
});
- return true;
- });
return outV;
}
- Reader::Result Reader::queryData(const Query& query) const
+ Reader::Result
+ Reader::queryData(const Query& query) const
{
const auto qb = buildQuery(query);
ARMARX_DEBUG << "[MappingDataReader] query ... ";
- const armem::client::QueryResult qResult =
- memoryReader.query(qb.buildQueryInput());
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
ARMARX_DEBUG << "[MappingDataReader] result: " << qResult;
if (not qResult.success)
{
- ARMARX_WARNING << "Failed to query data from memory: "
- << qResult.errorMessage;
- return {.laserScans = {},
- .sensors = {},
- .status = Result::Status::Error,
+ ARMARX_WARNING << "Failed to query data from memory: " << qResult.errorMessage;
+ return {.laserScans = {},
+ .sensors = {},
+ .status = Result::Status::Error,
.errorMessage = qResult.errorMessage};
}
@@ -195,16 +208,17 @@ namespace armarx::armem::laser_scans::client
const auto laserScans = asLaserScans(providerSegment);
std::vector<std::string> sensors;
- providerSegment.forEachEntity([&sensors](const wm::Entity & entity)
- {
- sensors.push_back(entity.name());
- return true;
- });
+ providerSegment.forEachEntity(
+ [&sensors](const wm::Entity& entity)
+ {
+ sensors.push_back(entity.name());
+ return true;
+ });
- return {.laserScans = laserScans,
- .sensors = sensors,
- .status = Result::Status::Success,
+ return {.laserScans = laserScans,
+ .sensors = sensors,
+ .status = Result::Status::Success,
.errorMessage = ""};
}
-} // namespace armarx::armem::vision::laser_scans::client
+} // namespace armarx::armem::laser_scans::client
diff --git a/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.h b/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.h
index 70703521336f8ac0b1fdf9649df4a3777d95f431..55beab204b12a7f72432ef2e38789f1856058934 100644
--- a/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.h
+++ b/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.h
@@ -72,7 +72,8 @@ namespace armarx::armem::laser_scans::client
{
std::string agent;
- TimeRange timeRange;
+ // if not provided, only latest is queried
+ std::optional<TimeRange> timeRange;
// if empty, all sensors will be queried
std::vector<std::string> sensorList;
diff --git a/source/RobotAPI/libraries/armem_laser_scans/server/Visu.cpp b/source/RobotAPI/libraries/armem_laser_scans/server/Visu.cpp
index 2387e7319e2646857d9c821647f32d136ffdd969..551e4792c2eba3b5e19d2ebf468f2d090a53af41 100644
--- a/source/RobotAPI/libraries/armem_laser_scans/server/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_laser_scans/server/Visu.cpp
@@ -301,8 +301,7 @@ namespace armarx::armem::server::laser_scans
if (robots.count(name) == 0)
{
ARMARX_CHECK_NOT_NULL(virtualRobotReader);
- const auto robot = virtualRobotReader->getRobot(
- name, timestamp, VirtualRobot::RobotIO::RobotDescription::eStructure);
+ const auto robot = virtualRobotReader->getRobot(name);
if (robot)
{
@@ -324,7 +323,7 @@ namespace armarx::armem::server::laser_scans
}
else
{
- ARMARX_VERBOSE << "Faield to synchronize robot `" << name << "`";
+ ARMARX_INFO << deactivateSpam(10) << "Failed to synchronize robot `" << name << "`";
}
}
return entry.first;
diff --git a/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml b/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml
index a90821817037a2deb733474743f33221048ab24b..e2d695429d3c4193ad97df106821f5bb912a0028 100644
--- a/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml
+++ b/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml
@@ -27,6 +27,22 @@ Core segment type of Object/Class.
<armarx::arondto::PackagePath />
</ObjectChild>
+ <ObjectChild key="urdfPath">
+ <armarx::arondto::PackagePath />
+ </ObjectChild>
+
+ <ObjectChild key="articulatedUrdfPath">
+ <armarx::arondto::PackagePath />
+ </ObjectChild>
+
+ <ObjectChild key="sdfPath">
+ <armarx::arondto::PackagePath />
+ </ObjectChild>
+
+ <ObjectChild key="articulatedSdfPath">
+ <armarx::arondto::PackagePath />
+ </ObjectChild>
+
<ObjectChild key="meshWrlPath">
<armarx::arondto::PackagePath />
</ObjectChild>
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
index c16ff257603b9fded0a5ea72737b92b40667fdb6..a2b0ddd6b2985997156cfa6c82998de249409e5a 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
@@ -47,7 +47,7 @@ namespace armarx::armem::articulated_object
ARMARX_DEBUG << "Object " << name << " available";
auto obj = VirtualRobot::RobotIO::loadRobot(
- ArmarXDataPath::resolvePath(it->xml.serialize().path),
+ it->xml.toSystemPath(),
VirtualRobot::RobotIO::eStructure);
if (not obj)
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
index 54049d8bbeb899b350dfab4906a125157ea527b9..0b466d404fbc41d5eee48819418dd5b5cae35d1f 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
@@ -3,44 +3,53 @@
#include <Eigen/Core>
#include <Eigen/Geometry>
+#include <VirtualRobot/Robot.h>
+
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
-
-#include <VirtualRobot/Robot.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
namespace armarx::armem::articulated_object
{
- armem::articulated_object::ArticulatedObject convert(
- const VirtualRobot::Robot& obj,
- const armem::Time& timestamp)
+ armem::articulated_object::ArticulatedObject
+ convert(const VirtualRobot::Robot& obj, const armem::Time& timestamp)
{
ARMARX_DEBUG << "Filename is " << obj.getFilename();
- // TODO(fabian.reister): remove "PriorKnowledgeData" below
+ const std::vector<std::string> packages =
+ armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
+ const std::string package = armarx::ArmarXDataPath::getProject(packages, obj.getFilename());
- return armem::articulated_object::ArticulatedObject
+ // make sure that the relative path is without the 'package/' prefix
+ const std::string fileRelPath = [&obj, &package]() -> std::string
{
- .description = {
- .name = obj.getType(),
- .xml = PackagePath(armarx::ArmarXDataPath::getProject(
- {"PriorKnowledgeData"}, obj.getFilename()),
- obj.getFilename())
- },
- .instance = obj.getName(),
- .config = {
- .timestamp = timestamp,
- .globalPose = Eigen::Affine3f(obj.getRootNode()->getGlobalPose()),
- .jointMap = obj.getJointValues()
- },
- .timestamp = timestamp};
+ if (simox::alg::starts_with(obj.getFilename(), package))
+ {
+ // remove "package" + "/"
+ const std::string fixedFilename = obj.getFilename().substr(package.size() + 1, -1);
+ return fixedFilename;
+ }
+
+ return obj.getFilename();
+ }();
+
+ return armem::articulated_object::ArticulatedObject{
+ .description = {.name = obj.getType(),
+ .xml = PackagePath(armarx::ArmarXDataPath::getProject(
+ {package}, fileRelPath),
+ obj.getFilename())},
+ .instance = obj.getName(),
+ .config = {.timestamp = timestamp,
+ .globalPose = Eigen::Affine3f(obj.getRootNode()->getGlobalPose()),
+ .jointMap = obj.getJointValues()},
+ .timestamp = timestamp};
}
bool
- ArticulatedObjectWriter::storeArticulatedObject(
- const VirtualRobot::RobotPtr& articulatedObject,
- const armem::Time& timestamp)
+ ArticulatedObjectWriter::storeArticulatedObject(const VirtualRobot::RobotPtr& articulatedObject,
+ const armem::Time& timestamp)
{
ARMARX_CHECK_NOT_NULL(articulatedObject);
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
index a95e98d011e52c7f6251a2119b66b971b9d5a045..4dcf3262724cd6a9626a967f9582d4514a478d89 100644
--- a/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
@@ -191,7 +191,11 @@ namespace armarx::armem::server::obj::clazz
}
};
setPathIfExists(data.simoxXmlPath, info.simoxXML());
+ setPathIfExists(data.urdfPath, info.urdf());
+ setPathIfExists(data.sdfPath, info.sdf());
setPathIfExists(data.articulatedSimoxXmlPath, info.articulatedSimoxXML());
+ setPathIfExists(data.articulatedUrdfPath, info.articulatedUrdf());
+ setPathIfExists(data.articulatedSdfPath, info.articulatedSdf());
setPathIfExists(data.meshObjPath, info.wavefrontObj());
setPathIfExists(data.meshWrlPath, info.meshWrl());
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index 887674e048a41f2a93996a0d6fbdb91e90b7f901..c17ce69020b2b7f2c363219339754919fc805723 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -222,12 +222,21 @@ namespace armarx::armem::server::obj::instance
stats.numUpdated++;
VirtualRobot::RobotPtr robot = robots.get(provided.robotName, provided.providerName);
+
+ if(not robot)
+ {
+ ARMARX_INFO << deactivateSpam(1) << "Failed to retrieve robot `" << provided.robotName << "`.";
+ }
+
// robot may be null!
// Update the robot to obtain correct local -> global transformation
if (robot and robotSyncTimestamp != timestamp)
{
- ARMARX_CHECK(robots.reader->synchronizeRobot(*robot, timestamp));
+ ARMARX_CHECK(robots.reader->synchronizeRobot(*robot, timestamp))
+ << "Failed to synchronize robot to timestamp " << timestamp
+ << ". This is " << (Clock::Now() - timestamp).toSecondsDouble() << " seconds in the past.";
+
robotSyncTimestamp = timestamp;
@@ -1302,14 +1311,13 @@ namespace armarx::armem::server::obj::instance
{
// Try to fetch the robot.
ARMARX_CHECK_NOT_NULL(reader);
- bool warnings = false;
VirtualRobot::RobotPtr robot = reader->getRobot(
robotName, Clock::Now(),
- VirtualRobot::RobotIO::RobotDescription::eStructure, warnings);
+ VirtualRobot::RobotIO::RobotDescription::eStructure);
if (robot)
{
- reader->synchronizeRobot(*robot, Clock::Now());
+ ARMARX_CHECK(reader->synchronizeRobot(*robot, Clock::Now()));
// Store robot if valid.
loaded.emplace(robotName, robot);
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
index 3a13f6722e5cd2c0f824c630a83367b47af1de72..514096731b83167b902dfe4198deae82725837ed 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
@@ -194,7 +194,7 @@ namespace armarx::armem::server::obj::instance
{
viz::Robot robot(key);
robot.pose(pose);
- robot.file(model->package, model->relativePath);
+ robot.file(model->package, model->package + "/" + model->relativePath);
robot.joints(objectPose.objectJointValues);
layer.add(robot);
diff --git a/source/RobotAPI/libraries/armem_robot/types.h b/source/RobotAPI/libraries/armem_robot/types.h
index 345564d4b859af306acecdc82d00096c4a559351..2bc45c144b53e3900e8c7041c54d33ee0d205f56 100644
--- a/source/RobotAPI/libraries/armem_robot/types.h
+++ b/source/RobotAPI/libraries/armem_robot/types.h
@@ -40,6 +40,15 @@ namespace armarx::armem::robot
Eigen::Vector3f torque;
};
+ using ToFArray = Eigen::MatrixXf;
+
+ struct ToF
+ {
+ ToFArray array;
+ std::string frame;
+ };
+
+
struct RobotState
{
using JointMap = std::map<std::string, float>;
diff --git a/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
index a9c0ceb1b8cd552915819a052a2ffd73bbfdec29..8dda90015cd030197cd4af43fd2b9d534968fc19 100644
--- a/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
@@ -65,6 +65,7 @@ armarx_enable_aron_file_generation_for_target(
ARON_FILES
aron/JointState.xml
aron/Proprioception.xml
+ aron/Exteroception.xml
aron/TransformHeader.xml
aron/Transform.xml
)
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron/Exteroception.xml b/source/RobotAPI/libraries/armem_robot_state/aron/Exteroception.xml
new file mode 100644
index 0000000000000000000000000000000000000000..80f72e06059a39e60e6709a5e7acb28c63ee3395
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/aron/Exteroception.xml
@@ -0,0 +1,35 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ <SystemInclude include="Eigen/Core" />
+ <SystemInclude include="Eigen/Geometry" />
+ </CodeIncludes>
+
+ <GenerateTypes>
+
+ <Object name="armarx::armem::exteroception::arondto::ToF">
+
+ <ObjectChild key="array">
+ <Matrix rows="-1" cols="-1" type="float32" />
+ </ObjectChild>
+
+ </Object>
+
+
+ <Object name="armarx::armem::arondto::Exteroception">
+
+ <ObjectChild key="iterationID">
+ <long />
+ </ObjectChild>
+
+ <ObjectChild key="tof">
+ <Dict>
+ <armarx::armem::exteroception::arondto::ToF/>
+ </Dict>
+ </ObjectChild>
+
+ </Object>
+
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml b/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml
index c7ac7670d399329bb6674dad3243a048153534f8..d5472cbde89434e346f116e6b6f461bf4f2ca34e 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml
+++ b/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml
@@ -234,8 +234,6 @@
</ObjectChild>
</Object>
-
-
<Object name="armarx::armem::arondto::Proprioception">
@@ -257,7 +255,6 @@
</Dict>
</ObjectChild>
-
<ObjectChild key="extraFloats">
<Dict>
<Float />
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
index 1c18fe5b78799b1b2f9555a33a8997e7edb60d28..3749c892d5e46e6ce0f7a848a86e8059ed70991c 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
@@ -6,6 +6,7 @@
#include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Exteroception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/types.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
@@ -97,4 +98,14 @@ namespace armarx::armem
aron::toAron(dto.torque, bo.torque);
}
+
+ void fromAron(const armarx::armem::exteroception::arondto::ToF& dto, robot::ToFArray& bo){
+ bo = dto.array;
+ }
+
+ void toAron(armarx::armem::exteroception::arondto::ToF& dto, const robot::ToFArray& bo){
+ dto.array = bo;
+ }
+
+
} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
index 75c339c7fc4aad7f24b5b52befdf2e7f1451b345..f78efc200ee18205523ddf7ea5c60e8c6949bf9f 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
@@ -40,6 +40,11 @@ namespace armarx::armem
struct ForceTorque;
}
+ namespace exteroception::arondto
+ {
+ struct ToF;
+ }
+
namespace arondto
{
struct Transform;
@@ -65,5 +70,8 @@ namespace armarx::armem
void fromAron(const armarx::armem::prop::arondto::ForceTorque& dto, robot::ForceTorque& bo);
void toAron(armarx::armem::prop::arondto::ForceTorque& dto, const robot::ForceTorque& bo);
+ void fromAron(const armarx::armem::exteroception::arondto::ToF& dto, robot::ToFArray& bo);
+ void toAron(armarx::armem::exteroception::arondto::ToF& dto, const robot::ToFArray& bo);
+
} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
index 611fa0e0d3f5bb931f5d64a60c70735d698c623f..eaa2e9ca07b063eaba3a71887eda2d14fa839c16 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -22,6 +22,7 @@
#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Exteroception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
@@ -121,7 +122,7 @@ namespace armarx::armem::robot_state
const auto elapsedTime = armem::Time::Now() - tsStartFunctionInvokation;
if (elapsedTime > syncTimeout)
{
- ARMARX_WARNING << "Could not synchronize object " << obj.description.name;
+ ARMARX_VERBOSE << "Could not synchronize object " << obj.description.name;
return false;
}
@@ -135,11 +136,7 @@ namespace armarx::armem::robot_state
}
std::optional<robot::RobotDescription>
- RobotReader::queryDescription(
- const std::string& name,
- const armem::Time& timestamp,
- bool warnings
- )
+ RobotReader::queryDescription(const std::string& name, const armem::Time& timestamp)
{
const auto sanitizedTimestamp = timestamp.isValid() ? timestamp : Clock::Now();
@@ -159,11 +156,8 @@ namespace armarx::armem::robot_state
if (not memoryReader)
{
- if (warnings)
- {
- ARMARX_WARNING << "Memory reader is null. Did you forget to call "
- "RobotReader::connect() in onConnectComponent()?";
- }
+ ARMARX_WARNING << "Memory reader is null. Did you forget to call "
+ "RobotReader::connect() in onConnectComponent()?";
return std::nullopt;
}
@@ -182,10 +176,7 @@ namespace armarx::armem::robot_state
}
catch (...)
{
- if (warnings)
- {
- ARMARX_WARNING << "query description failure" << GetHandledExceptionString();
- }
+ ARMARX_VERBOSE << "Query description failure" << GetHandledExceptionString();
}
return std::nullopt;
@@ -198,14 +189,14 @@ namespace armarx::armem::robot_state
const auto jointMap = queryJointState(description, timestamp);
if (not jointMap)
{
- ARMARX_WARNING << "Failed to query joint state for robot '" << description.name << "'.";
+ ARMARX_VERBOSE << "Failed to query joint state for robot '" << description.name << "'.";
return std::nullopt;
}
const auto globalPose = queryGlobalPose(description, timestamp);
if (not globalPose)
{
- ARMARX_WARNING << "Failed to query global pose for robot " << description.name;
+ ARMARX_VERBOSE << "Failed to query global pose for robot " << description.name;
return std::nullopt;
}
@@ -235,7 +226,7 @@ namespace armarx::armem::robot_state
}
catch (...)
{
- ARMARX_WARNING << GetHandledExceptionString();
+ ARMARX_VERBOSE << GetHandledExceptionString();
return std::nullopt;
}
}
@@ -268,7 +259,7 @@ namespace armarx::armem::robot_state
if (not qResult.success) /* c++20 [[unlikely]] */
{
- ARMARX_WARNING << qResult.errorMessage;
+ ARMARX_VERBOSE << qResult.errorMessage;
return std::nullopt;
}
@@ -276,7 +267,7 @@ namespace armarx::armem::robot_state
}
catch (...)
{
- ARMARX_WARNING << deactivateSpam(1) << "Failed to query joint state. Reason: "
+ ARMARX_VERBOSE << deactivateSpam(1) << "Failed to query joint state. Reason: "
<< GetHandledExceptionString();
return std::nullopt;
}
@@ -306,7 +297,7 @@ namespace armarx::armem::robot_state
if (not qResult.success) /* c++20 [[unlikely]] */
{
- ARMARX_WARNING << qResult.errorMessage;
+ ARMARX_VERBOSE << qResult.errorMessage;
return {};
}
@@ -339,7 +330,7 @@ namespace armarx::armem::robot_state
if (not qResult.success) /* c++20 [[unlikely]] */
{
- ARMARX_WARNING << qResult.errorMessage;
+ ARMARX_VERBOSE << qResult.errorMessage;
return std::nullopt;
}
@@ -352,7 +343,8 @@ namespace armarx::armem::robot_state
{
try
{
- const auto result = transformReader.getGlobalPose(description.name, constants::robotRootNodeName, timestamp);
+ const auto result = transformReader.getGlobalPose(
+ description.name, constants::robotRootNodeName, timestamp);
if (not result)
{
return std::nullopt;
@@ -362,7 +354,7 @@ namespace armarx::armem::robot_state
}
catch (...)
{
- ARMARX_WARNING << deactivateSpam(1) << "Failed to query global pose. Reason: "
+ ARMARX_VERBOSE << deactivateSpam(1) << "Failed to query global pose. Reason: "
<< GetHandledExceptionString();
return std::nullopt;
}
@@ -381,7 +373,7 @@ namespace armarx::armem::robot_state
if (providerSegment.empty())
{
- ARMARX_WARNING << "No entity found";
+ ARMARX_VERBOSE << "No entity found";
return std::nullopt;
}
@@ -394,9 +386,10 @@ namespace armarx::armem::robot_state
instance = &i;
return false; // break
});
- if (!instance)
+
+ if (instance == nullptr)
{
- ARMARX_WARNING << "No entity snapshots found";
+ ARMARX_VERBOSE << "No entity snapshots found";
return std::nullopt;
}
@@ -436,6 +429,11 @@ namespace armarx::armem::robot_state
coreSegment.forEachEntity(
[&jointMap](const wm::Entity& entity)
{
+ if (not entity.getLatestSnapshot().hasInstance(0))
+ {
+ return;
+ }
+
const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
const auto proprioception =
@@ -525,7 +523,7 @@ namespace armarx::armem::robot_state
if (not qResult.success) /* c++20 [[unlikely]] */
{
- ARMARX_WARNING << qResult.errorMessage;
+ ARMARX_VERBOSE << qResult.errorMessage;
return std::nullopt;
}
@@ -558,7 +556,7 @@ namespace armarx::armem::robot_state
if (not qResult.success) /* c++20 [[unlikely]] */
{
- ARMARX_WARNING << qResult.errorMessage;
+ ARMARX_VERBOSE << qResult.errorMessage;
return std::nullopt;
}
@@ -566,6 +564,37 @@ namespace armarx::armem::robot_state
}
+ std::optional<std::map<RobotReader::Hand, robot::ToFArray>>
+ RobotReader::queryToF(const robot::RobotDescription& description,
+ const armem::Time& timestamp) const
+ {
+ // Query all entities from provider.
+ armem::client::query::Builder qb;
+
+ ARMARX_DEBUG << "Querying ToF data for robot: " << description;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(constants::exteroceptionCoreSegment)
+ .providerSegments().withName(description.name) // agent
+ .entities().all() // TODO
+ .snapshots().beforeOrAtTime(timestamp);
+ // clang-format on
+
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ ARMARX_VERBOSE << qResult.errorMessage;
+ return std::nullopt;
+ }
+
+ return getToF(qResult.memory, description.name);
+ }
+
+
std::optional<robot::PlatformState>
RobotReader::getRobotPlatformState(const armarx::armem::wm::Memory& memory,
const std::string& name) const
@@ -580,6 +609,11 @@ namespace armarx::armem::robot_state
coreSegment.forEachEntity(
[&platformState](const wm::Entity& entity)
{
+ if (not entity.getLatestSnapshot().hasInstance(0))
+ {
+ return;
+ }
+
const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
const auto proprioception =
@@ -623,6 +657,11 @@ namespace armarx::armem::robot_state
coreSegment.forEachEntity(
[&forceTorques](const wm::Entity& entity)
{
+ if (not entity.getLatestSnapshot().hasInstance(0))
+ {
+ return;
+ }
+
const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
const auto proprioception =
@@ -675,6 +714,47 @@ namespace armarx::armem::robot_state
return forceTorques;
}
+ std::map<RobotReader::Hand, robot::ToFArray>
+ RobotReader::getToF(const armarx::armem::wm::Memory& memory, const std::string& name) const
+ {
+ std::map<RobotReader::Hand, robot::ToFArray> tofs;
+
+ // clang-format off
+ const armem::wm::CoreSegment& coreSegment = memory
+ .getCoreSegment(constants::exteroceptionCoreSegment);
+ // clang-format on
+
+ coreSegment.forEachEntity(
+ [&tofs](const wm::Entity& entity)
+ {
+ ARMARX_DEBUG << "Processing ToF element";
+
+ if (not entity.getLatestSnapshot().hasInstance(0))
+ {
+ return;
+ }
+ const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+
+ const auto exteroception =
+ tryCast<::armarx::armem::arondto::Exteroception>(entityInstance);
+ ARMARX_CHECK(exteroception.has_value());
+
+
+ for (const auto& [handName, dtoFt] : exteroception->tof)
+ {
+ ARMARX_DEBUG << "Processing ToF element for hand `" << handName << "`";
+
+ robot::ToFArray tof;
+ fromAron(dtoFt, tof);
+
+ const auto hand = fromHandName(handName);
+ tofs.emplace(hand, tof);
+ }
+ });
+
+ return tofs;
+ }
+
std::optional<robot::RobotDescription>
RobotReader::getRobotDescription(const armarx::armem::wm::Memory& memory,
@@ -691,11 +771,77 @@ namespace armarx::armem::robot_state
{ instance = &i; });
if (instance == nullptr)
{
- ARMARX_WARNING << "No entity snapshots found in provider segment `" << name << "`";
+ ARMARX_VERBOSE << "No entity snapshots found in provider segment `" << name << "`";
return std::nullopt;
}
return robot::convertRobotDescription(*instance);
}
+ std::vector<robot::RobotDescription>
+ RobotReader::getRobotDescriptions(const armarx::armem::wm::Memory& memory) const
+ {
+ const armem::wm::CoreSegment& coreSegment =
+ memory.getCoreSegment(constants::descriptionCoreSegment);
+
+ std::vector<robot::RobotDescription> descriptions;
+
+ coreSegment.forEachInstance(
+ [&descriptions](const wm::EntityInstance& instance)
+ {
+ if (const std::optional<robot::RobotDescription> desc =
+ robot::convertRobotDescription(instance))
+ {
+ descriptions.push_back(desc.value());
+ }
+ });
+
+ return descriptions;
+ }
+
+ std::vector<robot::RobotDescription>
+ RobotReader::queryDescriptions(const armem::Time& timestamp)
+ {
+ const auto sanitizedTimestamp = timestamp.isValid() ? timestamp : Clock::Now();
+
+ // Query all entities from provider.
+ armem::client::query::Builder qb;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(constants::descriptionCoreSegment)
+ .providerSegments().all()
+ .entities().all()
+ .snapshots().beforeOrAtTime(sanitizedTimestamp);
+ // clang-format on
+
+ ARMARX_DEBUG << "Lookup query in reader";
+
+ if (not memoryReader)
+ {
+ ARMARX_WARNING << "Memory reader is null. Did you forget to call "
+ "RobotReader::connect() in onConnectComponent()?";
+ return {};
+ }
+
+ try
+ {
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ return {};
+ }
+
+ return getRobotDescriptions(qResult.memory);
+ }
+ catch (...)
+ {
+ ARMARX_VERBOSE << "Query description failure" << GetHandledExceptionString();
+ }
+
+ return {};
+ }
} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
index bb63c714180325961c434d61e4a8e31b78a84045..a9cddc6b2afd5258271f5718b4f1c82d0dd6a995 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
@@ -23,6 +23,7 @@
#include <mutex>
#include <optional>
+#include <vector>
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
@@ -50,7 +51,7 @@ namespace armarx::armem::robot_state
void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def);
- bool synchronize(robot::Robot& obj, const armem::Time& timestamp) override;
+ [[nodiscard]] bool synchronize(robot::Robot& obj, const armem::Time& timestamp) override;
std::optional<robot::Robot> get(const std::string& name,
const armem::Time& timestamp) override;
@@ -58,8 +59,9 @@ namespace armarx::armem::robot_state
const armem::Time& timestamp) override;
std::optional<robot::RobotDescription> queryDescription(const std::string& name,
- const armem::Time& timestamp,
- bool warnings = true);
+ const armem::Time& timestamp);
+
+ std::vector<robot::RobotDescription> queryDescriptions(const armem::Time& timestamp);
std::optional<robot::RobotState> queryState(const robot::RobotDescription& description,
const armem::Time& timestamp);
@@ -100,6 +102,10 @@ namespace armarx::armem::robot_state
const armem::Time& start,
const armem::Time& end) const;
+
+ std::optional<std::map<Hand, robot::ToFArray>>
+ queryToF(const robot::RobotDescription& description, const armem::Time& timestamp) const;
+
/**
* @brief retrieve the robot's pose in the odometry frame.
*
@@ -125,6 +131,9 @@ namespace armarx::armem::robot_state
std::optional<robot::RobotDescription>
getRobotDescription(const armarx::armem::wm::Memory& memory, const std::string& name) const;
+ std::vector<robot::RobotDescription>
+ getRobotDescriptions(const armarx::armem::wm::Memory& memory) const;
+
std::optional<robot::RobotState::JointMap>
getRobotJointState(const armarx::armem::wm::Memory& memory, const std::string& name) const;
@@ -142,6 +151,9 @@ namespace armarx::armem::robot_state
std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>>
getForceTorques(const armarx::armem::wm::Memory& memory, const std::string& name) const;
+ std::map<RobotReader::Hand, robot::ToFArray> getToF(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const;
+
struct Properties
{
} properties;
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
index 8c1f8dfb0ad4f6f833d27191588fc8d766998235..225e048864e69a7c349b9e53163d41ed1e0eef6b 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
@@ -38,16 +38,16 @@ namespace armarx::armem::robot_state
bool
VirtualRobotReader::synchronizeRobot(VirtualRobot::Robot& robot, const armem::Time& timestamp)
{
- const static auto packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
- const auto package = armarx::ArmarXDataPath::getProject(packages, robot.getFilename());
+ // const static auto packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
+ // const auto package = armarx::ArmarXDataPath::getProject(packages, robot.getFilename());
const robot::RobotDescription robotDescription{
- .name = robot.getName(), .xml = PackagePath{package, robot.getFilename()}};
+ .name = robot.getName(), .xml = PackagePath{"", ""}};
const auto robotState = queryState(robotDescription, timestamp);
if (not robotState)
{
- ARMARX_WARNING << "Querying robot state failed for robot `" << robot.getName() << "` "
+ ARMARX_VERBOSE << "Querying robot state failed for robot `" << robot.getName() << "` "
<< "(type `"<< robot.getType() << "`)!";
return false;
}
@@ -61,19 +61,16 @@ namespace armarx::armem::robot_state
VirtualRobot::RobotPtr
VirtualRobotReader::getRobot(const std::string& name,
const armem::Time& timestamp,
- const VirtualRobot::RobotIO::RobotDescription& loadMode,
- bool warnings)
+ const VirtualRobot::RobotIO::RobotDescription& loadMode)
{
ARMARX_INFO << deactivateSpam(60)
<< "Querying robot description for robot '" << name << "'";
- const auto description = queryDescription(name, timestamp, warnings);
+ const auto description = queryDescription(name, timestamp);
if (not description)
{
- if (warnings)
- {
- ARMARX_WARNING << "The description of robot `" << name << "` is not a available!";
- }
+ ARMARX_VERBOSE << "The description of robot `" << name << "` is not a available!";
+
return nullptr;
}
@@ -134,7 +131,7 @@ namespace armarx::armem::robot_state
Clock::WaitFor(sleepAfterFailure);
}
- ARMARX_WARNING << "Failed to get synchronized robot `" << name << "`";
+ ARMARX_VERBOSE << "Failed to get synchronized robot `" << name << "`";
return nullptr;
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
index d19e0483fdfa04f00579ba85b1e830f58feee241..ab30cbc1317d98f802dc24cbe5d844da02b038e9 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
@@ -27,7 +27,6 @@
#include "RobotReader.h"
-
namespace armarx::armem::robot_state
{
/**
@@ -47,14 +46,14 @@ namespace armarx::armem::robot_state
void connect();
void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def);
- bool synchronizeRobot(VirtualRobot::Robot& robot, const armem::Time& timestamp);
+ [[nodiscard]] bool synchronizeRobot(VirtualRobot::Robot& robot,
+ const armem::Time& timestamp);
[[nodiscard]] VirtualRobot::RobotPtr
getRobot(const std::string& name,
const armem::Time& timestamp = armem::Time::Invalid(),
const VirtualRobot::RobotIO::RobotDescription& loadMode =
- VirtualRobot::RobotIO::RobotDescription::eStructure,
- bool warnings = true);
+ VirtualRobot::RobotIO::RobotDescription::eStructure);
[[nodiscard]] VirtualRobot::RobotPtr
getSynchronizedRobot(const std::string& name,
@@ -71,14 +70,12 @@ namespace armarx::armem::robot_state
private:
-
[[nodiscard]] VirtualRobot::RobotPtr
_getSynchronizedRobot(const std::string& name,
const armem::Time& timestamp = armem::Time::Invalid(),
const VirtualRobot::RobotIO::RobotDescription& loadMode =
- VirtualRobot::RobotIO::RobotDescription::eStructure,
+ VirtualRobot::RobotIO::RobotDescription::eStructure,
bool blocking = true);
-
};
} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/constants.h b/source/RobotAPI/libraries/armem_robot_state/client/common/constants.h
index d9efcbcf795b46ddb4e9647919d38c0c617c1234..5d093c195addda7c64c378d919d38f09987cdfc9 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/constants.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/constants.h
@@ -28,6 +28,7 @@ namespace armarx::armem::robot_state::constants
inline const std::string descriptionCoreSegment = "Description";
inline const std::string localizationCoreSegment = "Localization";
inline const std::string proprioceptionCoreSegment = "Proprioception";
+ inline const std::string exteroceptionCoreSegment = "Exteroception";
inline const std::string descriptionEntityName = "description";
diff --git a/source/RobotAPI/libraries/armem_robot_state/memory_ids.cpp b/source/RobotAPI/libraries/armem_robot_state/memory_ids.cpp
index 926c2e843e252efdc9c4711bbe2b03a3a7394bba..2f219eff39376ed3ecc44e47b0780fd1328785a0 100644
--- a/source/RobotAPI/libraries/armem_robot_state/memory_ids.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/memory_ids.cpp
@@ -30,6 +30,7 @@ namespace armarx::armem
const MemoryID robot_state::descriptionSegmentID { memoryID.withCoreSegmentName("Description") };
const MemoryID robot_state::proprioceptionSegmentID { memoryID.withCoreSegmentName("Proprioception") };
+ const MemoryID robot_state::exteroceptionSegmentID { memoryID.withCoreSegmentName("Exteroception") };
const MemoryID robot_state::localizationSegmentID { memoryID.withCoreSegmentName("Localization") };
diff --git a/source/RobotAPI/libraries/armem_robot_state/memory_ids.h b/source/RobotAPI/libraries/armem_robot_state/memory_ids.h
index 1d8008d4db8d7a71da0a8ca47420040060382ca6..f653236b1f62ec8aef905d45a7ee86af079b05b9 100644
--- a/source/RobotAPI/libraries/armem_robot_state/memory_ids.h
+++ b/source/RobotAPI/libraries/armem_robot_state/memory_ids.h
@@ -32,6 +32,7 @@ namespace armarx::armem::robot_state
extern const MemoryID descriptionSegmentID;
extern const MemoryID proprioceptionSegmentID;
+ extern const MemoryID exteroceptionSegmentID;
extern const MemoryID localizationSegmentID;
} // namespace armarx::armem::objects
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt
index 169d59f1a86d6a1bea1b9e54033409d23fb3cd7f..3ad1d50684d89c246e6123a6ef5d2055779eab6f 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt
@@ -43,10 +43,16 @@ armarx_add_library(
proprioception/RobotUnitData.h
proprioception/RobotUnitReader.h
+ proprioception/converters/ConverterInterface.h
proprioception/converters/Armar6Converter.h
proprioception/converters/ConverterTools.h
proprioception/converters/ConverterRegistry.h
- proprioception/converters/ConverterInterface.h
+
+ exteroception/converters/ConverterInterface.h
+ exteroception/converters/ConverterTools.h
+ exteroception/converters/ConverterRegistry.h
+ exteroception/converters/ArmarDEConverter.h
+ exteroception/Segment.h
description/Segment.h
@@ -67,6 +73,12 @@ armarx_add_library(
proprioception/converters/ConverterRegistry.cpp
proprioception/converters/ConverterInterface.cpp
+ exteroception/converters/ConverterInterface.cpp
+ exteroception/converters/ConverterTools.cpp
+ exteroception/converters/ConverterRegistry.cpp
+ exteroception/converters/ArmarDEConverter.cpp
+ exteroception/Segment.cpp
+
description/Segment.cpp
)
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..63f24d0f46a156adc023b3b64b4f27d344b151c6
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.cpp
@@ -0,0 +1,50 @@
+#include "Segment.h"
+
+#include <filesystem>
+
+#include <SimoxUtility/algorithm/string/string_tools.h>
+
+#include <ArmarXCore/core/application/properties/PluginAll.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
+#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Exteroception.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/memory_ids.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+
+
+namespace armarx::armem::server::robot_state::exteroception
+{
+
+ Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
+ Base(memoryToIceAdapter,
+ armem::robot_state::exteroceptionSegmentID.coreSegmentName,
+ arondto::Exteroception::ToAronType())
+ {
+ }
+
+ Segment::~Segment() = default;
+
+
+ // void
+ // Segment::onConnect(const RobotUnitInterfacePrx& robotUnitPrx)
+ // {
+ // robotUnit = robotUnitPrx;
+
+ // // store the robot description linked to the robot unit in this segment
+ // }
+
+
+} // namespace armarx::armem::server::robot_state::exteroception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.h
new file mode 100644
index 0000000000000000000000000000000000000000..e8471563d640800a44d4516d6eccdd7d7e355ccd
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.h
@@ -0,0 +1,55 @@
+/*
+ * 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 Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2023
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+#include <optional>
+#include <string>
+#include <unordered_map>
+
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+#include <RobotAPI/libraries/armem/server/segment/SpecializedCoreSegment.h>
+#include <RobotAPI/libraries/armem/server/segment/SpecializedProviderSegment.h>
+#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/server/forward_declarations.h>
+
+
+namespace armarx::armem::server::robot_state::exteroception
+{
+
+ class Segment : public segment::SpecializedCoreSegment
+ {
+ using Base = segment::SpecializedCoreSegment;
+
+ public:
+ Segment(server::MemoryToIceAdapter& iceMemory);
+ ~Segment() override;
+
+
+ // void onConnect(const RobotUnitInterfacePrx& robotUnitPrx);
+
+
+ private:
+ // RobotUnitInterfacePrx robotUnit;
+ };
+
+} // namespace armarx::armem::server::robot_state::exteroception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ArmarDEConverter.cpp b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ArmarDEConverter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d71bb045b32028df15bb37150892175e1ec8ec72
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ArmarDEConverter.cpp
@@ -0,0 +1,130 @@
+#include "ArmarDEConverter.h"
+#include <cmath>
+#include <string>
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/algorithm/advanced.h>
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+
+#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Exteroception.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
+#include <RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.h>
+
+#include "ConverterTools.h"
+
+
+namespace armarx::armem::server::robot_state::exteroception
+{
+
+ ArmarDEConverter::ArmarDEConverter() = default;
+ ArmarDEConverter::~ArmarDEConverter() = default;
+
+
+ aron::data::DictPtr
+ ArmarDEConverter::convert(
+ const RobotUnitDataStreaming::TimeStep& data,
+ const RobotUnitDataStreaming::DataStreamingDescription& description)
+ {
+ arondto::Exteroception dto;
+ dto.iterationID = data.iterationId;
+
+ for (const auto& [dataEntryName, dataEntry] : description.entries)
+ {
+ process(dto, dataEntryName, {data, dataEntry});
+ }
+
+ // resize to square
+ for(auto& [_, tof] : dto.tof)
+ {
+ const int sr = std::sqrt(tof.array.size());
+ const bool isSquare = (sr * sr == tof.array.size());
+
+ if(tof.array.size() > 0 and isSquare)
+ {
+ tof.array.resize(sr, sr);
+ }
+ }
+
+
+ return dto.toAron();
+ }
+
+
+ void ArmarDEConverter::process(
+ arondto::Exteroception& dto,
+ const std::string& entryName,
+ const ConverterValue& value)
+ {
+ const std::vector<std::string> split = simox::alg::split(entryName, ".", false, false);
+ ARMARX_CHECK_GREATER_EQUAL(split.size(), 3);
+ const std::set<size_t> acceptedSizes{3, 4, 5};
+ ARMARX_CHECK_GREATER(acceptedSizes.count(split.size()), 0)
+ << "Data entry name could not be parsed (exected 3 or 4 or 5 components between '.'): "
+ << "\n- split: '" << split << "'";
+
+ const std::string& category = split.at(0);
+ // const std::string& name = split.at(1);
+ const std::string& field = split.at(2);
+ ARMARX_CHECK_EQUAL(category, "sens") << category << " | " << entryName;
+
+ if (simox::alg::contains(field, "tofDepth"))
+ {
+ // ARMARX_DEBUG << "Processing ToF sensor data";
+ processToFEntry(dto.tof, split, value);
+ }
+
+ }
+
+
+
+
+ void ArmarDEConverter::processToFEntry(
+ std::map<std::string, armarx::armem::exteroception::arondto::ToF>& tofs,
+ const std::vector<std::string>& split,
+ const ConverterValue& value)
+ {
+ // split e.g. "sens.LeftHand.tofDepth.element_15" (split at dot)
+
+ ARMARX_CHECK_EQUAL(split.size(), 4);
+ ARMARX_CHECK_EQUAL(split.at(2), "tofDepth");
+
+ const std::string& name = split.at(1);
+
+ // element 0 sens
+ // element 1 is either LeftHand or RightHand
+
+ const std::map<std::string, std::string> sidePrefixMap
+ {
+ {"LeftHand", "Left"},
+ {"RightHand", "Right"}
+ };
+
+ auto it = sidePrefixMap.find(name);
+ ARMARX_CHECK(it != sidePrefixMap.end()) << name;
+
+ const std::string& side = it->second;
+ processToFEntry(tofs[side], split, value);
+ }
+
+ void ArmarDEConverter::processToFEntry(armarx::armem::exteroception::arondto::ToF& tof,
+ const std::vector<std::string>& split,
+ const ConverterValue& value)
+ {
+ // split, e.g., element_12
+ const std::vector<std::string> elements = simox::alg::split(split.back(), "_");
+ ARMARX_CHECK_EQUAL(elements.size(), 2);
+
+ const int idx = std::stoi(elements.at(1));
+
+ if(tof.array.size() < (idx +1))
+ {
+ tof.array.resize(idx+1, 1);
+ }
+
+ tof.array(idx) = getValueAs<float>(value);
+ }
+
+
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ArmarDEConverter.h b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ArmarDEConverter.h
new file mode 100644
index 0000000000000000000000000000000000000000..bf6c482a06bfd4be24fca5ad78a320b5b6834042
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ArmarDEConverter.h
@@ -0,0 +1,51 @@
+#pragma once
+
+#include <map>
+#include <string>
+
+#include <Eigen/Core>
+
+#include <RobotAPI/libraries/armem_robot_state/aron/Exteroception.aron.generated.h>
+
+#include "ConverterInterface.h"
+
+
+namespace armarx::armem::server::robot_state::exteroception
+{
+ struct ConverterValue;
+ // class ConverterTools;
+
+
+ class ArmarDEConverter : public ConverterInterface
+ {
+ public:
+ ArmarDEConverter();
+ ~ArmarDEConverter() override;
+
+
+ aron::data::DictPtr
+ convert(const RobotUnitDataStreaming::TimeStep& data,
+ const RobotUnitDataStreaming::DataStreamingDescription& description) override;
+
+
+ protected:
+ void process(arondto::Exteroception& dtoExteroception,
+ const std::string& entryName,
+ const ConverterValue& value);
+
+
+ private:
+
+
+ void processToFEntry(std::map<std::string, armarx::armem::exteroception::arondto::ToF>& fts,
+ const std::vector<std::string>& split,
+ const ConverterValue& value);
+
+ void processToFEntry(armarx::armem::exteroception::arondto::ToF& tof,
+ const std::vector<std::string>& split,
+ const ConverterValue& value);
+
+
+ // std::unique_ptr<ConverterTools> tools;
+ };
+} // namespace armarx::armem::server::robot_state::exteroception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterInterface.cpp b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterInterface.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4ba33e6c5f308cd6213c8ccf02018ad63ba9440b
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterInterface.cpp
@@ -0,0 +1,11 @@
+#include "ConverterInterface.h"
+
+
+namespace armarx::armem::server::robot_state::exteroception
+{
+
+ ConverterInterface::~ConverterInterface()
+ {
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterInterface.h b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterInterface.h
new file mode 100644
index 0000000000000000000000000000000000000000..5ba0ecc3c8aeaf5c2b3f3c03d9f36ed83269fb5e
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterInterface.h
@@ -0,0 +1,32 @@
+#pragma once
+
+#include <memory>
+
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
+namespace armarx::RobotUnitDataStreaming
+{
+ struct TimeStep;
+ struct DataStreamingDescription;
+ struct DataEntry;
+}
+namespace armarx::armem::arondto
+{
+ class Proprioception;
+}
+
+namespace armarx::armem::server::robot_state::exteroception
+{
+ class ConverterInterface
+ {
+ public:
+
+ virtual ~ConverterInterface();
+
+ virtual
+ aron::data::DictPtr convert(
+ const RobotUnitDataStreaming::TimeStep& data,
+ const RobotUnitDataStreaming::DataStreamingDescription& description) = 0;
+
+ };
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterRegistry.cpp b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterRegistry.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ef7296b669504e0f8d0aff8b4cd3991fe6ec10b3
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterRegistry.cpp
@@ -0,0 +1,31 @@
+#include "ConverterRegistry.h"
+
+#include "ArmarDEConverter.h"
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+
+
+namespace armarx::armem::server::robot_state::exteroception
+{
+
+ ConverterRegistry::ConverterRegistry()
+ {
+ add<ArmarDEConverter>("ArmarDE");
+ // add<Armar7Converter>("Armar7");
+ }
+
+
+ ConverterInterface*
+ ConverterRegistry::get(const std::string& key) const
+ {
+ auto it = converters.find(key);
+ return it != converters.end() ? it->second.get() : nullptr;
+ }
+
+
+ std::vector<std::string> ConverterRegistry::getKeys() const
+ {
+ return simox::alg::get_keys(converters);
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterRegistry.h b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterRegistry.h
new file mode 100644
index 0000000000000000000000000000000000000000..1e1cac9a18eb3d901b0ca0f61eccf08395a279b5
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterRegistry.h
@@ -0,0 +1,36 @@
+#pragma once
+
+#include <map>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "ConverterInterface.h"
+
+
+namespace armarx::armem::server::robot_state::exteroception
+{
+ class ConverterRegistry
+ {
+ public:
+
+ ConverterRegistry();
+
+
+ template <class ConverterT, class ...Args>
+ void add(const std::string& key, Args... args)
+ {
+ converters[key].reset(new ConverterT(args...));
+ }
+
+
+ ConverterInterface* get(const std::string& key) const;
+ std::vector<std::string> getKeys() const;
+
+
+ private:
+
+ std::map<std::string, std::unique_ptr<ConverterInterface>> converters;
+
+ };
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterTools.cpp b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterTools.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6086a9aee5f2adb8d997a65302084e0c902d747c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterTools.cpp
@@ -0,0 +1,179 @@
+#include "ConverterTools.h"
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/algorithm/advanced.h>
+
+#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
+
+namespace armarx::armem::server::robot_state
+{
+
+ std::optional<std::string>
+ exteroception::findByPrefix(const std::string& key, const std::set<std::string>& prefixes)
+ {
+ for (const auto& prefix : prefixes)
+ {
+ if (simox::alg::starts_with(key, prefix))
+ {
+ return prefix;
+ }
+ }
+ return std::nullopt;
+ }
+}
+
+
+namespace armarx::armem::server::robot_state::exteroception
+{
+ ConverterTools::ConverterTools()
+ {
+ {
+ vector3fSetters["X"] = [](Eigen::Vector3f & v, float f)
+ {
+ v.x() = f;
+ };
+ vector3fSetters["Y"] = [](Eigen::Vector3f & v, float f)
+ {
+ v.y() = f;
+ };
+ vector3fSetters["Z"] = [](Eigen::Vector3f & v, float f)
+ {
+ v.z() = f;
+ };
+ vector3fSetters["x"] = vector3fSetters["X"];
+ vector3fSetters["y"] = vector3fSetters["Y"];
+ vector3fSetters["z"] = vector3fSetters["Z"];
+ vector3fSetters["Rotation"] = vector3fSetters["Z"];
+ }
+ {
+ platformPoseGetters["acceleration"] = [](prop::arondto::Platform & p)
+ {
+ return &p.acceleration;
+ };
+ platformPoseGetters["relativePosition"] = [](prop::arondto::Platform & p)
+ {
+ return &p.relativePosition;
+ };
+ platformPoseGetters["velocity"] = [](prop::arondto::Platform & p)
+ {
+ return &p.velocity;
+ };
+ platformIgnored.insert("absolutePosition");
+ }
+ {
+ ftGetters["gravCompF"] = [](prop::arondto::ForceTorque & ft)
+ {
+ return &ft.gravityCompensationForce;
+ };
+ ftGetters["gravCompT"] = [](prop::arondto::ForceTorque & ft)
+ {
+ return &ft.gravityCompensationTorque;
+ };
+ ftGetters["f"] = [](prop::arondto::ForceTorque & ft)
+ {
+ return &ft.force;
+ };
+ ftGetters["t"] = [](prop::arondto::ForceTorque & ft)
+ {
+ return &ft.torque;
+ };
+ }
+ {
+ jointGetters["acceleration"] = [](prop::arondto::Joints & j)
+ {
+ return &j.acceleration;
+ };
+ jointGetters["gravityTorque"] = [](prop::arondto::Joints & j)
+ {
+ return &j.gravityTorque;
+ };
+ jointGetters["inertiaTorque"] = [](prop::arondto::Joints & j)
+ {
+ return &j.inertiaTorque;
+ };
+ jointGetters["inverseDynamicsTorque"] = [](prop::arondto::Joints & j)
+ {
+ return &j.inverseDynamicsTorque;
+ };
+ jointGetters["position"] = [](prop::arondto::Joints & j)
+ {
+ return &j.position;
+ };
+ jointGetters["torque"] = [](prop::arondto::Joints & j)
+ {
+ return &j.torque;
+ };
+ jointGetters["velocity"] = [](prop::arondto::Joints & j)
+ {
+ return &j.velocity;
+ };
+ }
+ {
+
+#define ADD_SCALAR_SETTER(container, name, type) \
+ container [ #name ] = []( \
+ prop::arondto::Joints & dto, \
+ const std::vector<std::string>& split, \
+ const ConverterValue & value) \
+ { \
+ dto. name [split.at(1)] = getValueAs< type >(value); \
+ }
+
+ ADD_SCALAR_SETTER(jointSetters, position, float);
+ ADD_SCALAR_SETTER(jointSetters, velocity, float);
+ ADD_SCALAR_SETTER(jointSetters, acceleration, float);
+
+ ADD_SCALAR_SETTER(jointSetters, relativePosition, float);
+ ADD_SCALAR_SETTER(jointSetters, filteredVelocity, float);
+
+ ADD_SCALAR_SETTER(jointSetters, currentTarget, float);
+ ADD_SCALAR_SETTER(jointSetters, positionTarget, float);
+ ADD_SCALAR_SETTER(jointSetters, velocityTarget, float);
+
+ ADD_SCALAR_SETTER(jointSetters, torque, float);
+ ADD_SCALAR_SETTER(jointSetters, inertiaTorque, float);
+ ADD_SCALAR_SETTER(jointSetters, gravityTorque, float);
+ ADD_SCALAR_SETTER(jointSetters, gravityCompensatedTorque, float);
+ ADD_SCALAR_SETTER(jointSetters, inverseDynamicsTorque, float);
+ ADD_SCALAR_SETTER(jointSetters, torqueTicks, int);
+
+ ADD_SCALAR_SETTER(jointSetters, positionTarget, float);
+ ADD_SCALAR_SETTER(jointSetters, currentTarget, float);
+ ADD_SCALAR_SETTER(jointSetters, positionTarget, float);
+
+ // "temperature" handled below
+ // ADD_SCALAR_SETTER(jointSetters, temperature, float);
+
+ ADD_SCALAR_SETTER(jointSetters, motorCurrent, float);
+ ADD_SCALAR_SETTER(jointSetters, maxTargetCurrent, float);
+
+ ADD_SCALAR_SETTER(jointSetters, sensorBoardUpdateRate, float);
+ ADD_SCALAR_SETTER(jointSetters, I2CUpdateRate, float);
+
+ ADD_SCALAR_SETTER(jointSetters, JointStatusEmergencyStop, bool);
+ ADD_SCALAR_SETTER(jointSetters, JointStatusEnabled, bool);
+ ADD_SCALAR_SETTER(jointSetters, JointStatusError, int);
+ ADD_SCALAR_SETTER(jointSetters, JointStatusOperation, int);
+
+
+#define ADD_VECTOR3_SETTER(container, name, type) \
+ container [ #name ] = [this]( \
+ prop::arondto::Joints & dto, \
+ const std::vector<std::string>& split, \
+ const ConverterValue & value) \
+ { \
+ auto& vec = dto. name [split.at(1)]; \
+ auto& setter = this->vector3fSetters.at(split.at(3)); \
+ setter(vec, getValueAs< type >(value)); \
+ }
+
+ ADD_VECTOR3_SETTER(jointSetters, angularVelocity, float);
+ ADD_VECTOR3_SETTER(jointSetters, linearAcceleration, float);
+
+ // ADD_GETTER(jointVectorGetters, orientation, float);
+ }
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterTools.h b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterTools.h
new file mode 100644
index 0000000000000000000000000000000000000000..7460ebb133a01387c0f29a472961366877254506
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterTools.h
@@ -0,0 +1,106 @@
+#pragma once
+
+#include <map>
+#include <set>
+#include <string>
+
+#include <Eigen/Core>
+
+#include <SimoxUtility/algorithm/string/string_tools.h>
+
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
+
+#include "ConverterInterface.h"
+
+
+namespace armarx::armem::server::robot_state::exteroception
+{
+
+ struct ConverterValue
+ {
+ const RobotUnitDataStreaming::TimeStep& data;
+ const RobotUnitDataStreaming::DataEntry& entry;
+ };
+
+
+ template <class T>
+ T
+ getValueAs(const ConverterValue& value)
+ {
+ return RobotUnitDataStreamingReceiver::GetAs<T>(value.data, value.entry);
+ }
+
+
+ /**
+ * @brief Search
+ * @param key
+ * @param prefixes
+ * @return
+ */
+ std::optional<std::string>
+ findByPrefix(const std::string& key, const std::set<std::string>& prefixes);
+
+
+ template <class ValueT>
+ ValueT
+ findByPrefix(const std::string& key, const std::map<std::string, ValueT>& map)
+ {
+ for (const auto& [prefix, value] : map)
+ {
+ if (simox::alg::starts_with(key, prefix))
+ {
+ return value;
+ }
+ }
+ return nullptr;
+ }
+
+
+ template <class ValueT>
+ ValueT
+ findBySuffix(const std::string& key, const std::map<std::string, ValueT>& map)
+ {
+ for (const auto& [suffix, value] : map)
+ {
+ if (simox::alg::ends_with(key, suffix))
+ {
+ return value;
+ }
+ }
+ return nullptr;
+ }
+
+
+
+ class ConverterTools
+ {
+ public:
+
+ ConverterTools();
+
+
+ public:
+
+ std::map<std::string, std::function<void(Eigen::Vector3f&, float)> > vector3fSetters;
+
+ std::map<std::string, std::function<std::map<std::string, float>*(prop::arondto::Joints&)> > jointGetters;
+ std::map<std::string, std::function<std::map<std::string, Eigen::Vector3f>*(prop::arondto::Joints&)> > jointVectorGetters;
+
+ using JointSetter = std::function<void(prop::arondto::Joints& dto, const std::vector<std::string>& split, const ConverterValue& value)>;
+ std::map<std::string, JointSetter> jointSetters;
+
+ std::map<std::string, std::function<Eigen::Vector3f*(prop::arondto::Platform&)> > platformPoseGetters;
+ std::set<std::string> platformIgnored;
+
+ std::map<std::string, std::function<Eigen::Vector3f*(prop::arondto::ForceTorque&)> > ftGetters;
+
+
+ std::map<std::string, std::string> sidePrefixMap
+ {
+ { "R", "Right" },
+ { "L", "Left" },
+ };
+
+ };
+}
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 52de49d1420a9bf194cf4071b4f23ea8e2944ad6..847ec98c82966408545a50bf9d7fa93fc7081d9f 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
@@ -29,6 +29,7 @@
#include <SimoxUtility/math/convert/rpy_to_mat3f.h>
// ArmarX
+#include "ArmarXCore/core/logging/Logging.h"
#include "ArmarXCore/core/time/Metronome.h"
#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
@@ -87,8 +88,11 @@ namespace armarx::armem::server::robot_state::proprioception
// Commits lock the core segments.
- // Proprioception
- armem::CommitResult result = memory.commitLocking(update.proprioception);
+ // Proprioception + Exteroception
+ armem::CommitResult resultProprioception = memory.commitLocking(update.proprioception);
+
+ ARMARX_DEBUG << deactivateSpam(1) << VAROUT(update.exteroception.updates.size());
+ armem::CommitResult resultExteroception = memory.commitLocking(update.exteroception);
endProprioception = std::chrono::high_resolution_clock::now();
// Localization
@@ -100,11 +104,18 @@ namespace armarx::armem::server::robot_state::proprioception
}
endLocalization = std::chrono::high_resolution_clock::now();
- if (not result.allSuccess())
+ if (not resultProprioception.allSuccess())
+ {
+ ARMARX_WARNING << "Could not commit data to proprioception segment in memory. Error message: "
+ << resultProprioception.allErrorMessages();
+ }
+
+ if (not resultExteroception.allSuccess())
{
- ARMARX_WARNING << "Could not commit data to memory. Error message: "
- << result.allErrorMessages();
+ ARMARX_WARNING << "Could not commit data to exteroception segment in memory. Error message: "
+ << resultExteroception.allErrorMessages();
}
+
if (debugObserver)
{
auto end = std::chrono::high_resolution_clock::now();
@@ -143,15 +154,27 @@ namespace armarx::armem::server::robot_state::proprioception
// Send batch to memory
Update update;
- {
+ if(data.proprioception){
armem::EntityUpdate& up = update.proprioception.add();
up.entityID = properties.robotUnitProviderID.withEntityName(
properties.robotUnitProviderID.providerSegmentName);
+ up.entityID.coreSegmentName =::armarx::armem::robot_state::constants::proprioceptionCoreSegment;
up.timeCreated = data.timestamp;
up.instancesData = {data.proprioception};
}
+ // Exteroception
+ if(data.exteroception){
+ armem::EntityUpdate& up = update.exteroception.add();
+ up.entityID = properties.robotUnitProviderID.withEntityName(
+ properties.robotUnitProviderID.providerSegmentName);
+ up.entityID.coreSegmentName = ::armarx::armem::robot_state::constants::exteroceptionCoreSegment;
+ up.timeCreated = data.timestamp;
+ up.instancesData = {data.exteroception};
+ }
+
// Extract odometry data.
+ ARMARX_CHECK_NOT_NULL(data.proprioception);
const std::string platformKey = "platform";
if (data.proprioception->hasElement(platformKey))
{
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 f911d43fc7fabf6626c5e6e230cf74a66753d1cf..78863697a61ecd20c4947188a04965d5990c6f7b 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
@@ -71,6 +71,7 @@ namespace armarx::armem::server::robot_state::proprioception
struct Update
{
armem::Commit proprioception;
+ armem::Commit exteroception;
std::vector<armem::robot_state::Transform> localization;
};
Update buildUpdate(const RobotUnitData& dataQueue);
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h
index f62d0b0c9c078185fe7959a229956f5f67b4afdc..8d904f249029cf9fd5ce010657cb4324f3ba5551 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h
@@ -14,6 +14,7 @@ namespace armarx::armem::server::robot_state::proprioception
{
Time timestamp;
aron::data::DictPtr proprioception;
+ aron::data::DictPtr exteroception;
};
using Queue = boost::sync_queue<RobotUnitData>;
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp
index c74f355c247eb6fca2ca348b5459527aef204260..53021dc7d88f9362a967f395b1381d07934f271e 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp
@@ -24,15 +24,23 @@ namespace armarx::armem::server::robot_state::proprioception
const std::string& robotTypeName)
{
{
- converter = converterRegistry.get(robotTypeName);
- ARMARX_CHECK_NOT_NULL(converter)
+ converterProprioception = proprioceptionConverterRegistry.get(robotTypeName);
+ ARMARX_CHECK_NOT_NULL(converterProprioception)
<< "No converter for robot type '" << robotTypeName << "' available. \n"
- << "Known are: " << converterRegistry.getKeys();
+ << "Known are: " << proprioceptionConverterRegistry.getKeys();
config.loggingNames.push_back(properties.sensorPrefix);
receiver = robotUnitPlugin.startDataStreaming(config);
description = receiver->getDataDescription();
}
+
+ {
+ converterExteroception = exteroceptionConverterRegistry.get(robotTypeName);
+ ARMARX_CHECK_NOT_NULL(converterProprioception)
+ << "No converter for robot type '" << robotTypeName << "' available. \n"
+ << "Known are: " << exteroceptionConverterRegistry.getKeys();
+ }
+
{
// Thread-local copy of debug observer helper.
debugObserver = DebugObserverHelper(
@@ -67,7 +75,7 @@ namespace armarx::armem::server::robot_state::proprioception
std::optional<RobotUnitData>
RobotUnitReader::fetchAndConvertLatestRobotUnitData()
{
- ARMARX_CHECK_NOT_NULL(converter);
+ ARMARX_CHECK_NOT_NULL(converterProprioception);
const std::optional<RobotUnitDataStreaming::TimeStep> data = fetchLatestData();
if (not data.has_value())
@@ -79,7 +87,17 @@ namespace armarx::armem::server::robot_state::proprioception
auto start = std::chrono::high_resolution_clock::now();
RobotUnitData result;
- result.proprioception = converter->convert(data.value(), description);
+
+ if(converterProprioception != nullptr)
+ {
+ result.proprioception = converterProprioception->convert(data.value(), description);
+ }
+
+ if(converterExteroception != nullptr)
+ {
+ result.exteroception = converterExteroception->convert(data.value(), description);
+ }
+
result.timestamp = Time(Duration::MicroSeconds(data->timestampUSec));
auto stop = std::chrono::high_resolution_clock::now();
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.h
index cc3cd27581951a0365cf028ff2bf2d7929568f1d..88313402f217eb8f46470aafd796465354f6cc68 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.h
@@ -15,7 +15,9 @@
#include "RobotUnitData.h"
#include "converters/ConverterInterface.h"
+#include "../exteroception/converters/ConverterInterface.h"
#include "converters/ConverterRegistry.h"
+#include "../exteroception/converters/ConverterRegistry.h"
namespace armarx::plugins
@@ -28,6 +30,8 @@ namespace armarx
using RobotUnitDataStreamingReceiverPtr = std::shared_ptr<class RobotUnitDataStreamingReceiver>;
}
+
+// TODO move this out of proprioception. it is now for proprioception and exteroception
namespace armarx::armem::server::robot_state::proprioception
{
class RobotUnitReader : public armarx::Logging
@@ -68,8 +72,10 @@ namespace armarx::armem::server::robot_state::proprioception
RobotUnitDataStreamingReceiverPtr receiver;
RobotUnitDataStreaming::DataStreamingDescription description;
- ConverterRegistry converterRegistry;
- ConverterInterface* converter = nullptr;
+ ConverterRegistry proprioceptionConverterRegistry;
+ exteroception::ConverterRegistry exteroceptionConverterRegistry;
+ proprioception::ConverterInterface* converterProprioception = nullptr;
+ exteroception::ConverterInterface* converterExteroception = nullptr;
armarx::SimpleRunningTask<>::pointer_type task = nullptr;
};
diff --git a/source/RobotAPI/libraries/aron/CMakeLists.txt b/source/RobotAPI/libraries/aron/CMakeLists.txt
index ce4f7bc99cb8d923decfdd7fdb79390837e79150..e1ccc8560ff75b018fbc958d0f1febf0512d9590 100644
--- a/source/RobotAPI/libraries/aron/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/CMakeLists.txt
@@ -2,3 +2,4 @@ add_subdirectory(core)
add_subdirectory(converter)
add_subdirectory(codegeneration)
add_subdirectory(common)
+add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/aron/core/aron_conversions.h b/source/RobotAPI/libraries/aron/core/aron_conversions.h
index d0f88cab840be59b86bbeb22d7229afb9f9f0b48..d27d6415877d4c15f9ddf36310e656590f8efd7e 100644
--- a/source/RobotAPI/libraries/aron/core/aron_conversions.h
+++ b/source/RobotAPI/libraries/aron/core/aron_conversions.h
@@ -3,12 +3,29 @@
#include <map>
#include <memory>
#include <optional>
+#include <type_traits>
#include <vector>
#include "Path.h"
namespace armarx::aron
{
+
+namespace detail
+{
+
+ // Helper concept to avoid ambiguities
+ template<typename DtoT, typename BoT>
+ concept DtoAndBoAreSame = std::is_same<DtoT, BoT>::value;
+
+ template <class ...>
+ struct is_optional : public std::false_type {};
+
+ template <class ...Ts>
+ struct is_optional<std::optional<Ts...>> : public std::true_type {};
+
+}
+
/**
* Framework for converting ARON DTOs (Data Transfer Objects) to C++ BOs
* (Business Objects) and back.
@@ -51,12 +68,6 @@ namespace armarx::aron
* }
* @endcode
*/
-
- // Helper concept to avoid ambiguities
- template<typename DtoT, typename BoT>
- concept DtoAndBoAreSame = std::is_same<DtoT, BoT>::value;
-
-
// Same type
template <class T>
void toAron(T& dto, const T& bo)
@@ -139,6 +150,24 @@ namespace armarx::aron
}
}
+
+ // One-sided optional
+ template <class DtoT, class BoT>
+ requires (not detail::is_optional<BoT>::value)
+ void toAron(std::optional<DtoT>& dto, const BoT& bo)
+ {
+ dto = DtoT{};
+ toAron(*dto, bo);
+ }
+ template <class DtoT, class BoT>
+ requires (not detail::is_optional<DtoT>::value)
+ void fromAron(DtoT& dto, const std::optional<BoT>& bo)
+ {
+ bo = BoT{};
+ fromAron(dto, *bo);
+ }
+
+
// Flag-controlled optional
template <class DtoT, class BoT>
void toAron(DtoT& dto, bool& dtoValid, const BoT& bo, bool boValid)
@@ -154,6 +183,7 @@ namespace armarx::aron
}
}
+
template <class DtoT, class BoT>
void fromAron(const DtoT& dto, bool dtoValid, BoT& bo, bool& boValid)
{
@@ -222,7 +252,7 @@ namespace armarx::aron
// std::map
template <class DtoKeyT, class DtoValueT, class BoKeyT, class BoValueT>
- requires (!(DtoAndBoAreSame<DtoKeyT, BoKeyT> and DtoAndBoAreSame<DtoValueT, BoValueT>))
+ requires (!(detail::DtoAndBoAreSame<DtoKeyT, BoKeyT> and detail::DtoAndBoAreSame<DtoValueT, BoValueT>))
void toAron(std::map<DtoKeyT, DtoValueT>& dtoMap,
const std::map<BoKeyT, BoValueT>& boMap)
{
@@ -236,7 +266,7 @@ namespace armarx::aron
}
}
template <class DtoKeyT, class DtoValueT, class BoKeyT, class BoValueT>
- requires (!(DtoAndBoAreSame<DtoKeyT, BoKeyT> and DtoAndBoAreSame<DtoValueT, BoValueT>))
+ requires (!(detail::DtoAndBoAreSame<DtoKeyT, BoKeyT> and detail::DtoAndBoAreSame<DtoValueT, BoValueT>))
void fromAron(const std::map<DtoKeyT, DtoValueT>& dtoMap,
std::map<BoKeyT, BoValueT>& boMap)
{
@@ -252,7 +282,7 @@ namespace armarx::aron
template <class DtoKeyT, class DtoValueT, class BoKeyT, class BoValueT>
- requires (!(DtoAndBoAreSame<DtoKeyT, BoKeyT> and DtoAndBoAreSame<DtoValueT, BoValueT>))
+ requires (!(detail::DtoAndBoAreSame<DtoKeyT, BoKeyT> and detail::DtoAndBoAreSame<DtoValueT, BoValueT>))
std::map<DtoKeyT, DtoValueT> toAron(const std::map<BoKeyT, BoValueT>& boMap)
{
std::map<DtoKeyT, DtoValueT> dtoMap;
@@ -343,20 +373,20 @@ namespace armarx
// std::vector
template <class DtoT, class BoT>
- requires (!aron::DtoAndBoAreSame<DtoT, BoT>)
+ requires (!aron::detail::DtoAndBoAreSame<DtoT, BoT>)
void toAron(std::vector<DtoT>& dtos, const std::vector<BoT>& bos)
{
armarx::aron::toAron(dtos, bos);
}
template <class DtoT, class BoT>
- requires (!aron::DtoAndBoAreSame<DtoT, BoT>)
+ requires (!aron::detail::DtoAndBoAreSame<DtoT, BoT>)
void fromAron(const std::vector<DtoT>& dtos, std::vector<BoT>& bos)
{
armarx::aron::fromAron(dtos, bos);
}
template <class DtoT, class BoT>
- requires (!aron::DtoAndBoAreSame<DtoT, BoT>)
+ requires (!aron::detail::DtoAndBoAreSame<DtoT, BoT>)
std::vector<DtoT> toAron(const std::vector<BoT>& bos)
{
return armarx::aron::toAron<DtoT, BoT>(bos);
@@ -365,13 +395,13 @@ namespace armarx
// std::map
template <class DtoKeyT, class DtoValueT, class BoKeyT, class BoValueT>
- requires (!(aron::DtoAndBoAreSame<DtoKeyT, BoKeyT> and aron::DtoAndBoAreSame<DtoValueT, BoValueT>))
+ requires (!(aron::detail::DtoAndBoAreSame<DtoKeyT, BoKeyT> and aron::detail::DtoAndBoAreSame<DtoValueT, BoValueT>))
void toAron(std::map<DtoKeyT, DtoValueT>& dtoMap, const std::map<BoKeyT, BoValueT>& boMap)
{
armarx::aron::toAron(dtoMap, boMap);
}
template <class DtoKeyT, class DtoValueT, class BoKeyT, class BoValueT>
- requires (!(aron::DtoAndBoAreSame<DtoKeyT, BoKeyT> and aron::DtoAndBoAreSame<DtoValueT, BoValueT>))
+ requires (!(aron::detail::DtoAndBoAreSame<DtoKeyT, BoKeyT> and aron::detail::DtoAndBoAreSame<DtoValueT, BoValueT>))
void fromAron(const std::map<DtoKeyT, DtoValueT>& dtoMap, std::map<BoKeyT, BoValueT>& boMap)
{
armarx::aron::fromAron(dtoMap, boMap);
@@ -379,7 +409,7 @@ namespace armarx
template <class DtoKeyT, class DtoValueT, class BoKeyT, class BoValueT>
- requires (!(aron::DtoAndBoAreSame<DtoKeyT, BoKeyT> and aron::DtoAndBoAreSame<DtoValueT, BoValueT>))
+ requires (!(aron::detail::DtoAndBoAreSame<DtoKeyT, BoKeyT> and aron::detail::DtoAndBoAreSame<DtoValueT, BoValueT>))
std::map<DtoKeyT, DtoValueT> toAron(const std::map<BoKeyT, BoValueT>& boMap)
{
armarx::aron::toAron<DtoKeyT, DtoValueT, BoKeyT, BoValueT>(boMap);
diff --git a/source/RobotAPI/libraries/aron/core/rw.h b/source/RobotAPI/libraries/aron/core/rw.h
index 6257f2422687591b0991afed8a0577ee127207ef..95b6aac606f47d3c46de34ac5ec077ded28c41a4 100644
--- a/source/RobotAPI/libraries/aron/core/rw.h
+++ b/source/RobotAPI/libraries/aron/core/rw.h
@@ -26,7 +26,7 @@ namespace armarx::aron
}
template<class ReaderT, class DtoT, class BoT>
- requires (data::isReader<ReaderT> && !DtoAndBoAreSame<DtoT, BoT>)
+ requires (data::isReader<ReaderT> && !detail::DtoAndBoAreSame<DtoT, BoT>)
inline void read(ReaderT& aron_r, typename ReaderT::InputType& input, BoT& ret)
{
DtoT aron;
@@ -36,7 +36,7 @@ namespace armarx::aron
}
template<class WriterT, class DtoT, class BoT>
- requires (data::isWriter<WriterT> && !DtoAndBoAreSame<DtoT, BoT>)
+ requires (data::isWriter<WriterT> && !detail::DtoAndBoAreSame<DtoT, BoT>)
inline void write(WriterT& aron_w, const BoT& input, typename WriterT::ReturnType& ret, const armarx::aron::Path& aron_p = armarx::aron::Path())
{
DtoT aron;
diff --git a/source/RobotAPI/libraries/aron/test/AronConversionTester.cpp b/source/RobotAPI/libraries/aron/test/AronConversionTester.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..aac543a04b6ba1696dedc7a21c7d4ea125896e4e
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/test/AronConversionTester.cpp
@@ -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/>.
+ *
+ * @package RobotAPI::ArmarXObjects::aron_cpp_to_python_conv_test
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2023
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#include "AronConversionTester.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+
+namespace armarx::aron::test
+{
+
+ AronConversionTester::AronConversionTester(dti::AronConversionTestInterfacePrx python) : interface(python)
+ {
+ ARMARX_CHECK(python);
+ }
+
+
+} // namespace armarx::aron::test
diff --git a/source/RobotAPI/libraries/aron/test/AronConversionTester.h b/source/RobotAPI/libraries/aron/test/AronConversionTester.h
new file mode 100644
index 0000000000000000000000000000000000000000..b7ad4b8dfdcf55a37f8efc3116c27440d925d428
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/test/AronConversionTester.h
@@ -0,0 +1,111 @@
+/**
+ * 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/>.
+ *
+ * @package RobotAPI::ArmarXObjects::aron_cpp_to_python_conv_test
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2023
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#pragma once
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <RobotAPI/interface/aron/test/AronConversionTestInterface.h>
+
+
+namespace armarx::aron::test
+{
+
+ /**
+ * @brief Helper class for implementing distributed ARON conversion tests
+ * based on the `dti::AronConversionTestInterfacePrx`.
+ *
+ * Example usage:
+ *
+ * @code
+ *
+ * armarx::aron::test::dti::AronConversionTestInterfacePrx pythonComponent = ...;
+ *
+ * auto myAronClassProbeFn = []()
+ * {
+ * my::arondto::MyAronClass probe;
+ * probe.data = "42";
+ * return probe,
+ * };
+ *
+ * armarx::aron::test::AronConversionTester tester(pythonComponent);
+ *
+ * tester.test<my::arondto::MyAronClass>(myAronClassProbeFn, "MyAronClass");
+ *
+ * @endcode
+ *
+ * Note that the `pythonComponent` must point to a corresponding
+ * implementation of the `AronConversionTestInterfacePrx`.
+ */
+ class AronConversionTester
+ {
+ public:
+ template <class AronClassT>
+ using ProbeFn = std::function<AronClassT()>;
+
+
+ public:
+ AronConversionTester(dti::AronConversionTestInterfacePrx interface);
+
+ /**
+ * @brief Test the conversion of a specific ARON class.
+ *
+ * @param probeFn A factory function creating a test instance of the
+ * ARON class.
+ * @param aronClassName The name of the ARON class. Can be used by the
+ * other component to decide which class to convert to.
+ */
+ template <class AronClassT>
+ void
+ test(ProbeFn<AronClassT> probeFn, const std::string& aronClassName)
+ {
+ std::stringstream ss;
+ ss << "Test for ARON class '" << aronClassName << "': ";
+
+ const AronClassT probe = probeFn();
+
+ dto::TestAronConversionRequest req;
+ req.aronClassName = aronClassName;
+ req.probe = probe.toAronDTO();
+
+ dto::TestAronConversionResponse res = interface->testAronConversion(req);
+
+ if (res.success)
+ {
+ const AronClassT probeOut = AronClassT::FromAron(res.probe);
+ ARMARX_CHECK(probeOut == probe);
+
+ ARMARX_IMPORTANT << ss.str() << "Success";
+ }
+ else
+ {
+ ARMARX_WARNING << ss.str() << "Conversion in Python component failed: \n" << res.errorMessage;
+ }
+ }
+
+ public:
+ dti::AronConversionTestInterfacePrx interface;
+ };
+
+} // namespace armarx::aron::test
diff --git a/source/RobotAPI/libraries/aron/test/CMakeLists.txt b/source/RobotAPI/libraries/aron/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..3a199acf0f19c13f6253e66947e540bfe3950efd
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/test/CMakeLists.txt
@@ -0,0 +1,25 @@
+set(LIB_NAME arontest)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+
+armarx_add_library(
+ LIBS
+ # ArmarXCore
+ ArmarXCore
+ # RobotAPI
+ RobotAPICore
+ RobotAPIInterfaces
+ aron
+
+ HEADERS
+ AronConversionTester.h
+
+ SOURCES
+ AronConversionTester.cpp
+)
+
+
+add_library(aron::test ALIAS arontest)
+add_library(${PROJECT_NAME}::Aron::test ALIAS arontest)