diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
index 45814a74f861e11701cee7db3463e62ce43bae27..aed5fec8081906601372f4851afeb44d25f82507 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
@@ -27,26 +27,24 @@
#include <ArmarXCore/core/Component.h>
+#include <RobotAPI/interface/core/RobotLocalization.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h>
-
+#include <RobotAPI/libraries/armem_robot_state/server/common/Visu.h>
#include <RobotAPI/libraries/armem_robot_state/server/description/Segment.h>
#include <RobotAPI/libraries/armem_robot_state/server/localization/Segment.h>
-#include <RobotAPI/libraries/armem_robot_state/server/common/Visu.h>
-#include <RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h>
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h>
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h>
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.h>
-
-#include <RobotAPI/interface/core/RobotLocalization.h>
+#include <RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h>
namespace armarx::plugins
{
class DebugObserverComponentPlugin;
class RobotUnitComponentPlugin;
-}
+} // namespace armarx::plugins
namespace armarx::armem::server::robot_state
{
@@ -68,7 +66,6 @@ namespace armarx::armem::server::robot_state
virtual public armarx::GlobalRobotPoseProvider
{
public:
-
RobotStateMemory();
virtual ~RobotStateMemory() override;
@@ -77,11 +74,12 @@ namespace armarx::armem::server::robot_state
// GlobalRobotPoseProvider interface
- armarx::PoseBasePtr getGlobalRobotPose(Ice::Long timestamp, const std::string& robotName, const ::Ice::Current&) override;
+ armarx::PoseBasePtr getGlobalRobotPose(Ice::Long timestamp,
+ const std::string& robotName,
+ const ::Ice::Current&) override;
protected:
-
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
void onInitComponent() override;
@@ -91,13 +89,11 @@ namespace armarx::armem::server::robot_state
private:
-
void startRobotUnitStream();
void stopRobotUnitStream();
private:
-
armarx::plugins::DebugObserverComponentPlugin* debugObserver = nullptr;
@@ -129,7 +125,7 @@ namespace armarx::armem::server::robot_state
proprioception::RobotStateWriter writer;
// queue
- using Queue = boost::lockfree::spsc_queue<proprioception::RobotUnitData, boost::lockfree::capacity<1024>>;
+ using Queue = armarx::armem::server::robot_state::proprioception::Queue;
Queue dataBuffer;
bool waitForRobotUnit = false;
@@ -137,4 +133,4 @@ namespace armarx::armem::server::robot_state
RobotUnit robotUnit;
};
-} // namespace armarx::armem::server::robot_state
+} // namespace armarx::armem::server::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
index 1e7e3d533c0251d3cbf1fb84962b4e11e182c35b..52de49d1420a9bf194cf4071b4f23ea8e2944ad6 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
@@ -30,49 +30,43 @@
// ArmarX
#include "ArmarXCore/core/time/Metronome.h"
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
#include "RobotAPI/libraries/armem_robot_state/client/common/constants.h"
-#include <RobotAPI/libraries/core/FramedPose.h>
-#include <RobotAPI/libraries/aron/core/data/variant/All.h>
-#include <RobotAPI/libraries/armem/core/Time.h>
-#include <RobotAPI/libraries/armem/core/ice_conversions.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
-#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/server/localization/Segment.h>
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
namespace armarx::armem::server::robot_state::proprioception
{
- void RobotStateWriter::connect(armarx::plugins::DebugObserverComponentPlugin& debugObserver)
+ void
+ RobotStateWriter::connect(armarx::plugins::DebugObserverComponentPlugin& debugObserverPlugin)
{
// Thread-local copy of debug observer helper.
this->debugObserver =
- DebugObserverHelper(Logging::tag.tagName, debugObserver.getDebugObserver(), true);
+ DebugObserverHelper(Logging::tag.tagName, debugObserverPlugin.getDebugObserver(), true);
}
- static float toDurationMs(
- std::chrono::high_resolution_clock::time_point start,
- std::chrono::high_resolution_clock::time_point end)
+ static float
+ toDurationMs(std::chrono::high_resolution_clock::time_point start,
+ std::chrono::high_resolution_clock::time_point end)
{
auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
return duration.count() / 1000.f;
}
- void RobotStateWriter::run(
- float pollFrequency,
- Queue& dataBuffer,
- MemoryToIceAdapter& memory,
- localization::Segment& localizationSegment)
+ void
+ RobotStateWriter::run(float pollFrequency,
+ Queue& dataBuffer,
+ MemoryToIceAdapter& memory,
+ localization::Segment& localizationSegment)
{
- Metronome metronome(Frequency::HertzDouble(pollFrequency));
-
while (task and not task->isStopped())
{
@@ -82,7 +76,8 @@ namespace armarx::armem::server::robot_state::proprioception
// }
RobotUnitData robotUnitData;
- if (dataBuffer.pop(robotUnitData))
+ if (const auto status = dataBuffer.wait_pull(robotUnitData);
+ status == boost::queue_op_status::success)
{
auto start = std::chrono::high_resolution_clock::now();
auto endBuildUpdate = start, endProprioception = start, endLocalization = start;
@@ -99,47 +94,61 @@ namespace armarx::armem::server::robot_state::proprioception
// Localization
for (const armem::robot_state::Transform& transform : update.localization)
{
- localizationSegment.doLocked([&localizationSegment, &transform]()
- {
- localizationSegment.commitTransform(transform);
- });
+ localizationSegment.doLocked(
+ [&localizationSegment, &transform]()
+ { localizationSegment.commitTransform(transform); });
}
endLocalization = std::chrono::high_resolution_clock::now();
if (not result.allSuccess())
{
- ARMARX_WARNING << "Could not commit data to memory. Error message: " << result.allErrorMessages();
+ ARMARX_WARNING << "Could not commit data to memory. Error message: "
+ << result.allErrorMessages();
}
if (debugObserver)
{
auto end = std::chrono::high_resolution_clock::now();
- debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit (ms)", toDurationMs(start, end));
- debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit 1. Build Update (ms)", toDurationMs(start, endBuildUpdate));
- debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit 2. Proprioception (ms)", toDurationMs(endBuildUpdate, endProprioception));
- debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit 3. Localization (ms)", toDurationMs(endProprioception, endLocalization));
+ debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit (ms)",
+ toDurationMs(start, end));
+ debugObserver->setDebugObserverDatafield(
+ "RobotStateWriter | t Commit 1. Build Update (ms)",
+ toDurationMs(start, endBuildUpdate));
+ debugObserver->setDebugObserverDatafield(
+ "RobotStateWriter | t Commit 2. Proprioception (ms)",
+ toDurationMs(endBuildUpdate, endProprioception));
+ debugObserver->setDebugObserverDatafield(
+ "RobotStateWriter | t Commit 3. Localization (ms)",
+ toDurationMs(endProprioception, endLocalization));
}
}
+ else
+ {
+ ARMARX_WARNING << "Queue pull was not successful. "
+ << static_cast<std::underlying_type<boost::queue_op_status>::type>(
+ status);
+ }
if (debugObserver)
{
debugObserver->sendDebugObserverBatch();
}
- metronome.waitForNextTick();
}
}
- RobotStateWriter::Update RobotStateWriter::buildUpdate(const RobotUnitData& data)
+ RobotStateWriter::Update
+ RobotStateWriter::buildUpdate(const RobotUnitData& data)
{
// Send batch to memory
Update update;
{
armem::EntityUpdate& up = update.proprioception.add();
- up.entityID = properties.robotUnitProviderID.withEntityName(properties.robotUnitProviderID.providerSegmentName);
+ up.entityID = properties.robotUnitProviderID.withEntityName(
+ properties.robotUnitProviderID.providerSegmentName);
up.timeCreated = data.timestamp;
- up.instancesData = { data.proprioception };
+ up.instancesData = {data.proprioception};
}
// Extract odometry data.
@@ -147,7 +156,8 @@ namespace armarx::armem::server::robot_state::proprioception
if (data.proprioception->hasElement(platformKey))
{
ARMARX_DEBUG << "Found odometry data.";
- auto platformData = aron::data::Dict::DynamicCastAndCheck(data.proprioception->getElement(platformKey));
+ auto platformData =
+ aron::data::Dict::DynamicCastAndCheck(data.proprioception->getElement(platformKey));
update.localization.emplace_back(getTransform(platformData, data.timestamp));
}
else
@@ -156,8 +166,7 @@ namespace armarx::armem::server::robot_state::proprioception
<< "(No element '" << platformKey << "' in proprioception data.)"
<< "\nIf you are using a mobile platform this should not have happened."
<< "\nThis error is only logged once."
- << "\nThese keys exist: " << data.proprioception->getAllKeys()
- ;
+ << "\nThese keys exist: " << data.proprioception->getAllKeys();
noOdometryDataLogged = true;
}
@@ -166,9 +175,8 @@ namespace armarx::armem::server::robot_state::proprioception
armem::robot_state::Transform
- RobotStateWriter::getTransform(
- const aron::data::DictPtr& platformData,
- const Time& timestamp) const
+ RobotStateWriter::getTransform(const aron::data::DictPtr& platformData,
+ const Time& timestamp) const
{
prop::arondto::Platform platform;
platform.fromAron(platformData);
@@ -176,7 +184,7 @@ namespace armarx::armem::server::robot_state::proprioception
const Eigen::Vector3f& relPose = platform.relativePosition;
Eigen::Affine3f odometryPose = Eigen::Affine3f::Identity();
- odometryPose.translation() << relPose.x(), relPose.y(), 0; // TODO set height
+ odometryPose.translation() << relPose.x(), relPose.y(), 0; // TODO set height
odometryPose.linear() = simox::math::rpy_to_mat3f(0.f, 0.f, relPose.z());
armem::robot_state::Transform transform;
@@ -189,4 +197,4 @@ namespace armarx::armem::server::robot_state::proprioception
return transform;
}
-}
+} // namespace armarx::armem::server::robot_state::proprioception
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 362bde7193fe40c9572bd4064437526bf4f0c7cb..f911d43fc7fabf6626c5e6e230cf74a66753d1cf 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
@@ -23,7 +23,6 @@
#pragma once
-#include <boost/lockfree/spsc_queue.hpp>
#include <optional>
#include "ArmarXCore/util/CPPUtility/TripleBuffer.h"
@@ -56,9 +55,9 @@ namespace armarx::armem::server::robot_state::proprioception
{
public:
- void connect(armarx::plugins::DebugObserverComponentPlugin& debugObserver);
+ void connect(armarx::plugins::DebugObserverComponentPlugin& debugObserverPlugin);
- using Queue = boost::lockfree::spsc_queue<RobotUnitData, boost::lockfree::capacity<1024>>;
+ using Queue = armarx::armem::server::robot_state::proprioception::Queue;
/// Reads data from `dataQueue` and commits to the memory.
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 c0029b4fbccece40156be49f05ca1efa4215850c..f62d0b0c9c078185fe7959a229956f5f67b4afdc 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h
@@ -2,8 +2,10 @@
#include <memory>
-#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+#include <boost/thread/concurrent_queues/sync_queue.hpp>
+
#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
namespace armarx::armem::server::robot_state::proprioception
{
@@ -13,5 +15,6 @@ namespace armarx::armem::server::robot_state::proprioception
Time timestamp;
aron::data::DictPtr proprioception;
};
-}
+ using Queue = boost::sync_queue<RobotUnitData>;
+} // namespace armarx::armem::server::robot_state::proprioception
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 ae44141606fb73011263bfe08fa474e7918ff023..c74f355c247eb6fca2ca348b5459527aef204260 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp
@@ -1,27 +1,15 @@
#include "RobotUnitReader.h"
-#include <RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6Converter.h>
-#include <RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.h>
-#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
-
-#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
-#include <RobotAPI/libraries/aron/core/data/variant/primitive/Long.h>
-#include <RobotAPI/libraries/aron/core/data/variant/primitive/String.h>
+#include <filesystem>
+#include <istream>
-#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
#include "ArmarXCore/core/time/Frequency.h"
#include "ArmarXCore/core/time/Metronome.h"
-#include "ArmarXCore/core/time/forward_declarations.h"
-#include "ArmarXCore/util/CPPUtility/TripleBuffer.h"
-#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
-#include <SimoxUtility/algorithm/string/string_tools.h>
-
-#include <istream>
-#include <filesystem>
-#include <fstream>
+#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
namespace armarx::armem::server::robot_state::proprioception
@@ -30,16 +18,16 @@ namespace armarx::armem::server::robot_state::proprioception
RobotUnitReader::RobotUnitReader() = default;
- void RobotUnitReader::connect(
- armarx::plugins::RobotUnitComponentPlugin& robotUnitPlugin,
- armarx::plugins::DebugObserverComponentPlugin& debugObserverPlugin,
- const std::string& robotTypeName)
+ void
+ RobotUnitReader::connect(armarx::plugins::RobotUnitComponentPlugin& robotUnitPlugin,
+ armarx::plugins::DebugObserverComponentPlugin& debugObserverPlugin,
+ const std::string& robotTypeName)
{
{
converter = converterRegistry.get(robotTypeName);
ARMARX_CHECK_NOT_NULL(converter)
- << "No converter for robot type '" << robotTypeName << "' available. \n"
- << "Known are: " << converterRegistry.getKeys();
+ << "No converter for robot type '" << robotTypeName << "' available. \n"
+ << "Known are: " << converterRegistry.getKeys();
config.loggingNames.push_back(properties.sensorPrefix);
receiver = robotUnitPlugin.startDataStreaming(config);
@@ -47,15 +35,14 @@ namespace armarx::armem::server::robot_state::proprioception
}
{
// Thread-local copy of debug observer helper.
- debugObserver =
- DebugObserverHelper(Logging::tag.tagName, debugObserverPlugin.getDebugObserver(), true);
+ debugObserver = DebugObserverHelper(
+ Logging::tag.tagName, debugObserverPlugin.getDebugObserver(), true);
}
}
- void RobotUnitReader::run(
- float pollFrequency,
- Queue& dataBuffer)
+ void
+ RobotUnitReader::run(float pollFrequency, Queue& dataBuffer)
{
Metronome metronome(Frequency::HertzDouble(pollFrequency));
@@ -63,6 +50,7 @@ namespace armarx::armem::server::robot_state::proprioception
{
if (std::optional<RobotUnitData> commit = fetchAndConvertLatestRobotUnitData())
{
+ // will lock a mutex
dataBuffer.push(std::move(commit.value()));
}
@@ -76,7 +64,8 @@ namespace armarx::armem::server::robot_state::proprioception
}
- std::optional<RobotUnitData> RobotUnitReader::fetchAndConvertLatestRobotUnitData()
+ std::optional<RobotUnitData>
+ RobotUnitReader::fetchAndConvertLatestRobotUnitData()
{
ARMARX_CHECK_NOT_NULL(converter);
@@ -95,18 +84,21 @@ namespace armarx::armem::server::robot_state::proprioception
auto stop = std::chrono::high_resolution_clock::now();
auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
- ARMARX_DEBUG << "RobotUnitReader: The total time needed to convert the data to commit: " << duration;
+ ARMARX_DEBUG << "RobotUnitReader: The total time needed to convert the data to commit: "
+ << duration;
if (debugObserver)
{
- debugObserver->setDebugObserverDatafield("RobotUnitReader | t Read+Group [ms]", duration.count() / 1000.f);
+ debugObserver->setDebugObserverDatafield("RobotUnitReader | t Read+Group [ms]",
+ duration.count() / 1000.f);
}
return result;
}
- std::optional<RobotUnitDataStreaming::TimeStep> RobotUnitReader::fetchLatestData()
+ std::optional<RobotUnitDataStreaming::TimeStep>
+ RobotUnitReader::fetchLatestData()
{
std::deque<RobotUnitDataStreaming::TimeStep>& data = receiver->getDataBuffer();
if (debugObserver)
@@ -126,4 +118,4 @@ namespace armarx::armem::server::robot_state::proprioception
}
-}
+} // namespace armarx::armem::server::robot_state::proprioception
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 9b1ea8861768f2a9bcd227c7be4e22d11ccef38c..cc3cd27581951a0365cf028ff2bf2d7929568f1d 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.h
@@ -1,13 +1,11 @@
#pragma once
-#include <queue>
#include <map>
#include <memory>
#include <optional>
+#include <queue>
#include <string>
-#include <boost/lockfree/spsc_queue.hpp>
-
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
#include <ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h>
@@ -16,15 +14,15 @@
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h>
#include "RobotUnitData.h"
-#include "converters/ConverterRegistry.h"
#include "converters/ConverterInterface.h"
+#include "converters/ConverterRegistry.h"
namespace armarx::plugins
{
class RobotUnitComponentPlugin;
class DebugObserverComponentPlugin;
-}
+} // namespace armarx::plugins
namespace armarx
{
using RobotUnitDataStreamingReceiverPtr = std::shared_ptr<class RobotUnitDataStreamingReceiver>;
@@ -35,34 +33,28 @@ namespace armarx::armem::server::robot_state::proprioception
class RobotUnitReader : public armarx::Logging
{
public:
-
RobotUnitReader();
- using Queue = boost::lockfree::spsc_queue<RobotUnitData, boost::lockfree::capacity<1024>>;
-
+ using Queue = armarx::armem::server::robot_state::proprioception::Queue;
- void connect(
- armarx::plugins::RobotUnitComponentPlugin& robotUnitPlugin,
- armarx::plugins::DebugObserverComponentPlugin& debugObserverPlugin,
- const std::string& robotTypeName);
+ void connect(armarx::plugins::RobotUnitComponentPlugin& robotUnitPlugin,
+ armarx::plugins::DebugObserverComponentPlugin& debugObserverPlugin,
+ const std::string& robotTypeName);
/// Reads data from `handler` and fills `dataQueue`.
- void run(float pollFrequency,
- Queue& dataBuffer);
+ void run(float pollFrequency, Queue& dataBuffer);
std::optional<RobotUnitData> fetchAndConvertLatestRobotUnitData();
private:
-
/// Fetch the latest timestep and clear the robot unit buffer.
std::optional<RobotUnitDataStreaming::TimeStep> fetchLatestData();
public:
-
struct Properties
{
std::string sensorPrefix = "sens.*";
@@ -80,7 +72,6 @@ namespace armarx::armem::server::robot_state::proprioception
ConverterInterface* converter = nullptr;
armarx::SimpleRunningTask<>::pointer_type task = nullptr;
-
};
-}
+} // namespace armarx::armem::server::robot_state::proprioception