diff --git a/.gitlab/CODEOWNERS b/.gitlab/CODEOWNERS
index af86e2ce7810f25704e7afbbbb3072e53e36b487..2a5d5735c0809d89c455cfced4ecd1ca96bc961d 100644
--- a/.gitlab/CODEOWNERS
+++ b/.gitlab/CODEOWNERS
@@ -1,17 +1,17 @@
# ARON
-/source/RobotAPI/libraries/aron/ @fratty @dreher
+/source/RobotAPI/libraries/aron/ @peller @dreher
-/source/RobotAPI/interface/aron/ @fratty @dreher
-/source/RobotAPI/interface/aron.ice @fratty @dreher
+/source/RobotAPI/interface/aron/ @peller @dreher
+/source/RobotAPI/interface/aron.ice @peller @dreher
# ArMem
-/source/RobotAPI/components/armem/ @fratty @RainerKartmann @dreher
+/source/RobotAPI/components/armem/ @peller @kartmann @dreher
-/source/RobotAPI/libraries/armem/ @fratty @RainerKartmann @dreher
-/source/RobotAPI/libraries/armem_robot_localization/ @FabianReister
-/source/RobotAPI/libraries/armem_robot_mapping/ @FabianReister
+/source/RobotAPI/libraries/armem/ @peller @kartmann @dreher
+/source/RobotAPI/libraries/armem_robot_localization/ @reister
+/source/RobotAPI/libraries/armem_robot_mapping/ @reister
/source/RobotAPI/interface/armem/ @fratty @RainerKartmann @dreher
/source/RobotAPI/interface/armem.ice @fratty @RainerKartmann @dreher
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
index 09280c4691a479a3bbd612131565775669bf21d1..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,11 +125,12 @@ namespace armarx::armem::server::robot_state
proprioception::RobotStateWriter writer;
// queue
- TripleBuffer<std::vector<proprioception::RobotUnitData>> dataBuffer;
+ using Queue = armarx::armem::server::robot_state::proprioception::Queue;
+ Queue dataBuffer;
bool waitForRobotUnit = false;
};
RobotUnit robotUnit;
};
-} // namespace armarx::armem::server::robot_state
+} // namespace armarx::armem::server::robot_state
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.cpp
index 240d0e8c31334129931c51254d70bff06f2826dd..e08feaa854d51acccf7439c9c2ff04c393958907 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.cpp
@@ -169,20 +169,21 @@ namespace armarx
void
AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::TuplePtr& i)
{
- // CAUTION; UNTESTED
- auto createdAronTuple = std::make_shared<aron::data::List>(i->getPath());
- createdAron = createdAronTuple;
- auto* currElem = parentItem->child(index);
- for (int j = 0; j < (int)i->childrenSize(); ++j)
+ auto createdAronList = std::make_shared<aron::data::List>(i->getPath());
+ createdAron = createdAronList;
+ QTreeWidgetItem* el = parentItem->child(index);
+
+ for (int x = 0; x < el->childCount(); ++x)
{
- AronTreeWidgetConverterVisitor convVisitor(currElem, j);
- aron::type::visit(convVisitor, i);
- handleErrors(convVisitor);
+ auto* it = el->child(x);
+ AronTreeWidgetConverterVisitor v(it, x);
+ aron::type::visit(v, i->getAcceptedType(x));
+ handleErrors(v);
- if (convVisitor.createdAron && convVisitor.isConversionSuccessful())
+ if (v.createdAron)
{
- createdAronTuple->addElement(convVisitor.createdAron);
+ createdAronList->addElement(v.createdAron);
}
}
}
diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index ecdd9f825e6442376cd19cd0c3a61a2ac590f51b..057440c94c97f8428c191c2d9c7a83edc95cbf51 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -38,3 +38,4 @@ add_subdirectory(skills)
add_subdirectory(RobotUnitDataStreamingReceiver)
add_subdirectory(GraspingUtility)
+add_subdirectory(obstacle_avoidance)
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp b/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp
index ee722453048da777dbad507f6f9b4e54e25a1865..c4b170313f7d9d71d09b055ffd497b9b658af5ac 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp
@@ -58,7 +58,8 @@ namespace armarx
GraspTrajectoryPtr
GraspTrajectory::ReadFromReader(const RapidXmlReaderPtr& reader)
{
- RapidXmlReaderNode root = reader->getRoot("GraspTrajectory");
+ RapidXmlReaderNode root = reader->getRoot();
+
GraspTrajectoryPtr traj;
for (const RapidXmlReaderNode& kpNode : root.nodes("Keypoint"))
{
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 87df7cb013d99543485fdc8868aa83254bf52ffe..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,64 +30,59 @@
// 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,
- TripleBuffer<std::vector<RobotUnitData>>& 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())
{
- const std::vector<RobotUnitData>& batch = dataBuffer.getUpToDateReadBuffer();
- if (debugObserver)
- {
- debugObserver->setDebugObserverDatafield("RobotStateWriter | Queue Size", batch.size());
- }
+ // if (debugObserver)
+ // {
+ // debugObserver->setDebugObserverDatafield("RobotStateWriter | Queue Size", dataBuffer.size());
+ // }
- if (not batch.empty())
+ RobotUnitData 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;
- Update update = buildUpdate(batch);
+ Update update = buildUpdate(robotUnitData);
endBuildUpdate = std::chrono::high_resolution_clock::now();
// Commits lock the core segments.
@@ -99,72 +94,80 @@ 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 std::vector<RobotUnitData>& dataQueue)
+ RobotStateWriter::Update
+ RobotStateWriter::buildUpdate(const RobotUnitData& data)
{
- ARMARX_CHECK_NOT_EMPTY(dataQueue);
- ARMARX_DEBUG << "RobotStateWriter: Commit batch of " << dataQueue.size() << " timesteps to memory...";
-
// Send batch to memory
Update update;
- for (const RobotUnitData& data: dataQueue)
{
- {
- armem::EntityUpdate& up = update.proprioception.add();
- up.entityID = properties.robotUnitProviderID.withEntityName(properties.robotUnitProviderID.providerSegmentName);
- up.timeCreated = data.timestamp;
- up.instancesData = { data.proprioception };
- }
+ armem::EntityUpdate& up = update.proprioception.add();
+ up.entityID = properties.robotUnitProviderID.withEntityName(
+ properties.robotUnitProviderID.providerSegmentName);
+ up.timeCreated = data.timestamp;
+ up.instancesData = {data.proprioception};
+ }
- // Extract odometry data.
- const std::string platformKey = "platform";
- if (data.proprioception->hasElement(platformKey))
- {
- ARMARX_DEBUG << "Found odometry data.";
- auto platformData = aron::data::Dict::DynamicCastAndCheck(data.proprioception->getElement(platformKey));
- update.localization.emplace_back(getTransform(platformData, data.timestamp));
- }
- else
- {
- ARMARX_INFO << "No odometry data! "
- << "(No element '" << platformKey << "' in proprioception data.)"
- << "\nIf you are using a mobile platform this should not have happened."
- << "\nThis error is only logged once."
- << "\nThese keys exist: " << data.proprioception->getAllKeys()
- ;
- noOdometryDataLogged = true;
- }
+ // Extract odometry data.
+ const std::string platformKey = "platform";
+ if (data.proprioception->hasElement(platformKey))
+ {
+ ARMARX_DEBUG << "Found odometry data.";
+ auto platformData =
+ aron::data::Dict::DynamicCastAndCheck(data.proprioception->getElement(platformKey));
+ update.localization.emplace_back(getTransform(platformData, data.timestamp));
+ }
+ else
+ {
+ ARMARX_INFO << "No odometry data! "
+ << "(No element '" << platformKey << "' in proprioception data.)"
+ << "\nIf you are using a mobile platform this should not have happened."
+ << "\nThis error is only logged once."
+ << "\nThese keys exist: " << data.proprioception->getAllKeys();
+ noOdometryDataLogged = true;
}
return update;
@@ -172,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);
@@ -182,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;
@@ -195,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 d70566ebf5c3aa6808d0e14af9c1af0471ac31c7..f911d43fc7fabf6626c5e6e230cf74a66753d1cf 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
@@ -55,11 +55,14 @@ namespace armarx::armem::server::robot_state::proprioception
{
public:
- void connect(armarx::plugins::DebugObserverComponentPlugin& debugObserver);
+ void connect(armarx::plugins::DebugObserverComponentPlugin& debugObserverPlugin);
+
+ using Queue = armarx::armem::server::robot_state::proprioception::Queue;
+
/// Reads data from `dataQueue` and commits to the memory.
void run(float pollFrequency,
- TripleBuffer<std::vector<RobotUnitData>>& dataBuffer,
+ Queue& dataBuffer,
MemoryToIceAdapter& memory,
localization::Segment& localizationSegment
);
@@ -70,7 +73,7 @@ namespace armarx::armem::server::robot_state::proprioception
armem::Commit proprioception;
std::vector<armem::robot_state::Transform> localization;
};
- Update buildUpdate(const std::vector<RobotUnitData>& dataQueue);
+ Update buildUpdate(const RobotUnitData& dataQueue);
private:
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 5794287c7231e07f9ffa0db967090f6d415c8f29..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,
- TripleBuffer<std::vector<RobotUnitData>>& dataBuffer)
+ void
+ RobotUnitReader::run(float pollFrequency, Queue& dataBuffer)
{
Metronome metronome(Frequency::HertzDouble(pollFrequency));
@@ -63,8 +50,8 @@ namespace armarx::armem::server::robot_state::proprioception
{
if (std::optional<RobotUnitData> commit = fetchAndConvertLatestRobotUnitData())
{
- dataBuffer.getWriteBuffer().push_back(std::move(commit.value()));
- dataBuffer.commitWrite();
+ // will lock a mutex
+ dataBuffer.push(std::move(commit.value()));
}
if (debugObserver)
@@ -77,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);
@@ -96,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)
@@ -127,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 5abf7378378828005795d10401a2ee7cc1ea5aa4..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,12 +1,11 @@
#pragma once
-#include <queue>
#include <map>
#include <memory>
#include <optional>
+#include <queue>
#include <string>
-#include "ArmarXCore/util/CPPUtility/TripleBuffer.h"
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
#include <ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h>
@@ -15,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>;
@@ -34,32 +33,28 @@ namespace armarx::armem::server::robot_state::proprioception
class RobotUnitReader : public armarx::Logging
{
public:
-
RobotUnitReader();
+ 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,
- TripleBuffer<std::vector<RobotUnitData>>& 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.*";
@@ -77,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
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp b/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp
index 478c6a2c80a4984fcc4fb19ee9dba611eb384af9..09cab7521090642ff509a0aa0fd9bdb17c99544c 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp
@@ -25,10 +25,12 @@
#include "Dict.h"
// ArmarX
-#include <RobotAPI/libraries/aron/core/data/variant/Factory.h>
-
#include <SimoxUtility/algorithm/string/string_conversion.h>
+#include <ArmarXCore/util/CPPUtility/trace.h>
+
+#include <RobotAPI/libraries/aron/core/data/variant/Factory.h>
+
namespace armarx::aron::data
{
@@ -216,19 +218,35 @@ namespace armarx::aron::data
{
case type::Descriptor::OBJECT:
{
+ ARMARX_TRACE;
auto objectTypeNav = type::Object::DynamicCastAndCheck(type);
for (const auto& [key, nav] : childrenNavigators)
{
if (!objectTypeNav->hasMemberType(key))
{
+ ARMARX_TRACE;
return false;
}
- if (!nav || !objectTypeNav->getMemberType(key))
+ if (!objectTypeNav->getMemberType(key))
{
+ ARMARX_TRACE;
return false;
}
+
+ if (!nav)
+ {
+ ARMARX_TRACE;
+ auto childTypeNav = objectTypeNav->getMemberType(key);
+ if (childTypeNav->getMaybe() == type::Maybe::NONE)
+ {
+ ARMARX_TRACE;
+ return false;
+ }
+ return true;
+ }
if (!nav->fullfillsType(objectTypeNav->getMemberType(key)))
{
+ ARMARX_TRACE;
return false;
}
}
@@ -236,16 +254,19 @@ namespace armarx::aron::data
}
case type::Descriptor::DICT:
{
+ ARMARX_TRACE;
auto dictTypeNav = type::Dict::DynamicCastAndCheck(type);
for (const auto& [key, nav] : childrenNavigators)
{
(void) key;
if (!nav || !dictTypeNav->getAcceptedType())
{
+ ARMARX_TRACE;
return false;
}
if (!nav->fullfillsType(dictTypeNav->getAcceptedType()))
{
+ ARMARX_TRACE;
return false;
}
}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/container/List.cpp b/source/RobotAPI/libraries/aron/core/data/variant/container/List.cpp
index 84bf1a2005b2d57f03065de58f65cf31870c8ea8..db80996a17bc20796dbca992bfd765ef31aad93d 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/container/List.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/container/List.cpp
@@ -26,10 +26,12 @@
// ArmarX
#include <ArmarXCore/core/exceptions/Exception.h>
+#include <ArmarXCore/util/CPPUtility/trace.h>
+
#include <RobotAPI/libraries/aron/core/data/variant/Factory.h>
#include <RobotAPI/libraries/aron/core/type/variant/container/List.h>
-#include <RobotAPI/libraries/aron/core/type/variant/container/Tuple.h>
#include <RobotAPI/libraries/aron/core/type/variant/container/Pair.h>
+#include <RobotAPI/libraries/aron/core/type/variant/container/Tuple.h>
namespace armarx::aron::data
{
@@ -218,15 +220,18 @@ namespace armarx::aron::data
{
case type::Descriptor::LIST:
{
+ ARMARX_TRACE;
auto listTypeNav = type::List::DynamicCastAndCheck(type);
for (const auto& nav : childrenNavigators)
{
if (!nav && !listTypeNav->getAcceptedType())
{
+ ARMARX_TRACE;
return false;
}
if (!nav->fullfillsType(listTypeNav->getAcceptedType()))
{
+ ARMARX_TRACE;
return false;
}
}
diff --git a/source/RobotAPI/libraries/obstacle_avoidance/CMakeLists.txt b/source/RobotAPI/libraries/obstacle_avoidance/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..dfd09c4a46abb303d29f4a0e5385fb6aa0a16b7b
--- /dev/null
+++ b/source/RobotAPI/libraries/obstacle_avoidance/CMakeLists.txt
@@ -0,0 +1,28 @@
+set(LIB_NAME obstacle_avoidance)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+armarx_add_library(
+ LIBS
+ # ArmarX
+ ArmarXCore
+ # This package
+ RobotAPI::Core
+ RobotAPI::armem
+ RobotAPI::armem_vision
+ RobotAPI::ArmarXObjects
+ aroncommon
+ # System / External
+# Eigen3::Eigen
+ HEADERS
+ CollisionModelHelper.h
+ SOURCES
+ CollisionModelHelper.cpp
+)
+
+add_library(
+ "RobotAPI::${LIB_NAME}"
+ ALIAS
+ ${LIB_NAME}
+)
diff --git a/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.cpp b/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d53a8dd6a89f0cd37f609c9a4742d97d86f4deba
--- /dev/null
+++ b/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.cpp
@@ -0,0 +1,179 @@
+/*
+ * 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
+ * @author Christoph Pohl ( christoph dot pohl at kit dot edu )
+ * @date 17.02.23
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "CollisionModelHelper.h"
+
+#include <VirtualRobot/ManipulationObject.h>
+#include <VirtualRobot/SceneObjectSet.h>
+#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
+#include <VirtualRobot/Visualization/TriMeshModel.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/components/ArViz/Client/elements/Mesh.h>
+
+
+namespace armarx::obstacle_avoidance
+{
+
+ VirtualRobot::ManipulationObjectPtr
+ CollisionModelHelper::asManipulationObject(const objpose::ObjectPose& objectPose)
+ {
+ const ObjectFinder finder;
+
+ const VirtualRobot::SceneObjectSetPtr sceneObjects(new VirtualRobot::SceneObjectSet);
+ if (auto obstacle = finder.loadManipulationObject(objectPose))
+ {
+ obstacle->setGlobalPose(objectPose.objectPoseGlobal);
+ return obstacle;
+ }
+
+ ARMARX_WARNING << "Failed to load scene object `" << objectPose.objectID << "`";
+ return nullptr;
+ }
+
+ VirtualRobot::SceneObjectSetPtr
+ CollisionModelHelper::asSceneObjects(const objpose::ObjectPoseSeq& objectPoses)
+ {
+ const ObjectFinder finder;
+
+ VirtualRobot::SceneObjectSetPtr sceneObjects(new VirtualRobot::SceneObjectSet);
+ for (const auto& objectPose : objectPoses)
+ {
+ if (auto obstacle = finder.loadManipulationObject(objectPose))
+ {
+ obstacle->setGlobalPose(objectPose.objectPoseGlobal);
+ sceneObjects->addSceneObject(obstacle);
+ }
+ }
+
+ return sceneObjects;
+ }
+
+ VirtualRobot::SceneObjectSetPtr
+ CollisionModelHelper::asSceneObjects(const OccupancyGrid& occupancyGrid,
+ const OccupancyGridHelper::Params& params)
+ {
+ const OccupancyGridHelper ocHelper(occupancyGrid, params);
+ const auto obstacles = ocHelper.obstacles();
+
+ const float boxSize = occupancyGrid.resolution;
+ const float resolution = occupancyGrid.resolution;
+
+ VirtualRobot::SceneObjectSetPtr sceneObjects(new VirtualRobot::SceneObjectSet);
+
+ ARMARX_CHECK_EQUAL(occupancyGrid.frame, GlobalFrame)
+ << "Only occupancy grid in global frame supported.";
+
+ VirtualRobot::CoinVisualizationFactory factory;
+
+ const auto& world_T_map = occupancyGrid.pose;
+
+ for (int x = 0; x < obstacles.rows(); x++)
+ {
+ for (int y = 0; y < obstacles.cols(); y++)
+ {
+ if (obstacles(x, y))
+ {
+ const Eigen::Vector3f pos{
+ static_cast<float>(x) * resolution, static_cast<float>(y) * resolution, 0};
+
+ // FIXME: change to Isometry3f
+ Eigen::Affine3f map_T_obj = Eigen::Affine3f::Identity();
+ map_T_obj.translation() = pos;
+
+ Eigen::Affine3f world_T_obj = world_T_map * map_T_obj;
+
+ // ARMARX_INFO << world_T_obj.translation();
+
+ auto cube = factory.createBox(boxSize, boxSize, boxSize);
+
+ const VirtualRobot::CollisionModelPtr collisionModel(
+ new VirtualRobot::CollisionModel(cube));
+
+ const VirtualRobot::SceneObjectPtr sceneObject(new VirtualRobot::SceneObject(
+ "box_" + std::to_string(sceneObjects->getSize()), cube, collisionModel));
+ sceneObject->setGlobalPose(world_T_obj.matrix());
+
+ sceneObjects->addSceneObject(sceneObject);
+ }
+ }
+ }
+
+ return sceneObjects;
+ }
+
+ CollisionModelHelper::CollisionModelHelper(const objpose::ObjectPoseClient& client) :
+ objectPoseClient_(client)
+ {
+ }
+
+ VirtualRobot::SceneObjectSetPtr
+ CollisionModelHelper::fetchSceneObjects()
+ {
+ const objpose::ObjectPoseSeq objectPoses = objectPoseClient_.fetchObjectPoses();
+ return asSceneObjects(objectPoses);
+ }
+
+ CollisionModelHelper::ManipulationObjectSetPtr
+ CollisionModelHelper::fetchManipulationObjects()
+ {
+ const objpose::ObjectPoseSeq objectPoses = objectPoseClient_.fetchObjectPoses();
+ return asManipulationObjects(objectPoses);
+ }
+
+ CollisionModelHelper::ManipulationObjectSetPtr
+ CollisionModelHelper::asManipulationObjects(const objpose::ObjectPoseSeq& objectPoses)
+ {
+ ManipulationObjectSet set;
+
+ for (const auto& pose : objectPoses)
+ {
+ set.emplace_back(*asManipulationObject(pose));
+ }
+
+ return std::make_shared<ManipulationObjectSet>(set);
+ }
+
+ void
+ CollisionModelHelper::visualizeCollisionModel(const VirtualRobot::CollisionModelPtr& model,
+ viz::Client& arviz)
+ {
+ armarx::viz::Mesh mesh(model->getName());
+ auto faces = model->getTriMeshModel()->faces;
+ std::vector<armarx::viz::data::Face> viz_faces;
+ std::transform(
+ faces.begin(),
+ faces.end(),
+ std::back_inserter(viz_faces),
+ [](const auto& face)
+ {
+ return armarx::viz::data::Face(
+ face.id1, face.id2, face.id3, face.idColor1, face.idColor2, face.idColor3);
+ });
+ mesh.vertices(model->getTriMeshModel()->vertices)
+ .faces(viz_faces)
+ .pose(model->getGlobalPose());
+ arviz.commitLayerContaining("CollisionModel", mesh);
+ }
+
+} // namespace armarx::obstacle_avoidance
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.h b/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.h
new file mode 100644
index 0000000000000000000000000000000000000000..d56a2d5bda01b7492f6b73f5ce3f9416bf7b0db7
--- /dev/null
+++ b/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.h
@@ -0,0 +1,62 @@
+/*
+ * 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
+ * @author Christoph Pohl ( christoph dot pohl at kit dot edu )
+ * @date 17.02.23
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h>
+#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
+#include <RobotAPI/libraries/armem_vision/OccupancyGridHelper.h>
+#include <RobotAPI/libraries/armem_vision/types.h>
+
+namespace armarx::obstacle_avoidance
+{
+
+ class CollisionModelHelper
+ {
+ public:
+ using ManipulationObjectSet = std::vector<VirtualRobot::ManipulationObject>;
+ using ManipulationObjectSetPtr = std::shared_ptr<ManipulationObjectSet>;
+
+ static VirtualRobot::ManipulationObjectPtr
+ asManipulationObject(const objpose::ObjectPose& objectPose);
+ static ManipulationObjectSetPtr
+ asManipulationObjects(const objpose::ObjectPoseSeq& objectPoses);
+ static VirtualRobot::SceneObjectSetPtr
+ asSceneObjects(const objpose::ObjectPoseSeq& objectPoses);
+ static VirtualRobot::SceneObjectSetPtr
+ asSceneObjects(const armem::vision::OccupancyGrid& occupancyGrid,
+ const OccupancyGridHelper::Params& params);
+ static void visualizeCollisionModel(const VirtualRobot::CollisionModelPtr& model,
+ viz::Client& arviz);
+
+
+ CollisionModelHelper(const objpose::ObjectPoseClient& client);
+ VirtualRobot::SceneObjectSetPtr fetchSceneObjects();
+ ManipulationObjectSetPtr fetchManipulationObjects();
+
+ private:
+ objpose::ObjectPoseClient objectPoseClient_;
+ };
+} // namespace armarx::obstacle_avoidance
diff --git a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
index 1fb8c3ac52010943e01b044d0e99ce144739a2b6..81e91b131294d542c286659e0691056ea0bc0894 100644
--- a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
+++ b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
@@ -86,7 +86,7 @@ namespace armarx
}
catch (const std::exception& ex)
{
- std::string message = "SkillError 101e: An error occured during the check whether skill '" + skillName + "' is available. The error was: " + ex.what();
+ std::string message = "SkillError 101e: An error occured during the check whether skill '" + skillName + "' is available. The error was: " + GetHandledExceptionString();
ARMARX_ERROR_S << message;
resetExecParam();
return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, TerminatedSkillStatus::Failed});
@@ -102,7 +102,7 @@ namespace armarx
}
catch (const std::exception& ex)
{
- std::string message = "SkillError 201e: An error occured during the reset of skill '" + skillName + "'. The error was: " + ex.what();
+ std::string message = "SkillError 201e: An error occured during the reset of skill '" + skillName + "'. The error was: " + GetHandledExceptionString();
ARMARX_ERROR_S << message;
updateStatus(SkillStatus::Failed);
@@ -116,7 +116,7 @@ namespace armarx
}
catch (const std::exception& ex)
{
- std::string message = "SkillError 301e: An error occured during waiting for skill dependencies of skill '" + skillName + "'. The error was: " + ex.what();
+ std::string message = "SkillError 301e: An error occured during waiting for skill dependencies of skill '" + skillName + "'. The error was: " + GetHandledExceptionString();
ARMARX_ERROR_S << message;
updateStatus(SkillStatus::Failed);
@@ -143,7 +143,7 @@ namespace armarx
}
catch (const std::exception& ex)
{
- std::string message = "SkillError 401e: An error occured during the initialization of skill '" + skillName + "'. The error was: " + ex.what();
+ std::string message = "SkillError 401e: An error occured during the initialization of skill '" + skillName + "'. The error was: " + GetHandledExceptionString();
ARMARX_ERROR_S << message;
skill->exitSkill({executorName, aron_params}); // try to exit skill. Ignore return value
@@ -174,7 +174,7 @@ namespace armarx
}
catch (const std::exception& ex)
{
- std::string message = "SkillError 501e: An error occured during the main method of skill '" + skillName + "'. The error was: " + ex.what();
+ std::string message = "SkillError 501e: An error occured during the main method of skill '" + skillName + "'. The error was: " + GetHandledExceptionString();
ARMARX_ERROR_S << message;
skill->exitSkill({executorName, aron_params}); // try to exit skill. Ignore return value
@@ -199,7 +199,7 @@ namespace armarx
}
catch (const std::exception& ex)
{
- std::string message = "SkillError 601e: An error occured during the exit method of skill '" + skillName + "'. The error was: " + ex.what();
+ std::string message = "SkillError 601e: An error occured during the exit method of skill '" + skillName + "'. The error was: " + GetHandledExceptionString();
ARMARX_ERROR_S << message;
updateStatus(SkillStatus::Failed);