diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
index c89a51ae47158d0c646a63f216f0a4ff397ba5d6..5a9f0e2e32e3f12ba9cf5ee9ba49c1b496fb09eb 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
@@ -90,11 +90,6 @@ namespace armarx::armem::server::robot_state
private:
- /// Reads from `robotUnitDataQueue` and commits into memory.
- //void convertRobotUnitStreamingDataToAronTask();
- /// Reads from `robotUnitDataQueue` and commits into memory.
- //void storeConvertedRobotUnitDataInMemoryTask();
-
void startRobotUnitStream();
void stopRobotUnitStream();
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateWriter.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateWriter.cpp
index 39439e2e77bbd8c3ebea44782ae44d62286c6ed9..39f9bc05bbab144befe114dc9944630cfda76a03 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateWriter.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateWriter.cpp
@@ -65,9 +65,12 @@ namespace armarx::armem::server::robot_state
}
- void RobotStateWriter::run(float pollFrequency,
- std::queue<RobotUnitData>& dataQueue, std::mutex& dataMutex,
- MemoryToIceAdapter& memory, localization::Segment& localizationSegment)
+ void RobotStateWriter::run(
+ float pollFrequency,
+ std::queue<RobotUnitData>& dataQueue,
+ std::mutex& dataMutex,
+ MemoryToIceAdapter& memory,
+ localization::Segment& localizationSegment)
{
CycleUtil cycle(static_cast<int>(1000.f / pollFrequency));
while (task and not task->isStopped())
@@ -86,7 +89,7 @@ namespace armarx::armem::server::robot_state
{
debugObserver->setDebugObserverDatafield("RobotStateWriter | Queue Size", queueSize);
}
- if (batch.size() > 0)
+ if (not batch.empty())
{
Update update = buildUpdate(batch);
@@ -126,7 +129,7 @@ namespace armarx::armem::server::robot_state
}
- RobotStateWriter::Update RobotStateWriter::buildUpdate(std::queue<RobotUnitData> dataQueue)
+ RobotStateWriter::Update RobotStateWriter::buildUpdate(std::queue<RobotUnitData>& dataQueue)
{
ARMARX_CHECK_GREATER(dataQueue.size(), 0);
@@ -138,32 +141,21 @@ namespace armarx::armem::server::robot_state
while (dataQueue.size() > 0)
{
- const RobotUnitData& convertedAndGroupedData = dataQueue.front();
+ const RobotUnitData& data = dataQueue.front();
- for (const auto& [encName, encTimestep] : convertedAndGroupedData.groups)
- {
- ARMARX_CHECK_EQUAL(encName, encTimestep.name);
-
- armem::EntityUpdate& up = update.proprioception.add();
- up.entityID = properties.robotUnitProviderID.withEntityName(encName);
- up.timeCreated = encTimestep.timestamp;
- up.instancesData = { encTimestep.data };
- }
+ armem::EntityUpdate& up = update.proprioception.add();
+ up.entityID = properties.robotUnitProviderID.withEntityName(properties.robotUnitProviderID.providerSegmentName);
+ up.timeCreated = data.timestamp;
+ up.instancesData = { data.proprioception.toAron() };
// odometry pose -> localization segment
- auto it = convertedAndGroupedData.groups.find(properties.platformGroupName);
- if (it != convertedAndGroupedData.groups.end())
{
ARMARX_DEBUG << "Found odometry data.";
-
- const RobotUnitData::RobotUnitDataGroup& timestep = it->second;
- const float relPosX = aron::datanavigator::FloatNavigator::DynamicCast(timestep.data->getElement("relativePositionX"))->getValue();
- const float relPosY = aron::datanavigator::FloatNavigator::DynamicCast(timestep.data->getElement("relativePositionY"))->getValue();
- const float relOrientation = aron::datanavigator::FloatNavigator::DynamicCast(timestep.data->getElement("relativePositionRotation"))->getValue();
+ const Eigen::Vector3f& relPose = data.proprioception.platform.relativePosition;
Eigen::Affine3f odometryPose = Eigen::Affine3f::Identity();
- odometryPose.translation() << relPosX, relPosY, 0; // TODO set height
- odometryPose.linear() = simox::math::rpy_to_mat3f(0.F, 0.F, relOrientation);
+ odometryPose.translation() << relPose.x(), relPose.y(), 0; // TODO set height
+ odometryPose.linear() = simox::math::rpy_to_mat3f(0.F, 0.F, relPose.z());
// const auto timestamp = armem::Time::microSeconds(it->second.timestamp);
@@ -171,15 +163,18 @@ namespace armarx::armem::server::robot_state
transform.header.parentFrame = armarx::OdometryFrame;
transform.header.frame = "root"; // TODO: robot root node
transform.header.agent = properties.robotUnitProviderID.providerSegmentName;
- transform.header.timestamp = it->second.timestamp;
+ transform.header.timestamp = data.timestamp;
transform.transform = odometryPose;
}
+ // ToDo: Detect that now odometry data was send
+ /*
else if (!noOdometryDataLogged)
{
ARMARX_INFO << "No odometry data! (No group with name '" << properties.platformGroupName << "'.). \n"
<< "If you are using a mobile platform this should not have happened.";
noOdometryDataLogged = true;
}
+ */
dataQueue.pop();
}
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateWriter.h b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateWriter.h
index 93ba74d2d9d622f4d72d1d5a4d08acd97f3eb64b..319a76893aeb7747e906c41f6ea2ea291799c692 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateWriter.h
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateWriter.h
@@ -70,7 +70,7 @@ namespace armarx::armem::server::robot_state
armem::Commit proprioception;
armem::robot_state::Transform localization;
};
- Update buildUpdate(std::queue<RobotUnitData> dataQueue);
+ Update buildUpdate(std::queue<RobotUnitData>& dataQueue);
public:
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitData.h b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitData.h
index dc43dcddca3eb693fe5200dcc0a256a15be23cac..5e5f810feb8e2ea63ce1deb563cd09494c481db0 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitData.h
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitData.h
@@ -1,30 +1,16 @@
#pragma once
-#include <map>
-#include <memory>
-#include <string>
-
#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
-namespace armarx::aron::datanavigator
-{
- using DictNavigatorPtr = std::shared_ptr<class DictNavigator>;
-}
-
namespace armarx::armem::server::robot_state
{
struct RobotUnitData
{
- struct RobotUnitDataGroup
- {
- armem::Time timestamp;
- std::string name;
- aron::datanavigator::DictNavigatorPtr data;
- };
-
- std::map<std::string, RobotUnitDataGroup> groups;
+ armem::Time timestamp;
+ arondto::Proprioception proprioception;
};
}
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitReader.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitReader.cpp
index 58b688557eb86e8efb28afd0329d8f3b50e8508c..b3c7b49baa8ff207034f8b51f054c0f6f0d50610 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitReader.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitReader.cpp
@@ -1,11 +1,14 @@
#include "RobotUnitReader.h"
#include "aron_conversions.h"
+#include "Armar6RobotUnitConverter.h"
#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
#include <RobotAPI/libraries/aron/core/navigator/data/primitive/Long.h>
#include <RobotAPI/libraries/aron/core/navigator/data/primitive/String.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+
#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
#include <ArmarXCore/core/time/CycleUtil.h>
@@ -60,6 +63,12 @@ namespace armarx::armem::server::robot_state
}
+ RobotUnitReader::RobotUnitReader() :
+ converter(new Armar6RobotUnitConverter())
+ {
+ }
+
+
void RobotUnitReader::connect(
armarx::plugins::RobotUnitComponentPlugin& robotUnitPlugin,
armarx::plugins::DebugObserverComponentPlugin& debugObserverPlugin)
@@ -86,10 +95,10 @@ namespace armarx::armem::server::robot_state
CycleUtil cycle(static_cast<int>(1000.f / pollFrequency));
while (task and not task->isStopped())
{
- if (std::optional<RobotUnitData> data = fetchAndGroupLatestRobotUnitData())
+ if (std::optional<RobotUnitData> commit = fetchAndConvertLatestRobotUnitData())
{
std::lock_guard g{dataMutex};
- dataQueue.push(data.value());
+ dataQueue.push(std::move(commit.value()));
}
if (debugObserver)
@@ -101,7 +110,7 @@ namespace armarx::armem::server::robot_state
}
- std::optional<RobotUnitData> RobotUnitReader::fetchAndGroupLatestRobotUnitData()
+ std::optional<RobotUnitData> RobotUnitReader::fetchAndConvertLatestRobotUnitData()
{
const std::optional<RobotUnitDataStreaming::TimeStep> data = fetchLatestData();
if (not data.has_value())
@@ -109,71 +118,23 @@ namespace armarx::armem::server::robot_state
return std::nullopt;
}
- ARMARX_DEBUG << "RobotUnitData: Generating new aron map for current timestep";
+ ARMARX_DEBUG << "RobotUnitReader: Converting data current timestep to commit";
auto start = std::chrono::high_resolution_clock::now();
- RobotUnitData convertedAndGroupedData;
- for (const auto& [nameEntry, dataEntry] : description.entries)
- {
- std::string name = nameEntry;
- std::string jointOrWhateverName;
-
- std::string groupName = "";
- if (auto it = configSensorMapping.find(name); it != configSensorMapping.end())
- {
- groupName = it->second;
- }
- else
- {
- const size_t first_dot_pos = name.find(".");
- const size_t second_dot_pos = name.find(".", first_dot_pos + 1); // find second occurence of "."
- if (second_dot_pos == std::string::npos)
- {
- ARMARX_WARNING << "Could not find a groupname for the sensor with name " << name << ". "
- << "All sensors must be called sens.X.Y where X is the name of the group "
- << "and Y is the actual sensor. Ignoring this sensor.";
- continue;
- }
- groupName = name.substr(0, second_dot_pos);
-
- jointOrWhateverName = name.substr(first_dot_pos + 1, second_dot_pos - first_dot_pos - 1);
-
- name = name.substr(second_dot_pos + 1); // remove the groupName, TODO check if +1 is valid
- }
-
- auto it = convertedAndGroupedData.groups.find(groupName);
- if (it == convertedAndGroupedData.groups.end())
- {
- namespace adn = aron::datanavigator;
-
- // Generate new dict for the group
- RobotUnitData::RobotUnitDataGroup group;
- group.timestamp = armem::Time::microSeconds(data->timestampUSec);
- group.name = groupName;
- group.data = std::make_shared<adn::DictNavigator>();
-
- group.data->addElement("EncoderGroupName", std::make_shared<adn::StringNavigator>(groupName));
- group.data->addElement("IterationId", std::make_shared<adn::LongNavigator>(data->iterationId));
- group.data->addElement("name", std::make_shared<adn::StringNavigator>(jointOrWhateverName));
-
- auto r = convertedAndGroupedData.groups.emplace(groupName, group);
- it = r.first;
- }
-
- RobotUnitData::RobotUnitDataGroup& group = it->second;
- group.data->addElement(name, RobotUnitDataStreaming::toAron(data.value(), dataEntry));
- }
+ RobotUnitData result;
+ result.proprioception = converter->convert(data.value(), description);
+ result.timestamp = Time::microSeconds(data->timestampUSec);;
auto stop = std::chrono::high_resolution_clock::now();
auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
- ARMARX_DEBUG << "RobotUnitData: The total time needed to convert the data to aron is: " << duration;
+ ARMARX_DEBUG << "RobotUnitReader: The total time needed to convert the data to commit s: " << duration;
if (debugObserver)
{
debugObserver->setDebugObserverDatafield("RobotUnitReader | t Read+Group [ms]", duration.count() / 1000.f);
}
- return convertedAndGroupedData;
+ return result;
}
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitReader.h b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitReader.h
index a44c9c0f19a586f48b3f899ae314ca70cc223abe..30bb083bcdb701c5aa73223cb73e03488733e6b8 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitReader.h
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitReader.h
@@ -10,11 +10,11 @@
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
#include <ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h>
-
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h>
#include "RobotUnitData.h"
+#include "RobotUnitConverterInterface.h"
namespace armarx::plugins
@@ -29,6 +29,7 @@ namespace armarx
namespace armarx::armem::server::robot_state
{
+ class Armar6RobotUnitConverter;
class RobotUnitReader : public armarx::Logging
{
@@ -39,17 +40,20 @@ namespace armarx::armem::server::robot_state
public:
+ RobotUnitReader();
+
+
void connect(armarx::plugins::RobotUnitComponentPlugin& robotUnitPlugin,
armarx::plugins::DebugObserverComponentPlugin& debugObserverPlugin);
- /// Reads data from `handler` and fills `robotUnitDataQueue`.
+ /// Reads data from `handler` and fills `dataQueue`.
void run(float pollFrequency,
std::queue<RobotUnitData>& dataQueue,
std::mutex& dataMutex);
- std::optional<RobotUnitData> fetchAndGroupLatestRobotUnitData();
+ std::optional<RobotUnitData> fetchAndConvertLatestRobotUnitData();
private:
@@ -75,6 +79,7 @@ namespace armarx::armem::server::robot_state
/// RobotUnit name -> group name.
std::map<std::string, std::string> configSensorMapping;
+ std::unique_ptr<RobotUnitConverterInterface> converter;
armarx::SimpleRunningTask<>::pointer_type task = nullptr;
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml b/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml
index 38eb6bb439197032cac8968a0dbee131eec8efbc..c3fb3d182e6110cc6cf8e6a5beb59ca19dbab80b 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml
+++ b/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml
@@ -43,7 +43,7 @@ Robot proprioception.
</Dict>
</ObjectChild>
- <ObjectChild key="inserveDynamicsTorques">
+ <ObjectChild key="inverseDynamicsTorques">
<Dict>
<Float />
</Dict>
@@ -54,26 +54,25 @@ Robot proprioception.
<Object name="armarx::armem::prop::arondto::Platform">
- <ObjectChild key="velocity">
+ <ObjectChild key="relativePosition">
<EigenMatrix rows="3" cols="1" type="float" />
</ObjectChild>
-
- <ObjectChild key="acceleration">
+
+ <ObjectChild key="absolutePosition">
<EigenMatrix rows="3" cols="1" type="float" />
</ObjectChild>
-
- <ObjectChild key="rot_velocity">
+
+ <ObjectChild key="velocity">
<EigenMatrix rows="3" cols="1" type="float" />
</ObjectChild>
-
- <ObjectChild key="rot_acceleration">
+
+ <ObjectChild key="acceleration">
<EigenMatrix rows="3" cols="1" type="float" />
</ObjectChild>
-
+
</Object>
-
<Object name="armarx::armem::prop::arondto::ForceTorque">
<ObjectChild key="force">
@@ -96,8 +95,12 @@ Robot proprioception.
- <Object name="armarx::armem::prop::arondto::Proprioception">
+ <Object name="armarx::armem::arondto::Proprioception">
+ <ObjectChild key="iterationID">
+ <long />
+ </ObjectChild>
+
<ObjectChild key="joints">
<armarx::armem::prop::arondto::Joints/>
</ObjectChild>
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
index de880c85df95dd8e0238defcce0a1831ddbbc0c9..9f2fadcf5b12906a63de4a08d05ce4b40ea33419 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
@@ -22,15 +22,16 @@
#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/types.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
-
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
namespace armarx::armem::server::robot_state::proprioception
{
Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
- Base(memoryToIceAdapter, "Proprioception", nullptr, 1024)
+ Base(memoryToIceAdapter, "Proprioception", arondto::Proprioception::toAronType(), 1024)
{
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
index cca0b35a0e179a15880bc38a08a1f1f20ce9f42f..708f252f79f1207a3b1219d71ab48b4f2f3edff9 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
@@ -22,20 +22,18 @@
#pragma once
// STD / STL
-#include <string>
#include <optional>
+#include <string>
#include <unordered_map>
// ArmarX
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+// RobotAPI
#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/server/segment/Segment.h>
-#include <RobotAPI/libraries/armem_objects/types.h>
-// Aron
-#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
namespace armarx
{