diff --git a/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.cpp b/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.cpp
index b63913fdc88671f583339c7850134e23d8c5fbef..cdc049a27ad64057d179096185a2a5968f34d0d0 100644
--- a/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.cpp
+++ b/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.cpp
@@ -26,6 +26,7 @@
// ArmarX
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
namespace armarx::armem
{
@@ -86,7 +87,7 @@ namespace armarx::armem
// is this corect??
prop.platform.acceleration = Eigen::Vector3f();
- prop.platform.relativePosition = Eigen::Vector3f(update.platformPose.x,
+ prop.platform.relativePosition = Eigen::Vector3f(update.platformPose.x, // this should be globasl AFAIK
update.platformPose.y,
update.platformPose.rotationAroundZ);
prop.platform.velocity = Eigen::Vector3f(std::get<0>(update.platformVelocity),
@@ -104,7 +105,7 @@ namespace armarx::armem
ARMARX_DEBUG << "Committing " << entityUpdate;
armem::EntityUpdateResult updateResult = memoryWriter.commit(entityUpdate);
- ARMARX_INFO << updateResult;
+ ARMARX_DEBUG << updateResult;
if (!updateResult.success)
{
ARMARX_ERROR << updateResult.errorMessage;
@@ -119,9 +120,9 @@ namespace armarx::armem
transform.header.timestamp = IceUtil::Time::microSeconds(_timestampUpdateFirstModifiedInUs);
Eigen::Isometry3f tf = Eigen::Isometry3f::Identity();
- tf.translation().x() = std::get<0>(update.platformOdometryPose);
- tf.translation().y() = std::get<1>(update.platformOdometryPose);
- tf.linear() = Eigen::AngleAxisf(std::get<2>(update.platformOdometryPose), Eigen::Vector3f::UnitZ()).toRotationMatrix();
+ tf.translation().x() = (update.platformPose.x);
+ tf.translation().y() = (update.platformPose.y);
+ tf.linear() = Eigen::AngleAxisf((update.platformPose.rotationAroundZ), Eigen::Vector3f::UnitZ()).toRotationMatrix();
transform.transform = tf.matrix();
@@ -135,7 +136,7 @@ namespace armarx::armem
ARMARX_DEBUG << "Committing " << entityUpdate;
armem::EntityUpdateResult updateResult = memoryWriter.commit(locUpdate);
- ARMARX_INFO << updateResult;
+ ARMARX_DEBUG << updateResult;
if (!updateResult.success)
{
ARMARX_ERROR << updateResult.errorMessage;
@@ -173,7 +174,7 @@ namespace armarx::armem
{
ARMARX_DEBUG << "Got an update for joint vels";
std::lock_guard l(updateMutex);
- update.jointAngles = m;
+ update.jointVelocities = m;
updateTimestamps(ts);
}
@@ -181,7 +182,7 @@ namespace armarx::armem
{
ARMARX_DEBUG << "Got an update for joint torques";
std::lock_guard l(updateMutex);
- update.jointAngles = m;
+ update.jointTorques = m;
updateTimestamps(ts);
}
@@ -189,7 +190,7 @@ namespace armarx::armem
{
ARMARX_DEBUG << "Got an update for joint accels";
std::lock_guard l(updateMutex);
- update.jointAngles = m;
+ update.jointAccelerations = m;
updateTimestamps(ts);
}
@@ -197,7 +198,7 @@ namespace armarx::armem
{
ARMARX_DEBUG << "Got an update for joint currents";
std::lock_guard l(updateMutex);
- update.jointAngles = m;
+ update.jointCurrents = m;
updateTimestamps(ts);
}
@@ -241,6 +242,33 @@ namespace armarx::armem
updateTimestamps(now);
}
+ void LegacyRobotStateMemoryAdapter::commitArmar3RobotDescription()
+ {
+ ARMARX_IMPORTANT << "Commiting Armar3 to descriptions";
+ armem::arondto::RobotDescription desc;
+ desc.name = "Armar3";
+ desc.timestamp = armem::Time::now();
+ desc.xml.package = "RobotAPI";
+ desc.xml.path = "RobotAPI/robots/Armar3/ArmarIII.xml";
+
+ armem::Commit c;
+ auto& entityUpdate = c.add();
+
+ entityUpdate.confidence = 1.0;
+ entityUpdate.entityID.memoryName = "RobotState";
+ entityUpdate.entityID.coreSegmentName = "Description";
+ entityUpdate.entityID.providerSegmentName = "Armar3";
+ entityUpdate.entityID.entityName = "Armar3";
+
+ entityUpdate.instancesData = { desc.toAron() };
+ entityUpdate.timeCreated = armem::Time::now();
+ auto res = memoryWriter.commit(c);
+ if (!res.allSuccess())
+ {
+ ARMARX_WARNING << "Could not send data to memory." << res.allErrorMessages();
+ }
+ }
+
void LegacyRobotStateMemoryAdapter::onInitComponent()
{
@@ -273,6 +301,9 @@ namespace armarx::armem
return;
}
+ // update RobotDescription (since there will be no robot unit, we have to submit the description by ourselves)
+ commitArmar3RobotDescription();
+
runningTask->start();
}
diff --git a/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.h b/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.h
index 10f6c20ee094a657806ab21217b0bcba79758ad9..50f2c1973ada697cf5747253ca298348e77f2e19 100644
--- a/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.h
+++ b/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.h
@@ -80,6 +80,8 @@ namespace armarx::armem
void onExitComponent() override;
private:
+ void commitArmar3RobotDescription();
+
void updateTimestamps(long dataGeneratedUs);
void checkUpdateAndSendToMemory();
diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
index cb2ba76bf3a1a2bf3f739c52692ef5ebf0f8cb4a..6c89282c5bc007f80915d56654d8d18706bf6609 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
@@ -27,7 +27,9 @@
#include <SimoxUtility/algorithm/string.h>
#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/ice_conversions.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
+#include <RobotAPI/libraries/armem/core/ice_conversions_templates.h>
#include <RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.aron.generated.h>
@@ -116,6 +118,88 @@ namespace armarx
+ // ACTIONS
+ armem::actions::GetActionsOutputSeq
+ ExampleMemory::getActions(
+ const armem::actions::GetActionsInputSeq& input)
+ {
+ using namespace armem::actions;
+ Action greeting{"hi", "Say hello to " + input[0].id.entityName};
+ Action failure{"fail", "Fail dramatically"};
+ Action nothing{"null", "Do nothing, but deeply nested"};
+
+ SubMenu one{"one", "One", { nothing } };
+ SubMenu two{"two", "Two", { one } };
+ SubMenu three{"three", "Three", { two } };
+ SubMenu four{"four", "Four", { three } };
+
+ Menu menu {
+ greeting,
+ failure,
+ four,
+ SubMenu{"mut", "Mutate", {
+ Action{"copy", "Copy latest instance"}
+ }}
+ };
+
+ return {{ menu.toIce() }};
+ }
+
+ armem::actions::ExecuteActionOutputSeq ExampleMemory::executeActions(
+ const armem::actions::ExecuteActionInputSeq& input)
+ {
+ using namespace armem::actions;
+
+ ExecuteActionOutputSeq output;
+ for (const auto& [id, path] : input)
+ {
+ armem::MemoryID memoryID = armem::fromIce<armem::MemoryID>(id);
+ if (path == ActionPath{"hi"})
+ {
+ ARMARX_INFO << "Hello, " << memoryID.str() << "!";
+ output.emplace_back(true, "");
+ }
+ else if (path == ActionPath{"fail"})
+ {
+ ARMARX_WARNING << "Alas, I am gravely wounded!";
+ output.emplace_back(false, "Why would you do that to him?");
+ }
+ else if (not path.empty() and path.front() == "four" and path.back() == "null")
+ {
+ // Do nothing.
+ ARMARX_INFO << "Nested action (path: " << path << ")";
+ output.emplace_back(true, "");
+ }
+ else if (path == ActionPath{"mut", "copy"})
+ {
+ auto* instance = workingMemory().findLatestInstance(memoryID);
+ if (instance != nullptr)
+ {
+ armem::EntityUpdate update;
+ armem::MemoryID newID = memoryID.getCoreSegmentID()
+ .withProviderSegmentName(memoryID.providerSegmentName)
+ .withEntityName(memoryID.entityName);
+ update.entityID = newID;
+ update.timeCreated = armem::Time::now();
+ update.instancesData = { instance->data() };
+
+ armem::Commit newCommit;
+ this->commit(armem::toIce<armem::data::Commit>(newCommit));
+
+ tab.rebuild = true;
+ ARMARX_INFO << "Duplicated " << memoryID;
+ output.emplace_back(true, "");
+ }
+ else
+ {
+ output.emplace_back(false, "Couldn't duplicate " + memoryID.str());
+ }
+ }
+ }
+
+ return output;
+ }
+
// REMOTE GUI
void ExampleMemory::createRemoteGuiTab()
diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.h b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.h
index a7dda191aa96c14567899a4f9064edf5f69d2bcf..66a500eeb43043ceb660d9ad9c720d2dc946fb66 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.h
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.h
@@ -58,7 +58,7 @@ namespace armarx
// WritingInterface interface
public:
armem::data::AddSegmentsResult addSegments(const armem::data::AddSegmentsInput& input, const Ice::Current&) override;
- armem::data::CommitResult commit(const armem::data::Commit& commit, const Ice::Current&) override;
+ armem::data::CommitResult commit(const armem::data::Commit& commit, const Ice::Current& = Ice::emptyCurrent) override;
// LightweightRemoteGuiComponentPluginUser interface
@@ -66,6 +66,11 @@ namespace armarx
void createRemoteGuiTab();
void RemoteGui_update() override;
+ // ActionsInterface interface
+ public:
+ armem::actions::GetActionsOutputSeq getActions(const armem::actions::GetActionsInputSeq& input) override;
+ armem::actions::ExecuteActionOutputSeq executeActions(const armem::actions::ExecuteActionInputSeq& input) override;
+
protected:
diff --git a/source/RobotAPI/components/armem/server/GraspMemory/CMakeLists.txt b/source/RobotAPI/components/armem/server/GraspMemory/CMakeLists.txt
index daf929f79016eb938ee1d3e26bb36aec90d5d1ba..711482a70e3caff55f4b0ab3689d86dc40a84e9e 100644
--- a/source/RobotAPI/components/armem/server/GraspMemory/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/server/GraspMemory/CMakeLists.txt
@@ -6,7 +6,7 @@ set(COMPONENT_LIBS
ArmarXGuiComponentPlugins
RobotAPICore RobotAPIInterfaces armem_server
RobotAPIComponentPlugins # for ArViz and other plugins
- GraspingUtility
+ armem_grasping
${IVT_LIBRARIES}
)
diff --git a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
index 0d47ff67f9bece55ef92b9e79c82088d2fce4c06..accfc00fec22ef5b1f1d454b4476d3306b198747 100644
--- a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
+++ b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
@@ -11,6 +11,8 @@
#include <RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.aron.generated.h>
#include <RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.h>
#include <RobotAPI/libraries/GraspingUtility/aron_conversions.h>
+#include <RobotAPI/libraries/armem_grasping/aron/KnownGraspCandidate.aron.generated.h>
+
namespace armarx::armem::server::grasp
{
@@ -31,6 +33,11 @@ namespace armarx::armem::server::grasp
return "GraspMemory";
}
+ GraspMemory::GraspMemory() :
+ knownGraspProviderSegment(iceAdapter())
+ {
+ }
+
void GraspMemory::onInitComponent()
{
workingMemory().name() = memoryName;
@@ -39,6 +46,10 @@ namespace armarx::armem::server::grasp
armarx::grasping::arondto::GraspCandidate::toAronType());
workingMemory().addCoreSegment("BimanualGraspCandidate",
armarx::grasping::arondto::BimanualGraspCandidate::toAronType());
+ workingMemory().addCoreSegment("KnownGraspCandidate",
+ armarx::armem::grasping::arondto::KnownGraspInfo::toAronType());
+
+ knownGraspProviderSegment.init();
}
void GraspMemory::onConnectComponent()
@@ -396,7 +407,7 @@ namespace armarx::armem::server::grasp
void GraspMemory::RemoteGui_update()
{
- grasping::GraspCandidateVisu visu = grasping::GraspCandidateVisu();
+ armarx::grasping::GraspCandidateVisu visu;
// if (gui.tab.selectCoreSegment.hasValueChanged())
// {
@@ -472,7 +483,7 @@ namespace armarx::armem::server::grasp
{
armarx::grasping::GraspCandidate candidate;
- grasping::arondto::GraspCandidate aronTransform;
+ armarx::grasping::arondto::GraspCandidate aronTransform;
armem::wm::EntityInstance instance = workingMemory().getInstance(armem::MemoryID::fromString(element.first));
@@ -496,7 +507,6 @@ namespace armarx::armem::server::grasp
{
createRemoteGuiTab();
}
-
}
}
diff --git a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
index dd1834b82f19b63ce6d27d0e0028751db2150507..867bf0a62575f2affd51e1842d6631ceb0204e8e 100644
--- a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
+++ b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
@@ -9,6 +9,8 @@
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h>
+#include <RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.h>
+
namespace armarx::armem::server::grasp
{
/**
@@ -30,6 +32,8 @@ namespace armarx::armem::server::grasp
{
public:
+ GraspMemory();
+
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
@@ -57,10 +61,10 @@ namespace armarx::armem::server::grasp
DebugObserverInterfacePrx debugObserver;
-
std::string memoryName = "Grasp";
-
+ // segments
+ armem::grasping::segment::KnownGraspProviderSegment knownGraspProviderSegment;
struct RemoteGuiTab : RemoteGui::Client::Tab
{
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
index c605df2efece207f0a0b11337dd96c61ca534470..1d9c4ac147ddc2407952ed6db08c8fd4cfcc279b 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
@@ -156,6 +156,62 @@ namespace armarx::armem::server::obj
{
}
+ armem::actions::GetActionsOutputSeq ObjectMemory::getActions(
+ const armem::actions::GetActionsInputSeq& inputs)
+ {
+ using namespace armem::actions;
+ GetActionsOutputSeq outputs;
+ for (const auto& input : inputs)
+ {
+ auto memoryID = fromIce<armem::MemoryID>(input.id);
+ if (armem::contains(armem::MemoryID("Object", "Class"), memoryID)
+ and memoryID.hasEntityName() and not memoryID.hasGap())
+ {
+ Menu menu {
+ Action{"vis", "Visualize object"},
+ };
+ // For strange C++ reasons, this works, but emplace_back doesn't.
+ outputs.push_back({ menu.toIce() });
+ }
+ }
+
+ return outputs;
+ }
+
+ armem::actions::ExecuteActionOutputSeq ObjectMemory::executeActions(
+ const armem::actions::ExecuteActionInputSeq& inputs)
+ {
+ using namespace armem::actions;
+ ExecuteActionOutputSeq outputs;
+
+ for (const auto& input : inputs)
+ {
+ auto memoryID = fromIce<armem::MemoryID>(input.id);
+ if (input.actionPath == ActionPath{"vis"})
+ {
+ if (armem::contains(armem::MemoryID("Object", "Class"), memoryID)
+ and memoryID.hasEntityName() and not memoryID.hasGap())
+ {
+ classSegment.visualizeClass(memoryID);
+ outputs.emplace_back(true, "");
+ }
+ else
+ {
+ std::stringstream sstream;
+ sstream << "MemoryID " << memoryID
+ << " does not refer to a valid object class.";
+ outputs.emplace_back(false, sstream.str());
+ }
+ }
+ else
+ {
+ std::stringstream sstream;
+ sstream << "Action path " << input.actionPath << " is not defined.";
+ outputs.emplace_back(false, sstream.str());
+ }
+ }
+ return outputs;
+ }
void ObjectMemory::createRemoteGuiTab()
{
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
index 91217c90cc0dcb5be302663a0b472cac3327f3c0..82f39db591bd56537fa280e0dd10a56fa9806ca3 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
@@ -93,6 +93,14 @@ namespace armarx::armem::server::obj
void onDisconnectComponent() override;
void onExitComponent() override;
+ // Without this, ObjectMemory draws the original
+ // methods from ObjectMemoryInterface and complains
+ // that an overload is being hidden.
+ using ReadWritePluginUser::getActions;
+ using ReadWritePluginUser::executeActions;
+
+ armem::actions::GetActionsOutputSeq getActions(const armem::actions::GetActionsInputSeq& inputs) override;
+ armem::actions::ExecuteActionOutputSeq executeActions(const armem::actions::ExecuteActionInputSeq& inputs) override;
// Remote GUI
void createRemoteGuiTab();
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
index 6cc5b6a88cc2e6540aa11067ef6addd3b2d6d4ef..0aef2635f6cec7714f654a3437fc95268e0f6963 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
@@ -119,10 +119,10 @@ namespace armarx::armem::server::robot_state
includePaths.insert(includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
}
- if (robotUnit.waitForRobotUnit)
- {
- usingProxy(robotUnit.plugin->getRobotUnitName());
- }
+ if (robotUnit.waitForRobotUnit)
+ {
+ usingProxy(robotUnit.plugin->getRobotUnitName());
+ }
}
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/CMakeLists.txt b/source/RobotAPI/components/skills/SkillProviderExample/CMakeLists.txt
index f3215ba1d02972ba2ce76353b681017601f71881..ee1ea4e51bb0156822513f8481cc13338cc79b62 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/CMakeLists.txt
+++ b/source/RobotAPI/components/skills/SkillProviderExample/CMakeLists.txt
@@ -21,5 +21,12 @@ set(HEADERS
armarx_add_component("${SOURCES}" "${HEADERS}")
+armarx_enable_aron_file_generation_for_target(
+ TARGET_NAME
+ ${ARMARX_COMPONENT_NAME}
+ ARON_FILES
+ aron/HelloWorldAcceptedType.xml
+)
+
#generate the application
armarx_generate_and_add_component_executable(COMPONENT_NAMESPACE "armarx::skills::provider")
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp
index 804b8d68002b2c43b8423015f466900e5ccb29eb..485a5e07d83fd42776d0f959186b4b4c764b1bf5 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp
+++ b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp
@@ -6,9 +6,20 @@
#include <RobotAPI/libraries/aron/core/type/variant/primitive/String.h>
#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
+#include <RobotAPI/components/skills/SkillProviderExample/aron/HelloWorldAcceptedType.aron.generated.h>
+
namespace armarx::skills::provider
{
- Skill::Status HelloWorldSkill::execute(const aron::data::DictPtr& d, const CallbackT&)
+ HelloWorldSkill::HelloWorldSkill() :
+ Skill(SkillDescription{
+ "HelloWorld",
+ "This skill logs a message on ARMARX_IMPORTANT",
+ {},
+ 1000,
+ armarx::skills::Example::HelloWorldAcceptedType::toAronType()
+ })
+ {}
+ Skill::Status HelloWorldSkill::_execute(const aron::data::DictPtr& d, const CallbackT&)
{
ARMARX_IMPORTANT << "Hi, from the Hello World Skill.\n" <<
"I received the following data: \n" <<
@@ -17,7 +28,16 @@ namespace armarx::skills::provider
return Skill::Status::Succeeded;
}
- Skill::Status TimeoutSkill::execute(const aron::data::DictPtr& d, const CallbackT&)
+ TimeoutSkill::TimeoutSkill() :
+ Skill(SkillDescription{
+ "Timeout",
+ "This fails with timeout reached",
+ {},
+ 1000,
+ nullptr
+ })
+ {}
+ Skill::Status TimeoutSkill::_execute(const aron::data::DictPtr& d, const CallbackT&)
{
int i = 0;
while (!timeoutReached)
@@ -26,12 +46,25 @@ namespace armarx::skills::provider
std::this_thread::sleep_for(std::chrono::milliseconds(200));
// check if work is done
- if(i++ > 15) return Skill::Status::Succeeded;
+ if(i++ > 15)
+ {
+ ARMARX_IMPORTANT << "Somehow the timeout check succeceded....";
+ return Skill::Status::Succeeded;
+ }
}
return Skill::Status::TimeoutReached;
}
- Skill::Status CallbackSkill::execute(const aron::data::DictPtr& d, const CallbackT& callback)
+ CallbackSkill::CallbackSkill() :
+ Skill(SkillDescription{
+ "ShowMeCallbacks",
+ "This skill does shows callbacks",
+ {},
+ 1000,
+ nullptr
+ })
+ {}
+ Skill::Status CallbackSkill::_execute(const aron::data::DictPtr& d, const CallbackT& callback)
{
ARMARX_IMPORTANT << "Logging three updates via the callback";
auto up1 = std::make_shared<aron::data::Dict>();
@@ -68,21 +101,7 @@ namespace armarx::skills::provider
void SkillProviderExample::onInitComponent()
{
// Add example skill
- {
- auto helloWorldAcceptedType = std::make_shared<aron::type::Object>("helloWorldAcceptedType");
- auto helloWorldAcceptedTypeText = std::make_shared<aron::type::String>(aron::Path({"text"}));
- helloWorldAcceptedType->addMemberType("text", helloWorldAcceptedTypeText);
-
- auto helloWorldAcceptedTypeInt = std::make_shared<aron::type::Int>(aron::Path({"int"}));
- helloWorldAcceptedType->addMemberType("int", helloWorldAcceptedTypeInt);
-
- skills::SkillDescription helloWorldDesc;
- helloWorldDesc.description = "This skill logs a message on ARMARX_IMPORTANT";
- helloWorldDesc.skillName = "HelloWorld";
- helloWorldDesc.acceptedType = helloWorldAcceptedType;
- helloWorldDesc.timeoutMs = 1000;
- addSkill(std::make_shared<HelloWorldSkill>(), helloWorldDesc);
- }
+ addSkill(std::make_shared<HelloWorldSkill>());
// Add another lambda example skill
{
@@ -95,24 +114,10 @@ namespace armarx::skills::provider
}
// Add another example skill
- {
- skills::SkillDescription cbDesc;
- cbDesc.acceptedType = nullptr; // accept everything
- cbDesc.description = "This skill does shows callbacks.";
- cbDesc.skillName = "ShowMeCallbacks";
- cbDesc.timeoutMs = 1000;
- addSkill(std::make_shared<CallbackSkill>(), cbDesc);
- }
+ addSkill(std::make_shared<CallbackSkill>());
// Add timeout skill
- {
- skills::SkillDescription tDesc;
- tDesc.acceptedType = nullptr; // accept everything
- tDesc.description = "This fails with timeout reached.";
- tDesc.skillName = "Timeout";
- tDesc.timeoutMs = 1000;
- addSkill(std::make_shared<TimeoutSkill>(), tDesc);
- }
+ addSkill(std::make_shared<TimeoutSkill>());
}
void SkillProviderExample::onConnectComponent()
@@ -126,5 +131,7 @@ namespace armarx::skills::provider
}
void SkillProviderExample::onExitComponent()
- {}
+ {
+
+ }
}
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h
index 42ad812f103cd230e0d6bd73c4a1c65a1dded774..2cd96b5ebea1cd9ab860521f1cc48b103ee21fec 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h
+++ b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h
@@ -35,21 +35,24 @@ namespace armarx::skills::provider
class HelloWorldSkill : public Skill
{
public:
- Status execute(const aron::data::DictPtr&, const CallbackT&) final;
+ HelloWorldSkill();
+ Status _execute(const aron::data::DictPtr&, const CallbackT&) final;
};
class TimeoutSkill : public Skill
{
public:
- Status execute(const aron::data::DictPtr&, const CallbackT&) final;
+ TimeoutSkill();
+ Status _execute(const aron::data::DictPtr&, const CallbackT&) final;
};
class CallbackSkill : public Skill
{
public:
- Status execute(const aron::data::DictPtr&, const CallbackT&) final;
+ CallbackSkill();
+ Status _execute(const aron::data::DictPtr&, const CallbackT&) final;
};
/**
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/aron/HelloWorldAcceptedType.xml b/source/RobotAPI/components/skills/SkillProviderExample/aron/HelloWorldAcceptedType.xml
new file mode 100644
index 0000000000000000000000000000000000000000..553a93ad0898ddc40d112e097af9c95ca801da55
--- /dev/null
+++ b/source/RobotAPI/components/skills/SkillProviderExample/aron/HelloWorldAcceptedType.xml
@@ -0,0 +1,17 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <GenerateTypes>
+ <Object name='armarx::skills::Example::HelloWorldAcceptedType'>
+ <ObjectChild key='some_int'>
+ <Int />
+ </ObjectChild>
+ <ObjectChild key='some_float'>
+ <Float />
+ </ObjectChild>
+ <ObjectChild key='some_text'>
+ <String />
+ </ObjectChild>
+ </Object>
+ </GenerateTypes>
+
+</AronTypeDefinition>
diff --git a/source/RobotAPI/components/units/CMakeLists.txt b/source/RobotAPI/components/units/CMakeLists.txt
index 3fa53590170e0ff9d288f7f130adae9814ee7446..f6710bc22978c2816aa8056da5795b0a84b5578a 100644
--- a/source/RobotAPI/components/units/CMakeLists.txt
+++ b/source/RobotAPI/components/units/CMakeLists.txt
@@ -6,6 +6,7 @@ set(LIBS
RobotAPICore
ArmarXCoreObservers
ArmarXCoreEigen3Variants
+ GraspingUtility
)
set(LIB_HEADERS
diff --git a/source/RobotAPI/components/units/GraspCandidateObserver.cpp b/source/RobotAPI/components/units/GraspCandidateObserver.cpp
index e780bbfe388f6e484673bbd44d5cc7c3bf5f0570..d4b5ea81cab4f670ba0a073b7ae120b82295de49 100644
--- a/source/RobotAPI/components/units/GraspCandidateObserver.cpp
+++ b/source/RobotAPI/components/units/GraspCandidateObserver.cpp
@@ -38,7 +38,7 @@
using namespace armarx;
using namespace armarx::grasping;
-GraspCandidateObserver::GraspCandidateObserver()
+GraspCandidateObserver::GraspCandidateObserver() : graspCandidateWriter(memoryNameSystem())
{
}
@@ -53,6 +53,7 @@ void GraspCandidateObserver::onInitObserver()
void GraspCandidateObserver::onConnectObserver()
{
configTopic = getTopic<GraspCandidateProviderInterfacePrx>(getProperty<std::string>("ConfigTopicName").getValue());
+ graspCandidateWriter.connect();
}
PropertyDefinitionsPtr GraspCandidateObserver::createPropertyDefinitions()
@@ -125,6 +126,7 @@ void GraspCandidateObserver::reportGraspCandidates(const std::string& providerNa
{
std::unique_lock lock(dataMutex);
this->candidates[providerName] = candidates;
+ graspCandidateWriter.commitGraspCandidateSeq(candidates, armarx::armem::Time::now(), providerName);
handleProviderUpdate(providerName, candidates.size());
}
diff --git a/source/RobotAPI/components/units/GraspCandidateObserver.h b/source/RobotAPI/components/units/GraspCandidateObserver.h
index f4bb0ba3e7886d970cd33a82a03fe9525057b6c9..91169049b9376b41a8deaddf7f93ed9100979c28 100644
--- a/source/RobotAPI/components/units/GraspCandidateObserver.h
+++ b/source/RobotAPI/components/units/GraspCandidateObserver.h
@@ -25,6 +25,8 @@
#include <ArmarXCore/observers/Observer.h>
#include <RobotAPI/interface/observers/GraspCandidateObserverInterface.h>
+#include <RobotAPI/libraries/armem/client/plugins/PluginUser.h>
+#include <RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.h>
#include <mutex>
@@ -52,6 +54,7 @@ namespace armarx
*/
class GraspCandidateObserver :
virtual public Observer,
+ virtual public armarx::armem::ClientPluginUser,
virtual public grasping::GraspCandidateObserverInterface
{
public:
@@ -124,7 +127,7 @@ namespace armarx
grasping::GraspCandidateSeq selectedCandidates;
grasping::BimanualGraspCandidateSeq selectedBimanualCandidates;
-
+ armarx::armem::GraspCandidateWriter graspCandidateWriter;
void handleProviderUpdate(const std::string& providerName, int candidateCount);
};
diff --git a/source/RobotAPI/components/units/RobotUnit/Devices/DeviceBase.h b/source/RobotAPI/components/units/RobotUnit/Devices/DeviceBase.h
index 2ba5dd359b22e50420b4da931b3344d884f52e2d..35958d52d5f984b14e5644e2372990d103eb935b 100644
--- a/source/RobotAPI/components/units/RobotUnit/Devices/DeviceBase.h
+++ b/source/RobotAPI/components/units/RobotUnit/Devices/DeviceBase.h
@@ -30,12 +30,14 @@ namespace armarx
{
public:
/// @brief Create a Device with the given name
- DeviceBase(const std::string& name): deviceName {name} {}
+ DeviceBase(const std::string& name) : deviceName{name}
+ {
+ }
virtual ~DeviceBase() = default;
/// @return The Device's name
const std::string& getDeviceName() const;
/// @return The Device's name
- const std::string& rtGetDeviceName() const;
+ const char* rtGetDeviceName() const;
//tags
/// @return Thes set of tags for this Device
const std::set<std::string>& getTags() const;
@@ -45,38 +47,43 @@ namespace armarx
protected:
/// @brief adds the given tag to the Device
void addDeviceTag(const std::string& tag);
+
private:
std::set<std::string> tags;
const std::string deviceName;
};
-}
+} // namespace armarx
//inline functions
namespace armarx
{
- inline bool DeviceBase::hasTag(const std::string& tag) const
+ inline bool
+ DeviceBase::hasTag(const std::string& tag) const
{
return getTags().count(tag);
}
- inline void DeviceBase::addDeviceTag(const std::string& tag)
+ inline void
+ DeviceBase::addDeviceTag(const std::string& tag)
{
tags.emplace(tag);
}
- inline const std::string& DeviceBase::getDeviceName() const
+ inline const std::string&
+ DeviceBase::getDeviceName() const
{
return deviceName;
}
- inline const std::string& DeviceBase::rtGetDeviceName() const
+ inline const char*
+ DeviceBase::rtGetDeviceName() const
{
- return deviceName;
+ return deviceName.c_str();
}
- inline const std::set<std::string>& DeviceBase::getTags() const
+ inline const std::set<std::string>&
+ DeviceBase::getTags() const
{
return tags;
}
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
index c4c7611f379c1ee00c106f2897212c580c25f853..6b251b9cbaa0db977e102580deeb17b4b0d2165e 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
@@ -20,18 +20,18 @@
* GNU General Public License
*/
+#include "RobotUnitModuleControlThread.h"
+
#include <VirtualRobot/XML/RobotIO.h>
-#include <ArmarXCore/core/util/OnScopeExit.h>
#include <ArmarXCore/core/time/TimeUtil.h>
-#include <RobotAPI/components/units/RobotUnit/util/DynamicsHelper.h>
+#include <ArmarXCore/core/util/OnScopeExit.h>
#include <ArmarXCore/observers/filters/rtfilters/AverageFilter.h>
#include <ArmarXCore/observers/filters/rtfilters/ButterworthFilter.h>
-
-#include "RobotUnitModuleControlThread.h"
-#include "../NJointControllers/NJointController.h"
+#include <RobotAPI/components/units/RobotUnit/util/DynamicsHelper.h>
#include "../Devices/RTThreadTimingsSensorDevice.h"
+#include "../NJointControllers/NJointController.h"
namespace armarx::RobotUnitModule
{
@@ -42,15 +42,18 @@ namespace armarx::RobotUnitModule
class NJointControllerAttorneyForControlThread
{
friend class ControlThread;
- static void RtDeactivateController(const NJointControllerBasePtr& nJointCtrl)
+ static void
+ RtDeactivateController(const NJointControllerBasePtr& nJointCtrl)
{
nJointCtrl->rtDeactivateController();
}
- static void RtActivateController(const NJointControllerBasePtr& nJointCtrl)
+ static void
+ RtActivateController(const NJointControllerBasePtr& nJointCtrl)
{
nJointCtrl->rtActivateController();
}
- static void RtDeactivateControllerBecauseOfError(const NJointControllerBasePtr& nJointCtrl)
+ static void
+ RtDeactivateControllerBecauseOfError(const NJointControllerBasePtr& nJointCtrl)
{
nJointCtrl->rtDeactivateControllerBecauseOfError();
}
@@ -62,73 +65,111 @@ namespace armarx::RobotUnitModule
class ControlThreadDataBufferAttorneyForControlThread
{
friend class ControlThread;
- static std::vector<JointController*>& GetActivatedJointControllers(ControlThread* p)
+ static std::vector<JointController*>&
+ GetActivatedJointControllers(ControlThread* p)
{
- return p->_module<ControlThreadDataBuffer>().controllersActivated.getWriteBuffer().jointControllers;
+ return p->_module<ControlThreadDataBuffer>()
+ .controllersActivated.getWriteBuffer()
+ .jointControllers;
}
- static std::vector<NJointControllerBase*>& GetActivatedNJointControllers(ControlThread* p)
+ static std::vector<NJointControllerBase*>&
+ GetActivatedNJointControllers(ControlThread* p)
{
- return p->_module<ControlThreadDataBuffer>().controllersActivated.getWriteBuffer().nJointControllers;
+ return p->_module<ControlThreadDataBuffer>()
+ .controllersActivated.getWriteBuffer()
+ .nJointControllers;
}
- static std::vector<std::size_t>& GetActivatedJointToNJointControllerAssignement(ControlThread* p)
+ static std::vector<std::size_t>&
+ GetActivatedJointToNJointControllerAssignement(ControlThread* p)
{
- return p->_module<ControlThreadDataBuffer>().controllersActivated.getWriteBuffer().jointToNJointControllerAssignement;
+ return p->_module<ControlThreadDataBuffer>()
+ .controllersActivated.getWriteBuffer()
+ .jointToNJointControllerAssignement;
}
- static const std::vector<JointController*>& GetActivatedJointControllers(const ControlThread* p)
+ static const std::vector<JointController*>&
+ GetActivatedJointControllers(const ControlThread* p)
{
- return p->_module<ControlThreadDataBuffer>().controllersActivated.getWriteBuffer().jointControllers;
+ return p->_module<ControlThreadDataBuffer>()
+ .controllersActivated.getWriteBuffer()
+ .jointControllers;
}
- static const std::vector<NJointControllerBase*>& GetActivatedNJointControllers(const ControlThread* p)
+ static const std::vector<NJointControllerBase*>&
+ GetActivatedNJointControllers(const ControlThread* p)
{
- return p->_module<ControlThreadDataBuffer>().controllersActivated.getWriteBuffer().nJointControllers;
+ return p->_module<ControlThreadDataBuffer>()
+ .controllersActivated.getWriteBuffer()
+ .nJointControllers;
}
- static const std::vector<std::size_t>& GetActivatedJointToNJointControllerAssignement(const ControlThread* p)
+ static const std::vector<std::size_t>&
+ GetActivatedJointToNJointControllerAssignement(const ControlThread* p)
{
- return p->_module<ControlThreadDataBuffer>().controllersActivated.getWriteBuffer().jointToNJointControllerAssignement;
+ return p->_module<ControlThreadDataBuffer>()
+ .controllersActivated.getWriteBuffer()
+ .jointToNJointControllerAssignement;
}
- static void AcceptRequestedJointToNJointControllerAssignement(ControlThread* p)
+ static void
+ AcceptRequestedJointToNJointControllerAssignement(ControlThread* p)
{
- p->_module<ControlThreadDataBuffer>().controllersActivated.getWriteBuffer().jointToNJointControllerAssignement =
- p->_module<ControlThreadDataBuffer>().controllersRequested.getReadBuffer().jointToNJointControllerAssignement;
+ p->_module<ControlThreadDataBuffer>()
+ .controllersActivated.getWriteBuffer()
+ .jointToNJointControllerAssignement = p->_module<ControlThreadDataBuffer>()
+ .controllersRequested.getReadBuffer()
+ .jointToNJointControllerAssignement;
}
- static void CommitActivatedControllers(ControlThread* p)
+ static void
+ CommitActivatedControllers(ControlThread* p)
{
return p->_module<ControlThreadDataBuffer>().controllersActivated.commitWrite();
}
- static void ResetActivatedControllerAssignement(ControlThread* p)
+ static void
+ ResetActivatedControllerAssignement(ControlThread* p)
{
- return p->_module<ControlThreadDataBuffer>().controllersActivated.getWriteBuffer().resetAssignement();
+ return p->_module<ControlThreadDataBuffer>()
+ .controllersActivated.getWriteBuffer()
+ .resetAssignement();
}
- static const std::vector<JointController*>& GetRequestedJointControllers(const ControlThread* p)
+ static const std::vector<JointController*>&
+ GetRequestedJointControllers(const ControlThread* p)
{
//do NOT update here!
- return p->_module<ControlThreadDataBuffer>().controllersRequested.getReadBuffer().jointControllers;
+ return p->_module<ControlThreadDataBuffer>()
+ .controllersRequested.getReadBuffer()
+ .jointControllers;
}
- static const std::vector<NJointControllerBase*>& GetRequestedNJointControllers(const ControlThread* p)
+ static const std::vector<NJointControllerBase*>&
+ GetRequestedNJointControllers(const ControlThread* p)
{
//do NOT update here!
- return p->_module<ControlThreadDataBuffer>().controllersRequested.getReadBuffer().nJointControllers;
+ return p->_module<ControlThreadDataBuffer>()
+ .controllersRequested.getReadBuffer()
+ .nJointControllers;
}
- static const std::vector<std::size_t>& GetRequestedJointToNJointControllerAssignement(const ControlThread* p)
+ static const std::vector<std::size_t>&
+ GetRequestedJointToNJointControllerAssignement(const ControlThread* p)
{
//do NOT update here!
- return p->_module<ControlThreadDataBuffer>().controllersRequested.getReadBuffer().jointToNJointControllerAssignement;
+ return p->_module<ControlThreadDataBuffer>()
+ .controllersRequested.getReadBuffer()
+ .jointToNJointControllerAssignement;
}
- static bool RequestedControllersChanged(const ControlThread* p)
+ static bool
+ RequestedControllersChanged(const ControlThread* p)
{
//only place allowed to update this buffer!
return p->_module<ControlThreadDataBuffer>().controllersRequested.updateReadBuffer();
}
/// Activate a joint controller from the rt loop (only works in switch mode RTThread)
- static void RTSetJointController(ControlThread* p, JointController* c, std::size_t index)
+ static void
+ RTSetJointController(ControlThread* p, JointController* c, std::size_t index)
{
ARMARX_CHECK_NOT_NULL(c);
//do NOT update here!
- auto& readbuf = p->_module<ControlThreadDataBuffer>().controllersRequested._getNonConstReadBuffer();
+ auto& readbuf =
+ p->_module<ControlThreadDataBuffer>().controllersRequested._getNonConstReadBuffer();
auto& j = readbuf.jointControllers;
auto& assig = readbuf.jointToNJointControllerAssignement;
auto& nj = readbuf.nJointControllers;
@@ -156,31 +197,40 @@ namespace armarx::RobotUnitModule
class DevicesAttorneyForControlThread
{
friend class ControlThread;
- static const std::vector<ControlDevicePtr>& GetControlDevices(const ControlThread* p)
+ static const std::vector<ControlDevicePtr>&
+ GetControlDevices(const ControlThread* p)
{
return p->_module<Devices>().controlDevices.values();
}
- static const std::vector<SensorDevicePtr>& GetSensorDevices(const ControlThread* p)
+ static const std::vector<SensorDevicePtr>&
+ GetSensorDevices(const ControlThread* p)
{
return p->_module<Devices>().sensorDevices.values();
}
- static const std::vector<const SensorValueBase*>& GetSensorValues(const ControlThread* p)
+ static const std::vector<const SensorValueBase*>&
+ GetSensorValues(const ControlThread* p)
{
return p->_module<Devices>().sensorValues;
}
- static const std::vector<std::vector<PropagateConst<ControlTargetBase*>>>& GetControlTargets(const ControlThread* p)
+ static const std::vector<std::vector<PropagateConst<ControlTargetBase*>>>&
+ GetControlTargets(const ControlThread* p)
{
return p->_module<Devices>().controlTargets;
}
- static RTThreadTimingsSensorDevice& GetThreadTimingsSensorDevice(const ControlThread* p)
+ static RTThreadTimingsSensorDevice&
+ GetThreadTimingsSensorDevice(const ControlThread* p)
{
return *(p->_module<Devices>().rtThreadTimingsSensorDevice);
}
- static void UpdateRobotWithSensorValues(const ControlThread* p, const VirtualRobot::RobotPtr& robot, const std::vector<VirtualRobot::RobotNodePtr>& robotNodes)
+ static void
+ UpdateRobotWithSensorValues(const ControlThread* p,
+ const VirtualRobot::RobotPtr& robot,
+ const std::vector<VirtualRobot::RobotNodePtr>& robotNodes)
{
- p->_module<Devices>().updateVirtualRobotFromSensorValues(robot, robotNodes, p->_module<Devices>().sensorValues);
+ p->_module<Devices>().updateVirtualRobotFromSensorValues(
+ robot, robotNodes, p->_module<Devices>().sensorValues);
}
};
/**
@@ -190,7 +240,8 @@ namespace armarx::RobotUnitModule
class ManagementAttorneyForControlThread
{
friend class ControlThread;
- static bool HeartbeatMissing(const ControlThread* p)
+ static bool
+ HeartbeatMissing(const ControlThread* p)
{
const Management& m = p->_module<Management>();
long now = TimeUtil::GetTime(true).toMilliSeconds();
@@ -201,14 +252,18 @@ namespace armarx::RobotUnitModule
return (now - m.lastHeartbeat) > m.heartbeatMaxCycleMS;
}
};
-}
+} // namespace armarx::RobotUnitModule
namespace armarx::RobotUnitModule
{
- bool ControlThread::rtSwitchControllerSetup(SwitchControllerMode mode)
+ bool
+ ControlThread::rtSwitchControllerSetup(SwitchControllerMode mode)
{
rtGetThreadTimingsSensorDevice().rtMarkRtSwitchControllerSetupStart();
- ARMARX_ON_SCOPE_EXIT { rtGetThreadTimingsSensorDevice().rtMarkRtSwitchControllerSetupEnd(); };
+ ARMARX_ON_SCOPE_EXIT
+ {
+ rtGetThreadTimingsSensorDevice().rtMarkRtSwitchControllerSetupEnd();
+ };
//store controllers activated before switching controllers
{
@@ -250,27 +305,25 @@ namespace armarx::RobotUnitModule
}
else
{
- ARMARX_RT_LOGF_ERROR("NJoint controller that is neither SynchronousNJointController"
- " nor AsynchronousNJointController: %s", actNJC.at(i)->rtGetClassName().c_str());
+ ARMARX_RT_LOGF_ERROR(
+ "NJoint controller that is neither SynchronousNJointController"
+ " nor AsynchronousNJointController: %s",
+ actNJC.at(i)->rtGetClassName().c_str());
// Throwing exceptions in a destructor causes std::abort to be called
//throw std::logic_error{};
}
}
- for (
- std::size_t i = numSyncNj;
- i < _maxControllerCount &&
- _activatedSynchronousNJointControllersIdx.at(i) != _maxControllerCount;
- ++i
- )
+ for (std::size_t i = numSyncNj;
+ i < _maxControllerCount &&
+ _activatedSynchronousNJointControllersIdx.at(i) != _maxControllerCount;
+ ++i)
{
_activatedSynchronousNJointControllersIdx.at(i) = _maxControllerCount;
}
- for (
- std::size_t i = numAsyncNj;
- i < _maxControllerCount &&
- _activatedAsynchronousNJointControllersIdx.at(i) != _maxControllerCount;
- ++i
- )
+ for (std::size_t i = numAsyncNj;
+ i < _maxControllerCount &&
+ _activatedAsynchronousNJointControllersIdx.at(i) != _maxControllerCount;
+ ++i)
{
_activatedAsynchronousNJointControllersIdx.at(i) = _maxControllerCount;
}
@@ -283,7 +336,8 @@ namespace armarx::RobotUnitModule
if (!emergencyStop && ManagementAttorneyForControlThread::HeartbeatMissing(this))
{
rtSetEmergencyStopState(EmergencyStopState::eEmergencyStopActive);
- ARMARX_RT_LOGF_ERROR("Emergency Stop was activated because heartbeat is missing!").deactivateSpam(1);
+ ARMARX_RT_LOGF_ERROR("Emergency Stop was activated because heartbeat is missing!")
+ .deactivateSpam(1);
}
const bool rtThreadOverridesControl = mode != SwitchControllerMode::IceRequests;
@@ -325,17 +379,16 @@ namespace armarx::RobotUnitModule
}
if (rtSwitchControllerSetupChangedControllers)
{
- ControlThreadDataBufferAttorneyForControlThread::ResetActivatedControllerAssignement(this);
+ ControlThreadDataBufferAttorneyForControlThread::
+ ResetActivatedControllerAssignement(this);
// the activated controllers are committed in rtUpdateSensorAndControlBuffer(...)
}
return rtSwitchControllerSetupChangedControllers;
}
- if (
- !rtThreadOverridesControl &&
+ if (!rtThreadOverridesControl &&
!ControlThreadDataBufferAttorneyForControlThread::RequestedControllersChanged(this) &&
- !rtIsInEmergencyStop()
- )
+ !rtIsInEmergencyStop())
{
return false;
}
@@ -356,8 +409,10 @@ namespace armarx::RobotUnitModule
{
++idxAct;
}
- const NJointControllerBasePtr& req = idxReq < n ? allReqNJ.at(idxReq) : nullptr; //may be null
- const NJointControllerBasePtr& act = idxAct < n ? allActdNJ.at(idxAct) : nullptr; //may be null if it is the last
+ const NJointControllerBasePtr& req =
+ idxReq < n ? allReqNJ.at(idxReq) : nullptr; //may be null
+ const NJointControllerBasePtr& act =
+ idxAct < n ? allActdNJ.at(idxAct) : nullptr; //may be null if it is the last
const auto reqId = reinterpret_cast<std::uintptr_t>(req.get());
const auto actId = reinterpret_cast<std::uintptr_t>(act.get());
@@ -402,12 +457,14 @@ namespace armarx::RobotUnitModule
allActdJ.at(i) = requestedJointCtrl;
}
}
- ControlThreadDataBufferAttorneyForControlThread::AcceptRequestedJointToNJointControllerAssignement(this);
+ ControlThreadDataBufferAttorneyForControlThread::
+ AcceptRequestedJointToNJointControllerAssignement(this);
// the activated controllers are committed in rtUpdateSensorAndControlBuffer(...)
return true;
}
- void ControlThread::rtResetAllTargets()
+ void
+ ControlThread::rtResetAllTargets()
{
rtGetThreadTimingsSensorDevice().rtMarkRtResetAllTargetsStart();
for (const ControlDevicePtr& controlDev : rtGetControlDevices())
@@ -417,7 +474,8 @@ namespace armarx::RobotUnitModule
rtGetThreadTimingsSensorDevice().rtMarkRtResetAllTargetsEnd();
}
- void ControlThread::rtHandleInvalidTargets()
+ void
+ ControlThread::rtHandleInvalidTargets()
{
rtGetThreadTimingsSensorDevice().rtMarkRtHandleInvalidTargetsStart();
numberOfInvalidTargetsInThisIteration = 0;
@@ -426,8 +484,11 @@ namespace armarx::RobotUnitModule
{
if (!rtGetActivatedJointControllers().at(i)->rtIsTargetValid())
{
- ARMARX_RT_LOGF_ERROR("INVALID TARGET for JointController (idx = %lu) '%s'", i, cdevs.at(i)->rtGetDeviceName().c_str());
- ARMARX_ERROR << ">>>INVALID TARGET for JointController (idx = " << i << ") '" << cdevs.at(i)->rtGetDeviceName().c_str() << "'";
+ ARMARX_RT_LOGF_ERROR("INVALID TARGET for JointController (idx = %lu) '%s'",
+ i,
+ cdevs.at(i)->rtGetDeviceName());
+ ARMARX_ERROR << ">>>INVALID TARGET for JointController (idx = " << i << ") '"
+ << cdevs.at(i)->getDeviceName() << "'";
rtDeactivateAssignedNJointControllerBecauseOfError(i);
++numberOfInvalidTargetsInThisIteration;
}
@@ -436,7 +497,9 @@ namespace armarx::RobotUnitModule
rtGetThreadTimingsSensorDevice().rtMarkRtHandleInvalidTargetsEnd();
}
- void ControlThread::rtReadSensorDeviceValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ ControlThread::rtReadSensorDeviceValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
rtGetThreadTimingsSensorDevice().rtMarkRtReadSensorDeviceValuesStart();
for (const SensorDevicePtr& device : rtGetSensorDevices())
@@ -448,7 +511,9 @@ namespace armarx::RobotUnitModule
rtGetThreadTimingsSensorDevice().rtMarkRtReadSensorDeviceValuesEnd();
}
- void ControlThread::rtPostReadSensorDeviceValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ ControlThread::rtPostReadSensorDeviceValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
if (dynamicsHelpers)
{
@@ -459,7 +524,9 @@ namespace armarx::RobotUnitModule
}
}
- void ControlThread::rtRunJointControllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ ControlThread::rtRunJointControllers(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
rtGetThreadTimingsSensorDevice().rtMarkRtRunJointControllersStart();
for (const ControlDevicePtr& device : rtGetControlDevices())
@@ -469,7 +536,9 @@ namespace armarx::RobotUnitModule
rtGetThreadTimingsSensorDevice().rtMarkRtRunJointControllersEnd();
}
- void ControlThread::rtRunNJointControllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ ControlThread::rtRunNJointControllers(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
rtGetThreadTimingsSensorDevice().rtMarkRtRunNJointControllersStart();
// bool activeControllersChanged = false;
@@ -481,7 +550,8 @@ namespace armarx::RobotUnitModule
{
break;
}
- auto nJointCtrl = static_cast<AsynchronousNJointController*>(activatedNjointCtrls.at(nJointCtrlIndex));
+ auto nJointCtrl = static_cast<AsynchronousNJointController*>(
+ activatedNjointCtrls.at(nJointCtrlIndex));
if (!nJointCtrl)
{
continue;
@@ -492,8 +562,7 @@ namespace armarx::RobotUnitModule
{
ARMARX_RT_LOGF_ERROR(
"NJointControllerBase '%s' requested deactivation while activating it",
- nJointCtrl->rtGetInstanceName().c_str()
- );
+ nJointCtrl->rtGetInstanceName().c_str());
rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex);
}
auto start = TimeUtil::GetTime(true);
@@ -504,25 +573,24 @@ namespace armarx::RobotUnitModule
{
ARMARX_RT_LOGF_ERROR(
"NJointControllerBase '%s' requested deactivation while running it",
- nJointCtrl->rtGetInstanceName().c_str()
- );
+ nJointCtrl->rtGetInstanceName().c_str());
rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex);
}
- if (static_cast<std::size_t>(duration.toMicroSeconds()) > nJointCtrl->rtGetNumberOfUsedControlDevices() * usPerDevUntilError)
+ if (static_cast<std::size_t>(duration.toMicroSeconds()) >
+ nJointCtrl->rtGetNumberOfUsedControlDevices() * usPerDevUntilError)
{
- ARMARX_RT_LOGF_ERROR(
- "The NJointControllerBase '%s' took %ld µs to run!",
- nJointCtrl->rtGetInstanceName().c_str(),
- duration.toMicroSeconds()
- ).deactivateSpam(5);
+ ARMARX_RT_LOGF_ERROR("The NJointControllerBase '%s' took %ld µs to run!",
+ nJointCtrl->rtGetInstanceName().c_str(),
+ duration.toMicroSeconds())
+ .deactivateSpam(5);
}
- else if (static_cast<std::size_t>(duration.toMicroSeconds()) > nJointCtrl->rtGetNumberOfUsedControlDevices() * usPerDevUntilWarn)
+ else if (static_cast<std::size_t>(duration.toMicroSeconds()) >
+ nJointCtrl->rtGetNumberOfUsedControlDevices() * usPerDevUntilWarn)
{
- ARMARX_RT_LOGF_WARNING(
- "The NJointControllerBase '%s' took %ld µs to run!",
- nJointCtrl->rtGetInstanceName().c_str(),
- duration.toMicroSeconds()
- ).deactivateSpam(5);
+ ARMARX_RT_LOGF_WARNING("The NJointControllerBase '%s' took %ld µs to run!",
+ nJointCtrl->rtGetInstanceName().c_str(),
+ duration.toMicroSeconds())
+ .deactivateSpam(5);
}
}
catch (...)
@@ -541,7 +609,8 @@ namespace armarx::RobotUnitModule
{
break;
}
- auto nJointCtrl = static_cast<SynchronousNJointController*>(activatedNjointCtrls.at(nJointCtrlIndex));
+ auto nJointCtrl =
+ static_cast<SynchronousNJointController*>(activatedNjointCtrls.at(nJointCtrlIndex));
try
{
if (nJointCtrl)
@@ -550,8 +619,7 @@ namespace armarx::RobotUnitModule
{
ARMARX_RT_LOGF_ERROR(
"NJointControllerBase '%s' requested deactivation while activating it",
- nJointCtrl->rtGetInstanceName().c_str()
- );
+ nJointCtrl->rtGetInstanceName().c_str());
rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex);
// activeControllersChanged = true;
}
@@ -564,26 +632,25 @@ namespace armarx::RobotUnitModule
{
ARMARX_RT_LOGF_ERROR(
"NJointControllerBase '%s' requested deactivation while running it",
- nJointCtrl->rtGetInstanceName().c_str()
- );
+ nJointCtrl->rtGetInstanceName().c_str());
rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex);
// activeControllersChanged = true;
}
- if (static_cast<std::size_t>(duration.toMicroSeconds()) > nJointCtrl->rtGetNumberOfUsedControlDevices() * usPerDevUntilError)
+ if (static_cast<std::size_t>(duration.toMicroSeconds()) >
+ nJointCtrl->rtGetNumberOfUsedControlDevices() * usPerDevUntilError)
{
- ARMARX_RT_LOGF_ERROR(
- "The NJointControllerBase '%s' took %ld µs to run!",
- nJointCtrl->rtGetInstanceName().c_str(),
- duration.toMicroSeconds()
- ).deactivateSpam(5);
+ ARMARX_RT_LOGF_ERROR("The NJointControllerBase '%s' took %ld µs to run!",
+ nJointCtrl->rtGetInstanceName().c_str(),
+ duration.toMicroSeconds())
+ .deactivateSpam(5);
}
- else if (static_cast<std::size_t>(duration.toMicroSeconds()) > nJointCtrl->rtGetNumberOfUsedControlDevices() * usPerDevUntilWarn)
+ else if (static_cast<std::size_t>(duration.toMicroSeconds()) >
+ nJointCtrl->rtGetNumberOfUsedControlDevices() * usPerDevUntilWarn)
{
- ARMARX_RT_LOGF_WARNING(
- "The NJointControllerBase '%s' took %ld µs to run!",
- nJointCtrl->rtGetInstanceName().c_str(),
- duration.toMicroSeconds()
- ).deactivateSpam(5);
+ ARMARX_RT_LOGF_WARNING("The NJointControllerBase '%s' took %ld µs to run!",
+ nJointCtrl->rtGetInstanceName().c_str(),
+ duration.toMicroSeconds())
+ .deactivateSpam(5);
}
}
}
@@ -603,7 +670,8 @@ namespace armarx::RobotUnitModule
{
break;
}
- auto nJointCtrl = static_cast<AsynchronousNJointController*>(activatedNjointCtrls.at(nJointCtrlIndex));
+ auto nJointCtrl = static_cast<AsynchronousNJointController*>(
+ activatedNjointCtrls.at(nJointCtrlIndex));
if (!nJointCtrl)
{
continue;
@@ -614,8 +682,7 @@ namespace armarx::RobotUnitModule
{
ARMARX_RT_LOGF_ERROR(
"NJointControllerBase '%s' requested deactivation while activating it",
- nJointCtrl->rtGetInstanceName().c_str()
- );
+ nJointCtrl->rtGetInstanceName().c_str());
rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex);
}
auto start = TimeUtil::GetTime(true);
@@ -626,25 +693,24 @@ namespace armarx::RobotUnitModule
{
ARMARX_RT_LOGF_ERROR(
"NJointControllerBase '%s' requested deactivation while running it",
- nJointCtrl->rtGetInstanceName().c_str()
- );
+ nJointCtrl->rtGetInstanceName().c_str());
rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex);
}
- if (static_cast<std::size_t>(duration.toMicroSeconds()) > nJointCtrl->rtGetNumberOfUsedControlDevices() * usPerDevUntilError)
+ if (static_cast<std::size_t>(duration.toMicroSeconds()) >
+ nJointCtrl->rtGetNumberOfUsedControlDevices() * usPerDevUntilError)
{
- ARMARX_RT_LOGF_ERROR(
- "The NJointControllerBase '%s' took %ld µs to run!",
- nJointCtrl->rtGetInstanceName().c_str(),
- duration.toMicroSeconds()
- ).deactivateSpam(5);
+ ARMARX_RT_LOGF_ERROR("The NJointControllerBase '%s' took %ld µs to run!",
+ nJointCtrl->rtGetInstanceName().c_str(),
+ duration.toMicroSeconds())
+ .deactivateSpam(5);
}
- else if (static_cast<std::size_t>(duration.toMicroSeconds()) > nJointCtrl->rtGetNumberOfUsedControlDevices() * usPerDevUntilWarn)
+ else if (static_cast<std::size_t>(duration.toMicroSeconds()) >
+ nJointCtrl->rtGetNumberOfUsedControlDevices() * usPerDevUntilWarn)
{
- ARMARX_RT_LOGF_WARNING(
- "The NJointControllerBase '%s' took %ld µs to run!",
- nJointCtrl->rtGetInstanceName().c_str(),
- duration.toMicroSeconds()
- ).deactivateSpam(5);
+ ARMARX_RT_LOGF_WARNING("The NJointControllerBase '%s' took %ld µs to run!",
+ nJointCtrl->rtGetInstanceName().c_str(),
+ duration.toMicroSeconds())
+ .deactivateSpam(5);
}
}
catch (...)
@@ -659,21 +725,22 @@ namespace armarx::RobotUnitModule
rtGetThreadTimingsSensorDevice().rtMarkRtRunNJointControllersEnd();
}
- void ControlThread::rtDeactivateNJointControllerBecauseOfError(std::size_t nJointCtrlIndex)
+ void
+ ControlThread::rtDeactivateNJointControllerBecauseOfError(std::size_t nJointCtrlIndex)
{
- const NJointControllerBasePtr& nJointCtrl = rtGetActivatedNJointControllers().at(nJointCtrlIndex);
+ const NJointControllerBasePtr& nJointCtrl =
+ rtGetActivatedNJointControllers().at(nJointCtrlIndex);
NJointControllerAttorneyForControlThread::RtDeactivateControllerBecauseOfError(nJointCtrl);
for (auto ctrlDevIdx : nJointCtrl->rtGetControlDeviceUsedIndices())
{
const ControlDevicePtr& controlDevice = rtGetControlDevices().at(ctrlDevIdx);
JointController* es = controlDevice->rtGetJointEmergencyStopController();
- ARMARX_CHECK_EQUAL(
- rtGetActivatedJointToNJointControllerAssignement().at(ctrlDevIdx),
- nJointCtrlIndex) <<
- VAROUT(ctrlDevIdx) << "\n"
- << VAROUT(controlDevice->getDeviceName()) << "\n"
- << dumpRtState();
+ ARMARX_CHECK_EQUAL(rtGetActivatedJointToNJointControllerAssignement().at(ctrlDevIdx),
+ nJointCtrlIndex)
+ << VAROUT(ctrlDevIdx) << "\n"
+ << VAROUT(controlDevice->getDeviceName()) << "\n"
+ << dumpRtState();
controlDevice->rtSetActiveJointController(es);
rtGetActivatedJointControllers().at(ctrlDevIdx) = es;
@@ -690,14 +757,14 @@ namespace armarx::RobotUnitModule
{
continue;
}
- ControlDevice* const dev = rtGetControlDevices().at(ctrlDevIdx).get();
+ ControlDevice* const dev = rtGetControlDevices().at(ctrlDevIdx).get();
JointController* const jointCtrl = dev->rtGetActiveJointController();
const auto hwModeHash = jointCtrl->rtGetHardwareControlModeHash();
//this device is in a group!
// -> check all other devices
for (const auto otherIdx : ctrlModeGroups.deviceIndices.at(groupIdx))
{
- ControlDevice* const otherDev = rtGetControlDevices().at(otherIdx).get();
+ ControlDevice* const otherDev = rtGetControlDevices().at(otherIdx).get();
JointController* const otherJointCtrl = otherDev->rtGetActiveJointController();
const auto otherHwModeHash = otherJointCtrl->rtGetHardwareControlModeHash();
if (hwModeHash == otherHwModeHash)
@@ -705,11 +772,13 @@ namespace armarx::RobotUnitModule
//the assigend ctrl has the same hwMode -> don't do anything
continue;
}
- const auto otherNJointCtrl1Idx = rtGetActivatedJointToNJointControllerAssignement().at(otherIdx);
+ const auto otherNJointCtrl1Idx =
+ rtGetActivatedJointToNJointControllerAssignement().at(otherIdx);
if (otherNJointCtrl1Idx == IndexSentinel())
{
//the hwmodes are different! (hence the other ctrl must be in stop movement
- ARMARX_CHECK_EXPRESSION(otherJointCtrl == otherDev->rtGetJointStopMovementController());
+ ARMARX_CHECK_EXPRESSION(otherJointCtrl ==
+ otherDev->rtGetJointStopMovementController());
//we need to activate the es contoller
JointController* const es = otherDev->rtGetJointEmergencyStopController();
otherDev->rtSetActiveJointController(es);
@@ -725,22 +794,26 @@ namespace armarx::RobotUnitModule
rtDeactivatedNJointControllerBecauseOfError(nJointCtrl);
}
- void ControlThread::rtDeactivateAssignedNJointControllerBecauseOfError(std::size_t ctrlDevIndex)
+ void
+ ControlThread::rtDeactivateAssignedNJointControllerBecauseOfError(std::size_t ctrlDevIndex)
{
- std::size_t nJointCtrlIndex = rtGetActivatedJointToNJointControllerAssignement().at(ctrlDevIndex);
- ARMARX_CHECK_LESS(
- nJointCtrlIndex, rtGetControlDevices().size()) <<
- "no NJoint controller controls this device (name = "
- << rtGetControlDevices().at(ctrlDevIndex)->rtGetDeviceName()
- << ", ControlMode = " << rtGetActivatedJointControllers().at(ctrlDevIndex)->getControlMode() << ")!" << "\n"
- << "This means an invariant is violated! Dumping data for debugging:\n"
- << VAROUT(ctrlDevIndex) << "\n"
- << dumpRtState();
+ std::size_t nJointCtrlIndex =
+ rtGetActivatedJointToNJointControllerAssignement().at(ctrlDevIndex);
+ ARMARX_CHECK_LESS(nJointCtrlIndex, rtGetControlDevices().size())
+ << "no NJoint controller controls this device (name = "
+ << rtGetControlDevices().at(ctrlDevIndex)->getDeviceName() << ", ControlMode = "
+ << rtGetActivatedJointControllers().at(ctrlDevIndex)->getControlMode() << ")!"
+ << "\n"
+ << "This means an invariant is violated! Dumping data for debugging:\n"
+ << VAROUT(ctrlDevIndex) << "\n"
+ << dumpRtState();
rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex);
}
- void ControlThread::rtUpdateSensorAndControlBuffer(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ ControlThread::rtUpdateSensorAndControlBuffer(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
rtGetThreadTimingsSensorDevice().rtMarkRtUpdateSensorAndControlBufferStart();
//commit all changes to activated controllers (joint, njoint, assignement)
@@ -762,8 +835,10 @@ namespace armarx::RobotUnitModule
for (std::size_t ctrlIdx = 0; ctrlIdx < rtGetControlDevices().size(); ++ctrlIdx)
{
ControlDevice& controlDevice = *rtGetControlDevices().at(ctrlIdx);
- ARMARX_CHECK_EQUAL(controlDevice.rtGetJointControllers().size(), sc.control.at(ctrlIdx).size());
- for (std::size_t targIdx = 0; targIdx < controlDevice.rtGetJointControllers().size(); ++targIdx)
+ ARMARX_CHECK_EQUAL(controlDevice.rtGetJointControllers().size(),
+ sc.control.at(ctrlIdx).size());
+ for (std::size_t targIdx = 0; targIdx < controlDevice.rtGetJointControllers().size();
+ ++targIdx)
{
JointController& jointCtrl = *controlDevice.rtGetJointControllers().at(targIdx);
jointCtrl.getControlTarget()->_copyTo(sc.control.at(ctrlIdx).at(targIdx));
@@ -775,22 +850,26 @@ namespace armarx::RobotUnitModule
rtGetThreadTimingsSensorDevice().rtMarkRtUpdateSensorAndControlBufferEnd();
}
- const std::vector<ControlDevicePtr>& ControlThread::rtGetControlDevices() const
+ const std::vector<ControlDevicePtr>&
+ ControlThread::rtGetControlDevices() const
{
return DevicesAttorneyForControlThread::GetControlDevices(this);
}
- const std::vector<SensorDevicePtr>& ControlThread::rtGetSensorDevices()
+ const std::vector<SensorDevicePtr>&
+ ControlThread::rtGetSensorDevices()
{
return DevicesAttorneyForControlThread::GetSensorDevices(this);
}
- RTThreadTimingsSensorDevice& ControlThread::rtGetThreadTimingsSensorDevice()
+ RTThreadTimingsSensorDevice&
+ ControlThread::rtGetThreadTimingsSensorDevice()
{
return DevicesAttorneyForControlThread::GetThreadTimingsSensorDevice(this);
}
- void ControlThread::rtSetEmergencyStopState(EmergencyStopState state)
+ void
+ ControlThread::rtSetEmergencyStopState(EmergencyStopState state)
{
if (state == EmergencyStopState::eEmergencyStopActive)
{
@@ -802,14 +881,16 @@ namespace armarx::RobotUnitModule
}
}
- void ControlThread::_preFinishControlThreadInitialization()
+ void
+ ControlThread::_preFinishControlThreadInitialization()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
controlThreadId = std::this_thread::get_id();
_maxControllerCount = rtGetActivatedJointControllers().size();
- ARMARX_CHECK_EQUAL(_maxControllerCount, rtGetActivatedJointToNJointControllerAssignement().size());
+ ARMARX_CHECK_EQUAL(_maxControllerCount,
+ rtGetActivatedJointToNJointControllerAssignement().size());
ARMARX_CHECK_EQUAL(_maxControllerCount, rtGetActivatedNJointControllers().size());
//resize buffers used for error oputput
preSwitchSetup_ActivatedJointControllers.resize(_maxControllerCount);
@@ -832,32 +913,40 @@ namespace armarx::RobotUnitModule
std::shared_ptr<DynamicsHelper> dynamicsHelpers(new DynamicsHelper(robotUnit));
auto bodySetName = getProperty<std::string>("InverseDynamicsRobotBodySet").getValue();
auto rtRobotBodySet = rtGetRobot()->getRobotNodeSet(bodySetName);
- ARMARX_CHECK_EXPRESSION(rtRobotBodySet) << "could not find robot node set with name: " << bodySetName << " - Check property InverseDynamicsRobotBodySet";
+ ARMARX_CHECK_EXPRESSION(rtRobotBodySet)
+ << "could not find robot node set with name: " << bodySetName
+ << " - Check property InverseDynamicsRobotBodySet";
// rtfilters::RTFilterBasePtr accFilter(new rtfilters::ButterworthFilter(100, 1000, Lowpass, 1));
// rtfilters::RTFilterBasePtr velFilter(new rtfilters::ButterworthFilter(100, 1000, Lowpass, 1));
rtfilters::RTFilterBasePtr accFilter(new rtfilters::AverageFilter(30));
rtfilters::RTFilterBasePtr velFilter(new rtfilters::AverageFilter(30));
- auto setList = armarx::Split(getProperty<std::string>("InverseDynamicsRobotJointSets").getValue(), ",", true, true);
+ auto setList =
+ armarx::Split(getProperty<std::string>("InverseDynamicsRobotJointSets").getValue(),
+ ",",
+ true,
+ true);
for (auto& set : setList)
{
auto rns = rtGetRobot()->getRobotNodeSet(set);
- ARMARX_CHECK_EXPRESSION(rns) << "could not find robot node set with name: " << set << " - Check property InverseDynamicsRobotJointSets";
+ ARMARX_CHECK_EXPRESSION(rns) << "could not find robot node set with name: " << set
+ << " - Check property InverseDynamicsRobotJointSets";
dynamicsHelpers->addRobotPart(rns, rtRobotBodySet, velFilter, accFilter);
ARMARX_VERBOSE << "Added nodeset " << set << " for inverse dynamics";
}
this->dynamicsHelpers = dynamicsHelpers;
}
-
}
- void ControlThread::_preOnInitRobotUnit()
+ void
+ ControlThread::_preOnInitRobotUnit()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
try
{
- rtRobot = VirtualRobot::RobotIO::loadRobot(_module<RobotData>().getRobotFileName(), VirtualRobot::RobotIO::eStructure);
+ rtRobot = VirtualRobot::RobotIO::loadRobot(_module<RobotData>().getRobotFileName(),
+ VirtualRobot::RobotIO::eStructure);
rtRobot->setUpdateCollisionModel(false);
rtRobot->setUpdateVisualization(false);
rtRobot->setThreadsafe(false);
@@ -867,37 +956,49 @@ namespace armarx::RobotUnitModule
{
throw UserException(e.what());
}
- usPerDevUntilWarn = getProperty<std::size_t>("NjointController_AllowedExecutionTimePerControlDeviceUntilWarning").getValue();
- usPerDevUntilError = getProperty<std::size_t>("NjointController_AllowedExecutionTimePerControlDeviceUntilError").getValue();
+ usPerDevUntilWarn = getProperty<std::size_t>(
+ "NjointController_AllowedExecutionTimePerControlDeviceUntilWarning")
+ .getValue();
+ usPerDevUntilError = getProperty<std::size_t>(
+ "NjointController_AllowedExecutionTimePerControlDeviceUntilError")
+ .getValue();
}
- void ControlThread::setEmergencyStopState(EmergencyStopState state, const Ice::Current&)
+ void
+ ControlThread::setEmergencyStopState(EmergencyStopState state, const Ice::Current&)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
_module<Units>().getEmergencyStopMaster()->setEmergencyStopState(state);
}
- EmergencyStopState ControlThread::getEmergencyStopState(const Ice::Current&) const
+ EmergencyStopState
+ ControlThread::getEmergencyStopState(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- return emergencyStop ? EmergencyStopState::eEmergencyStopActive : EmergencyStopState::eEmergencyStopInactive;
+ return emergencyStop ? EmergencyStopState::eEmergencyStopActive
+ : EmergencyStopState::eEmergencyStopInactive;
}
- EmergencyStopState ControlThread::getRtEmergencyStopState(const Ice::Current&) const
+ EmergencyStopState
+ ControlThread::getRtEmergencyStopState(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- return rtIsInEmergencyStop() ? EmergencyStopState::eEmergencyStopActive : EmergencyStopState::eEmergencyStopInactive;
+ return rtIsInEmergencyStop() ? EmergencyStopState::eEmergencyStopActive
+ : EmergencyStopState::eEmergencyStopInactive;
}
- void ControlThread::setEmergencyStopStateNoReportToTopic(EmergencyStopState state)
+ void
+ ControlThread::setEmergencyStopStateNoReportToTopic(EmergencyStopState state)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
emergencyStop = (state == EmergencyStopState::eEmergencyStopActive);
}
- void ControlThread::processEmergencyStopRequest()
+ void
+ ControlThread::processEmergencyStopRequest()
{
- const auto state = emergencyStopStateRequest.exchange(EmergencyStopStateRequest::RequestNone);
+ const auto state =
+ emergencyStopStateRequest.exchange(EmergencyStopStateRequest::RequestNone);
switch (state)
{
case EmergencyStopStateRequest::RequestActive:
@@ -909,56 +1010,70 @@ namespace armarx::RobotUnitModule
case EmergencyStopStateRequest::RequestNone:
break;
default:
- ARMARX_CHECK_EXPRESSION(!static_cast<int>(state)) << "Unkown value for EmergencyStopStateRequest";
+ ARMARX_CHECK_EXPRESSION(!static_cast<int>(state))
+ << "Unkown value for EmergencyStopStateRequest";
}
}
- const std::vector<JointController*>& ControlThread::rtGetActivatedJointControllers() const
+ const std::vector<JointController*>&
+ ControlThread::rtGetActivatedJointControllers() const
{
return ControlThreadDataBufferAttorneyForControlThread::GetActivatedJointControllers(this);
}
- const std::vector<NJointControllerBase*>& ControlThread::rtGetActivatedNJointControllers() const
+ const std::vector<NJointControllerBase*>&
+ ControlThread::rtGetActivatedNJointControllers() const
{
return ControlThreadDataBufferAttorneyForControlThread::GetActivatedNJointControllers(this);
}
- const std::vector<std::size_t>& ControlThread::rtGetActivatedJointToNJointControllerAssignement() const
+ const std::vector<std::size_t>&
+ ControlThread::rtGetActivatedJointToNJointControllerAssignement() const
{
- return ControlThreadDataBufferAttorneyForControlThread::GetActivatedJointToNJointControllerAssignement(this);
+ return ControlThreadDataBufferAttorneyForControlThread::
+ GetActivatedJointToNJointControllerAssignement(this);
}
- std::vector<JointController*>& ControlThread::rtGetActivatedJointControllers()
+ std::vector<JointController*>&
+ ControlThread::rtGetActivatedJointControllers()
{
return ControlThreadDataBufferAttorneyForControlThread::GetActivatedJointControllers(this);
}
- std::vector<NJointControllerBase*>& ControlThread::rtGetActivatedNJointControllers()
+ std::vector<NJointControllerBase*>&
+ ControlThread::rtGetActivatedNJointControllers()
{
return ControlThreadDataBufferAttorneyForControlThread::GetActivatedNJointControllers(this);
}
- std::vector<std::size_t>& ControlThread::rtGetActivatedJointToNJointControllerAssignement()
+ std::vector<std::size_t>&
+ ControlThread::rtGetActivatedJointToNJointControllerAssignement()
{
- return ControlThreadDataBufferAttorneyForControlThread::GetActivatedJointToNJointControllerAssignement(this);
+ return ControlThreadDataBufferAttorneyForControlThread::
+ GetActivatedJointToNJointControllerAssignement(this);
}
- const std::vector<JointController*>& ControlThread::rtGetRequestedJointControllers() const
+ const std::vector<JointController*>&
+ ControlThread::rtGetRequestedJointControllers() const
{
return ControlThreadDataBufferAttorneyForControlThread::GetRequestedJointControllers(this);
}
- const std::vector<NJointControllerBase*>& ControlThread::rtGetRequestedNJointControllers() const
+ const std::vector<NJointControllerBase*>&
+ ControlThread::rtGetRequestedNJointControllers() const
{
return ControlThreadDataBufferAttorneyForControlThread::GetRequestedNJointControllers(this);
}
- const std::vector<std::size_t>& ControlThread::rtGetRequestedJointToNJointControllerAssignement() const
+ const std::vector<std::size_t>&
+ ControlThread::rtGetRequestedJointToNJointControllerAssignement() const
{
- return ControlThreadDataBufferAttorneyForControlThread::GetRequestedJointToNJointControllerAssignement(this);
+ return ControlThreadDataBufferAttorneyForControlThread::
+ GetRequestedJointToNJointControllerAssignement(this);
}
- void ControlThread::rtSyncNJointControllerRobot(NJointControllerBase* njctrl)
+ void
+ ControlThread::rtSyncNJointControllerRobot(NJointControllerBase* njctrl)
{
if (njctrl->rtGetRobot())
{
@@ -972,7 +1087,8 @@ namespace armarx::RobotUnitModule
}
- void ControlThread::dumpRtControllerSetup(
+ void
+ ControlThread::dumpRtControllerSetup(
std::ostream& out,
const std::string& indent,
const std::vector<JointController*>& activeJCtrls,
@@ -985,8 +1101,9 @@ namespace armarx::RobotUnitModule
for (std::size_t i = 0; i < cdevs.size(); ++i)
{
const JointController* jctrl = activeJCtrls.at(i);
- out << indent << "\t(" << i << ")\t" << cdevs.at(i)->rtGetDeviceName() << ":\n"
- << indent << "\t\t Controller: " << jctrl->getControlMode() << " (" << jctrl << ")\n"
+ out << indent << "\t(" << i << ")\t" << cdevs.at(i)->getDeviceName() << ":\n"
+ << indent << "\t\t Controller: " << jctrl->getControlMode() << " (" << jctrl
+ << ")\n"
<< indent << "\t\t Assigned NJoint: " << assignement.at(i) << "\n";
}
}
@@ -995,7 +1112,7 @@ namespace armarx::RobotUnitModule
for (std::size_t i = 0; i < activeNJCtrls.size(); ++i)
{
const auto* njctrl = activeNJCtrls.at(i);
- out << indent << "\t(" << i << ")\t";
+ out << indent << "\t(" << i << ")\t";
if (njctrl)
{
out << njctrl->rtGetInstanceName() << " (" << njctrl << "):"
@@ -1003,57 +1120,58 @@ namespace armarx::RobotUnitModule
}
else
{
- out << " (" << njctrl << ")\n";
+ out << " (" << njctrl << ")\n";
}
}
}
-
}
- std::string ControlThread::dumpRtState() const
+ std::string
+ ControlThread::dumpRtState() const
{
std::stringstream str;
str << "state requested\n";
- dumpRtControllerSetup(
- str, "\t",
- rtGetRequestedJointControllers(),
- rtGetRequestedJointToNJointControllerAssignement(),
- rtGetRequestedNJointControllers());
+ dumpRtControllerSetup(str,
+ "\t",
+ rtGetRequestedJointControllers(),
+ rtGetRequestedJointToNJointControllerAssignement(),
+ rtGetRequestedNJointControllers());
str << "state before rtSwitchControllerSetup() was called\n";
- dumpRtControllerSetup(
- str, "\t",
- preSwitchSetup_ActivatedJointControllers,
- preSwitchSetup_ActivatedJointToNJointControllerAssignement,
- preSwitchSetup_ActivatedNJointControllers);
+ dumpRtControllerSetup(str,
+ "\t",
+ preSwitchSetup_ActivatedJointControllers,
+ preSwitchSetup_ActivatedJointToNJointControllerAssignement,
+ preSwitchSetup_ActivatedNJointControllers);
str << "state after rtSwitchControllerSetup() was called\n";
- dumpRtControllerSetup(
- str, "\t",
- postSwitchSetup_ActivatedJointControllers,
- postSwitchSetup_ActivatedJointToNJointControllerAssignement,
- postSwitchSetup_ActivatedNJointControllers);
+ dumpRtControllerSetup(str,
+ "\t",
+ postSwitchSetup_ActivatedJointControllers,
+ postSwitchSetup_ActivatedJointToNJointControllerAssignement,
+ postSwitchSetup_ActivatedNJointControllers);
str << "state now\n";
- dumpRtControllerSetup(
- str, "\t",
- rtGetActivatedJointControllers(),
- rtGetActivatedJointToNJointControllerAssignement(),
- rtGetActivatedNJointControllers());
+ dumpRtControllerSetup(str,
+ "\t",
+ rtGetActivatedJointControllers(),
+ rtGetActivatedJointToNJointControllerAssignement(),
+ rtGetActivatedNJointControllers());
str << VAROUT(rtSwitchControllerSetupChangedControllers) << "\n";
str << VAROUT(numberOfInvalidTargetsInThisIteration) << "\n";
return str.str();
}
- void ControlThread::rtSetRobotGlobalPose(const Eigen::Matrix4f& gp, bool updateRobot)
+ void
+ ControlThread::rtSetRobotGlobalPose(const Eigen::Matrix4f& gp, bool updateRobot)
{
rtRobot->setGlobalPose(gp, updateRobot);
}
- void ControlThread::rtSetJointController(JointController* c, std::size_t index)
+ void
+ ControlThread::rtSetJointController(JointController* c, std::size_t index)
{
ControlThreadDataBufferAttorneyForControlThread::RTSetJointController(this, c, index);
-
}
-}
+} // namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
index 1ca557e8d84cd565d689445a3f4b0c5e79a256aa..5cf5fed082abdcb79f4f1de97ec9982c90f10d92 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
@@ -21,19 +21,18 @@
*/
-#include <ArmarXCore/util/CPPUtility/trace.h>
-#include <ArmarXCore/util/CPPUtility/Iterator.h>
-#include <ArmarXCore/core/util/FileSystemPathBuilder.h>
-#include <ArmarXCore/core/ArmarXManager.h>
-
#include "RobotUnitModuleLogging.h"
-#include "RobotUnitModuleControlThreadDataBuffer.h"
-#include "RobotUnitModuleDevices.h"
+#include <regex>
-#include "../util/ControlThreadOutputBuffer.h"
+#include <ArmarXCore/core/ArmarXManager.h>
+#include <ArmarXCore/core/util/FileSystemPathBuilder.h>
+#include <ArmarXCore/util/CPPUtility/Iterator.h>
+#include <ArmarXCore/util/CPPUtility/trace.h>
-#include <regex>
+#include "../util/ControlThreadOutputBuffer.h"
+#include "RobotUnitModuleControlThreadDataBuffer.h"
+#include "RobotUnitModuleDevices.h"
namespace armarx::RobotUnitModule::details
{
@@ -46,16 +45,19 @@ namespace armarx::RobotUnitModule::details
t = IceUtil::Time::seconds(0);
}
IceUtil::Time t;
- void start()
+ void
+ start()
{
t -= IceUtil::Time::now();
++n;
}
- void stop()
+ void
+ stop()
{
t += IceUtil::Time::now();
}
- double ms() const
+ double
+ ms() const
{
return t.toMilliSecondsDouble();
}
@@ -75,22 +77,28 @@ namespace armarx::RobotUnitModule::details
DurationsEntry backlog;
DurationsEntry msg;
};
-}
+} // namespace armarx::RobotUnitModule::details
namespace armarx::RobotUnitModule
{
- void Logging::addMarkerToRtLog(const SimpleRemoteReferenceCounterBasePtr& token, const std::string& marker, const Ice::Current&)
+ void
+ Logging::addMarkerToRtLog(const SimpleRemoteReferenceCounterBasePtr& token,
+ const std::string& marker,
+ const Ice::Current&)
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::lock_guard<std::mutex> guard {rtLoggingMutex};
+ std::lock_guard<std::mutex> guard{rtLoggingMutex};
if (!rtLoggingEntries.count(token->getId()))
{
- throw InvalidArgumentException {"addMarkerToRtLog called for a nonexistent log"};
+ throw InvalidArgumentException{"addMarkerToRtLog called for a nonexistent log"};
}
rtLoggingEntries.at(token->getId())->marker = marker;
}
- SimpleRemoteReferenceCounterBasePtr Logging::startRtLogging(const std::string& formatString, const Ice::StringSeq& loggingNames, const Ice::Current&)
+ SimpleRemoteReferenceCounterBasePtr
+ Logging::startRtLogging(const std::string& formatString,
+ const Ice::StringSeq& loggingNames,
+ const Ice::Current&)
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
@@ -102,28 +110,31 @@ namespace armarx::RobotUnitModule
return startRtLoggingWithAliasNames(formatString, alias, Ice::emptyCurrent);
}
- void Logging::stopRtLogging(const SimpleRemoteReferenceCounterBasePtr& token, const Ice::Current&)
+ void
+ Logging::stopRtLogging(const SimpleRemoteReferenceCounterBasePtr& token, const Ice::Current&)
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::lock_guard<std::mutex> guard {rtLoggingMutex};
+ std::lock_guard<std::mutex> guard{rtLoggingMutex};
ARMARX_CHECK_NOT_NULL(token);
- try {
+ try
+ {
if (!rtLoggingEntries.count(token->getId()))
{
- throw InvalidArgumentException {"stopRtLogging called for a nonexistent log"};
+ throw InvalidArgumentException{"stopRtLogging called for a nonexistent log"};
}
- ARMARX_DEBUG_S << "RobotUnit: stop RtLogging for file " << rtLoggingEntries.at(token->getId())->filename;
+ ARMARX_DEBUG_S << "RobotUnit: stop RtLogging for file "
+ << rtLoggingEntries.at(token->getId())->filename;
rtLoggingEntries.at(token->getId())->stopLogging = true;
}
catch (...)
{
ARMARX_WARNING << "Error during attempting to stop rt logging";
}
-
}
- Ice::StringSeq Logging::getLoggingNames(const Ice::Current&) const
+ Ice::StringSeq
+ Logging::getLoggingNames(const Ice::Current&) const
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
@@ -148,18 +159,19 @@ namespace armarx::RobotUnitModule
return result;
}
- SimpleRemoteReferenceCounterBasePtr Logging::startRtLoggingWithAliasNames(
- const std::string& formatString,
- const StringStringDictionary& aliasNames,
- const Ice::Current&)
+ SimpleRemoteReferenceCounterBasePtr
+ Logging::startRtLoggingWithAliasNames(const std::string& formatString,
+ const StringStringDictionary& aliasNames,
+ const Ice::Current&)
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- FileSystemPathBuilder pb {formatString};
- std::lock_guard<std::mutex> guard {rtLoggingMutex};
+ FileSystemPathBuilder pb{formatString};
+ std::lock_guard<std::mutex> guard{rtLoggingMutex};
if (rtLoggingEntries.count(pb.getPath()))
{
- throw InvalidArgumentException {"There already is a logger for the path '" + pb.getPath() + "'"};
+ throw InvalidArgumentException{"There already is a logger for the path '" +
+ pb.getPath() + "'"};
}
rtLoggingEntries[pb.getPath()].reset(new CSVLoggingEntry());
auto ptr = rtLoggingEntries[pb.getPath()];
@@ -170,14 +182,14 @@ namespace armarx::RobotUnitModule
if (!e.stream)
{
rtLoggingEntries.erase(pb.getPath());
- throw LogicError {"RtLogging could not open filestream for '" + pb.getPath() + "'"};
+ throw LogicError{"RtLogging could not open filestream for '" + pb.getPath() + "'"};
}
- ARMARX_INFO << "Start logging to " << e.filename
- << ". Names (pattern, replacement name): " << aliasNames;
+ ARMARX_VERBOSE << "Start logging to " << e.filename
+ << ". Names (pattern, replacement name): " << aliasNames;
std::stringstream header;
header << "marker;iteration;timestamp;TimeSinceLastIteration";
- auto logDev = [&](const std::string & dev)
+ auto logDev = [&](const std::string& dev)
{
ARMARX_TRACE_LITE;
for (const auto& [key, value] : aliasNames)
@@ -231,38 +243,45 @@ namespace armarx::RobotUnitModule
ARMARX_TRACE;
//write header
- e.stream << header.str() << std::flush; // newline is written at the beginning of each log line
+ e.stream << header.str()
+ << std::flush; // newline is written at the beginning of each log line
//create and return handle
auto block = getArmarXManager()->createSimpleRemoteReferenceCount(
- [ptr]()
- {
- ARMARX_DEBUG_S << "RobotUnit: stop RtLogging for file " << ptr->filename;
- ptr->stopLogging = true;
- }, e.filename, IceUtil::Time::milliSeconds(100));
+ [ptr]()
+ {
+ ARMARX_DEBUG_S << "RobotUnit: stop RtLogging for file " << ptr->filename;
+ ptr->stopLogging = true;
+ },
+ e.filename,
+ IceUtil::Time::milliSeconds(100));
auto counter = block->getReferenceCounter();
block->activateCounting();
ARMARX_DEBUG_S << "RobotUnit: start RtLogging for file " << ptr->filename;
return counter;
}
- void Logging::writeRecentIterationsToFile(const std::string& formatString, const Ice::Current&) const
+ void
+ Logging::writeRecentIterationsToFile(const std::string& formatString, const Ice::Current&) const
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::lock_guard<std::mutex> guard {rtLoggingMutex};
- FileSystemPathBuilder pb {formatString};
+ std::lock_guard<std::mutex> guard{rtLoggingMutex};
+ FileSystemPathBuilder pb{formatString};
pb.createParentDirectories();
- std::ofstream outCSV {pb.getPath() + ".csv"};
+ std::ofstream outCSV{pb.getPath() + ".csv"};
if (!outCSV)
{
- throw LogicError {"writeRecentIterationsToFile could not open filestream for '" + pb.getPath() + ".csv'"};
+ throw LogicError{"writeRecentIterationsToFile could not open filestream for '" +
+ pb.getPath() + ".csv'"};
}
- std::ofstream outMsg {pb.getPath() + ".messages"};
+ std::ofstream outMsg{pb.getPath() + ".messages"};
if (!outMsg)
{
- throw LogicError {"writeRecentIterationsToFile could not open filestream for '" + pb.getPath() + ".messages'"};
+ throw LogicError{"writeRecentIterationsToFile could not open filestream for '" +
+ pb.getPath() + ".messages'"};
}
- ARMARX_INFO << "writing the last " << backlog.size() << " iterations to " << pb.getPath() << ".{csv, messages}";
+ ARMARX_INFO << "writing the last " << backlog.size() << " iterations to " << pb.getPath()
+ << ".{csv, messages}";
//write csv header
{
outCSV << "iteration;timestamp";
@@ -290,16 +309,17 @@ namespace armarx::RobotUnitModule
{
//write csv data
{
- outCSV << iteration.iteration << ";" << iteration.sensorValuesTimestamp;
+ outCSV << iteration.iteration << ";" << iteration.sensorValuesTimestamp;
//sens
{
for (const SensorValueBase* val : iteration.sensors)
{
- for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++ idxField)
+ for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields();
+ ++idxField)
{
std::string s;
val->getDataFieldAs(idxField, s);
- outCSV << ";" << s;
+ outCSV << ";" << s;
}
}
}
@@ -309,11 +329,12 @@ namespace armarx::RobotUnitModule
{
for (const ControlTargetBase* val : vals)
{
- for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++ idxField)
+ for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields();
+ ++idxField)
{
std::string s;
val->getDataFieldAs(idxField, s);
- outCSV << ";" << s;
+ outCSV << ";" << s;
}
}
}
@@ -323,7 +344,8 @@ namespace armarx::RobotUnitModule
//write message data
{
bool atLeastOneMessage = false;
- for (const ::armarx::detail::RtMessageLogEntryBase* msg : iteration.messages.getEntries())
+ for (const ::armarx::detail::RtMessageLogEntryBase* msg :
+ iteration.messages.getEntries())
{
if (!msg)
{
@@ -339,27 +361,28 @@ namespace armarx::RobotUnitModule
outMsg << "\nmessages lost: " << iteration.messages.messagesLost
<< " (required additional "
<< iteration.messages.requiredAdditionalBufferSpace << " bytes, "
- << iteration.messages.requiredAdditionalEntries << " message entries)\n" << std::endl;
+ << iteration.messages.requiredAdditionalEntries << " message entries)\n"
+ << std::endl;
}
}
}
}
- RobotUnitDataStreaming::DataStreamingDescription Logging::startDataStreaming(
- const RobotUnitDataStreaming::ReceiverPrx& receiver,
- const RobotUnitDataStreaming::Config& config,
- const Ice::Current&)
+ RobotUnitDataStreaming::DataStreamingDescription
+ Logging::startDataStreaming(const RobotUnitDataStreaming::ReceiverPrx& receiver,
+ const RobotUnitDataStreaming::Config& config,
+ const Ice::Current&)
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
if (!receiver)
{
- throw InvalidArgumentException {"Receiver proxy is NULL!"};
+ throw InvalidArgumentException{"Receiver proxy is NULL!"};
}
- std::lock_guard<std::mutex> guard {rtLoggingMutex};
+ std::lock_guard<std::mutex> guard{rtLoggingMutex};
if (rtDataStreamingEntry.count(receiver))
{
- throw InvalidArgumentException {"There already is a logger for the given receiver"};
+ throw InvalidArgumentException{"There already is a logger for the given receiver"};
}
RobotUnitDataStreaming::DataStreamingDescription result;
@@ -369,7 +392,7 @@ namespace armarx::RobotUnitModule
ARMARX_INFO << "start data streaming to " << receiver->ice_getIdentity().name
<< ". Values: " << config.loggingNames;
- auto devMatchesAnyKey = [&](const std::string & dev)
+ auto devMatchesAnyKey = [&](const std::string& dev)
{
for (const auto& key : config.loggingNames)
{
@@ -381,11 +404,10 @@ namespace armarx::RobotUnitModule
return false;
};
- const auto handleVal = [&](
- const ValueMetaData & valData,
- DataStreamingEntry & streamingEntry,
- RobotUnitDataStreaming::DataStreamingDescription & descr
- ) -> std::vector<DataStreamingEntry::OutVal>
+ const auto handleVal = [&](const ValueMetaData& valData,
+ DataStreamingEntry& streamingEntry,
+ RobotUnitDataStreaming::DataStreamingDescription& descr)
+ -> std::vector<DataStreamingEntry::OutVal>
{
ARMARX_TRACE_LITE;
std::vector<DataStreamingEntry::OutVal> result;
@@ -397,35 +419,34 @@ namespace armarx::RobotUnitModule
continue; //do not add to result and skipp during processing
}
auto& descrEntr = descr.entries[valData.fields.at(i).name];
- //formatter failes here!
- //*INDENT-OFF*
- #define make_case(Type, TName) \
- (typeid(Type) == *valData.fields.at(i).type) \
- { \
- descrEntr.index = streamingEntry.num##TName##s; \
- descrEntr.type = RobotUnitDataStreaming::NodeType##TName; \
- result.at(i).idx = streamingEntry.num##TName##s; \
- result.at(i).value = DataStreamingEntry::ValueT::TName; \
- ++streamingEntry.num##TName##s; \
- }
- if make_case(bool, Bool)
- else if make_case(Ice::Byte, Byte)
- else if make_case(Ice::Short, Short)
- else if make_case(Ice::Int, Int)
- else if make_case(Ice::Long, Long)
- else if make_case(Ice::Float, Float)
- else if make_case(Ice::Double, Double)
- else if make_case(std::uint16_t, Long)
- else if make_case(std::uint32_t, Long)
- else
- {
- ARMARX_CHECK_EXPRESSION(false)
+//formatter failes here!
+//*INDENT-OFF*
+#define make_case(Type, TName) \
+ (typeid(Type) == *valData.fields.at(i).type) \
+ { \
+ descrEntr.index = streamingEntry.num##TName##s; \
+ descrEntr.type = RobotUnitDataStreaming::NodeType##TName; \
+ result.at(i).idx = streamingEntry.num##TName##s; \
+ result.at(i).value = DataStreamingEntry::ValueT::TName; \
+ ++streamingEntry.num##TName##s; \
+ }
+ if make_case (bool, Bool)
+ else if make_case (Ice::Byte, Byte) else if make_case (Ice::Short, Short) else if make_case (Ice::Int, Int) else if make_case (
+ Ice::Long,
+ Long) else if make_case (Ice::Float,
+ Float) else if make_case (Ice::Double,
+ Double) else if make_case (std::
+ uint16_t,
+ Long) else if make_case (std::
+ uint32_t,
+ Long) else
+ {
+ ARMARX_CHECK_EXPRESSION(false)
<< "This code sould be unreachable! "
"The type of "
- << valData.fields.at(i).name
- << " is not handled correctly!";
- }
- #undef make_case
+ << valData.fields.at(i).name << " is not handled correctly!";
+ }
+#undef make_case
//*INDENT-ON*
}
return result;
@@ -437,8 +458,7 @@ namespace armarx::RobotUnitModule
streamingEntry.sensDevs.reserve(sensorDeviceValueMetaData.size());
for (const auto& valData : sensorDeviceValueMetaData)
{
- streamingEntry.sensDevs.emplace_back(
- handleVal(valData, streamingEntry, result));
+ streamingEntry.sensDevs.emplace_back(handleVal(valData, streamingEntry, result));
}
}
//get logged control device values
@@ -452,8 +472,7 @@ namespace armarx::RobotUnitModule
ctrlDevEntrs.reserve(devData.size());
for (const auto& valData : devData)
{
- ctrlDevEntrs.emplace_back(
- handleVal(valData, streamingEntry, result));
+ ctrlDevEntrs.emplace_back(handleVal(valData, streamingEntry, result));
}
}
}
@@ -461,20 +480,23 @@ namespace armarx::RobotUnitModule
return result;
}
- void Logging::stopDataStreaming(const RobotUnitDataStreaming::ReceiverPrx& receiver, const Ice::Current&)
+ void
+ Logging::stopDataStreaming(const RobotUnitDataStreaming::ReceiverPrx& receiver,
+ const Ice::Current&)
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::lock_guard<std::mutex> guard {rtLoggingMutex};
+ std::lock_guard<std::mutex> guard{rtLoggingMutex};
if (!rtDataStreamingEntry.count(receiver))
{
- throw InvalidArgumentException {"stopDataStreaming called for a nonexistent log"};
+ throw InvalidArgumentException{"stopDataStreaming called for a nonexistent log"};
}
ARMARX_INFO_S << "RobotUnit: request to stop DataStreaming for " << receiver->ice_id();
rtDataStreamingEntry.at(receiver).stopStreaming = true;
}
- void Logging::_preFinishRunning()
+ void
+ Logging::_preFinishRunning()
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
@@ -488,46 +510,56 @@ namespace armarx::RobotUnitModule
}
}
- void Logging::_preFinishControlThreadInitialization()
+ void
+ Logging::_preFinishControlThreadInitialization()
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
controlThreadId = LogSender::getThreadId();
- ControlThreadOutputBuffer::RtLoggingInstance = &(_module<ControlThreadDataBuffer>().getControlThreadOutputBuffer());
+ ControlThreadOutputBuffer::RtLoggingInstance =
+ &(_module<ControlThreadDataBuffer>().getControlThreadOutputBuffer());
ARMARX_INFO << "starting rt logging with timestep " << rtLoggingTimestepMs;
stopRtLoggingTask = false;
- rtLoggingTask = std::thread{[&]{
+ rtLoggingTask = std::thread{
+ [&]
+ {
using clock_t = std::chrono::steady_clock;
- const auto now = []{return clock_t::now();};
+ const auto now = [] { return clock_t::now(); };
while (!stopRtLoggingTask)
{
const auto start = now();
doLogging();
- const std::uint64_t ms = std::chrono::duration_cast<std::chrono::milliseconds>(now() - start).count();
+ const std::uint64_t ms =
+ std::chrono::duration_cast<std::chrono::milliseconds>(now() - start)
+ .count();
if (ms < rtLoggingTimestepMs)
{
- std::this_thread::sleep_for(std::chrono::milliseconds{rtLoggingTimestepMs - ms});
+ std::this_thread::sleep_for(
+ std::chrono::milliseconds{rtLoggingTimestepMs - ms});
continue;
}
- ARMARX_WARNING << deactivateSpam(10) << "logging thread took " << ms
- << " ms > " << rtLoggingTimestepMs << " ms (message printed every 10 seconds)";
+ ARMARX_WARNING << deactivateSpam(10) << "logging thread took " << ms << " ms > "
+ << rtLoggingTimestepMs
+ << " ms (message printed every 10 seconds)";
}
}};
}
- void Logging::doLogging()
+ void
+ Logging::doLogging()
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::lock_guard<std::mutex> guard {rtLoggingMutex};
+ std::lock_guard<std::mutex> guard{rtLoggingMutex};
const auto now = IceUtil::Time::now();
// entries are removed last
//remove backlog entries
const auto start_time_remove_backlog_entries = IceUtil::Time::now();
{
- while (!backlog.empty() && backlog.front().writeTimestamp + rtLoggingBacklogRetentionTime < now)
+ while (!backlog.empty() &&
+ backlog.front().writeTimestamp + rtLoggingBacklogRetentionTime < now)
{
backlog.pop_front();
}
@@ -539,19 +571,21 @@ namespace armarx::RobotUnitModule
ARMARX_TRACE;
if (!rtLoggingEntries.empty() || !rtDataStreamingEntry.empty())
{
- ARMARX_INFO << deactivateSpam()
- << "Number of logs " << rtLoggingEntries.size() << '\n'
- << "Number of streams " << rtDataStreamingEntry.size();
+ ARMARX_DEBUG << deactivateSpam() << "Number of logs " << rtLoggingEntries.size()
+ << '\n'
+ << "Number of streams " << rtDataStreamingEntry.size();
}
- _module<ControlThreadDataBuffer>().getControlThreadOutputBuffer().foreachNewLoggingEntry(
- [&](const auto & data, auto i, auto num)
- {
- ARMARX_TRACE;
- doLogging(dlogdurs, now, data, i, num);
- }
- );
+ _module<ControlThreadDataBuffer>()
+ .getControlThreadOutputBuffer()
+ .foreachNewLoggingEntry(
+ [&](const auto& data, auto i, auto num)
+ {
+ ARMARX_TRACE;
+ doLogging(dlogdurs, now, data, i, num);
+ });
}
- ARMARX_DEBUG_S << ::deactivateSpam() << "the last " << backlog.size() << " iterations are stored";
+ ARMARX_DEBUG << ::deactivateSpam() << "the last " << backlog.size()
+ << " iterations are stored";
//flush all files
const auto start_time_flush_all_files = IceUtil::Time::now();
{
@@ -604,33 +638,38 @@ namespace armarx::RobotUnitModule
rtDataStreamingEntry.erase(prx);
}
}
+ // clang-format off
const auto end_time = IceUtil::Time::now();
const auto time_total = (end_time - now).toMilliSecondsDouble();
- ARMARX_DEBUG_S << "rtlogging time required: " << time_total << "ms\n"
- << " time_remove_backlog_entries " << (start_time_log_all - start_time_remove_backlog_entries).toMilliSecondsDouble() << "ms\n"
- << " time_log_all " << (start_time_flush_all_files - start_time_log_all).toMilliSecondsDouble() << "ms\n"
- << " header " << dlogdurs.header.ms() << "ms\t(" << dlogdurs.header.n << " calls)\n"
- << " csv " << dlogdurs.header_csv.ms() << "ms\t(" << dlogdurs.header_csv.n << " calls)\n"
- << " stream " << dlogdurs.header_stream.ms() << "ms\t(" << dlogdurs.header_stream.n << " calls)\n"
- << " sens " << dlogdurs.sens.ms() << "ms\t(" << dlogdurs.sens.n << " calls)\n"
- << " csv " << dlogdurs.sens_csv.ms() << "ms\t(" << dlogdurs.sens_csv.n << " calls)\n"
- << " stream " << dlogdurs.sens_stream.ms() << "ms\t(" << dlogdurs.sens_stream.n << " calls)\n"
- << " per elem " << dlogdurs.sens_stream_elem.ms() << "ms\t(" << dlogdurs.sens_stream_elem.n << " calls)\n"
- << " ctrl " << dlogdurs.ctrl.ms() << "ms\t(" << dlogdurs.ctrl.n << " calls)\n"
- << " csv " << dlogdurs.ctrl_csv.ms() << "ms\t(" << dlogdurs.ctrl_csv.n << " calls)\n"
- << " stream " << dlogdurs.ctrl_stream.ms() << "ms\t(" << dlogdurs.ctrl_stream.n << " calls)\n"
- << " per elem " << dlogdurs.ctrl_stream_elem.ms() << "ms\t(" << dlogdurs.ctrl_stream_elem.n << " calls)\n"
- << " backlog " << dlogdurs.backlog.ms() << "ms\t(" << dlogdurs.backlog.n << " calls)\n"
- << " msg " << dlogdurs.msg.ms() << "ms\t(" << dlogdurs.msg.n << " calls)\n"
- << " time_flush_all_files " << (start_time_remove_entries - start_time_flush_all_files).toMilliSecondsDouble() << "ms\n"
- << " time_remove_entries " << (start_time_data_streaming - start_time_remove_entries).toMilliSecondsDouble() << "ms\n"
- << " time_data_streaming " << (end_time - start_time_data_streaming).toMilliSecondsDouble() << "ms\n";
+ ARMARX_DEBUG << deactivateSpam(1)
+ << "rtlogging time required: " << time_total << "ms\n"
+ << " time_remove_backlog_entries " << (start_time_log_all - start_time_remove_backlog_entries).toMilliSecondsDouble() << "ms\n"
+ << " time_log_all " << (start_time_flush_all_files - start_time_log_all).toMilliSecondsDouble() << "ms\n"
+ << " header " << dlogdurs.header.ms() << "ms\t(" << dlogdurs.header.n << " calls)\n"
+ << " csv " << dlogdurs.header_csv.ms() << "ms\t(" << dlogdurs.header_csv.n << " calls)\n"
+ << " stream " << dlogdurs.header_stream.ms() << "ms\t(" << dlogdurs.header_stream.n << " calls)\n"
+ << " sens " << dlogdurs.sens.ms() << "ms\t(" << dlogdurs.sens.n << " calls)\n"
+ << " csv " << dlogdurs.sens_csv.ms() << "ms\t(" << dlogdurs.sens_csv.n << " calls)\n"
+ << " stream " << dlogdurs.sens_stream.ms() << "ms\t(" << dlogdurs.sens_stream.n << " calls)\n"
+ << " per elem " << dlogdurs.sens_stream_elem.ms() << "ms\t(" << dlogdurs.sens_stream_elem.n << " calls)\n"
+ << " ctrl " << dlogdurs.ctrl.ms() << "ms\t(" << dlogdurs.ctrl.n << " calls)\n"
+ << " csv " << dlogdurs.ctrl_csv.ms() << "ms\t(" << dlogdurs.ctrl_csv.n << " calls)\n"
+ << " stream " << dlogdurs.ctrl_stream.ms() << "ms\t(" << dlogdurs.ctrl_stream.n << " calls)\n"
+ << " per elem " << dlogdurs.ctrl_stream_elem.ms() << "ms\t(" << dlogdurs.ctrl_stream_elem.n << " calls)\n"
+ << " backlog " << dlogdurs.backlog.ms() << "ms\t(" << dlogdurs.backlog.n << " calls)\n"
+ << " msg " << dlogdurs.msg.ms() << "ms\t(" << dlogdurs.msg.n << " calls)\n"
+ << " time_flush_all_files " << (start_time_remove_entries - start_time_flush_all_files).toMilliSecondsDouble() << "ms\n"
+ << " time_remove_entries " << (start_time_data_streaming - start_time_remove_entries).toMilliSecondsDouble() << "ms\n"
+ << " time_data_streaming " << (end_time - start_time_data_streaming).toMilliSecondsDouble() << "ms\n";
+ // clang-format on
}
- void Logging::doLogging(details::DoLoggingDurations& durations,
- const IceUtil::Time& now, const
- ControlThreadOutputBuffer::Entry& data,
- std::size_t i, std::size_t num)
+ void
+ Logging::doLogging(details::DoLoggingDurations& durations,
+ const IceUtil::Time& now,
+ const ControlThreadOutputBuffer::Entry& data,
+ std::size_t i,
+ std::size_t num)
{
ARMARX_TRACE;
@@ -645,8 +684,7 @@ namespace armarx::RobotUnitModule
for (auto& [_, e] : rtLoggingEntries)
{
e->stream << "\n"
- << e->marker << ";"
- << data.iteration << ";"
+ << e->marker << ";" << data.iteration << ";"
<< data.sensorValuesTimestamp.toMicroSeconds() << ";"
<< data.timeSinceLastIteration.toMicroSeconds();
e->marker.clear();
@@ -666,416 +704,425 @@ namespace armarx::RobotUnitModule
durations.header.stop();
}
//process devices
+ {//sens
+ {ARMARX_TRACE;
+ durations.sens.start();
+ //sensors
+ for (std::size_t idxDev = 0; idxDev < data.sensors.size(); ++idxDev)
{
- //sens
+ const SensorValueBase* val = data.sensors.at(idxDev);
+ //dimensions of sensor value (e.g. vel, tor, f_x, f_y, ...)
+ for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++idxField)
{
- ARMARX_TRACE;
- durations.sens.start();
- //sensors
- for (std::size_t idxDev = 0; idxDev < data.sensors.size(); ++ idxDev)
+ if (!rtLoggingEntries.empty())
{
- const SensorValueBase* val = data.sensors.at(idxDev);
- //dimensions of sensor value (e.g. vel, tor, f_x, f_y, ...)
- for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++ idxField)
+ durations.sens_csv.start();
+ const auto str = val->getDataFieldAs<std::string>(idxField);
+ for (auto& [_, entry] : rtLoggingEntries)
{
- if (!rtLoggingEntries.empty())
- {
- durations.sens_csv.start();
- const auto str = val->getDataFieldAs<std::string>(idxField);
- for (auto& [_, entry] : rtLoggingEntries)
- {
- if (entry->loggedSensorDeviceValues.at(idxDev).at(idxField))
- {
- entry->stream << ";" << str;
- }
- }
- durations.sens_csv.stop();
- }
- if (!rtDataStreamingEntry.empty())
+ if (entry->loggedSensorDeviceValues.at(idxDev).at(idxField))
{
- durations.sens_stream.start();
- for (auto& [_, data] : rtDataStreamingEntry)
- {
- durations.sens_stream_elem.start();
- data.processSens(*val, idxDev, idxField);
- durations.sens_stream_elem.stop();
- }
- durations.sens_stream.stop();
+ entry->stream << ";" << str;
}
}
+ durations.sens_csv.stop();
}
- durations.sens.stop();
- }
- //ctrl
- {
- durations.ctrl.start();
- ARMARX_TRACE;
- //joint controllers
- for (std::size_t idxDev = 0; idxDev < data.control.size(); ++ idxDev)
+ if (!rtDataStreamingEntry.empty())
{
- const auto& vals = data.control.at(idxDev);
- //control value (e.g. v_platform)
- for (std::size_t idxVal = 0; idxVal < vals.size(); ++idxVal)
+ durations.sens_stream.start();
+ for (auto& [_, data] : rtDataStreamingEntry)
{
- const ControlTargetBase* val = vals.at(idxVal);
- //dimensions of control value (e.g. v_platform_x, v_platform_y, v_platform_rotate)
- for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++ idxField)
- {
- if (!rtLoggingEntries.empty())
- {
- durations.ctrl_csv.start();
- std::string str;
- val->getDataFieldAs(idxField, str); // expensive function
- for (auto& [_, entry] : rtLoggingEntries)
- {
- if (entry->loggedControlDeviceValues.at(idxDev).at(idxVal).at(idxField))
- {
- entry->stream << ";" << str;
- }
- }
- durations.ctrl_csv.stop();
- }
- if (!rtDataStreamingEntry.empty())
- {
- durations.ctrl_stream.start();
- for (auto& [_, data] : rtDataStreamingEntry)
- {
- durations.ctrl_stream_elem.start();
- data.processCtrl(*val, idxDev, idxVal, idxField);
- durations.ctrl_stream_elem.stop();
- }
- durations.ctrl_stream.stop();
- }
- }
+ durations.sens_stream_elem.start();
+ data.processSens(*val, idxDev, idxField);
+ durations.sens_stream_elem.stop();
}
+ durations.sens_stream.stop();
}
- durations.ctrl.stop();
}
}
- //finish processing
+ durations.sens.stop();
+ }
+ //ctrl
+ {
+ durations.ctrl.start();
+ ARMARX_TRACE;
+ //joint controllers
+ for (std::size_t idxDev = 0; idxDev < data.control.size(); ++idxDev)
{
- //store data to backlog
+ const auto& vals = data.control.at(idxDev);
+ //control value (e.g. v_platform)
+ for (std::size_t idxVal = 0; idxVal < vals.size(); ++idxVal)
{
- durations.backlog.start();
- ARMARX_TRACE;
- if (data.writeTimestamp + rtLoggingBacklogRetentionTime >= now)
+ const ControlTargetBase* val = vals.at(idxVal);
+ //dimensions of control value (e.g. v_platform_x, v_platform_y, v_platform_rotate)
+ for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++idxField)
{
- backlog.emplace_back(data, true); //true for minimal copy
- }
- durations.backlog.stop();
- }
- //print + reset messages
- {
- durations.msg.start();
- ARMARX_TRACE;
- for (const ::armarx::detail::RtMessageLogEntryBase* ptr : data.messages.getEntries())
- {
- if (!ptr)
+ if (!rtLoggingEntries.empty())
{
- break;
+ durations.ctrl_csv.start();
+ std::string str;
+ val->getDataFieldAs(idxField, str); // expensive function
+ for (auto& [_, entry] : rtLoggingEntries)
+ {
+ if (entry->loggedControlDeviceValues.at(idxDev).at(idxVal).at(idxField))
+ {
+ entry->stream << ";" << str;
+ }
+ }
+ durations.ctrl_csv.stop();
+ }
+ if (!rtDataStreamingEntry.empty())
+ {
+ durations.ctrl_stream.start();
+ for (auto& [_, data] : rtDataStreamingEntry)
+ {
+ durations.ctrl_stream_elem.start();
+ data.processCtrl(*val, idxDev, idxVal, idxField);
+ durations.ctrl_stream_elem.stop();
+ }
+ durations.ctrl_stream.stop();
}
- ptr->print(controlThreadId);
}
- durations.msg.stop();
}
}
+ durations.ctrl.stop();
}
-
- bool Logging::MatchName(const std::string& pattern, const std::string& name)
+} // namespace armarx::RobotUnitModule
+//finish processing
+{
+ //store data to backlog
{
+ durations.backlog.start();
ARMARX_TRACE;
- if (pattern.empty())
+ if (data.writeTimestamp + rtLoggingBacklogRetentionTime >= now)
{
- return false;
+ backlog.emplace_back(data, true); //true for minimal copy
}
- static const std::regex pattern_regex{R"(^\^?[- ._*a-zA-Z0-9]+\$?$)"};
- if (!std::regex_match(pattern, pattern_regex))
+ durations.backlog.stop();
+ }
+ //print + reset messages
+ {
+ durations.msg.start();
+ ARMARX_TRACE;
+ for (const ::armarx::detail::RtMessageLogEntryBase* ptr : data.messages.getEntries())
{
- throw InvalidArgumentException {"Pattern '" + pattern + "' is invalid"};
+ if (!ptr)
+ {
+ break;
+ }
+ ptr->print(controlThreadId);
}
- static const std::regex reg_dot{"[.]"};
- static const std::regex reg_star{"[*]"};
- const std::string rpl1 = std::regex_replace(pattern, reg_dot, "\\.");
- const std::string rpl2 = std::regex_replace(rpl1, reg_star, ".*");
- const std::regex key_regex{rpl2};
- return std::regex_search(name, key_regex);
+ durations.msg.stop();
}
+}
+}
- void Logging::_postOnInitRobotUnit()
+bool
+Logging::MatchName(const std::string& pattern, const std::string& name)
+{
+ ARMARX_TRACE;
+ if (pattern.empty())
{
- ARMARX_TRACE;
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- rtLoggingTimestepMs = getProperty<std::size_t>("RTLogging_PeriodMs");
- ARMARX_CHECK_LESS(0, rtLoggingTimestepMs) << "The property RTLoggingPeriodMs must not be 0";
+ return false;
+ }
+ static const std::regex pattern_regex{R"(^\^?[- ._*a-zA-Z0-9]+\$?$)"};
+ if (!std::regex_match(pattern, pattern_regex))
+ {
+ throw InvalidArgumentException{"Pattern '" + pattern + "' is invalid"};
+ }
+ static const std::regex reg_dot{"[.]"};
+ static const std::regex reg_star{"[*]"};
+ const std::string rpl1 = std::regex_replace(pattern, reg_dot, "\\.");
+ const std::string rpl2 = std::regex_replace(rpl1, reg_star, ".*");
+ const std::regex key_regex{rpl2};
+ return std::regex_search(name, key_regex);
+}
- messageBufferSize = getProperty<std::size_t>("RTLogging_MessageBufferSize");
- messageBufferMaxSize = getProperty<std::size_t>("RTLogging_MaxMessageBufferSize");
- ARMARX_CHECK_LESS_EQUAL(messageBufferSize, messageBufferMaxSize);
+void
+Logging::_postOnInitRobotUnit()
+{
+ ARMARX_TRACE;
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ rtLoggingTimestepMs = getProperty<std::size_t>("RTLogging_PeriodMs");
+ ARMARX_CHECK_LESS(0, rtLoggingTimestepMs) << "The property RTLoggingPeriodMs must not be 0";
- messageBufferNumberEntries = getProperty<std::size_t>("RTLogging_MessageNumber");
- messageBufferMaxNumberEntries = getProperty<std::size_t>("RTLogging_MaxMessageNumber");
- ARMARX_CHECK_LESS_EQUAL(messageBufferNumberEntries, messageBufferMaxNumberEntries);
+ messageBufferSize = getProperty<std::size_t>("RTLogging_MessageBufferSize");
+ messageBufferMaxSize = getProperty<std::size_t>("RTLogging_MaxMessageBufferSize");
+ ARMARX_CHECK_LESS_EQUAL(messageBufferSize, messageBufferMaxSize);
- rtLoggingBacklogRetentionTime = IceUtil::Time::milliSeconds(getProperty<std::size_t>("RTLogging_KeepIterationsForMs"));
+ messageBufferNumberEntries = getProperty<std::size_t>("RTLogging_MessageNumber");
+ messageBufferMaxNumberEntries = getProperty<std::size_t>("RTLogging_MaxMessageNumber");
+ ARMARX_CHECK_LESS_EQUAL(messageBufferNumberEntries, messageBufferMaxNumberEntries);
- ARMARX_CHECK_GREATER(getControlThreadTargetPeriod().toMicroSeconds(), 0);
- }
+ rtLoggingBacklogRetentionTime =
+ IceUtil::Time::milliSeconds(getProperty<std::size_t>("RTLogging_KeepIterationsForMs"));
- void Logging::_postFinishDeviceInitialization()
+ ARMARX_CHECK_GREATER(getControlThreadTargetPeriod().toMicroSeconds(), 0);
+}
+
+void
+Logging::_postFinishDeviceInitialization()
+{
+ ARMARX_TRACE;
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ //init buffer
{
ARMARX_TRACE;
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- //init buffer
- {
- ARMARX_TRACE;
- std::size_t ctrlThreadPeriodUs = static_cast<std::size_t>(getControlThreadTargetPeriod().toMicroSeconds());
- std::size_t logThreadPeriodUs = rtLoggingTimestepMs * 1000;
- std::size_t nBuffers = (logThreadPeriodUs / ctrlThreadPeriodUs + 1) * 100;
+ std::size_t ctrlThreadPeriodUs =
+ static_cast<std::size_t>(getControlThreadTargetPeriod().toMicroSeconds());
+ std::size_t logThreadPeriodUs = rtLoggingTimestepMs * 1000;
+ std::size_t nBuffers = (logThreadPeriodUs / ctrlThreadPeriodUs + 1) * 100;
- const auto bufferSize = _module<ControlThreadDataBuffer>().getControlThreadOutputBuffer().initialize(
- nBuffers, _module<Devices>().getControlDevices(), _module<Devices>().getSensorDevices(),
- messageBufferSize, messageBufferNumberEntries,
- messageBufferMaxSize, messageBufferMaxNumberEntries);
- ARMARX_INFO << "RTLogging activated! using "
- << nBuffers << "buffers "
- << "(buffersize = " << bufferSize << " bytes)";
- }
- //init logging names + field types
+ const auto bufferSize =
+ _module<ControlThreadDataBuffer>().getControlThreadOutputBuffer().initialize(
+ nBuffers,
+ _module<Devices>().getControlDevices(),
+ _module<Devices>().getSensorDevices(),
+ messageBufferSize,
+ messageBufferNumberEntries,
+ messageBufferMaxSize,
+ messageBufferMaxNumberEntries);
+ ARMARX_INFO << "RTLogging activated! using " << nBuffers << "buffers "
+ << "(buffersize = " << bufferSize << " bytes)";
+ }
+ //init logging names + field types
+ {
+ ARMARX_TRACE;
+ const auto makeValueMetaData = [&](auto* val, const std::string& namePre)
{
- ARMARX_TRACE;
- const auto makeValueMetaData = [&](auto * val, const std::string & namePre)
+ ValueMetaData data;
+ const auto names = val->getDataFieldNames();
+ data.fields.resize(names.size());
+ for (const auto& [fieldIdx, fieldName] : MakeIndexedContainer(names))
{
- ValueMetaData data;
- const auto names = val->getDataFieldNames();
- data.fields.resize(names.size());
- for (const auto& [fieldIdx, fieldName] : MakeIndexedContainer(names))
- {
- data.fields.at(fieldIdx).name = namePre + '.' + fieldName;
- data.fields.at(fieldIdx).type = &(val->getDataFieldType(fieldIdx));
- }
- return data;
- };
-
- //sensorDevices
- controlDeviceValueMetaData.reserve(_module<Devices>().getControlDevices().size());
- for (const auto& cd : _module<Devices>().getControlDevices().values())
- {
- ARMARX_TRACE;
- controlDeviceValueMetaData.emplace_back();
- auto& dataForDev = controlDeviceValueMetaData.back();
- dataForDev.reserve(cd->getJointControllers().size());
- for (auto jointC : cd->getJointControllers())
- {
- dataForDev.emplace_back(
- makeValueMetaData(jointC->getControlTarget(),
- "ctrl." +
- cd->getDeviceName() + "." +
- jointC->getControlMode()));
- }
+ data.fields.at(fieldIdx).name = namePre + '.' + fieldName;
+ data.fields.at(fieldIdx).type = &(val->getDataFieldType(fieldIdx));
}
- //sensorDevices
- sensorDeviceValueMetaData.reserve(_module<Devices>().getSensorDevices().size());
- for (const auto& sd : _module<Devices>().getSensorDevices().values())
+ return data;
+ };
+
+ //sensorDevices
+ controlDeviceValueMetaData.reserve(_module<Devices>().getControlDevices().size());
+ for (const auto& cd : _module<Devices>().getControlDevices().values())
+ {
+ ARMARX_TRACE;
+ controlDeviceValueMetaData.emplace_back();
+ auto& dataForDev = controlDeviceValueMetaData.back();
+ dataForDev.reserve(cd->getJointControllers().size());
+ for (auto jointC : cd->getJointControllers())
{
- ARMARX_TRACE;
- sensorDeviceValueMetaData.emplace_back(
- makeValueMetaData(sd->getSensorValue(),
- "sens." +
- sd->getDeviceName()));
+ dataForDev.emplace_back(makeValueMetaData(jointC->getControlTarget(),
+ "ctrl." + cd->getDeviceName() + "." +
+ jointC->getControlMode()));
}
}
- //start logging thread is done in rtinit
- //maybe add the default log
+ //sensorDevices
+ sensorDeviceValueMetaData.reserve(_module<Devices>().getSensorDevices().size());
+ for (const auto& sd : _module<Devices>().getSensorDevices().values())
{
ARMARX_TRACE;
- const auto loggingpath = getProperty<std::string>("RTLogging_DefaultLog").getValue();
- if (!loggingpath.empty())
- {
- defaultLogHandle = startRtLogging(loggingpath, getLoggingNames());
- }
+ sensorDeviceValueMetaData.emplace_back(
+ makeValueMetaData(sd->getSensorValue(), "sens." + sd->getDeviceName()));
}
}
-
- void Logging::DataStreamingEntry::clearResult()
+ //start logging thread is done in rtinit
+ //maybe add the default log
{
ARMARX_TRACE;
- for (auto& e : result)
+ const auto loggingpath = getProperty<std::string>("RTLogging_DefaultLog").getValue();
+ if (!loggingpath.empty())
{
- entryBuffer.emplace_back(std::move(e));
+ defaultLogHandle = startRtLogging(loggingpath, getLoggingNames());
}
- result.clear();
}
+}
- RobotUnitDataStreaming::TimeStep Logging::DataStreamingEntry::allocateResultElement() const
+void
+Logging::DataStreamingEntry::clearResult()
+{
+ ARMARX_TRACE;
+ for (auto& e : result)
{
- ARMARX_TRACE;
- RobotUnitDataStreaming::TimeStep data;
- data.bools .resize(numBools);
- data.bytes .resize(numBytes);
- data.shorts .resize(numShorts);
- data.ints .resize(numInts);
- data.longs .resize(numLongs);
- data.floats .resize(numFloats);
- data.doubles.resize(numDoubles);
- return data;
+ entryBuffer.emplace_back(std::move(e));
}
+ result.clear();
+}
+
+RobotUnitDataStreaming::TimeStep
+Logging::DataStreamingEntry::allocateResultElement() const
+{
+ ARMARX_TRACE;
+ RobotUnitDataStreaming::TimeStep data;
+ data.bools.resize(numBools);
+ data.bytes.resize(numBytes);
+ data.shorts.resize(numShorts);
+ data.ints.resize(numInts);
+ data.longs.resize(numLongs);
+ data.floats.resize(numFloats);
+ data.doubles.resize(numDoubles);
+ return data;
+}
- RobotUnitDataStreaming::TimeStep Logging::DataStreamingEntry::getResultElement()
+RobotUnitDataStreaming::TimeStep
+Logging::DataStreamingEntry::getResultElement()
+{
+ ARMARX_TRACE;
+ if (entryBuffer.empty())
{
- ARMARX_TRACE;
- if (entryBuffer.empty())
- {
- return allocateResultElement();
- }
- auto e = std::move(entryBuffer.back());
- entryBuffer.pop_back();
- return e;
+ return allocateResultElement();
}
+ auto e = std::move(entryBuffer.back());
+ entryBuffer.pop_back();
+ return e;
+}
- void Logging::DataStreamingEntry::processHeader(const ControlThreadOutputBuffer::Entry& e)
+void
+Logging::DataStreamingEntry::processHeader(const ControlThreadOutputBuffer::Entry& e)
+{
+ ARMARX_TRACE;
+ if (stopStreaming)
{
- ARMARX_TRACE;
- if (stopStreaming)
- {
- return;
- }
- result.emplace_back(getResultElement());
-
- auto& data = result.back();
- data.iterationId = e.iteration;
- data.timestampUSec = e.sensorValuesTimestamp.toMicroSeconds();
- data.timesSinceLastIterationUSec = e.timeSinceLastIteration.toMicroSeconds();
+ return;
}
+ result.emplace_back(getResultElement());
- void WriteTo(const auto& dentr,
- const Logging::DataStreamingEntry::OutVal& out,
- const auto& val,
- std::size_t fidx,
- auto& data)
+ auto& data = result.back();
+ data.iterationId = e.iteration;
+ data.timestampUSec = e.sensorValuesTimestamp.toMicroSeconds();
+ data.timesSinceLastIterationUSec = e.timeSinceLastIteration.toMicroSeconds();
+}
+
+void
+WriteTo(const auto& dentr,
+ const Logging::DataStreamingEntry::OutVal& out,
+ const auto& val,
+ std::size_t fidx,
+ auto& data)
+{
+ ARMARX_TRACE;
+ using enum_t = Logging::DataStreamingEntry::ValueT;
+ try
{
ARMARX_TRACE;
- using enum_t = Logging::DataStreamingEntry::ValueT;
- try
+ switch (out.value)
{
- ARMARX_TRACE;
- switch (out.value)
- {
- case enum_t::Bool :
- bool b;
- val.getDataFieldAs(fidx, b);
- data.bools.at(out.idx) = b;
- return;
- case enum_t::Byte :
- val.getDataFieldAs(fidx, data.bytes.at(out.idx));
- return;
- case enum_t::Short :
- val.getDataFieldAs(fidx, data.shorts.at(out.idx));
- return;
- case enum_t::Int :
- val.getDataFieldAs(fidx, data.ints.at(out.idx));
- return;
- case enum_t::Long :
- val.getDataFieldAs(fidx, data.longs.at(out.idx));
- return;
- case enum_t::Float :
- val.getDataFieldAs(fidx, data.floats.at(out.idx));
- return;
- case enum_t::Double :
- val.getDataFieldAs(fidx, data.doubles.at(out.idx));
- return;
- case enum_t::Skipped:
- return;
- }
- }
- catch (...)
- {
- ARMARX_ERROR << GetHandledExceptionString()
- << "\ntype " << static_cast<int>(out.value)
- << "\n" << VAROUT(data.bools.size()) << " " << VAROUT(dentr.numBools)
- << "\n" << VAROUT(data.bytes.size()) << " " << VAROUT(dentr.numBytes)
- << "\n" << VAROUT(data.shorts.size()) << " " << VAROUT(dentr.numShorts)
- << "\n" << VAROUT(data.ints.size()) << " " << VAROUT(dentr.numInts)
- << "\n" << VAROUT(data.longs.size()) << " " << VAROUT(dentr.numLongs)
- << "\n" << VAROUT(data.floats.size()) << " " << VAROUT(dentr.numFloats)
- << "\n" << VAROUT(data.doubles.size()) << " " << VAROUT(dentr.numDoubles);
- throw;
+ case enum_t::Bool:
+ bool b;
+ val.getDataFieldAs(fidx, b);
+ data.bools.at(out.idx) = b;
+ return;
+ case enum_t::Byte:
+ val.getDataFieldAs(fidx, data.bytes.at(out.idx));
+ return;
+ case enum_t::Short:
+ val.getDataFieldAs(fidx, data.shorts.at(out.idx));
+ return;
+ case enum_t::Int:
+ val.getDataFieldAs(fidx, data.ints.at(out.idx));
+ return;
+ case enum_t::Long:
+ val.getDataFieldAs(fidx, data.longs.at(out.idx));
+ return;
+ case enum_t::Float:
+ val.getDataFieldAs(fidx, data.floats.at(out.idx));
+ return;
+ case enum_t::Double:
+ val.getDataFieldAs(fidx, data.doubles.at(out.idx));
+ return;
+ case enum_t::Skipped:
+ return;
}
}
-
- void Logging::DataStreamingEntry::processCtrl(
- const ControlTargetBase& val,
- std::size_t didx,
- std::size_t vidx,
- std::size_t fidx)
+ catch (...)
{
- ARMARX_TRACE;
- if (stopStreaming)
- {
- return;
- }
- auto& data = result.back();
- const OutVal& o = ctrlDevs.at(didx).at(vidx).at(fidx);
- WriteTo(*this, o, val, fidx, data);
+ ARMARX_ERROR << GetHandledExceptionString() << "\ntype " << static_cast<int>(out.value)
+ << "\n"
+ << VAROUT(data.bools.size()) << " " << VAROUT(dentr.numBools) << "\n"
+ << VAROUT(data.bytes.size()) << " " << VAROUT(dentr.numBytes) << "\n"
+ << VAROUT(data.shorts.size()) << " " << VAROUT(dentr.numShorts) << "\n"
+ << VAROUT(data.ints.size()) << " " << VAROUT(dentr.numInts) << "\n"
+ << VAROUT(data.longs.size()) << " " << VAROUT(dentr.numLongs) << "\n"
+ << VAROUT(data.floats.size()) << " " << VAROUT(dentr.numFloats) << "\n"
+ << VAROUT(data.doubles.size()) << " " << VAROUT(dentr.numDoubles);
+ throw;
}
+}
- void Logging::DataStreamingEntry::processSens(
- const SensorValueBase& val,
- std::size_t didx,
- std::size_t fidx)
+void
+Logging::DataStreamingEntry::processCtrl(const ControlTargetBase& val,
+ std::size_t didx,
+ std::size_t vidx,
+ std::size_t fidx)
+{
+ ARMARX_TRACE;
+ if (stopStreaming)
{
- ARMARX_TRACE;
- if (stopStreaming)
- {
- return;
- }
- auto& data = result.back();
- const OutVal& o = sensDevs.at(didx).at(fidx);
- WriteTo(*this, o, val, fidx, data);
+ return;
}
+ auto& data = result.back();
+ const OutVal& o = ctrlDevs.at(didx).at(vidx).at(fidx);
+ WriteTo(*this, o, val, fidx, data);
+}
- void Logging::DataStreamingEntry::send(const RobotUnitDataStreaming::ReceiverPrx& r,
- std::uint64_t msgId)
+void
+Logging::DataStreamingEntry::processSens(const SensorValueBase& val,
+ std::size_t didx,
+ std::size_t fidx)
+{
+ ARMARX_TRACE;
+ if (stopStreaming)
{
- ARMARX_TRACE;
- const auto start_send = IceUtil::Time::now();
- updateCalls.emplace_back(r->begin_update(result, static_cast<Ice::Long>(msgId)));
- const auto start_clear = IceUtil::Time::now();
- clearResult();
- const auto end = IceUtil::Time::now();
- ARMARX_DEBUG_S << "Logging::DataStreamingEntry::send"
- << "\n update " << (start_clear - start_send).toMilliSecondsDouble() << "ms (" << result.size() << " timesteps)"
- << "\n clear " << (end - start_clear).toMilliSecondsDouble() << "ms";
+ return;
+ }
+ auto& data = result.back();
+ const OutVal& o = sensDevs.at(didx).at(fidx);
+ WriteTo(*this, o, val, fidx, data);
+}
+
+void
+Logging::DataStreamingEntry::send(const RobotUnitDataStreaming::ReceiverPrx& r, std::uint64_t msgId)
+{
+ ARMARX_TRACE;
+ const auto start_send = IceUtil::Time::now();
+ updateCalls.emplace_back(r->begin_update(result, static_cast<Ice::Long>(msgId)));
+ const auto start_clear = IceUtil::Time::now();
+ clearResult();
+ const auto end = IceUtil::Time::now();
+ ARMARX_DEBUG_S << "Logging::DataStreamingEntry::send"
+ << "\n update " << (start_clear - start_send).toMilliSecondsDouble() << "ms ("
+ << result.size() << " timesteps)"
+ << "\n clear " << (end - start_clear).toMilliSecondsDouble() << "ms";
- //now execute all ready callbacks
- std::size_t i = 0;
- for (; i < updateCalls.size(); ++i)
+ //now execute all ready callbacks
+ std::size_t i = 0;
+ for (; i < updateCalls.size(); ++i)
+ {
+ try
{
- try
- {
- if (!updateCalls.at(i)->isCompleted())
- {
- break;
- }
- r->end_update(updateCalls.at(i));
- connectionFailures = 0;
- }
- catch (...)
+ if (!updateCalls.at(i)->isCompleted())
{
- ARMARX_TRACE;
- ++connectionFailures;
- if (connectionFailures > rtStreamMaxClientErrors)
- {
- stopStreaming = true;
- ARMARX_WARNING_S << "DataStreaming Receiver was not reachable for "
- << connectionFailures << " iterations! Removing receiver";
- updateCalls.clear();
- break;
- }
+ break;
}
+ r->end_update(updateCalls.at(i));
+ connectionFailures = 0;
}
- if (!updateCalls.empty())
+ catch (...)
{
- updateCalls.erase(updateCalls.begin(), updateCalls.begin() + i);
+ ARMARX_TRACE;
+ ++connectionFailures;
+ if (connectionFailures > rtStreamMaxClientErrors)
+ {
+ stopStreaming = true;
+ ARMARX_WARNING_S << "DataStreaming Receiver was not reachable for "
+ << connectionFailures << " iterations! Removing receiver";
+ updateCalls.clear();
+ break;
+ }
}
}
+ if (!updateCalls.empty())
+ {
+ updateCalls.erase(updateCalls.begin(), updateCalls.begin() + i);
+ }
+}
}
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/AronTreeWidgetEditor.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/AronTreeWidgetEditor.cpp
deleted file mode 100644
index 0df59db31a93124f7ec5b776fe7e876afe2fb414..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/AronTreeWidgetEditor.cpp
+++ /dev/null
@@ -1,349 +0,0 @@
-/*
- * 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::gui-plugins::SkillManagerMonitorWidgetController
- * \author Raphael Grimm ( raphael dot grimm at kit dot edu )
- * \date 2020
- * \copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include <string>
-
-#include <ArmarXCore/util/CPPUtility/Iterator.h>
-
-#include "AronTreeWidgetEditor.h"
-
-// debug
-#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
-
-//visitors
-namespace armarx
-{
-
- void
- AronTreeViewWidgetCreatorVisitor::createSimpleTreeViewWidget(Input& i)
- {
- ARMARX_CHECK_NOT_NULL(i);
- ARMARX_CHECK_NOT_NULL(parentItem);
-
- auto key = i->getPath().getLastElement();
- createdQWidgetItem = new QTreeWidgetItem(parentItem);
- createdQWidgetItem->setText(0, QString::fromStdString(key));
- createdQWidgetItem->setText(2, QString::fromStdString(i->getShortName()));
- createdQWidgetItem->setText(3, QString::fromStdString(i->getDefaultFromString()));
- createdQWidgetItem->setFlags(createdQWidgetItem->flags() | Qt::ItemIsEditable);
- }
-
- void
- AronTreeViewWidgetCreatorVisitor::visitAronVariant(const aron::type::ObjectPtr& i)
- {
- ARMARX_CHECK_NOT_NULL(i);
- ARMARX_CHECK_NOT_NULL(parentItem);
-
- auto key = i->getObjectName();
- if (i->getPath().hasElement())
- {
- key = i->getPath().getLastElement();
- }
-
- createdQWidgetItem = new QTreeWidgetItem(parentItem);
- createdQWidgetItem->setText(0, QString::fromStdString(key));
-
- for (const auto& [key, value] : i->getMemberTypes())
- {
- AronTreeViewWidgetCreatorVisitor v(createdQWidgetItem);
- aron::type::visit(v, value);
-
- if (v.createdQWidgetItem)
- {
- createdQWidgetItem->addChild(v.createdQWidgetItem);
- }
- }
- }
-
- void
- AronTreeViewWidgetCreatorVisitor::visitAronVariant(const aron::type::DictPtr& i)
- { createSimpleTreeViewWidget(i); }
- void
- AronTreeViewWidgetCreatorVisitor::visitAronVariant(const aron::type::PairPtr& i)
- { createSimpleTreeViewWidget(i); }
- void
- AronTreeViewWidgetCreatorVisitor::visitAronVariant(const aron::type::TuplePtr& i)
- { createSimpleTreeViewWidget(i); }
- void
- AronTreeViewWidgetCreatorVisitor::visitAronVariant(const aron::type::ListPtr& i)
- { createSimpleTreeViewWidget(i); }
- void
- AronTreeViewWidgetCreatorVisitor::visitAronVariant(const aron::type::NDArrayPtr& i)
- { createSimpleTreeViewWidget(i); }
- void
- AronTreeViewWidgetCreatorVisitor::visitAronVariant(const aron::type::MatrixPtr& i)
- { createSimpleTreeViewWidget(i); }
- void
- AronTreeViewWidgetCreatorVisitor::visitAronVariant(const aron::type::QuaternionPtr& i)
- { createSimpleTreeViewWidget(i); }
- void
- AronTreeViewWidgetCreatorVisitor::visitAronVariant(const aron::type::PositionPtr& i)
- { createSimpleTreeViewWidget(i); }
- void
- AronTreeViewWidgetCreatorVisitor::visitAronVariant(const aron::type::OrientationPtr& i)
- { createSimpleTreeViewWidget(i); }
- void
- AronTreeViewWidgetCreatorVisitor::visitAronVariant(const aron::type::PosePtr& i)
- { createSimpleTreeViewWidget(i); }
- void
- AronTreeViewWidgetCreatorVisitor::visitAronVariant(const aron::type::ImagePtr& i)
- { createSimpleTreeViewWidget(i); }
- void
- AronTreeViewWidgetCreatorVisitor::visitAronVariant(const aron::type::PointCloudPtr& i)
- { createSimpleTreeViewWidget(i); }
- void
- AronTreeViewWidgetCreatorVisitor::visitAronVariant(const aron::type::IntEnumPtr& i)
- { createSimpleTreeViewWidget(i); }
- void
- AronTreeViewWidgetCreatorVisitor::visitAronVariant(const aron::type::IntPtr& i)
- { createSimpleTreeViewWidget(i); }
- void
- AronTreeViewWidgetCreatorVisitor::visitAronVariant(const aron::type::LongPtr& i)
- { createSimpleTreeViewWidget(i); }
- void
- AronTreeViewWidgetCreatorVisitor::visitAronVariant(const aron::type::FloatPtr& i)
- { createSimpleTreeViewWidget(i); }
- void
- AronTreeViewWidgetCreatorVisitor::visitAronVariant(const aron::type::DoublePtr& i)
- { createSimpleTreeViewWidget(i); }
- void
- AronTreeViewWidgetCreatorVisitor::visitAronVariant(const aron::type::BoolPtr& i)
- { createSimpleTreeViewWidget(i); }
- void
- AronTreeViewWidgetCreatorVisitor::visitAronVariant(const aron::type::StringPtr& i)
- { createSimpleTreeViewWidget(i); }
- void
- AronTreeViewWidgetCreatorVisitor::visitAronVariant(const aron::type::TimePtr& i)
- { createSimpleTreeViewWidget(i); }
-
- void AronTreeViewWidgetCreatorVisitor::visitUnknown(Input&)
- {
- ARMARX_WARNING_S << "Received an unknown type when trying to create a tree view widget for a skill argument type.";
- }
-
-
-
-
- void
- AronTreeViewWidgetConverterVisitor::visitAronVariant(const aron::type::ObjectPtr& i)
- {
- auto createdAronDict = std::make_shared<aron::data::Dict>();
- createdAron = createdAronDict;
- QTreeWidgetItem* el = parentItem->child(index);
-
- unsigned int x = 0;
- for (const auto& [key, value] : i->getMemberTypes())
- {
- AronTreeViewWidgetConverterVisitor v(el, x++);
- aron::type::visit(v, value);
-
- if (v.createdAron)
- {
- createdAronDict->addElement(key, v.createdAron);
- }
- }
- }
-
- void
- AronTreeViewWidgetConverterVisitor::visitAronVariant(const aron::type::DictPtr& i)
- {
- // TODO
- }
-
- void
- AronTreeViewWidgetConverterVisitor::visitAronVariant(const aron::type::PairPtr& i)
- {
- // TODO
- }
-
- void
- AronTreeViewWidgetConverterVisitor::visitAronVariant(const aron::type::TuplePtr& i)
- {
- // TODO
- }
-
- void
- AronTreeViewWidgetConverterVisitor::visitAronVariant(const aron::type::ListPtr& i)
- {
- // TODO
- }
-
- void
- AronTreeViewWidgetConverterVisitor::visitAronVariant(const aron::type::NDArrayPtr& i)
- {
- // TODO
- }
-
- void
- AronTreeViewWidgetConverterVisitor::visitAronVariant(const aron::type::MatrixPtr& i)
- {
- // TODO
- }
-
- void
- AronTreeViewWidgetConverterVisitor::visitAronVariant(const aron::type::QuaternionPtr& i)
- {
- // TODO
- }
-
- void
- AronTreeViewWidgetConverterVisitor::visitAronVariant(const aron::type::PositionPtr& i)
- {
- // TODO
- }
-
- void
- AronTreeViewWidgetConverterVisitor::visitAronVariant(const aron::type::OrientationPtr& i)
- {
- // TODO
- }
-
- void
- AronTreeViewWidgetConverterVisitor::visitAronVariant(const aron::type::PosePtr& i)
- {
- // TODO
- }
-
- void
- AronTreeViewWidgetConverterVisitor::visitAronVariant(const aron::type::ImagePtr& i)
- {
- // TODO
- }
-
- void
- AronTreeViewWidgetConverterVisitor::visitAronVariant(const aron::type::PointCloudPtr& i)
- {
- // TODO
- }
-
- void
- AronTreeViewWidgetConverterVisitor::visitAronVariant(const aron::type::IntEnumPtr& i)
- {
- // TODO
- }
-
- void
- AronTreeViewWidgetConverterVisitor::visitAronVariant(const aron::type::IntPtr& i)
- {
- auto createdAronInt = std::make_shared<aron::data::Int>();
- createdAron = createdAronInt;
- QTreeWidgetItem* el = parentItem->child(index);
-
- std::string str = el->text(1).toStdString();
- if (str.empty())
- {
- createdAronInt->setValue(0);
- return;
- }
-
- int val = std::stoi(str);
- createdAronInt->setValue(val);
- }
-
- void
- AronTreeViewWidgetConverterVisitor::visitAronVariant(const aron::type::LongPtr& i)
- {
- auto createdAronLong = std::make_shared<aron::data::Long>();
- createdAron = createdAronLong;
- QTreeWidgetItem* el = parentItem->child(index);
-
- std::string str = el->text(1).toStdString();
- if (str.empty())
- {
- str = el->text(3).toStdString();
- }
-
- createdAronLong->fromString(str);
- }
-
- void
- AronTreeViewWidgetConverterVisitor::visitAronVariant(const aron::type::FloatPtr& i)
- {
- auto createdAronFloat = std::make_shared<aron::data::Float>();
- createdAron = createdAronFloat;
- QTreeWidgetItem* el = parentItem->child(index);
-
- std::string str = el->text(1).toStdString();
- if (str.empty())
- {
- str = el->text(3).toStdString();
- }
-
- createdAronFloat->fromString(str);
- }
-
- void
- AronTreeViewWidgetConverterVisitor::visitAronVariant(const aron::type::DoublePtr& i)
- {
- auto createdAronDouble = std::make_shared<aron::data::Double>();
- createdAron = createdAronDouble;
- QTreeWidgetItem* el = parentItem->child(index);
-
- std::string str = el->text(1).toStdString();
- if (str.empty())
- {
- str = el->text(3).toStdString();
- }
-
- createdAronDouble->fromString(str);
- }
-
- void
- AronTreeViewWidgetConverterVisitor::visitAronVariant(const aron::type::BoolPtr& i)
- {
- auto createdAronBool = std::make_shared<aron::data::Bool>();
- createdAron = createdAronBool;
- QTreeWidgetItem* el = parentItem->child(index);
-
- std::string str = el->text(1).toStdString();
- if (str.empty())
- {
- str = el->text(3).toStdString();
- }
-
- createdAronBool->fromString(str);
- }
-
- void
- AronTreeViewWidgetConverterVisitor::visitAronVariant(const aron::type::StringPtr& i)
- {
- auto createdAronString = std::make_shared<aron::data::String>();
- createdAron = createdAronString;
- QTreeWidgetItem* el = parentItem->child(index);
-
- std::string str = el->text(1).toStdString();
- createdAronString->fromString(str);
- }
-
- void
- AronTreeViewWidgetConverterVisitor::visitAronVariant(const aron::type::TimePtr& i)
- {
- auto l = std::make_shared<aron::type::Long>();
- visitLong(l);
- }
-
- void AronTreeViewWidgetConverterVisitor::visitUnknown(Input&)
- {
- ARMARX_WARNING_S << "Received an unknown type when trying to convert a skill argument type to an aron data object.";
- }
-}
-
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/AronTreeWidgetInputModalController.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/AronTreeWidgetInputModalController.cpp
deleted file mode 100644
index 25ec39a7056226f53ae8882e60877f1c512d551c..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/AronTreeWidgetInputModalController.cpp
+++ /dev/null
@@ -1,38 +0,0 @@
-#include "AronTreeWidgetInputModalController.h"
-
-namespace armarx
-{
- AronTreeWidgetInputModalController::AronTreeWidgetInputModalController(const std::string& name, const std::string& placeholder, QWidget * parent) :
- QDialog(parent),
- placeholder(placeholder)
- {
- widget.setupUi(this);
-
- // set header
- widget.groupBoxInput->setTitle(QString::fromStdString(name));
- widget.textEditInput->setPlainText(QString::fromStdString(placeholder));
-
- // connect signals
- connect(widget.pushButtonReset, &QPushButton::clicked,
- this, &AronTreeWidgetInputModalController::reset);
- connect(widget.pushButtonSubmit, &QPushButton::clicked,
- this, &AronTreeWidgetInputModalController::submit);
- }
-
- QString AronTreeWidgetInputModalController::getInput() const
- {
- return widget.textEditInput->toPlainText();
- }
-
-
- void AronTreeWidgetInputModalController::reset()
- {
- // reset to initial value
- widget.textEditInput->setPlainText(QString::fromStdString(placeholder));
- }
-
- void AronTreeWidgetInputModalController::submit()
- {
- accept();
- }
-}
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/AronTreeWidgetInputModalController.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/AronTreeWidgetInputModalController.h
deleted file mode 100644
index 1f4a0d0f93f4cc1cece3468070dd1e3721633473..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/AronTreeWidgetInputModalController.h
+++ /dev/null
@@ -1,34 +0,0 @@
-#pragma once
-
-#include <stack>
-#include <ArmarXCore/core/system/ImportExportComponent.h>
-
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
-#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
-
-#include <RobotAPI/gui-plugins/SkillManagerPlugin/ui_AronTreeWidgetInputModal.h>
-
-#include <QDialog>
-
-namespace armarx
-{
- class ARMARXCOMPONENT_IMPORT_EXPORT
- AronTreeWidgetInputModalController :
- public QDialog
- {
- Q_OBJECT
- public:
- AronTreeWidgetInputModalController(const std::string& label, const std::string& placeholder = "", QWidget * parent = 0);
-
- QString getInput() const;
-
- private slots:
- void reset();
- void submit();
-
- private:
- std::string placeholder;
- Ui::AronTreeWidgetInputModalWidget widget;
- };
-}
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/CMakeLists.txt b/source/RobotAPI/gui-plugins/SkillManagerPlugin/CMakeLists.txt
index b23252198252a9799bc69de20581a675057cf26e..749a9e26330f60a2ffaee4ae251a65495887f771 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/CMakeLists.txt
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/CMakeLists.txt
@@ -5,18 +5,36 @@ armarx_build_if(ArmarXGui_FOUND "ArmarXGui not available")
# do not rename this variable, it is used in armarx_gui_library()...
set(SOURCES
- AronTreeWidgetEditor.cpp
- AronTreeWidgetInputModalController.cpp
+ aronTreeWidget/visitors/AronTreeWidgetCreator.cpp
+ aronTreeWidget/visitors/AronTreeWidgetConverter.cpp
+ aronTreeWidget/visitors/AronTreeWidgetModalCreator.cpp
+ aronTreeWidget/Data.cpp
+ aronTreeWidget/AronTreeWidgetItem.cpp
+ aronTreeWidget/AronTreeWidgetController.cpp
+ aronTreeWidget/modal/AronTreeWidgetModal.cpp
+ aronTreeWidget/modal/text/AronTreeWidgetTextInputModalController.cpp
+ aronTreeWidget/modal/dict/AronTreeWidgetDictInputModalController.cpp
SkillManagerMonitorWidgetController.cpp
)
set(HEADERS
- AronTreeWidgetEditor.h
- AronTreeWidgetInputModalController.h
+ aronTreeWidget/visitors/AronTreeWidgetCreator.h
+ aronTreeWidget/visitors/AronTreeWidgetConverter.h
+ aronTreeWidget/visitors/AronTreeWidgetModalCreator.h
+ aronTreeWidget/Data.h
+ aronTreeWidget/AronTreeWidgetItem.h
+ aronTreeWidget/AronTreeWidgetController.h
+ aronTreeWidget/modal/AronTreeWidgetModal.h
+ aronTreeWidget/modal/text/AronTreeWidgetTextInputModalController.h
+ aronTreeWidget/modal/dict/AronTreeWidgetDictInputModalController.h
SkillManagerMonitorWidgetController.h
)
-set(GUI_UIS SkillManagerMonitorWidget.ui AronTreeWidgetInputModal.ui)
+set(GUI_UIS
+ SkillManagerMonitorWidget.ui
+ aronTreeWidget/modal/text/AronTreeWidgetTextInputModal.ui
+ aronTreeWidget/modal/dict/AronTreeWidgetDictInputModal.ui
+)
# Add more libraries you depend on here, e.g. ${QT_LIBRARIES}.
set(COMPONENT_LIBS RobotAPIInterfaces aron skills aronjsonconverter SimpleConfigDialog)
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui
index 927d6daf0431c4988ab05bb62171910aac25dbe8..15678facbdf134eb61542300528e6aa3290a0dc9 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui
@@ -24,14 +24,14 @@
<string>Manager</string>
</property>
<layout class="QGridLayout" name="gridLayout">
- <item row="0" column="0">
- <widget class="QPushButton" name="pushButtonRefreshProvidedSkills">
+ <item row="2" column="0">
+ <widget class="QLabel" name="label">
<property name="text">
- <string>Refresh</string>
+ <string>Update Frequency:</string>
</property>
</widget>
</item>
- <item row="1" column="0">
+ <item row="3" column="0" colspan="3">
<widget class="QTreeWidget" name="treeWidgetSkills">
<column>
<property name="text">
@@ -50,6 +50,9 @@
</column>
</widget>
</item>
+ <item row="2" column="1">
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxUpdateFreq"/>
+ </item>
</layout>
</widget>
<widget class="QGroupBox" name="groupBoxSkillDetails">
@@ -57,21 +60,28 @@
<string>Skill Details</string>
</property>
<layout class="QGridLayout" name="gridLayout_2">
- <item row="5" column="0">
+ <item row="0" column="0">
+ <widget class="QPushButton" name="pushButtonPaste">
+ <property name="text">
+ <string>Set from clipboard</string>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="0">
<widget class="QPushButton" name="pushButtonStopSkill">
<property name="text">
<string>Stop</string>
</property>
</widget>
</item>
- <item row="1" column="0">
- <widget class="QPushButton" name="pushButtonExecuteSkill">
+ <item row="0" column="1">
+ <widget class="QPushButton" name="pushButtonCopy">
<property name="text">
- <string>Execute</string>
+ <string>Copy args to clipboard</string>
</property>
</widget>
</item>
- <item row="0" column="0">
+ <item row="1" column="0" colspan="2">
<widget class="QTreeWidget" name="treeWidgetSkillDetails">
<column>
<property name="text">
@@ -95,6 +105,13 @@
</column>
</widget>
</item>
+ <item row="6" column="1">
+ <widget class="QPushButton" name="pushButtonExecuteSkill">
+ <property name="text">
+ <string>Execute</string>
+ </property>
+ </widget>
+ </item>
</layout>
</widget>
</widget>
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
index 720304e622303a74a0390c17297b0ec4a17dd91c..4a458d690633e611e64a4402238ec6793ae388d6 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
@@ -26,12 +26,21 @@
#include "SkillManagerMonitorWidgetController.h"
-#include "AronTreeWidgetEditor.h"
-#include "AronTreeWidgetInputModalController.h"
+#include "aronTreeWidget/visitors/AronTreeWidgetCreator.h"
+#include "aronTreeWidget/visitors/AronTreeWidgetConverter.h"
+#include "aronTreeWidget/visitors/AronTreeWidgetModalCreator.h"
+
+// modals
+#include "aronTreeWidget/modal/text/AronTreeWidgetTextInputModalController.h"
// debug
#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
+#include <QDoubleSpinBox>
+#include <QClipboard>
+
+#include "aronTreeWidget/Data.h"
+
//config
namespace armarx
{
@@ -64,9 +73,27 @@ namespace armarx
SkillManagerMonitorWidgetController::SkillManagerMonitorWidgetController()
{
widget.setupUi(getWidget());
- connect(widget.pushButtonRefreshProvidedSkills, &QPushButton::clicked,
+
+ widget.doubleSpinBoxUpdateFreq->setValue(5.0);
+ widget.doubleSpinBoxUpdateFreq->setMinimum(0);
+ widget.doubleSpinBoxUpdateFreq->setMaximum(100);
+ widget.doubleSpinBoxUpdateFreq->setSingleStep(0.5);
+ widget.doubleSpinBoxUpdateFreq->setSuffix(" Hz");
+
+ refreshSkillsResultTimer = new QTimer(this);
+ refreshSkillsResultTimer->setInterval(1000 / 5); // Keep this stable.
+ refreshSkillsResultTimer->start();
+
+ connect(widget.doubleSpinBoxUpdateFreq, &QDoubleSpinBox::editingFinished,
+ this, &SkillManagerMonitorWidgetController::updateTimerFrequency);
+ connect(refreshSkillsResultTimer, &QTimer::timeout,
this, &SkillManagerMonitorWidgetController::refreshSkills);
+ connect(widget.pushButtonCopy, &QPushButton::clicked,
+ this, &SkillManagerMonitorWidgetController::copyCurrentConfig);
+ connect(widget.pushButtonPaste, &QPushButton::clicked,
+ this, &SkillManagerMonitorWidgetController::pasteCurrentConfig);
+
connect(widget.pushButtonExecuteSkill, &QPushButton::clicked,
this, &SkillManagerMonitorWidgetController::executeSkill);
connect(widget.pushButtonStopSkill, &QPushButton::clicked,
@@ -91,14 +118,19 @@ namespace armarx
void SkillManagerMonitorWidgetController::onConnectComponent()
- { getProxy(manager, observerName);
+ {
+ getProxy(manager, observerName);
widget.groupBoxSkills->setTitle(QString::fromStdString(observerName));
widget.treeWidgetSkillDetails->setEditTriggers(QAbstractItemView::EditTrigger::NoEditTriggers);
widget.treeWidgetSkillDetails->setColumnHidden(3, true);
+
+ connected = true;
}
void SkillManagerMonitorWidgetController::onDisconnectComponent()
{
+ connected = false;
+
// reset all
skills.clear();
widget.treeWidgetSkills->clear();
@@ -108,29 +140,73 @@ namespace armarx
selectedSkill.skillName = "";
}
+ void SkillManagerMonitorWidgetController::updateTimerFrequency()
+ {
+ int f = static_cast<int>(std::round(1000 / widget.doubleSpinBoxUpdateFreq->value()));
+ refreshSkillsResultTimer->setInterval(f);
+ }
+
void SkillManagerMonitorWidgetController::refreshSkills()
{
- // get all skills
- skills.clear();
- for (const auto& [providerName, provider] : manager->getSkillProviders())
+ if (!connected)
+ return;
+
+ // remove non-existing ones
+ auto managerSkills = manager->getSkillProviders();
+ std::vector<std::string> removedProviders;
+ for (auto it = skills.begin(); it != skills.end();)
{
- SkillProviderData providerData;
- providerData.providerName = providerName;
- providerData.skillDescriptions = provider->getSkills();
- skills.insert(std::make_pair(providerName, providerData));
+ // TODO: iterate over skills, not just over providers!
+ std::string providerName = it->first;
+ if (managerSkills.find(providerName) == managerSkills.end())
+ {
+ removedProviders.push_back(providerName);
+ it = skills.erase(it);
+ }
+ else
+ {
+ it++;
+ }
}
- // put skills into tree view
- widget.treeWidgetSkills->clear();
+ // add new ones
+ std::vector<std::string> newProviders;
+ for (const auto& [providerName, provider] : managerSkills)
+ {
+ if (skills.find(providerName) == skills.end())
+ {
+ SkillProviderData providerData;
+ providerData.providerName = providerName;
+ providerData.skillDescriptions = provider->getSkills();
+ skills.insert(std::make_pair(providerName, providerData));
+
+ newProviders.push_back(providerName);
+ }
+ }
+
+ // remove providers from tree
+ for (int i = 0; i < widget.treeWidgetSkills->topLevelItemCount(); ++i)
+ {
+ QTreeWidgetItem* item = widget.treeWidgetSkills->topLevelItem(i);
+ if (std::find(removedProviders.begin(), removedProviders.end(), item->text(0).toStdString()) != removedProviders.end())
+ {
+ delete widget.treeWidgetSkills->takeTopLevelItem(i);
+ }
+ }
+
+ // add new providers
for (const auto& [providerName, provider] : skills)
{
- auto it = new QTreeWidgetItem(widget.treeWidgetSkills);
- it->setText(0, QString::fromStdString(providerName));
- for (const auto& [name, sk] : provider.skillDescriptions)
+ if (std::find(newProviders.begin(), newProviders.end(), providerName) != newProviders.end())
{
- auto itsk = new QTreeWidgetItem(it);
- it->addChild(itsk);
- itsk->setText(0, QString::fromStdString(name));
+ auto it = new QTreeWidgetItem(widget.treeWidgetSkills);
+ it->setText(0, QString::fromStdString(providerName));
+ for (const auto& [name, sk] : provider.skillDescriptions)
+ {
+ auto itsk = new QTreeWidgetItem(it);
+ it->addChild(itsk);
+ itsk->setText(0, QString::fromStdString(name));
+ }
}
}
}
@@ -148,22 +224,12 @@ namespace armarx
return;
}
- const auto& skillDescription = prv.skillDescriptions.at(selectedSkill.skillName);
+ auto data = getConfigAsAron();
+
skills::manager::dto::SkillExecutionInfo exInfo;
exInfo.providerName = selectedSkill.providerName;
exInfo.skillName = selectedSkill.skillName;
-
- // create argument aron (if there is an accepted type set)
- if (skillsArgumentsTreeWidgetItem)
- {
- auto aron_accepted_type = std::make_shared<aron::type::Object>(*skillDescription.acceptedType);
-
- AronTreeViewWidgetConverterVisitor v(skillsArgumentsTreeWidgetItem, 0);
- aron::type::visit(v, aron_accepted_type);
-
- auto aron_args = aron::data::Dict::DynamicCastAndCheck(v.createdAron);
- exInfo.params = aron_args->toAronDictPtr();
- }
+ exInfo.params = aron::data::Dict::ToAronDictDTO(data);
ARMARX_INFO << "Executing skill from GUI: " << selectedSkill.providerName << "/" << selectedSkill.skillName;
manager->executeSkill(exInfo);
@@ -221,6 +287,7 @@ namespace armarx
// setup table view
widget.treeWidgetSkillDetails->clear();
+ aronTreeWidgetController = nullptr;
skillsArgumentsTreeWidgetItem = nullptr;
auto skillDesc = skills.at(selectedSkill.providerName).skillDescriptions.at(selectedSkill.skillName);
@@ -256,13 +323,7 @@ namespace armarx
skillsArgumentsTreeWidgetItem = new QTreeWidgetItem(widget.treeWidgetSkillDetails,
{QString::fromStdString("Arguments")});
- AronTreeViewWidgetCreatorVisitor v(skillsArgumentsTreeWidgetItem);
- aron::type::visit(v, aron_args);
-
- if (v.createdQWidgetItem)
- {
- skillsArgumentsTreeWidgetItem->addChild(v.createdQWidgetItem);
- }
+ aronTreeWidgetController = std::make_shared<AronTreeWidgetController>(widget.treeWidgetSkillDetails, skillsArgumentsTreeWidgetItem, aron_args);
}
else
{
@@ -271,24 +332,64 @@ namespace armarx
}
}
- void SkillManagerMonitorWidgetController::onTreeWidgetItemDoubleClicked(QTreeWidgetItem * item, int column)
+ aron::data::DictPtr SkillManagerMonitorWidgetController::getConfigAsAron() const
+ {
+ // create argument aron (if there is an accepted type set)
+ if (aronTreeWidgetController)
+ {
+ return aronTreeWidgetController->convertToAron();
+ }
+ return nullptr;
+ }
+
+ void SkillManagerMonitorWidgetController::copyCurrentConfig()
+ {
+ auto data = getConfigAsAron();
+ if (!data)
+ {
+ return;
+ }
+
+ auto json = aron::converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(data);
+ QClipboard* clipboard = QApplication::clipboard();
+ clipboard->setText(QString::fromStdString(json.dump(2)));
+ }
+
+ void SkillManagerMonitorWidgetController::pasteCurrentConfig()
+ {
+ QClipboard* clipboard = QApplication::clipboard();
+ std::string s = clipboard->text().toStdString();
+ nlohmann::json json = nlohmann::json::parse(s);
+ auto data = aron::converter::AronNlohmannJSONConverter::ConvertFromNlohmannJSONObject(json);
+
+ if (!aronTreeWidgetController)
+ {
+ return;
+ }
+
+ aronTreeWidgetController->setFromAron(data);
+ }
+
+ void SkillManagerMonitorWidgetController::onTreeWidgetItemDoubleClicked(QTreeWidgetItem* item, int column)
{
+ if (!item)
+ {
+ return;
+ }
+
if (column == 1)
{
if (item->flags() & Qt::ItemIsEditable) // we use the flag to indicate whether the item is editable or not
{
- std::string name = item->text(0).toStdString();
- std::string placeholder = item->text(1).toStdString();
- if (placeholder.empty())
- {
- placeholder = item->text(3).toStdString();
- ARMARX_IMPORTANT << "Setting placeholder to " << placeholder;
- }
-
- auto modal = AronTreeWidgetInputModalController(name, placeholder, getWidget());
- modal.exec();
-
- item->setText(1, modal.getInput());
+ // we assume its aron item
+ AronTreeWidgetItem* aItem = AronTreeWidgetItem::DynamicCastAndCheck(item);
+ std::string name = aItem->text(aron_tree_widget::constantes::TREE_WIDGET_ITEM_NAME).toStdString();
+ std::string type = aItem->text(aron_tree_widget::constantes::TREE_WIDGET_ITEM_TYPE).toStdString();
+
+ AronTreeWidgetModalCreatorVisitor v(name, aItem, widget.treeWidgetSkillDetails);
+ aron::type::visit(v, aItem->aronType);
+ auto modal = v.createdModal;
+ modal->exec();
}
}
}
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h
index ae65fcbcbfac3b839163244015174d3bfdbb2e59..9ba0d8a378411a688f650012ec96d55525a164c2 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h
@@ -32,11 +32,14 @@
#include <RobotAPI/gui-plugins/SkillManagerPlugin/ui_SkillManagerMonitorWidget.h>
+#include "aronTreeWidget/AronTreeWidgetController.h"
#include <RobotAPI/libraries/aron/core/type/variant/All.h>
#include <RobotAPI/libraries/aron/core/data/variant/All.h>
#include <RobotAPI/libraries/aron/core/type/visitor/variant/VariantVisitor.h>
+#include <QTimer>
+
namespace armarx
{
class ARMARXCOMPONENT_IMPORT_EXPORT
@@ -76,10 +79,17 @@ namespace armarx
void stopSkill();
void executeSkill();
+ void updateTimerFrequency();
void refreshSkills();
+ void copyCurrentConfig();
+ void pasteCurrentConfig();
+
void onTreeWidgetItemDoubleClicked(QTreeWidgetItem * item, int column);
+ private:
+ aron::data::DictPtr getConfigAsAron() const;
+
private:
/**
* Widget Form
@@ -110,6 +120,11 @@ namespace armarx
// Helper to get the treeWidgetItem easily
QTreeWidgetItem* skillsArgumentsTreeWidgetItem = nullptr;
+ AronTreeWidgetControllerPtr aronTreeWidgetController = nullptr;
+
+ // others
+ std::atomic_bool connected = false;
+ QTimer* refreshSkillsResultTimer;
};
}
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetController.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1eeaf00ff53e332fb43999311fdf8e4b11ea7d5e
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetController.cpp
@@ -0,0 +1,38 @@
+#include "AronTreeWidgetController.h"
+
+#include "visitors/AronTreeWidgetConverter.h"
+
+namespace armarx
+{
+ AronTreeWidgetController::AronTreeWidgetController(QTreeWidget* tree, QTreeWidgetItem* parent, const aron::type::ObjectPtr& type):
+ parent(parent),
+ tree(tree),
+ type(type)
+ {
+ AronTreeWidgetCreatorVisitor v;
+ aron::type::visit(v, type);
+
+ if (v.createdQWidgetItem)
+ {
+ parent->addChild(v.createdQWidgetItem);
+ }
+ }
+
+ aron::data::DictPtr AronTreeWidgetController::convertToAron() const
+ {
+ if (parent)
+ {
+ AronTreeWidgetConverterVisitor v(parent, 0);
+ aron::type::visit(v, type);
+
+ auto aron_args = aron::data::Dict::DynamicCastAndCheck(v.createdAron);
+ return aron_args;
+ }
+ return nullptr;
+ }
+
+ void AronTreeWidgetController::setFromAron(const aron::data::DictPtr&)
+ {
+
+ }
+}
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetController.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetController.h
new file mode 100644
index 0000000000000000000000000000000000000000..4e73deb0592b30548bd72011ace95d29db070edf
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetController.h
@@ -0,0 +1,34 @@
+#pragma once
+
+#include <stack>
+#include <ArmarXCore/core/system/ImportExportComponent.h>
+
+#include "Data.h"
+
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+
+#include <QTreeWidget>
+#include "visitors/AronTreeWidgetCreator.h"
+#include "AronTreeWidgetItem.h"
+
+namespace armarx
+{
+ class AronTreeWidgetController
+ {
+
+ public:
+ AronTreeWidgetController(QTreeWidget* tree, QTreeWidgetItem* parent, const aron::type::ObjectPtr& type);
+
+ aron::data::DictPtr convertToAron() const;
+ void setFromAron(const aron::data::DictPtr&);
+
+ private:
+ QTreeWidgetItem* parent;
+ QTreeWidget* tree;
+
+ aron::type::ObjectPtr type;
+ };
+
+ using AronTreeWidgetControllerPtr = std::shared_ptr<AronTreeWidgetController>;
+}
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetItem.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetItem.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e53161bb8b2ee4aee0c34ac2fc82c3214894f0cd
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetItem.cpp
@@ -0,0 +1,28 @@
+#include "AronTreeWidgetItem.h"
+
+#include <RobotAPI/libraries/aron/core/type/variant/All.h>
+
+namespace armarx
+{
+ AronTreeWidgetItem* AronTreeWidgetItem::DynamicCast(QTreeWidgetItem* i)
+ {
+ return dynamic_cast<AronTreeWidgetItem*>(i);
+ }
+
+ AronTreeWidgetItem* AronTreeWidgetItem::copy()
+ {
+ AronTreeWidgetItem* ret = new AronTreeWidgetItem(*this);
+ return ret;
+ }
+
+ AronTreeWidgetItem* AronTreeWidgetItem::DynamicCastAndCheck(QTreeWidgetItem* i)
+ {
+ if (!i)
+ {
+ return nullptr;
+ }
+ auto c = DynamicCast(i);
+ ARMARX_CHECK_NOT_NULL(c);
+ return c;
+ }
+}
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetItem.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetItem.h
new file mode 100644
index 0000000000000000000000000000000000000000..3d6d0c2a329c5a4376fb24a406dadf706d6b898b
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetItem.h
@@ -0,0 +1,39 @@
+#pragma once
+
+#include <stack>
+#include <ArmarXCore/core/system/ImportExportComponent.h>
+
+#include "Data.h"
+
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+
+#include <RobotAPI/libraries/aron/core/type/variant/Variant.h>
+
+#include <QTreeWidget>
+
+namespace armarx
+{
+ class AronTreeWidgetItem : public QObject, public QTreeWidgetItem
+ {
+ Q_OBJECT
+ public:
+ AronTreeWidgetItem(const AronTreeWidgetItem& other) :
+ QObject(),
+ QTreeWidgetItem(other)
+ {
+ aronType = other.aronType;
+ }
+
+ using QTreeWidgetItem::QTreeWidgetItem;
+
+ AronTreeWidgetItem* copy(); // differs from clone!!!!
+
+ static AronTreeWidgetItem* DynamicCast(QTreeWidgetItem*);
+
+ static AronTreeWidgetItem* DynamicCastAndCheck(QTreeWidgetItem*);
+
+ aron::type::VariantPtr aronType;
+
+ };
+}
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/Data.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/Data.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..48b5816e7be127db579245f1044e4dd35aef4d77
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/Data.cpp
@@ -0,0 +1,6 @@
+#include "Data.h"
+
+namespace armarx
+{
+
+}
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/Data.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/Data.h
new file mode 100644
index 0000000000000000000000000000000000000000..3fea187d34f34cfcffa79a12ead1af3e5d97a55d
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/Data.h
@@ -0,0 +1,13 @@
+#pragma once
+
+#include <string>
+
+namespace armarx::aron_tree_widget::constantes
+{
+ const int TREE_WIDGET_ITEM_NAME = 0;
+ const int TREE_WIDGET_ITEM_VALUE = 1;
+ const int TREE_WIDGET_ITEM_TYPE = 2;
+
+ const std::string ITEM_EMPTY_MESSAGE = "(double click to edit)";
+ const std::string NEW_ITEM_DEFAULT_MESSAGE = "(Please set via main GUI (not in modal))";
+}
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/AronTreeWidgetModal.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/AronTreeWidgetModal.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8fbd9c5ba1abdb5e2d2175b1ff545579034c7376
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/AronTreeWidgetModal.cpp
@@ -0,0 +1,20 @@
+#include "AronTreeWidgetModal.h"
+
+namespace armarx
+{
+ AronTreeWidgetModal::AronTreeWidgetModal(const std::string& label, AronTreeWidgetItem* item, QTreeWidget* parent) :
+ QDialog(parent),
+ item(item),
+ label(label),
+ parent(parent)
+ {
+ init.aronType = item->aronType;
+ init.setText(aron_tree_widget::constantes::TREE_WIDGET_ITEM_NAME, item->text(aron_tree_widget::constantes::TREE_WIDGET_ITEM_NAME));
+ init.setText(aron_tree_widget::constantes::TREE_WIDGET_ITEM_VALUE, item->text(aron_tree_widget::constantes::TREE_WIDGET_ITEM_VALUE));
+ init.setText(aron_tree_widget::constantes::TREE_WIDGET_ITEM_TYPE, item->text(aron_tree_widget::constantes::TREE_WIDGET_ITEM_TYPE));
+ for (int i = 0; i < item->childCount(); ++i)
+ {
+ init.addChild(item->child(i)->clone());
+ }
+ }
+}
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/AronTreeWidgetModal.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/AronTreeWidgetModal.h
new file mode 100644
index 0000000000000000000000000000000000000000..cda9f7e728df6da713fb1e09001bc37e8502b9e6
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/AronTreeWidgetModal.h
@@ -0,0 +1,57 @@
+#pragma once
+
+#include <stack>
+#include <ArmarXCore/core/system/ImportExportComponent.h>
+
+#include "../AronTreeWidgetItem.h"
+#include "../Data.h"
+
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+
+#include <QDialog>
+#include <QTreeWidget>
+
+namespace armarx
+{
+ class AronTreeWidgetModal :
+ public QDialog
+ {
+ Q_OBJECT
+
+ public:
+ AronTreeWidgetModal(const std::string& label, AronTreeWidgetItem* item, QTreeWidget* parent);
+
+ protected slots:
+ virtual void reset()
+ {
+ item->aronType = init.aronType;
+ item->setText(aron_tree_widget::constantes::TREE_WIDGET_ITEM_NAME, init.text(aron_tree_widget::constantes::TREE_WIDGET_ITEM_NAME));
+ item->setText(aron_tree_widget::constantes::TREE_WIDGET_ITEM_VALUE, init.text(aron_tree_widget::constantes::TREE_WIDGET_ITEM_VALUE));
+ item->setText(aron_tree_widget::constantes::TREE_WIDGET_ITEM_TYPE, init.text(aron_tree_widget::constantes::TREE_WIDGET_ITEM_TYPE));
+ for (int i = 0; i < item->childCount(); ++i)
+ {
+ item->removeChild(item->child(i));
+ }
+ for (int i = 0; i < init.childCount(); ++i)
+ {
+ item->addChild(init.child(i)->clone());
+ }
+ }
+ virtual void submit()
+ {
+ accept();
+ }
+
+ protected:
+
+ AronTreeWidgetItem init;
+ AronTreeWidgetItem* item;
+
+ private:
+ std::string label;
+ QTreeWidget* parent;
+ };
+
+ using AronTreeWidgetModalControllerPtr = std::shared_ptr<AronTreeWidgetModal>;
+}
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/dict/AronTreeWidgetDictInputModal.ui b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/dict/AronTreeWidgetDictInputModal.ui
new file mode 100644
index 0000000000000000000000000000000000000000..b9b800ed26a2b2170b8ab06633e8e795b1957be4
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/dict/AronTreeWidgetDictInputModal.ui
@@ -0,0 +1,88 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>AronTreeWidgetDictInputModalWidget</class>
+ <widget class="QWidget" name="AronTreeWidgetDictInputModalWidget">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>1015</width>
+ <height>498</height>
+ </rect>
+ </property>
+ <property name="windowTitle">
+ <string>SkillManagerMonitorWidget</string>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout">
+ <item>
+ <widget class="QSplitter" name="splitter">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <widget class="QGroupBox" name="groupBoxInput">
+ <property name="title">
+ <string>InputField</string>
+ </property>
+ <layout class="QGridLayout" name="gridLayout">
+ <item row="0" column="0" colspan="5">
+ <widget class="QTreeWidget" name="treeWidgetDict">
+ <column>
+ <property name="text">
+ <string>Key</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string>Value</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string>Type</string>
+ </property>
+ </column>
+ </widget>
+ </item>
+ <item row="1" column="3" colspan="2">
+ <widget class="QPushButton" name="pushButtonAddElement">
+ <property name="text">
+ <string>+ Add element</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0" colspan="3">
+ <widget class="QLineEdit" name="lineEditKey">
+ <property name="maximumSize">
+ <size>
+ <width>880</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Enter Key</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="0">
+ <widget class="QPushButton" name="pushButtonReset">
+ <property name="text">
+ <string>Reset</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="1" colspan="4">
+ <widget class="QPushButton" name="pushButtonSubmit">
+ <property name="text">
+ <string>Submit</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/dict/AronTreeWidgetDictInputModalController.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/dict/AronTreeWidgetDictInputModalController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f7d83d98b6cde491cad878822b69b50adb34d355
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/dict/AronTreeWidgetDictInputModalController.cpp
@@ -0,0 +1,73 @@
+#include "AronTreeWidgetDictInputModalController.h"
+
+#include <RobotAPI/libraries/aron/core/type/variant/All.h>
+#include "../../visitors/AronTreeWidgetCreator.h"
+
+namespace armarx
+{
+ AronTreeWidgetDictInputModalController::AronTreeWidgetDictInputModalController(const std::string& label, AronTreeWidgetItem* item, QTreeWidget* parent) :
+ AronTreeWidgetModal(label, item, parent)
+ {
+ widget.setupUi(this);
+
+ // set header
+ widget.groupBoxInput->setTitle(QString::fromStdString(label));
+ reset();
+
+ // connect signals
+ connect(widget.pushButtonAddElement, &QPushButton::clicked,
+ this, &AronTreeWidgetDictInputModalController::addEmptyElement);
+
+ connect(widget.pushButtonReset, &QPushButton::clicked,
+ this, &AronTreeWidgetDictInputModalController::reset);
+ connect(widget.pushButtonSubmit, &QPushButton::clicked,
+ this, &AronTreeWidgetDictInputModalController::submit);
+
+ }
+
+ void AronTreeWidgetDictInputModalController::submit()
+ {
+ for (const auto& added : addedItems)
+ {
+ item->addChild(added->copy());
+ }
+
+ AronTreeWidgetModal::submit();
+ }
+
+ void AronTreeWidgetDictInputModalController::reset()
+ {
+ AronTreeWidgetModal::reset();
+
+ // reset to initial value
+ widget.treeWidgetDict->clear();
+ for (int i = 0; i < init.childCount(); ++i)
+ {
+ auto el = init.child(i);
+ widget.treeWidgetDict->addTopLevelItem(el->clone());
+ }
+ }
+
+ void AronTreeWidgetDictInputModalController::addEmptyElement()
+ {
+ QString s = widget.lineEditKey->text();
+ widget.lineEditKey->setText("Enter Key");
+
+ if (widget.treeWidgetDict->findItems(s, Qt::MatchFlag::MatchExactly, 0).empty())
+ {
+ auto t = item->aronType;
+ auto d = aron::type::Dict::DynamicCastAndCheck(t);
+ auto ac = d->getAcceptedType();
+
+ AronTreeWidgetCreatorVisitor v;
+ aron::type::visit(v, ac);
+
+ if (v.createdQWidgetItem)
+ {
+ v.createdQWidgetItem->setText(aron_tree_widget::constantes::TREE_WIDGET_ITEM_NAME, s);
+ addedItems.push_back(v.createdQWidgetItem);
+ widget.treeWidgetDict->addTopLevelItem(v.createdQWidgetItem);
+ }
+ }
+ }
+}
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/dict/AronTreeWidgetDictInputModalController.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/dict/AronTreeWidgetDictInputModalController.h
new file mode 100644
index 0000000000000000000000000000000000000000..f5d94a04b46994220fb0ff9c81e1fff50a9c3091
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/dict/AronTreeWidgetDictInputModalController.h
@@ -0,0 +1,36 @@
+#pragma once
+
+#include <stack>
+#include <ArmarXCore/core/system/ImportExportComponent.h>
+
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
+
+#include "../AronTreeWidgetModal.h"
+
+#include <RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/dict/ui_AronTreeWidgetDictInputModal.h>
+
+#include <QDialog>
+
+namespace armarx
+{
+ class AronTreeWidgetDictInputModalController :
+ public AronTreeWidgetModal
+ {
+ public:
+
+ AronTreeWidgetDictInputModalController(const std::string& label, AronTreeWidgetItem* item, QTreeWidget* parent);
+
+ private slots:
+
+ void submit() final;
+ void reset() final;
+
+ void addEmptyElement();
+
+ private:
+ std::vector<AronTreeWidgetItem*> addedItems;
+ Ui::AronTreeWidgetDictInputModalWidget widget;
+ };
+}
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/others_are_TODO b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/others_are_TODO
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/AronTreeWidgetInputModal.ui b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/text/AronTreeWidgetTextInputModal.ui
similarity index 91%
rename from source/RobotAPI/gui-plugins/SkillManagerPlugin/AronTreeWidgetInputModal.ui
rename to source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/text/AronTreeWidgetTextInputModal.ui
index baec62815f1b8b7f2c6b46b7e2f3847130a9509e..ed40962f96639ff34eed30b85331d7774b2eef9d 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/AronTreeWidgetInputModal.ui
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/text/AronTreeWidgetTextInputModal.ui
@@ -1,7 +1,7 @@
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
- <class>AronTreeWidgetInputModalWidget</class>
- <widget class="QWidget" name="AronTreeWidgetInputModalWidget">
+ <class>AronTreeWidgetTextInputModalWidget</class>
+ <widget class="QWidget" name="AronTreeWidgetTextInputModalWidget">
<property name="geometry">
<rect>
<x>0</x>
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/text/AronTreeWidgetTextInputModalController.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/text/AronTreeWidgetTextInputModalController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..972fe6ff98b20e2bfe0970455df1ed760a4c4203
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/text/AronTreeWidgetTextInputModalController.cpp
@@ -0,0 +1,35 @@
+#include "AronTreeWidgetTextInputModalController.h"
+
+namespace armarx
+{
+ AronTreeWidgetTextInputModalController::AronTreeWidgetTextInputModalController(const std::string& label, AronTreeWidgetItem* item, QTreeWidget* parent) :
+ AronTreeWidgetModal(label, item, parent)
+ {
+ widget.setupUi(this);
+
+ // set header
+ widget.groupBoxInput->setTitle(QString::fromStdString(label));
+ reset();
+
+ // connect signals
+ connect(widget.pushButtonReset, &QPushButton::clicked,
+ this, &AronTreeWidgetTextInputModalController::reset);
+ connect(widget.pushButtonSubmit, &QPushButton::clicked,
+ this, &AronTreeWidgetTextInputModalController::submit);
+ }
+
+ void AronTreeWidgetTextInputModalController::submit()
+ {
+ item->setText(aron_tree_widget::constantes::TREE_WIDGET_ITEM_VALUE, widget.textEditInput->toPlainText());
+
+ AronTreeWidgetModal::submit();
+ }
+
+ void AronTreeWidgetTextInputModalController::reset()
+ {
+ AronTreeWidgetModal::reset();
+
+ // reset to initial value
+ widget.textEditInput->setPlainText(init.text(aron_tree_widget::constantes::TREE_WIDGET_ITEM_VALUE));
+ }
+}
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/text/AronTreeWidgetTextInputModalController.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/text/AronTreeWidgetTextInputModalController.h
new file mode 100644
index 0000000000000000000000000000000000000000..216451f4ad761080357b368744b3c7c1a65e77dc
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/text/AronTreeWidgetTextInputModalController.h
@@ -0,0 +1,34 @@
+#pragma once
+
+#include <stack>
+#include <ArmarXCore/core/system/ImportExportComponent.h>
+
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
+
+#include "../AronTreeWidgetModal.h"
+
+#include <RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/text/ui_AronTreeWidgetTextInputModal.h>
+
+#include <QDialog>
+
+namespace armarx
+{
+ class AronTreeWidgetTextInputModalController :
+ public AronTreeWidgetModal
+ {
+
+ public:
+
+ AronTreeWidgetTextInputModalController(const std::string& label, AronTreeWidgetItem* item, QTreeWidget* parent);
+
+ private slots:
+
+ void submit() final;
+ void reset() final;
+
+ private:
+ Ui::AronTreeWidgetTextInputModalWidget widget;
+ };
+}
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e9effb321210d3c3201adaa40368a359785a1524
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.cpp
@@ -0,0 +1,252 @@
+/*
+ * 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::gui-plugins::SkillManagerMonitorWidgetController
+ * \author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * \date 2020
+ * \copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include <string>
+
+#include <ArmarXCore/util/CPPUtility/Iterator.h>
+
+#include "AronTreeWidgetConverter.h"
+
+// debug
+#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
+
+//visitors
+namespace armarx
+{
+ void
+ AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::ObjectPtr& i)
+ {
+ auto createdAronDict = std::make_shared<aron::data::Dict>();
+ createdAron = createdAronDict;
+ QTreeWidgetItem* el = parentItem->child(index);
+
+ unsigned int x = 0;
+ for (const auto& [key, value] : i->getMemberTypes())
+ {
+ AronTreeWidgetConverterVisitor v(el, x++);
+ aron::type::visit(v, value);
+
+ if (v.createdAron)
+ {
+ createdAronDict->addElement(key, v.createdAron);
+ }
+ }
+ }
+
+ void
+ AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::DictPtr& i)
+ {
+ auto createdAronDict = std::make_shared<aron::data::Dict>();
+ createdAron = createdAronDict;
+ QTreeWidgetItem* el = parentItem->child(index);
+
+ for (int x = 0; x < el->childCount(); ++x)
+ {
+ auto it = el->child(x);
+ AronTreeWidgetConverterVisitor v(el, x);
+ aron::type::visit(v, i->getAcceptedType());
+
+ if (v.createdAron)
+ {
+ createdAronDict->addElement(it->text(0).toStdString(), v.createdAron);
+ }
+ }
+ }
+
+ void
+ AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::PairPtr& i)
+ {
+ // TODO
+ }
+
+ void
+ AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::TuplePtr& i)
+ {
+ // TODO
+ }
+
+ void
+ AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::ListPtr& i)
+ {
+ // TODO
+ }
+
+ void
+ AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::NDArrayPtr& i)
+ {
+ // TODO
+ }
+
+ void
+ AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::MatrixPtr& i)
+ {
+ // TODO
+ }
+
+ void
+ AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::QuaternionPtr& i)
+ {
+ // TODO
+ }
+
+ void
+ AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::PositionPtr& i)
+ {
+ // TODO
+ }
+
+ void
+ AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::OrientationPtr& i)
+ {
+ // TODO
+ }
+
+ void
+ AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::PosePtr& i)
+ {
+ // TODO
+ }
+
+ void
+ AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::ImagePtr& i)
+ {
+ // TODO
+ }
+
+ void
+ AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::PointCloudPtr& i)
+ {
+ // TODO
+ }
+
+ void
+ AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::IntEnumPtr& i)
+ {
+ // TODO
+ }
+
+ void
+ AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::IntPtr& i)
+ {
+ auto createdAronInt = std::make_shared<aron::data::Int>();
+ createdAron = createdAronInt;
+ QTreeWidgetItem* el = parentItem->child(index);
+
+ std::string str = el->text(1).toStdString();
+ if (str.empty())
+ {
+ createdAronInt->setValue(0);
+ return;
+ }
+
+ int val = std::stoi(str);
+ createdAronInt->setValue(val);
+ }
+
+ void
+ AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::LongPtr& i)
+ {
+ auto createdAronLong = std::make_shared<aron::data::Long>();
+ createdAron = createdAronLong;
+ QTreeWidgetItem* el = parentItem->child(index);
+
+ std::string str = el->text(1).toStdString();
+ if (str.empty())
+ {
+ str = el->text(3).toStdString();
+ }
+
+ createdAronLong->fromString(str);
+ }
+
+ void
+ AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::FloatPtr& i)
+ {
+ auto createdAronFloat = std::make_shared<aron::data::Float>();
+ createdAron = createdAronFloat;
+ QTreeWidgetItem* el = parentItem->child(index);
+
+ std::string str = el->text(1).toStdString();
+ if (str.empty())
+ {
+ str = el->text(3).toStdString();
+ }
+
+ createdAronFloat->fromString(str);
+ }
+
+ void
+ AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::DoublePtr& i)
+ {
+ auto createdAronDouble = std::make_shared<aron::data::Double>();
+ createdAron = createdAronDouble;
+ QTreeWidgetItem* el = parentItem->child(index);
+
+ std::string str = el->text(1).toStdString();
+ if (str.empty())
+ {
+ str = el->text(3).toStdString();
+ }
+
+ createdAronDouble->fromString(str);
+ }
+
+ void
+ AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::BoolPtr& i)
+ {
+ auto createdAronBool = std::make_shared<aron::data::Bool>();
+ createdAron = createdAronBool;
+ QTreeWidgetItem* el = parentItem->child(index);
+
+ std::string str = el->text(1).toStdString();
+ if (str.empty())
+ {
+ str = el->text(3).toStdString();
+ }
+
+ createdAronBool->fromString(str);
+ }
+
+ void
+ AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::StringPtr& i)
+ {
+ auto createdAronString = std::make_shared<aron::data::String>();
+ createdAron = createdAronString;
+ QTreeWidgetItem* el = parentItem->child(index);
+
+ std::string str = el->text(1).toStdString();
+ createdAronString->fromString(str);
+ }
+
+ void
+ AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::TimePtr& i)
+ {
+ auto l = std::make_shared<aron::type::Long>();
+ visitLong(l);
+ }
+
+ void AronTreeWidgetConverterVisitor::visitUnknown(Input&)
+ {
+ ARMARX_WARNING_S << "Received an unknown type when trying to convert a skill argument type to an aron data object.";
+ }
+}
+
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/AronTreeWidgetEditor.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.h
similarity index 60%
rename from source/RobotAPI/gui-plugins/SkillManagerPlugin/AronTreeWidgetEditor.h
rename to source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.h
index 3d391783077458df2e9257d40e188cd81bf36188..5021cb5ae473c3212ec3bcde32708940fcb23f9d 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/AronTreeWidgetEditor.h
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.h
@@ -39,48 +39,8 @@
namespace armarx
{
- // Convert aron type to tree widget
- class AronTreeViewWidgetCreatorVisitor :
- public armarx::aron::type::ConstVariantVisitor
- {
- public:
- QTreeWidgetItem* parentItem;
- QTreeWidgetItem* createdQWidgetItem;
-
- AronTreeViewWidgetCreatorVisitor() = delete;
- AronTreeViewWidgetCreatorVisitor(QTreeWidgetItem* i) :
- parentItem(i)
- {}
-
- void createSimpleTreeViewWidget(Input& i);
-
- void visitAronVariant(const aron::type::ObjectPtr&) override;
- void visitAronVariant(const aron::type::DictPtr& i) override;
- void visitAronVariant(const aron::type::PairPtr& i) override;
- void visitAronVariant(const aron::type::TuplePtr& i) override;
- void visitAronVariant(const aron::type::ListPtr& i) override;
- void visitAronVariant(const aron::type::NDArrayPtr& i) override;
- void visitAronVariant(const aron::type::MatrixPtr& i) override;
- void visitAronVariant(const aron::type::QuaternionPtr& i) override;
- void visitAronVariant(const aron::type::PositionPtr& i) override;
- void visitAronVariant(const aron::type::OrientationPtr& i) override;
- void visitAronVariant(const aron::type::PosePtr& i) override;
- void visitAronVariant(const aron::type::ImagePtr& i) override;
- void visitAronVariant(const aron::type::PointCloudPtr& i) override;
- void visitAronVariant(const aron::type::IntEnumPtr& i) override;
- void visitAronVariant(const aron::type::IntPtr& i) override;
- void visitAronVariant(const aron::type::LongPtr& i) override;
- void visitAronVariant(const aron::type::FloatPtr& i) override;
- void visitAronVariant(const aron::type::DoublePtr& i) override;
- void visitAronVariant(const aron::type::BoolPtr& i) override;
- void visitAronVariant(const aron::type::StringPtr& i) override;
- void visitAronVariant(const aron::type::TimePtr& i) override;
- void visitUnknown(Input&) override;
- };
-
-
// Conversion from TreeView to aron data
- class AronTreeViewWidgetConverterVisitor :
+ class AronTreeWidgetConverterVisitor :
public armarx::aron::type::ConstVariantVisitor
{
public:
@@ -88,8 +48,8 @@ namespace armarx
int index;
aron::data::VariantPtr createdAron;
- AronTreeViewWidgetConverterVisitor() = delete;
- AronTreeViewWidgetConverterVisitor(QTreeWidgetItem* i, int x) :
+ AronTreeWidgetConverterVisitor() = delete;
+ AronTreeWidgetConverterVisitor(QTreeWidgetItem* i, int x) :
parentItem(i), index(x)
{}
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetCreator.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetCreator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b3a8682081312bc1f3dbc7f036db10dbe40fb5f9
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetCreator.cpp
@@ -0,0 +1,144 @@
+/*
+ * 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::gui-plugins::SkillManagerMonitorWidgetController
+ * \author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * \date 2020
+ * \copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include <string>
+
+#include <ArmarXCore/util/CPPUtility/Iterator.h>
+
+#include "AronTreeWidgetCreator.h"
+
+// debug
+#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
+
+#include "../Data.h"
+
+//visitors
+namespace armarx
+{
+ void
+ AronTreeWidgetCreatorVisitor::createSimpleTreeViewWidget(Input& i, const std::string& defaul)
+ {
+ ARMARX_CHECK_NOT_NULL(i);
+
+ auto key = i->getPath().getLastElement();
+ createdQWidgetItem = new AronTreeWidgetItem();
+ createdQWidgetItem->aronType = i;
+ createdQWidgetItem->setText(0, QString::fromStdString(key));
+ createdQWidgetItem->setText(1, QString::fromStdString(defaul));
+ createdQWidgetItem->setText(2, QString::fromStdString(i->getShortName()));
+ createdQWidgetItem->setText(3, QString::fromStdString(aron_tree_widget::constantes::ITEM_EMPTY_MESSAGE) /*QString::fromStdString(i->getDefaultFromString())*/);
+ createdQWidgetItem->setFlags(createdQWidgetItem->flags() | Qt::ItemIsEditable);
+ }
+
+ void
+ AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::ObjectPtr& i)
+ {
+ ARMARX_CHECK_NOT_NULL(i);
+
+ auto key = i->getObjectName();
+ if (i->getPath().hasElement())
+ {
+ key = i->getPath().getLastElement();
+ }
+
+ createdQWidgetItem = new AronTreeWidgetItem();
+ createdQWidgetItem->setText(0, QString::fromStdString(key));
+
+ for (const auto& [key, value] : i->getMemberTypes())
+ {
+ AronTreeWidgetCreatorVisitor v;
+ aron::type::visit(v, value);
+
+ if (v.createdQWidgetItem)
+ {
+ createdQWidgetItem->addChild(v.createdQWidgetItem);
+ }
+ }
+ }
+
+ void
+ AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::DictPtr& i)
+ { createSimpleTreeViewWidget(i, ""); }
+ void
+ AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::PairPtr& i)
+ { createSimpleTreeViewWidget(i, ""); }
+ void
+ AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::TuplePtr& i)
+ { createSimpleTreeViewWidget(i, ""); }
+ void
+ AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::ListPtr& i)
+ { createSimpleTreeViewWidget(i, ""); }
+ void
+ AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::NDArrayPtr& i)
+ { createSimpleTreeViewWidget(i, ""); }
+ void
+ AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::MatrixPtr& i)
+ { createSimpleTreeViewWidget(i, ""); }
+ void
+ AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::QuaternionPtr& i)
+ { createSimpleTreeViewWidget(i, ""); }
+ void
+ AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::PositionPtr& i)
+ { createSimpleTreeViewWidget(i, ""); }
+ void
+ AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::OrientationPtr& i)
+ { createSimpleTreeViewWidget(i, ""); }
+ void
+ AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::PosePtr& i)
+ { createSimpleTreeViewWidget(i, ""); }
+ void
+ AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::ImagePtr& i)
+ { createSimpleTreeViewWidget(i, ""); }
+ void
+ AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::PointCloudPtr& i)
+ { createSimpleTreeViewWidget(i, ""); }
+ void
+ AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::IntEnumPtr& i)
+ { createSimpleTreeViewWidget(i, ""); }
+ void
+ AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::IntPtr& i)
+ { createSimpleTreeViewWidget(i, "0"); }
+ void
+ AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::LongPtr& i)
+ { createSimpleTreeViewWidget(i, "0"); }
+ void
+ AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::FloatPtr& i)
+ { createSimpleTreeViewWidget(i, "0.0"); }
+ void
+ AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::DoublePtr& i)
+ { createSimpleTreeViewWidget(i, "0.0"); }
+ void
+ AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::BoolPtr& i)
+ { createSimpleTreeViewWidget(i, "false"); }
+ void
+ AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::StringPtr& i)
+ { createSimpleTreeViewWidget(i, ""); }
+ void
+ AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::TimePtr& i)
+ { createSimpleTreeViewWidget(i, "0"); }
+
+ void AronTreeWidgetCreatorVisitor::visitUnknown(Input&)
+ {
+ ARMARX_WARNING_S << "Received an unknown type when trying to create a tree view widget for a skill argument type.";
+ }
+}
+
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetCreator.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetCreator.h
new file mode 100644
index 0000000000000000000000000000000000000000..a81d1137df6f29636f790cc5083f744c7b13f1b0
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetCreator.h
@@ -0,0 +1,79 @@
+/*
+ * 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::gui-plugins::SkillManagerMonitorWidgetController
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#pragma once
+
+#include <stack>
+#include <ArmarXCore/core/system/ImportExportComponent.h>
+
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
+
+#include <RobotAPI/interface/skills/SkillMemoryInterface.h>
+
+#include <RobotAPI/gui-plugins/SkillManagerPlugin/ui_SkillManagerMonitorWidget.h>
+
+#include "../AronTreeWidgetItem.h"
+
+#include <RobotAPI/libraries/aron/core/type/variant/All.h>
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+#include <RobotAPI/libraries/aron/core/type/visitor/variant/VariantVisitor.h>
+
+namespace armarx
+{
+ // Convert aron type to tree widget
+ class AronTreeWidgetCreatorVisitor :
+ public armarx::aron::type::ConstVariantVisitor
+ {
+ public:
+ AronTreeWidgetItem* createdQWidgetItem;
+
+ AronTreeWidgetCreatorVisitor() = default;
+
+ void createSimpleTreeViewWidget(Input& i, const std::string&);
+
+ void visitAronVariant(const aron::type::ObjectPtr&) override;
+ void visitAronVariant(const aron::type::DictPtr& i) override;
+ void visitAronVariant(const aron::type::PairPtr& i) override;
+ void visitAronVariant(const aron::type::TuplePtr& i) override;
+ void visitAronVariant(const aron::type::ListPtr& i) override;
+ void visitAronVariant(const aron::type::NDArrayPtr& i) override;
+ void visitAronVariant(const aron::type::MatrixPtr& i) override;
+ void visitAronVariant(const aron::type::QuaternionPtr& i) override;
+ void visitAronVariant(const aron::type::PositionPtr& i) override;
+ void visitAronVariant(const aron::type::OrientationPtr& i) override;
+ void visitAronVariant(const aron::type::PosePtr& i) override;
+ void visitAronVariant(const aron::type::ImagePtr& i) override;
+ void visitAronVariant(const aron::type::PointCloudPtr& i) override;
+ void visitAronVariant(const aron::type::IntEnumPtr& i) override;
+ void visitAronVariant(const aron::type::IntPtr& i) override;
+ void visitAronVariant(const aron::type::LongPtr& i) override;
+ void visitAronVariant(const aron::type::FloatPtr& i) override;
+ void visitAronVariant(const aron::type::DoublePtr& i) override;
+ void visitAronVariant(const aron::type::BoolPtr& i) override;
+ void visitAronVariant(const aron::type::StringPtr& i) override;
+ void visitAronVariant(const aron::type::TimePtr& i) override;
+ void visitUnknown(Input&) override;
+ };
+}
+
+
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetModalCreator.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetModalCreator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a2a2240d354758a257a022fa07e211b1d993495b
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetModalCreator.cpp
@@ -0,0 +1,146 @@
+/*
+ * 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::gui-plugins::SkillManagerMonitorWidgetController
+ * \author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * \date 2020
+ * \copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include <string>
+
+#include <ArmarXCore/util/CPPUtility/Iterator.h>
+
+#include "AronTreeWidgetModalCreator.h"
+
+#include <SimoxUtility/algorithm/string.h>
+
+// debug
+#include "../modal/text/AronTreeWidgetTextInputModalController.h"
+#include "../modal/dict/AronTreeWidgetDictInputModalController.h"
+
+//visitors
+namespace armarx
+{
+
+ void AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::ObjectPtr& i)
+ {
+ // should not happen, right?
+ }
+
+ void
+ AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::DictPtr& i)
+ {
+ createdModal = std::make_shared<AronTreeWidgetDictInputModalController>(label, item, parent);
+ }
+
+ void
+ AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::PairPtr& i)
+ {
+ }
+
+ void
+ AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::TuplePtr& i)
+ { }
+
+ void
+ AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::ListPtr& i)
+ { }
+
+ void
+ AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::NDArrayPtr& i)
+ { }
+
+ void
+ AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::MatrixPtr& i)
+ { }
+
+ void
+ AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::QuaternionPtr& i)
+ { }
+
+ void
+ AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::PositionPtr& i)
+ { }
+
+ void
+ AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::OrientationPtr& i)
+ { }
+
+ void
+ AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::PosePtr& i)
+ { }
+
+ void
+ AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::ImagePtr& i)
+ { }
+
+ void
+ AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::PointCloudPtr& i)
+ { }
+
+ void
+ AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::IntEnumPtr& i)
+ { }
+
+ void
+ AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::IntPtr& i)
+ {
+ createdModal = std::make_shared<AronTreeWidgetTextInputModalController>(label, item, parent);
+ }
+
+ void
+ AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::LongPtr& i)
+ {
+ createdModal = std::make_shared<AronTreeWidgetTextInputModalController>(label, item, parent);
+ }
+
+ void
+ AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::FloatPtr& i)
+ {
+ createdModal = std::make_shared<AronTreeWidgetTextInputModalController>(label, item, parent);
+ }
+
+ void
+ AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::DoublePtr& i)
+ {
+ createdModal = std::make_shared<AronTreeWidgetTextInputModalController>(label, item, parent);
+ }
+
+ void
+ AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::BoolPtr& i)
+ {
+ createdModal = std::make_shared<AronTreeWidgetTextInputModalController>(label, item, parent);
+ }
+
+ void
+ AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::StringPtr& i)
+ {
+ createdModal = std::make_shared<AronTreeWidgetTextInputModalController>(label, item, parent);
+ }
+
+ void
+ AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::TimePtr& i)
+ {
+ createdModal = std::make_shared<AronTreeWidgetTextInputModalController>(label, item, parent);
+ }
+
+ void AronTreeWidgetModalCreatorVisitor::visitUnknown(Input&)
+ {
+ ARMARX_WARNING_S << "Received an unknown type when trying to create a tree view widget modal for a skill argument type.";
+ }
+}
+
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetModalCreator.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetModalCreator.h
new file mode 100644
index 0000000000000000000000000000000000000000..c1bba0790bb2ddeb00c3d6c166242f324bf74922
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetModalCreator.h
@@ -0,0 +1,87 @@
+/*
+ * 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::gui-plugins::SkillManagerMonitorWidgetController
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#pragma once
+
+#include <stack>
+#include <ArmarXCore/core/system/ImportExportComponent.h>
+
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
+
+#include <RobotAPI/interface/skills/SkillMemoryInterface.h>
+
+#include <RobotAPI/gui-plugins/SkillManagerPlugin/ui_SkillManagerMonitorWidget.h>
+
+#include "../modal/AronTreeWidgetModal.h"
+
+#include <RobotAPI/libraries/aron/core/type/variant/All.h>
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+#include <RobotAPI/libraries/aron/core/type/visitor/variant/VariantVisitor.h>
+
+#include <QTreeWidget>
+
+namespace armarx
+{
+ // Convert aron type to tree widget
+ class AronTreeWidgetModalCreatorVisitor :
+ public armarx::aron::type::ConstVariantVisitor
+ {
+ public:
+ std::string label;
+ AronTreeWidgetItem* item;
+ QTreeWidget* parent;
+ AronTreeWidgetModalControllerPtr createdModal;
+
+ AronTreeWidgetModalCreatorVisitor() = delete;
+ AronTreeWidgetModalCreatorVisitor(const std::string& label, AronTreeWidgetItem* item, QTreeWidget* parent) :
+ label(label),
+ item(item),
+ parent(parent)
+ {}
+
+ void visitAronVariant(const aron::type::ObjectPtr&) override;
+ void visitAronVariant(const aron::type::DictPtr& i) override;
+ void visitAronVariant(const aron::type::PairPtr& i) override;
+ void visitAronVariant(const aron::type::TuplePtr& i) override;
+ void visitAronVariant(const aron::type::ListPtr& i) override;
+ void visitAronVariant(const aron::type::NDArrayPtr& i) override;
+ void visitAronVariant(const aron::type::MatrixPtr& i) override;
+ void visitAronVariant(const aron::type::QuaternionPtr& i) override;
+ void visitAronVariant(const aron::type::PositionPtr& i) override;
+ void visitAronVariant(const aron::type::OrientationPtr& i) override;
+ void visitAronVariant(const aron::type::PosePtr& i) override;
+ void visitAronVariant(const aron::type::ImagePtr& i) override;
+ void visitAronVariant(const aron::type::PointCloudPtr& i) override;
+ void visitAronVariant(const aron::type::IntEnumPtr& i) override;
+ void visitAronVariant(const aron::type::IntPtr& i) override;
+ void visitAronVariant(const aron::type::LongPtr& i) override;
+ void visitAronVariant(const aron::type::FloatPtr& i) override;
+ void visitAronVariant(const aron::type::DoublePtr& i) override;
+ void visitAronVariant(const aron::type::BoolPtr& i) override;
+ void visitAronVariant(const aron::type::StringPtr& i) override;
+ void visitAronVariant(const aron::type::TimePtr& i) override;
+ void visitUnknown(Input&) override;
+ };
+}
+
+
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index 638e9737b6e47043b1f6c253f6bb640b5f596a00..36442cf0bdaa4238a8a832fe39f6c244569fe830 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -72,6 +72,7 @@ set(SLICE_FILES
units/RobotUnit/NJointCartesianWaypointController.ice
units/RobotUnit/NJointCartesianNaturalPositionController.ice
units/RobotUnit/RobotUnitInterface.ice
+ units/RobotUnit/GazeController.ice
units/RobotUnit/NJointBimanualForceController.ice
units/RobotUnit/NJointBimanualObjLevelController.ice
@@ -106,6 +107,7 @@ set(SLICE_FILES
armem.ice
+ armem/actions.ice
armem/io.ice
armem/commit.ice
armem/memory.ice
@@ -115,6 +117,7 @@ set(SLICE_FILES
armem/client/MemoryListenerInterface.ice
armem/server.ice
+ armem/server/ActionsInterface.ice
armem/server/StoringMemoryInterface.ice
armem/server/LoadingMemoryInterface.ice
armem/server/MemoryInterface.ice
diff --git a/source/RobotAPI/interface/armem.ice b/source/RobotAPI/interface/armem.ice
index 79c54360ec8cdea2189d6f748cbadc4544379130..f7621099de52b5a2f15b3f0f6367ec5c19471fb3 100644
--- a/source/RobotAPI/interface/armem.ice
+++ b/source/RobotAPI/interface/armem.ice
@@ -1,5 +1,6 @@
#pragma once
+#include <RobotAPI/interface/armem/actions.ice>
#include <RobotAPI/interface/armem/client.ice>
#include <RobotAPI/interface/armem/commit.ice>
#include <RobotAPI/interface/armem/memory.ice>
diff --git a/source/RobotAPI/interface/armem/actions.ice b/source/RobotAPI/interface/armem/actions.ice
new file mode 100644
index 0000000000000000000000000000000000000000..5bb06fc4c9ec815dba87fef49a31bc13498b66fc
--- /dev/null
+++ b/source/RobotAPI/interface/armem/actions.ice
@@ -0,0 +1,74 @@
+#pragma once
+
+#include <RobotAPI/interface/armem/memory.ice>
+
+module armarx
+{
+ module armem
+ {
+ module actions
+ {
+ module data
+ {
+
+ class MenuEntry
+ {
+ /// Item in the created action path.
+ string id;
+ /// Human-readable text displayed to the user.
+ string text;
+ };
+ sequence<MenuEntry> MenuEntrySeq;
+
+ /**
+ * Clickable / executable action.
+ */
+ class Action extends MenuEntry
+ {
+ };
+
+ /**
+ * Expandable sub-menu, but cannot be executed itself.
+ */
+ class SubMenu extends MenuEntry
+ {
+ MenuEntrySeq entries;
+ };
+
+
+ class Menu
+ {
+ MenuEntrySeq entries;
+ };
+
+ sequence<string> ActionPath;
+
+ struct GetActionsInput
+ {
+ armem::data::MemoryID id;
+ };
+ sequence<GetActionsInput> GetActionsInputSeq;
+
+ struct GetActionsOutput
+ {
+ Menu menu;
+ };
+ sequence<GetActionsOutput> GetActionsOutputSeq;
+
+ struct ExecuteActionInput
+ {
+ armem::data::MemoryID id;
+ ActionPath actionPath;
+ };
+ sequence<ExecuteActionInput> ExecuteActionInputSeq;
+
+ struct ExecuteActionOutput
+ {
+ bool success = false;
+ string errorMessage;
+ };
+ sequence<ExecuteActionOutput> ExecuteActionOutputSeq;
+ }
+ }
+ }
+}
diff --git a/source/RobotAPI/interface/armem/mns/MemoryNameSystemInterface.ice b/source/RobotAPI/interface/armem/mns/MemoryNameSystemInterface.ice
index 95b6a9fa6b03d8f7d4644d2538ecfd64d5258229..2d6b220a2050001960fcc4b564d54b3526f4ed44 100644
--- a/source/RobotAPI/interface/armem/mns/MemoryNameSystemInterface.ice
+++ b/source/RobotAPI/interface/armem/mns/MemoryNameSystemInterface.ice
@@ -1,5 +1,6 @@
#pragma once
+#include <RobotAPI/interface/armem/server/ActionsInterface.ice>
#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.ice>
#include <RobotAPI/interface/armem/server/WritingMemoryInterface.ice>
@@ -15,11 +16,14 @@ module armarx
/**
* A memory server can implement the reading and/or writing
* memory interface. They should be handled individually.
+ * Additionally, it can implement an actions interface,
+ * which is currently the case for all reading and writing memories.
*/
struct MemoryServerInterfaces
{
server::ReadingMemoryInterface* reading;
server::WritingMemoryInterface* writing;
+ server::actions::ActionsInterface* actions;
};
dictionary<string, MemoryServerInterfaces> MemoryServerInterfacesMap;
diff --git a/source/RobotAPI/interface/armem/server/ActionsInterface.ice b/source/RobotAPI/interface/armem/server/ActionsInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..8fa6754f7e1e0c961c4a5ff9c9bb317dc9fc3aef
--- /dev/null
+++ b/source/RobotAPI/interface/armem/server/ActionsInterface.ice
@@ -0,0 +1,21 @@
+#pragma once
+
+#include <RobotAPI/interface/armem/actions.ice>
+
+module armarx
+{
+ module armem
+ {
+ module server
+ {
+ module actions
+ {
+ interface ActionsInterface
+ {
+ actions::data::GetActionsOutputSeq getActions(actions::data::GetActionsInputSeq inputs);
+ actions::data::ExecuteActionOutputSeq executeActions(actions::data::ExecuteActionInputSeq inputs);
+ };
+ }
+ }
+ }
+}
diff --git a/source/RobotAPI/interface/armem/server/MemoryInterface.ice b/source/RobotAPI/interface/armem/server/MemoryInterface.ice
index ec83f439d8a91e8e57fe4c1767a8ba27bfab7c5d..05b2ea8c54207f6bcfa7a2d708f8fd7e59df416b 100644
--- a/source/RobotAPI/interface/armem/server/MemoryInterface.ice
+++ b/source/RobotAPI/interface/armem/server/MemoryInterface.ice
@@ -5,6 +5,7 @@
#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.ice>
#include <RobotAPI/interface/armem/server/WritingMemoryInterface.ice>
+#include <RobotAPI/interface/armem/server/ActionsInterface.ice>
#include <RobotAPI/interface/armem/client/MemoryListenerInterface.ice>
@@ -26,6 +27,7 @@ module armarx
interface MemoryInterface extends
WorkingMemoryInterface,
LongTermMemoryInterface,
+ actions::ActionsInterface,
client::MemoryListenerInterface
{
};
diff --git a/source/RobotAPI/interface/units/RobotUnit/GazeController.ice b/source/RobotAPI/interface/units/RobotUnit/GazeController.ice
new file mode 100644
index 0000000000000000000000000000000000000000..c09e65097f5bd4458a2c074177b7652a8181ad03
--- /dev/null
+++ b/source/RobotAPI/interface/units/RobotUnit/GazeController.ice
@@ -0,0 +1,63 @@
+
+/*
+* 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 ArmarX::RobotAPI
+* @author Raphael Grimm
+* @copyright 2019 Humanoids Group, H2T, KIT
+* @license http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+#include <RobotAPI/interface/core/FramedPoseBase.ice>
+#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
+
+module armarx { module control { module gaze_controller {
+
+ class GazeControllerConfig extends NJointControllerConfig
+ {
+ string cameraFrameName = "DepthCamera";
+ string yawNodeName = "Neck_1_Yaw";
+ string pitchNodeName = "Neck_2_Pitch";
+ string cameraNodeName = "DepthCamera";
+ string torsoNodeName = "TorsoJoint";
+
+ float Kp = 1.9f;
+ float Ki = 0.0f;
+ float Kd = 0.0f;
+ double maxControlValue = 1.0;
+ double maxDerivation = 0.5;
+
+ float yawAngleTolerance = 0.005;
+ float pitchAngleTolerance = 0.005;
+ bool abortIfUnreachable = false;
+ };
+
+ interface GazeControllerInterface extends
+ NJointControllerInterface
+ {
+ void submitTarget(FramedPositionBase target);
+ void removeTarget();
+ void removeTargetAfter(long durationMilliSeconds);
+ };
+
+ interface GazeControllerListener
+ {
+ void reportGazeTarget(long requestTimestamp, long reachedTimestamp, long releasedTimestamp, long abortedTimestamp, FramedPositionBase target);
+ };
+
+};};};
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
index ebd0fc51b4ddb660613dd0c5f18aa322c2942795..70fd46b47fb7a8a3c992dfe5d6c3d4676915aed7 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
@@ -61,7 +61,6 @@ namespace armarx
void setLogError(bool enabled);
-
std::string package() const;
std::string dataset() const;
@@ -129,9 +128,11 @@ namespace armarx
virtual bool checkPaths() const;
+
private:
path objectDirectory() const;
+
std::optional<std::vector<std::string>> loadNames(const std::string& jsonKey) const;
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp
index 0ec32dda6089fc533d4afe2f7aed6668ff28137f..8f875542bce9f7a5ed3fe91bc697eb3cd76e1edf 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp
@@ -188,7 +188,7 @@ namespace armarx::armem
ARMARX_DEBUG << "Query for memory name: " << properties.memoryName;
- if (provider != "")
+ if (!provider.empty())
{
qb
.coreSegments().withName(properties.graspCandidateMemoryName)
@@ -219,7 +219,7 @@ namespace armarx::armem
ARMARX_DEBUG << "Query for memory name: " << properties.memoryName;
- if (provider != "")
+ if (!provider.empty())
{
qb
.coreSegments().withName(properties.bimanualGraspCandidateMemoryName)
diff --git a/source/RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.xml b/source/RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.xml
index 592c9fe677ecb0e8ad9b4e55c59a9560f336f439..070806e0cd1b9718999b4034b5bd5ee3dc046b62 100644
--- a/source/RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.xml
+++ b/source/RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.xml
@@ -40,7 +40,7 @@
<Object name="armarx::grasping::arondto::GraspCandidateSourceInfo">
<ObjectChild key='referenceObjectPose'>
- <Pose />
+ <Pose optional="true" />
</ObjectChild>
<ObjectChild key='referenceObjectName'>
<string />
@@ -49,7 +49,7 @@
<int />
</ObjectChild>
<ObjectChild key='bbox'>
- <armarx::grasping::arondto::BoundingBox />
+ <armarx::grasping::arondto::BoundingBox optional="true" />
</ObjectChild>
</Object>
@@ -96,11 +96,11 @@
</ObjectChild>
<ObjectChild key='tcpPoseInHandRoot'>
- <Pose />
+ <Pose optional="true" />
</ObjectChild>
<ObjectChild key='approachVector'>
- <Position />
+ <Position optional="true" />
</ObjectChild>
<ObjectChild key='sourceFrame'>
@@ -116,7 +116,7 @@
</ObjectChild>
<ObjectChild key='graspSuccessProbability'>
- <float />
+ <float optional="true" />
</ObjectChild>
<ObjectChild key='objectType'>
@@ -124,7 +124,7 @@
</ObjectChild>
<ObjectChild key='groupNr'>
- <int />
+ <int optional="true" />
</ObjectChild>
<ObjectChild key='providerName'>
@@ -135,21 +135,21 @@
<bool />
</ObjectChild>
<ObjectChild key='sourceInfo'>
- <armarx::grasping::arondto::GraspCandidateSourceInfo />
+ <armarx::grasping::arondto::GraspCandidateSourceInfo optional="true" />
</ObjectChild>
<ObjectChild key='executionHintsValid'>
<bool />
</ObjectChild>
<ObjectChild key='executionHints'>
- <armarx::grasping::arondto::GraspCandidateExecutionHints />
+ <armarx::grasping::arondto::GraspCandidateExecutionHints optional="true" />
</ObjectChild>
<ObjectChild key='reachabilityInfoValid'>
<bool />
</ObjectChild>
<ObjectChild key='reachabilityInfo'>
- <armarx::grasping::arondto::GraspCandidateReachabilityInfo />
+ <armarx::grasping::arondto::GraspCandidateReachabilityInfo optional="true" />
</ObjectChild>
</Object>
diff --git a/source/RobotAPI/libraries/GraspingUtility/aron_conversions.cpp b/source/RobotAPI/libraries/GraspingUtility/aron_conversions.cpp
index 01f6adc1a9fe479580104e9e7de110de6b09aaef..0b367d3cbd4203474aeeeaa9cf8bd30c08c0fc30 100644
--- a/source/RobotAPI/libraries/GraspingUtility/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/aron_conversions.cpp
@@ -5,18 +5,18 @@
#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
#include <ArmarXCore/core/exceptions/local/UnexpectedEnumValueException.h>
+#include <ArmarXCore/core/logging/Logging.h>
-
-void armarx::grasping::fromAron(const armarx::grasping::arondto::BoundingBox& dto,
- armarx::grasping::BoundingBox& bo)
+void armarx::grasping::fromAron(const armarx::grasping::arondto::BoundingBox& dto, armarx::grasping::BoundingBox& bo)
{
+ ARMARX_TRACE;
bo = BoundingBox(toIce(dto.center), toIce(dto.ha1), toIce(dto.ha2), toIce(dto.ha3));
}
-void armarx::grasping::toAron(armarx::grasping::arondto::BoundingBox& dto,
- const armarx::grasping::BoundingBox& bo)
+void armarx::grasping::toAron(armarx::grasping::arondto::BoundingBox& dto, const armarx::grasping::BoundingBox& bo)
{
+ ARMARX_TRACE;
dto.center = fromIce(bo.center);
dto.ha1 = fromIce(bo.ha1);
dto.ha2 = fromIce(bo.ha2);
@@ -26,26 +26,41 @@ void armarx::grasping::toAron(armarx::grasping::arondto::BoundingBox& dto,
void armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidateSourceInfo& dto,
armarx::grasping::GraspCandidateSourceInfo& bo)
{
+ ARMARX_TRACE;
bo.bbox = new BoundingBox();
- fromAron(dto.bbox, *bo.bbox);
+ if (dto.bbox)
+ {
+ fromAron(dto.bbox.value(), *bo.bbox);
+ } else bo.bbox = nullptr;
bo.referenceObjectName = dto.referenceObjectName;
- bo.referenceObjectPose = toIce(dto.referenceObjectPose);
+ if (dto.referenceObjectPose)
+ {
+ bo.referenceObjectPose = toIce(dto.referenceObjectPose.value());
+ } else bo.referenceObjectPose = nullptr;
bo.segmentationLabelID = dto.segmentationLabelID;
}
void armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidateSourceInfo& dto,
const armarx::grasping::GraspCandidateSourceInfo& bo)
{
- toAron(dto.bbox, *bo.bbox);
+ ARMARX_TRACE;
+ if (bo.bbox)
+ {
+ dto.bbox = arondto::BoundingBox();
+ toAron(dto.bbox.value(), *bo.bbox);
+ }
dto.referenceObjectName = bo.referenceObjectName;
- dto.referenceObjectPose = fromIce(bo.referenceObjectPose);
+ if (bo.referenceObjectPose)
+ {
+ dto.referenceObjectPose = fromIce(bo.referenceObjectPose);
+ }
dto.segmentationLabelID = bo.segmentationLabelID;
-
}
void armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidateReachabilityInfo& dto,
armarx::grasping::GraspCandidateReachabilityInfo& bo)
{
+ ARMARX_TRACE;
bo = GraspCandidateReachabilityInfo(dto.reachable, dto.minimumJointLimitMargin, dto.jointLimitMargins,
dto.maxPosError, dto.maxOriError);
}
@@ -53,6 +68,7 @@ void armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidateR
void armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidateReachabilityInfo& dto,
const armarx::grasping::GraspCandidateReachabilityInfo& bo)
{
+ ARMARX_TRACE;
dto.jointLimitMargins = bo.jointLimitMargins;
dto.maxOriError = bo.maxOriError;
dto.maxPosError = bo.maxPosError;
@@ -60,92 +76,110 @@ void armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidateReachabil
dto.reachable = bo.reachable;
}
-void armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidate& dto,
- armarx::grasping::GraspCandidate& bo)
+void
+armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidate& dto, armarx::grasping::GraspCandidate& bo)
{
+ ARMARX_TRACE;
bo = GraspCandidate();
bo.graspPose = toIce(dto.graspPose);
bo.robotPose = toIce(dto.robotPose);
- bo.tcpPoseInHandRoot = toIce(dto.tcpPoseInHandRoot);
- bo.approachVector = toIce(dto.approachVector);
+ bo.tcpPoseInHandRoot = dto.tcpPoseInHandRoot ? toIce(dto.tcpPoseInHandRoot.value()) : nullptr;
+ bo.approachVector = dto.approachVector ? toIce(dto.approachVector.value()) : nullptr;
bo.sourceFrame = dto.sourceFrame;
bo.targetFrame = dto.targetFrame;
bo.side = dto.side;
- bo.graspSuccessProbability = dto.graspSuccessProbability;
+ bo.graspSuccessProbability = dto.graspSuccessProbability ? dto.graspSuccessProbability.value() : -1.0;
fromAron(dto.objectType, bo.objectType);
- if (dto.executionHintsValid)
+ if (dto.executionHints && dto.executionHintsValid)
{
bo.executionHints = new GraspCandidateExecutionHints();
- fromAron(dto.executionHints, *bo.executionHints);
- }
- else
+ fromAron(dto.executionHints.value(), *bo.executionHints);
+ } else
{
bo.executionHints = nullptr;
}
- bo.groupNr = dto.groupNr;
+ bo.groupNr = dto.groupNr ? dto.groupNr.value() : -1;
bo.providerName = dto.providerName;
- if (dto.reachabilityInfoValid)
+ if (dto.reachabilityInfo && dto.reachabilityInfoValid)
{
bo.reachabilityInfo = new GraspCandidateReachabilityInfo();
- fromAron(dto.reachabilityInfo, *bo.reachabilityInfo);
- }
- else
+ fromAron(dto.reachabilityInfo.value(), *bo.reachabilityInfo);
+ } else
{
bo.reachabilityInfo = nullptr;
}
- if (dto.sourceInfoValid)
+ if (dto.sourceInfo && dto.sourceInfoValid)
{
bo.sourceInfo = new GraspCandidateSourceInfo();
- fromAron(dto.sourceInfo, *bo.sourceInfo);
- }
- else
+ fromAron(dto.sourceInfo.value(), *bo.sourceInfo);
+ } else
{
bo.sourceInfo = nullptr;
}
}
-void armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidate& dto,
- const armarx::grasping::GraspCandidate& bo)
+void
+armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidate& dto, const armarx::grasping::GraspCandidate& bo)
{
- dto.approachVector = fromIce(bo.approachVector);
+ ARMARX_TRACE;
+ if (bo.approachVector)
+ {
+ dto.approachVector = fromIce(bo.approachVector);
+ }
if (bo.executionHints)
{
+ dto.executionHints = arondto::GraspCandidateExecutionHints();
dto.executionHintsValid = true;
- toAron(dto.executionHints, *bo.executionHints);
- }
- else
+ toAron(dto.executionHints.value(), *bo.executionHints);
+ } else
{
dto.executionHintsValid = false;
- dto.executionHints.toAron();
+ dto.executionHints = std::nullopt;
}
dto.graspPose = fromIce(bo.graspPose);
- dto.graspSuccessProbability = bo.graspSuccessProbability;
- dto.groupNr = bo.groupNr;
+ if (bo.graspSuccessProbability < 0 || bo.graspSuccessProbability > 1.0)
+ {
+ dto.graspSuccessProbability = std::nullopt;
+ } else
+ {
+ dto.graspSuccessProbability = bo.graspSuccessProbability;
+ }
+ if (bo.groupNr < 0)
+ {
+ dto.groupNr = std::nullopt;
+ } else
+ {
+ dto.groupNr = bo.groupNr;
+ }
toAron(dto.objectType, bo.objectType);
dto.providerName = bo.providerName;
if (bo.reachabilityInfo)
{
+ dto.reachabilityInfo = arondto::GraspCandidateReachabilityInfo();
dto.reachabilityInfoValid = true;
- toAron(dto.reachabilityInfo, *bo.reachabilityInfo);
- }
- else
+ toAron(dto.reachabilityInfo.value(), *bo.reachabilityInfo);
+ } else
{
dto.reachabilityInfoValid = false;
- dto.reachabilityInfo.toAron();
+ dto.reachabilityInfo = std::nullopt;
}
dto.robotPose = fromIce(bo.robotPose);
- dto.tcpPoseInHandRoot = fromIce(bo.tcpPoseInHandRoot);
+ if (bo.tcpPoseInHandRoot)
+ {
+ dto.tcpPoseInHandRoot = fromIce(bo.tcpPoseInHandRoot);
+ }
+
dto.side = bo.side;
dto.sourceFrame = bo.sourceFrame;
if (bo.sourceInfo)
{
+ dto.sourceInfo = arondto::GraspCandidateSourceInfo();
dto.sourceInfoValid = true;
- toAron(dto.sourceInfo, *bo.sourceInfo);
- }
- else
+ toAron(dto.sourceInfo.value(), *bo.sourceInfo);
+ } else
{
dto.sourceInfoValid = false;
- dto.sourceInfo.toAron();
+ dto.sourceInfo = std::nullopt;
}
dto.targetFrame = bo.targetFrame;
}
@@ -153,6 +187,7 @@ void armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidate& dto,
void armarx::grasping::fromAron(const armarx::grasping::arondto::BimanualGraspCandidate& dto,
armarx::grasping::BimanualGraspCandidate& bo)
{
+ ARMARX_TRACE;
bo = BimanualGraspCandidate();
bo.graspPoseRight = toIce(dto.graspPoseRight);
bo.graspPoseLeft = toIce(dto.graspPoseLeft);
@@ -170,8 +205,7 @@ void armarx::grasping::fromAron(const armarx::grasping::arondto::BimanualGraspCa
{
bo.executionHintsRight = new GraspCandidateExecutionHints();
fromAron(dto.executionHintsRight, *bo.executionHintsRight);
- }
- else
+ } else
{
bo.executionHintsRight = nullptr;
}
@@ -179,8 +213,7 @@ void armarx::grasping::fromAron(const armarx::grasping::arondto::BimanualGraspCa
{
bo.executionHintsLeft = new GraspCandidateExecutionHints();
fromAron(dto.executionHintsLeft, *bo.executionHintsLeft);
- }
- else
+ } else
{
bo.executionHintsLeft = nullptr;
}
@@ -190,8 +223,7 @@ void armarx::grasping::fromAron(const armarx::grasping::arondto::BimanualGraspCa
{
bo.reachabilityInfoRight = new GraspCandidateReachabilityInfo();
fromAron(dto.reachabilityInfoRight, *bo.reachabilityInfoRight);
- }
- else
+ } else
{
bo.reachabilityInfoRight = nullptr;
}
@@ -199,8 +231,7 @@ void armarx::grasping::fromAron(const armarx::grasping::arondto::BimanualGraspCa
{
bo.reachabilityInfoLeft = new GraspCandidateReachabilityInfo();
fromAron(dto.reachabilityInfoLeft, *bo.reachabilityInfoLeft);
- }
- else
+ } else
{
bo.reachabilityInfoLeft = nullptr;
}
@@ -208,8 +239,7 @@ void armarx::grasping::fromAron(const armarx::grasping::arondto::BimanualGraspCa
{
bo.sourceInfo = new GraspCandidateSourceInfo();
fromAron(dto.sourceInfo, *bo.sourceInfo);
- }
- else
+ } else
{
bo.sourceInfo = nullptr;
}
@@ -225,8 +255,7 @@ void armarx::grasping::toAron(armarx::grasping::arondto::BimanualGraspCandidate&
{
dto.executionHintsRightValid = true;
toAron(dto.executionHintsRight, *bo.executionHintsRight);
- }
- else
+ } else
{
dto.executionHintsRightValid = false;
dto.executionHintsRight.toAron();
@@ -235,8 +264,7 @@ void armarx::grasping::toAron(armarx::grasping::arondto::BimanualGraspCandidate&
{
dto.executionHintsLeftValid = true;
toAron(dto.executionHintsLeft, *bo.executionHintsLeft);
- }
- else
+ } else
{
dto.executionHintsLeftValid = false;
dto.executionHintsLeft.toAron();
@@ -251,8 +279,7 @@ void armarx::grasping::toAron(armarx::grasping::arondto::BimanualGraspCandidate&
{
dto.reachabilityInfoRightValid = true;
toAron(dto.reachabilityInfoRight, *bo.reachabilityInfoRight);
- }
- else
+ } else
{
dto.reachabilityInfoRightValid = false;
dto.reachabilityInfoRight.toAron();
@@ -261,8 +288,7 @@ void armarx::grasping::toAron(armarx::grasping::arondto::BimanualGraspCandidate&
{
dto.reachabilityInfoLeftValid = true;
toAron(dto.reachabilityInfoLeft, *bo.reachabilityInfoLeft);
- }
- else
+ } else
{
dto.reachabilityInfoLeftValid = false;
dto.reachabilityInfoLeft.toAron();
@@ -273,8 +299,7 @@ void armarx::grasping::toAron(armarx::grasping::arondto::BimanualGraspCandidate&
{
dto.sourceInfoValid = true;
toAron(dto.sourceInfo, *bo.sourceInfo);
- }
- else
+ } else
{
dto.sourceInfoValid = false;
dto.sourceInfo.toAron();
@@ -302,8 +327,7 @@ void armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidateExecution
dto.graspTrajectoryName = bo.graspTrajectoryName;
}
-void armarx::grasping::fromAron(const armarx::grasping::arondto::ApproachType& dto,
- armarx::grasping::ApproachType& bo)
+void armarx::grasping::fromAron(const armarx::grasping::arondto::ApproachType& dto, armarx::grasping::ApproachType& bo)
{
switch (dto.value)
{
@@ -321,8 +345,7 @@ void armarx::grasping::fromAron(const armarx::grasping::arondto::ApproachType& d
}
-void armarx::grasping::toAron(armarx::grasping::arondto::ApproachType& dto,
- const armarx::grasping::ApproachType& bo)
+void armarx::grasping::toAron(armarx::grasping::arondto::ApproachType& dto, const armarx::grasping::ApproachType& bo)
{
switch (bo)
{
@@ -340,8 +363,7 @@ void armarx::grasping::toAron(armarx::grasping::arondto::ApproachType& dto,
}
-void armarx::grasping::fromAron(const armarx::grasping::arondto::ApertureType& dto,
- armarx::grasping::ApertureType& bo)
+void armarx::grasping::fromAron(const armarx::grasping::arondto::ApertureType& dto, armarx::grasping::ApertureType& bo)
{
switch (dto.value)
{
@@ -358,8 +380,7 @@ void armarx::grasping::fromAron(const armarx::grasping::arondto::ApertureType& d
ARMARX_UNEXPECTED_ENUM_VALUE(arondto::ObjectType, dto.value);
}
-void armarx::grasping::toAron(armarx::grasping::arondto::ApertureType& dto,
- const armarx::grasping::ApertureType& bo)
+void armarx::grasping::toAron(armarx::grasping::arondto::ApertureType& dto, const armarx::grasping::ApertureType& bo)
{
switch (bo)
{
diff --git a/source/RobotAPI/libraries/PriorKnowledge/CMakeLists.txt b/source/RobotAPI/libraries/PriorKnowledge/CMakeLists.txt
index 596f554c1f0ec2689d12cc9776539b0d4fd7377f..7c22f824a7161e844590073789f55b4a3e7f5ef9 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/CMakeLists.txt
+++ b/source/RobotAPI/libraries/PriorKnowledge/CMakeLists.txt
@@ -1,4 +1,5 @@
add_subdirectory(core)
add_subdirectory(motions)
+add_subdirectory(objects)
#add_subdirectory(subjects)
diff --git a/source/RobotAPI/libraries/PriorKnowledge/objects/CMakeLists.txt b/source/RobotAPI/libraries/PriorKnowledge/objects/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..cdf52fa7d1aa8dff4eda392dcb0cebdd77ec8f00
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/objects/CMakeLists.txt
@@ -0,0 +1,19 @@
+set(LIB_NAME ${PROJECT_NAME}PriorKnowledgeObjects)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+armarx_add_library(
+ LIBS
+ RobotAPI::Core
+ RobotAPI::Aron::Common
+ SOURCES
+ ObjectFinder.cpp
+ HEADERS
+ ObjectFinder.h
+)
+
+add_library(${PROJECT_NAME}::PriorKnowledge::Objects ALIAS ${PROJECT_NAME}PriorKnowledgeObjects)
+
+# add unit tests
+#add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/PriorKnowledge/objects/ObjectFinder.cpp b/source/RobotAPI/libraries/PriorKnowledge/objects/ObjectFinder.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ed1b6768e170f9416db52ba6f27fe44ac18b8adc
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/objects/ObjectFinder.cpp
@@ -0,0 +1,71 @@
+// Simox
+#include <SimoxUtility/algorithm/vector.hpp>
+
+// BaseClass
+#include "ObjectFinder.h"
+
+// ArmarX
+
+namespace armarx::priorknowledge::objects
+{
+ std::vector<ObjectFinderInfo> ObjectFinder::findAll() const
+ {
+ const std::filesystem::path absPath = Base::Base::getFullPath();
+ if (std::filesystem::is_regular_file(absPath))
+ {
+ ARMARX_WARNING << "The entered path is leading to a file!";
+ return {};
+ }
+
+ std::vector<ObjectFinderInfo> ret;
+ for (const auto& d : std::filesystem::directory_iterator(absPath))
+ {
+ if (!d.is_directory())
+ {
+ ARMARX_WARNING << "Found invalid path: " << d.path();
+ continue;
+ }
+ std::string k = d.path().filename();
+ if (simox::alg::contains(DATASET_FOLDERS_BLACKLIST, k))
+ {
+ continue;
+ }
+
+ auto motionsForDataset = this->findAll(k);
+ simox::alg::append(ret, motionsForDataset);
+ }
+ return ret;
+ }
+
+ std::vector<ObjectFinderInfo> ObjectFinder::findAll(const std::string &dataset) const
+ {
+ const std::filesystem::path absPathToDataset = this->getFullPath(dataset);
+ if (std::filesystem::is_regular_file(absPathToDataset))
+ {
+ ARMARX_WARNING << "The entered path is leading to a file!";
+ return {};
+ }
+
+ std::vector<ObjectFinderInfo> ret;
+ for (const auto& d : std::filesystem::directory_iterator(absPathToDataset))
+ {
+ if (!d.is_directory())
+ {
+ ARMARX_WARNING << "Found invalid path: " << d.path();
+ continue;
+ }
+ std::string k = d.path().filename();
+ if (simox::alg::contains(ID_FOLDERS_BLACKLIST, k))
+ {
+ continue;
+ }
+
+ if(auto op = this->find(dataset, k); op.has_value())
+ {
+ ret.emplace_back(op.value());
+ }
+ }
+ return ret;
+ }
+
+}
diff --git a/source/RobotAPI/libraries/PriorKnowledge/objects/ObjectFinder.h b/source/RobotAPI/libraries/PriorKnowledge/objects/ObjectFinder.h
new file mode 100644
index 0000000000000000000000000000000000000000..a0e5b1820af5a9cafa561273aafabcad099820b3
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/objects/ObjectFinder.h
@@ -0,0 +1,41 @@
+#include <RobotAPI/libraries/PriorKnowledge/core/FinderBase.h>
+#include <RobotAPI/libraries/PriorKnowledge/core/FinderInfoBase.h>
+
+// TODO!!!
+namespace armarx::priorknowledge::objects
+{
+ class ObjectFinderInfo : public core::DatasetFinderInfoBase<std::string, std::string>
+ {
+ using Base = core::DatasetFinderInfoBase<std::string, std::string>;
+
+ public:
+ ObjectFinderInfo(const std::string& packageName,
+ const std::filesystem::path& absPackageDataDir,
+ const std::filesystem::path& relPackageDataPath,
+ const std::filesystem::path& relDatasetPath,
+ const std::string& dataset,
+ const std::string& id) :
+ Base(packageName, absPackageDataDir, relPackageDataPath, relDatasetPath, dataset, id)
+ {
+ }
+ };
+
+ class ObjectFinder : public core::DatasetFinderBase<std::string, std::string, ObjectFinderInfo>
+ {
+ using Base = core::DatasetFinderBase<std::string, std::string, ObjectFinderInfo>;
+
+ public:
+ ObjectFinder(const std::string& packageName, const std::filesystem::path& relDir) :
+ Base(packageName, relDir)
+ {}
+
+ std::vector<ObjectFinderInfo> findAll() const;
+
+ std::vector<ObjectFinderInfo> findAll(const std::string& dataset) const;
+
+ private:
+ std::vector<std::string> DATASET_FOLDERS_BLACKLIST = {};
+ std::vector<std::string> ID_FOLDERS_BLACKLIST = {"script"};
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem/CMakeLists.txt b/source/RobotAPI/libraries/armem/CMakeLists.txt
index ba231f670b3342011fd228da611d2e9d1b8d87cc..5feda749b6fc3eb3e1f601812b778e71b20c099b 100644
--- a/source/RobotAPI/libraries/armem/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/CMakeLists.txt
@@ -12,6 +12,7 @@ set(LIBS
)
set(LIB_FILES
+ core/actions.cpp
core/Commit.cpp
core/MemoryID.cpp
core/MemoryID_operators.cpp
@@ -77,6 +78,7 @@ set(LIB_HEADERS
core.h
core/forward_declarations.h
+ core/actions.h
core/Commit.h
core/DataMode.h
core/MemoryID.h
diff --git a/source/RobotAPI/libraries/armem/client/MemoryNameSystem.h b/source/RobotAPI/libraries/armem/client/MemoryNameSystem.h
index 01bf3aecfdc821d41f2902efd8329fa9e509152a..9422d575cd9b32d58c64110d6d3ef83a8deabe56 100644
--- a/source/RobotAPI/libraries/armem/client/MemoryNameSystem.h
+++ b/source/RobotAPI/libraries/armem/client/MemoryNameSystem.h
@@ -181,7 +181,15 @@ namespace armarx::armem::client
std::map<MemoryID, wm::EntityInstance> resolveEntityInstances(const std::vector<MemoryID>& ids);
-
+ /**
+ * @brief Resolve the given memory server for the given memory ID.
+ *
+ * @param memoryID The memory ID.
+ * @return The memory server proxy.
+ *
+ * @throw `error::CouldNotResolveMemoryServer` If the memory name could not be resolved.
+ */
+ mns::dto::MemoryServerInterfaces resolveServer(const MemoryID& memoryID);
// Registration - only for memory servers
@@ -217,16 +225,6 @@ namespace armarx::armem::client
private:
- /**
- * @brief Resolve the given memory server for the given memory ID.
- *
- * @param memoryID The memory ID.
- * @return The memory server proxy.
- *
- * @throw `error::CouldNotResolveMemoryServer` If the memory name could not be resolved.
- */
- mns::dto::MemoryServerInterfaces resolveServer(const MemoryID& memoryID);
-
/**
* @brief Wait for the given memory server.
*
diff --git a/source/RobotAPI/libraries/armem/client/Reader.cpp b/source/RobotAPI/libraries/armem/client/Reader.cpp
index 2662f86e0d7f36827f9d5cff67f963ac29613de9..b7a1c85dd5925353a360058cc5359a312f6e42c0 100644
--- a/source/RobotAPI/libraries/armem/client/Reader.cpp
+++ b/source/RobotAPI/libraries/armem/client/Reader.cpp
@@ -39,6 +39,7 @@ namespace armarx::armem::client
};
armem::query::data::Result result;
+ ARMARX_CHECK_NOT_NULL(memoryPrx);
try
{
result = memoryPrx->query(input);
diff --git a/source/RobotAPI/libraries/armem/client/util/MemoryListener.cpp b/source/RobotAPI/libraries/armem/client/util/MemoryListener.cpp
index f0ca213f3873dd81ca3b3a02972d04fde3f0cbee..a305de2cbcdd9aa040e6f8b14eb33a905519ec05 100644
--- a/source/RobotAPI/libraries/armem/client/util/MemoryListener.cpp
+++ b/source/RobotAPI/libraries/armem/client/util/MemoryListener.cpp
@@ -2,6 +2,7 @@
#include <sstream>
+#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <RobotAPI/libraries/armem/core/ice_conversions.h>
#include <RobotAPI/libraries/armem/core/error.h>
@@ -63,13 +64,40 @@ namespace armarx::armem::client::util
<< " with " << matchingSnapshotIDs.size() << " snapshot IDs ...";
for (auto& callback : subCallbacks)
{
- callback(subscription, matchingSnapshotIDs);
+ try
+ {
+ callback(subscription, matchingSnapshotIDs);
+ }
+ catch (const armarx::LocalException& e)
+ {
+ error << "Calling callback subscribing " << subscription << " failed."
+ << "\nCaught armarx::LocalException:"
+ << "\n" << e.getReason()
+ << "\n Stacktrace: \n" << e.generateBacktrace()
+ << "\n"
+ ;
+ }
+ catch (const std::exception& e)
+ {
+ error << "Calling callback subscribing " << subscription << " failed."
+ << "\nCaught armarx::Exception:"
+ << "\n" << e.what()
+ << "\n"
+ ;
+ }
+ catch (...)
+ {
+ error << "Calling callback subscribing " << subscription << " failed."
+ << "\nCaught unknown exception."
+ << "\n"
+ ;
+ }
}
}
}
if (error.str().size() > 0)
{
- ARMARX_VERBOSE << "The following issues were encountered during MemoryListener::" << __FUNCTION__ << "(): \n\n"
+ ARMARX_WARNING << "The following issues were encountered during MemoryListener::" << __FUNCTION__ << "(): \n\n"
<< error.str();
}
}
diff --git a/source/RobotAPI/libraries/armem/core/actions.cpp b/source/RobotAPI/libraries/armem/core/actions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9eb28f7187d9ac51295dfa3be351f14fc97ef2cd
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/actions.cpp
@@ -0,0 +1,112 @@
+#include "actions.h"
+
+#include <RobotAPI/interface/armem/actions.h>
+
+namespace armarx::armem::actions
+{
+
+ MenuEntry::MenuEntry(const std::string& id,
+ const std::string& text,
+ const std::vector<MenuEntry>& entries) :
+ id(id), text(text), entries(entries)
+ {
+ }
+
+ MenuEntry&
+ MenuEntry::add(const std::string& id,
+ const std::string& text,
+ const std::vector<MenuEntry>& entries)
+ {
+ return this->entries.emplace_back(id, text, entries);
+ }
+
+ data::MenuEntryPtr
+ MenuEntry::toIce() const // NOLINT (recursive call chain)
+ {
+ if (entries.empty())
+ {
+ return new data::Action{this->id, this->text};
+ }
+
+ data::SubMenuPtr ice = new data::SubMenu{this->id, this->text, {}};
+ for (const MenuEntry& entry : entries)
+ {
+ ice->entries.push_back(entry.toIce());
+ }
+ return ice;
+ }
+
+ MenuEntry
+ MenuEntry::fromIce(const data::MenuEntryPtr& ice) // NOLINT (recursive call chain)
+ {
+ if (ice->ice_isA(data::SubMenu::ice_staticId()))
+ {
+ auto ptr = IceUtil::Handle<data::SubMenu>::dynamicCast(ice);
+ MenuEntry subMenu{ptr->id, ptr->text, {}};
+ for (const auto& entry : ptr->entries)
+ {
+ subMenu.entries.push_back(MenuEntry::fromIce(entry));
+ }
+ return subMenu;
+ }
+
+ return MenuEntry{ice->id, ice->text, {}};
+ }
+
+ Action::Action(const std::string& id, const std::string& text) : MenuEntry(id, text)
+ {
+ }
+
+ SubMenu::SubMenu(const std::string& id,
+ const std::string& text,
+ const std::vector<MenuEntry>& entries) :
+ MenuEntry(id, text, entries)
+ {
+ }
+
+ MenuEntry&
+ SubMenu::add(const std::string& id,
+ const std::string& text,
+ const std::vector<MenuEntry>& entries)
+ {
+ return this->entries.emplace_back(id, text, entries);
+ }
+
+
+ Menu::Menu(std::initializer_list<MenuEntry> entries) : entries(entries)
+ {
+ }
+
+ Menu::Menu(const std::vector<MenuEntry>& entries) : entries(entries)
+ {
+ }
+
+
+ MenuEntry&
+ Menu::add(const std::string& id, const std::string& text, const std::vector<MenuEntry>& entries)
+ {
+ return this->entries.emplace_back(id, text, entries);
+ }
+
+ data::MenuPtr
+ Menu::toIce() const
+ {
+ data::MenuPtr ice{new data::Menu};
+ for (const MenuEntry& entry : entries)
+ {
+ ice->entries.push_back(entry.toIce());
+ }
+ return ice;
+ }
+
+ Menu
+ Menu::fromIce(const data::MenuPtr& ice)
+ {
+ Menu menu;
+ for (const auto& entry : ice->entries)
+ {
+ menu.entries.push_back(MenuEntry::fromIce(entry));
+ }
+ return menu;
+ }
+} // namespace armarx::armem::actions
diff --git a/source/RobotAPI/libraries/armem/core/actions.h b/source/RobotAPI/libraries/armem/core/actions.h
new file mode 100644
index 0000000000000000000000000000000000000000..2d96dbbac3f5b8f614772a881b504793d78b40a9
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/actions.h
@@ -0,0 +1,77 @@
+#pragma once
+
+#include <vector>
+
+#include <RobotAPI/interface/armem/actions.h>
+
+namespace armarx::armem::actions
+{
+ // Business objects to make working with the Armem
+ // action interface easier. To see an example usage,
+ // check the ExampleMemory.
+
+ struct MenuEntry
+ {
+ MenuEntry(const std::string& id,
+ const std::string& text,
+ const std::vector<MenuEntry>& entries = {});
+
+ MenuEntry& add(const std::string& id,
+ const std::string& text,
+ const std::vector<MenuEntry>& entries = {});
+
+ data::MenuEntryPtr toIce() const;
+
+ static MenuEntry fromIce(const data::MenuEntryPtr& ice);
+
+ public:
+ std::string id;
+ std::string text;
+ std::vector<MenuEntry> entries;
+ };
+
+
+ struct Action : public MenuEntry
+ {
+ Action(const std::string& id, const std::string& text);
+ };
+
+ struct SubMenu : public MenuEntry
+ {
+ SubMenu(const std::string& id,
+ const std::string& text,
+ const std::vector<MenuEntry>& entries = {});
+
+ MenuEntry& add(const std::string& id,
+ const std::string& text,
+ const std::vector<MenuEntry>& entries = {});
+ };
+
+
+ class Menu
+ {
+
+ public:
+ Menu(std::initializer_list<MenuEntry> entries);
+
+ Menu(const std::vector<MenuEntry>& entries = {});
+
+ MenuEntry& add(const std::string& id,
+ const std::string& text,
+ const std::vector<MenuEntry>& entries = {});
+
+ data::MenuPtr toIce() const;
+
+ static Menu fromIce(const data::MenuPtr& ice);
+
+ public:
+ std::vector<MenuEntry> entries;
+ };
+
+ using data::ExecuteActionInputSeq;
+ using data::ExecuteActionOutputSeq;
+ using data::GetActionsInputSeq;
+ using data::GetActionsOutputSeq;
+ using data::ActionPath;
+
+} // namespace armarx::armem::actions
diff --git a/source/RobotAPI/libraries/armem/core/base/ice_conversions.cpp b/source/RobotAPI/libraries/armem/core/base/ice_conversions.cpp
index d2339f87f009bd35a3b78812555e652884719238..1e19f25a7b08671b1e3f65ea2649f056a107361a 100644
--- a/source/RobotAPI/libraries/armem/core/base/ice_conversions.cpp
+++ b/source/RobotAPI/libraries/armem/core/base/ice_conversions.cpp
@@ -22,13 +22,13 @@ namespace armarx::armem::base
void detail::toIce(aron::data::dto::DictPtr& ice, const aron::data::DictPtr& data)
{
- ice = data ? data->toAronDictPtr() : nullptr;
+ ice = data ? data->toAronDictDTO() : nullptr;
}
void detail::fromIce(const aron::data::dto::DictPtr& ice, aron::data::DictPtr& data)
{
if (ice)
{
- data = aron::data::Dict::FromAronDictPtr(ice);
+ data = aron::data::Dict::FromAronDictDTO(ice);
}
else
{
diff --git a/source/RobotAPI/libraries/armem/core/ice_conversions.cpp b/source/RobotAPI/libraries/armem/core/ice_conversions.cpp
index f019f3d78a9ed3a38116ee86a11ef42ef942e9d4..20df9176bb562ab9025c001451d6fef23793d504 100644
--- a/source/RobotAPI/libraries/armem/core/ice_conversions.cpp
+++ b/source/RobotAPI/libraries/armem/core/ice_conversions.cpp
@@ -83,7 +83,7 @@ namespace armarx
update.instancesData.clear();
update.instancesData.reserve(ice.instancesData.size());
std::transform(ice.instancesData.begin(), ice.instancesData.end(), std::back_inserter(update.instancesData),
- aron::data::Dict::FromAronDictPtr);
+ aron::data::Dict::FromAronDictDTO);
update.timeCreated = Time::microSeconds(ice.timeCreatedMicroSeconds);
@@ -98,7 +98,7 @@ namespace armarx
ice.instancesData.clear();
ice.instancesData.reserve(update.instancesData.size());
std::transform(update.instancesData.begin(), update.instancesData.end(), std::back_inserter(ice.instancesData),
- aron::data::Dict::ToAronDictPtr);
+ aron::data::Dict::ToAronDictDTO);
ice.timeCreatedMicroSeconds = update.timeCreated.toMicroSeconds();
@@ -138,5 +138,14 @@ namespace armarx
update.timeArrived = timeArrived;
}
+ void armem::fromIce(const actions::data::MenuPtr& ice, actions::Menu& menu)
+ {
+ menu = actions::Menu::fromIce(ice);
+ }
+
+ void armem::toIce(actions::data::MenuPtr& ice, const actions::Menu& menu)
+ {
+ ice = menu.toIce();
+ }
}
diff --git a/source/RobotAPI/libraries/armem/core/ice_conversions.h b/source/RobotAPI/libraries/armem/core/ice_conversions.h
index e8f104fe04ebcb721b0ffbf4b70b18ada0af234c..a8ef35e7938f07f14a81b8ecb731e2470b395c97 100644
--- a/source/RobotAPI/libraries/armem/core/ice_conversions.h
+++ b/source/RobotAPI/libraries/armem/core/ice_conversions.h
@@ -1,10 +1,12 @@
#pragma once
+#include <RobotAPI/interface/armem/actions.h>
#include <RobotAPI/interface/armem/commit.h>
#include <RobotAPI/interface/armem/memory.h>
#include "ice_conversions_templates.h"
+#include "actions.h"
#include "Commit.h"
#include "MemoryID.h"
#include "Time.h"
@@ -43,5 +45,8 @@ namespace armarx::armem
void fromIce(const data::Commit& ice, Commit& commit, Time timeArrived);
void fromIce(const data::EntityUpdate& ice, EntityUpdate& update, Time timeArrived);
+
+ void fromIce(const actions::data::MenuPtr& ice, actions::Menu& menu);
+ void toIce(actions::data::MenuPtr& ice, const actions::Menu& menu);
}
diff --git a/source/RobotAPI/libraries/armem/core/wm/aron_conversions.cpp b/source/RobotAPI/libraries/armem/core/wm/aron_conversions.cpp
index 91b1a6584269820bfc020b36145cd656feec76d4..dacedd5e05177028fea42114a3dd02d4b609ad9a 100644
--- a/source/RobotAPI/libraries/armem/core/wm/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem/core/wm/aron_conversions.cpp
@@ -23,16 +23,16 @@ void armarx::armem::from_aron(const aron::data::DictPtr& metadata, const aron::d
e.data() = data;
auto timeCreated = aron::data::Long::DynamicCastAndCheck(metadata->getElement(DATA_WRAPPER_TIME_CREATED_FIELD));
- m.timeCreated = Time::microSeconds(timeCreated->toAronLongPtr()->value);
+ m.timeCreated = Time::microSeconds(timeCreated->toAronLongDTO()->value);
auto timeSent = aron::data::Long::DynamicCastAndCheck(metadata->getElement(DATA_WRAPPER_TIME_SENT_FIELD));
- m.timeSent = Time::microSeconds(timeSent->toAronLongPtr()->value);
+ m.timeSent = Time::microSeconds(timeSent->toAronLongDTO()->value);
auto timeArrived = aron::data::Long::DynamicCastAndCheck(metadata->getElement(DATA_WRAPPER_TIME_ARRIVED_FIELD));
- m.timeArrived = Time::microSeconds(timeArrived->toAronLongPtr()->value);
+ m.timeArrived = Time::microSeconds(timeArrived->toAronLongDTO()->value);
auto confidence = aron::data::Double::DynamicCastAndCheck(metadata->getElement(DATA_WRAPPER_CONFIDENCE_FIELD));
- m.confidence = static_cast<float>(confidence->toAronDoublePtr()->value);
+ m.confidence = static_cast<float>(confidence->toAronDoubleDTO()->value);
}
diff --git a/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.cpp b/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.cpp
index a0c606a990fd4635a5fa2cded1c0e5be8f7566a8..566cf7e0bcbdba70e8e16ca5c527120628c89f32 100644
--- a/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.cpp
+++ b/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.cpp
@@ -59,7 +59,7 @@ namespace armarx::armem::mns
int row = 0;
grid.add(Label("Memory Name"), {row, 0})
.add(Label("Component Name"), {row, 1})
- .add(Label("R/W"), {row, 2})
+ .add(Label("R/W/A"), {row, 2})
.add(Label("Registration Time"), {row, 3})
;
row++;
@@ -79,6 +79,11 @@ namespace armarx::armem::mns
componentName = info.server.writing->ice_getIdentity().name;
mode += "W";
}
+ if (info.server.actions)
+ {
+ componentName = info.server.actions->ice_getIdentity().name;
+ mode += "A";
+ }
int col = 0;
grid.add(Label(name), {row, col});
diff --git a/source/RobotAPI/libraries/armem/mns/Registry.cpp b/source/RobotAPI/libraries/armem/mns/Registry.cpp
index 260cfcd818a426ae7b4f18c6cde5a4180a459f0b..b8b2d02c807750aad2cb36c28cd55013b2a0b496 100644
--- a/source/RobotAPI/libraries/armem/mns/Registry.cpp
+++ b/source/RobotAPI/libraries/armem/mns/Registry.cpp
@@ -1,6 +1,7 @@
#include "Registry.h"
#include <ArmarXCore/core/logging/Logging.h>
+#include <RobotAPI/interface/armem/server/ActionsInterface.h>
namespace armarx::armem::mns
@@ -158,5 +159,11 @@ namespace armarx::armem::mns
return server.writing;
}
+
+ server::actions::ActionsInterfacePrx getActionsInterface(const dto::MemoryServerInterfaces& server)
+ {
+ return server.actions;
+ }
+
}
diff --git a/source/RobotAPI/libraries/armem/mns/Registry.h b/source/RobotAPI/libraries/armem/mns/Registry.h
index c1965bdac24fcdf943ad2b7bc945698358dd3f0c..7fcd1c04b95b5b0fe938ae4fdc383ada5bd147d5 100644
--- a/source/RobotAPI/libraries/armem/mns/Registry.h
+++ b/source/RobotAPI/libraries/armem/mns/Registry.h
@@ -3,6 +3,7 @@
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/interface/armem/server/ActionsInterface.h>
#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
#include <RobotAPI/interface/armem/server/WritingMemoryInterface.h>
@@ -68,4 +69,5 @@ namespace armarx::armem::mns
server::ReadingMemoryInterfacePrx getReadingInterface(const dto::MemoryServerInterfaces& server);
server::WritingMemoryInterfacePrx getWritingInterface(const dto::MemoryServerInterfaces& server);
+ server::actions::ActionsInterfacePrx getActionsInterface(const dto::MemoryServerInterfaces& server);
}
diff --git a/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp b/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp
index 5c8387726e6eb65307e66173bcb7e7a0984cdd6b..7ae0bc4bac3f4e3166f47fd4777d44d68f41a86f 100644
--- a/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp
+++ b/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp
@@ -77,6 +77,7 @@ namespace armarx::armem::server::plugins
mns::dto::MemoryServerInterfaces server;
server.reading = ReadingMemoryInterfacePrx::uncheckedCast(parent.getProxy());
server.writing = WritingMemoryInterfacePrx::uncheckedCast(parent.getProxy());
+ server.actions = actions::ActionsInterfacePrx::uncheckedCast(parent.getProxy());
mns::dto::RegisterServerResult result;
try
diff --git a/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.cpp b/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.cpp
index 285c65e0b02895bbb59663a461db1249330daf13..41e09c13be1cbc68f0281023596be9c275f16dc5 100644
--- a/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.cpp
+++ b/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.cpp
@@ -91,4 +91,30 @@ namespace armarx::armem::server::plugins
return plugin->longtermMemory;
}
-}
+ // ACTIONS
+ armem::actions::GetActionsOutputSeq ReadWritePluginUser::getActions(
+ const armem::actions::GetActionsInputSeq& inputs, const ::Ice::Current& /*unused*/)
+ {
+ return getActions(inputs);
+ }
+
+ armem::actions::GetActionsOutputSeq ReadWritePluginUser::getActions(
+ const armem::actions::GetActionsInputSeq& inputs)
+ {
+ (void) inputs;
+ return {};
+ }
+
+ armem::actions::ExecuteActionOutputSeq ReadWritePluginUser::executeActions(
+ const armem::actions::ExecuteActionInputSeq& inputs, const ::Ice::Current& /*unused*/)
+ {
+ return executeActions(inputs);
+ }
+
+ armem::actions::ExecuteActionOutputSeq ReadWritePluginUser::executeActions(
+ const armem::actions::ExecuteActionInputSeq& inputs)
+ {
+ return {};
+ }
+
+} // namespace armarx::armem::server::plugins
diff --git a/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h b/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h
index 8d3dad4c98d3f11592b250ce11a10fc96dc8cd81..83e761281bf064a111a90efe15a9148689daa2ff 100644
--- a/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h
+++ b/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h
@@ -3,6 +3,7 @@
#include <RobotAPI/libraries/armem/server/forward_declarations.h>
#include <RobotAPI/libraries/armem/client/plugins/ListeningPluginUser.h>
+#include <RobotAPI/libraries/armem/core/actions.h>
#include <RobotAPI/interface/armem/server/MemoryInterface.h>
#include <ArmarXCore/core/ManagedIceObject.h>
@@ -47,6 +48,12 @@ namespace armarx::armem::server::plugins
// StoringInterface interface
virtual data::StoreResult store(const data::StoreInput&, const Ice::Current& = Ice::emptyCurrent) override;
+ // ActionsInterface interface
+ virtual armem::actions::GetActionsOutputSeq getActions(const armem::actions::GetActionsInputSeq& inputs);
+ virtual armem::actions::ExecuteActionOutputSeq executeActions(const armem::actions::ExecuteActionInputSeq& inputs);
+
+ virtual armem::actions::GetActionsOutputSeq getActions(const armem::actions::GetActionsInputSeq& inputs, const ::Ice::Current&) override;
+ virtual armem::actions::ExecuteActionOutputSeq executeActions(const armem::actions::ExecuteActionInputSeq& inputs, const ::Ice::Current&) override;
public:
diff --git a/source/RobotAPI/libraries/armem_grasping/CMakeLists.txt b/source/RobotAPI/libraries/armem_grasping/CMakeLists.txt
index f79f688800f1129a2c63a8da65d3c560b043bad2..c555283cc5506ae12391f4acacf94cf40acd4488 100644
--- a/source/RobotAPI/libraries/armem_grasping/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_grasping/CMakeLists.txt
@@ -6,37 +6,36 @@ armarx_set_target("Library: ${LIB_NAME}")
armarx_add_library(
LIBS
+ armem
+ armem_server
+ aron
+ RobotAPI::ArmarXObjects
GraspingUtility
HEADERS
armem_grasping.h
- aron_conversions.h
- GraspCandidateWriter.h
- GraspCandidateReader.h
- aron/GraspCandidate.aron.generated.h
+ server/KnownGraspProviderSegment.h
+
+ client/KnownGraspCandidateReader.h
+
+ # TODO: move client code in client folder
+ aron_conversions.h # TODO: Please also provide cpp files (they can be empty except of include<header>)
+ GraspCandidateWriter.h # dito
+ GraspCandidateReader.h # dito
+ aron/GraspCandidate.aron.generated.h # Please only use the xml and let the aron converter generate the header file.
SOURCES
armem_grasping.cpp
+
+ server/KnownGraspProviderSegment.cpp
+
+ client/KnownGraspCandidateReader.cpp
+
+ ARON_FILES
+ aron/KnownGraspCandidate.xml
)
add_library(${PROJECT_NAME}::armem_grasping ALIAS armem_grasping)
-#armarx_add_library(armem_grasping
-# SOURCES
-# ./armem_grasping.cpp
-# HEADERS
-# ./armem_grasping.h
-# DEPENDENCIES
-# ArmarXCoreInterfaces
-# ArmarXCore
-#)
-
-
-#armarx_add_test(armem_graspingTest
-# TEST_FILES
-# test/armem_graspingTest.cpp
-# DEPENDENCIES
-# RobotAPI::armem_grasping
-#)
diff --git a/source/RobotAPI/libraries/armem_grasping/aron/KnownGraspCandidate.xml b/source/RobotAPI/libraries/armem_grasping/aron/KnownGraspCandidate.xml
new file mode 100644
index 0000000000000000000000000000000000000000..8397a5d2c65f9d1145b69ab45f0b8e21bba33c20
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_grasping/aron/KnownGraspCandidate.xml
@@ -0,0 +1,62 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <AronIncludes>
+ <Include include="<RobotAPI/libraries/armem/aron/MemoryID.xml>" autoinclude="true" />
+ <Include include="<RobotAPI/libraries/aron/common/aron/PackagePath.xml>" autoinclude="true" />
+ </AronIncludes>
+ <GenerateTypes>
+ <Object name='armarx::armem::grasping::arondto::KnownGrasp'>
+
+ <ObjectChild key='name'>
+ <string />
+ </ObjectChild>
+
+ <ObjectChild key='creator'>
+ <string />
+ </ObjectChild>
+
+ <ObjectChild key='quality'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='pose'>
+ <Pose />
+ </ObjectChild>
+
+ </Object>
+
+ <Object name='armarx::armem::grasping::arondto::KnownGraspSet'>
+
+ <ObjectChild key='name'>
+ <string />
+ </ObjectChild>
+
+ <ObjectChild key='grasps'>
+ <Dict>
+ <armarx::armem::grasping::arondto::KnownGrasp />
+ </Dict>
+ </ObjectChild>
+
+ </Object>
+
+ <Object name='armarx::armem::grasping::arondto::KnownGraspInfo'>
+
+ <ObjectChild key='correspondingObject'>
+ <armarx::armem::arondto::MemoryID />
+ </ObjectChild>
+
+ <ObjectChild key='graspSets'>
+ <Dict>
+ <armarx::armem::grasping::arondto::KnownGraspSet />
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key='xml'>
+ <armarx::arondto::PackagePath />
+ </ObjectChild>
+
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
+
diff --git a/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.cpp b/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..17dfd176e77f561e744be85802df356edbf4d804
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.cpp
@@ -0,0 +1,101 @@
+#include "KnownGraspCandidateReader.h"
+
+#include <mutex>
+#include <optional>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/PackagePath.h>
+
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/util/util.h>
+#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+
+
+namespace armarx::armem::grasping::known_grasps
+{
+ Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem) :
+ memoryNameSystem(memoryNameSystem)
+ {}
+
+ void Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ {
+ ARMARX_DEBUG << "Reader: registerPropertyDefinitions";
+
+ const std::string prefix = propertyPrefix;
+
+ def->optional(properties.memoryName, prefix + "MemoryName");
+
+ def->optional(properties.coreSegmentName,
+ prefix + "CoreSegment",
+ "Name of the memory core segment to use for object instances.");
+ }
+
+
+ void Reader::connect()
+ {
+ // Wait for the memory to become available and add it as dependency.
+ ARMARX_IMPORTANT << "Reader: Waiting for memory '" << properties.memoryName << "' ...";
+ try
+ {
+ memoryReader = memoryNameSystem.useReader(properties.memoryName);
+ ARMARX_IMPORTANT << "Reader: Connected to memory '" << properties.memoryName << "'";
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
+ {
+ ARMARX_ERROR << e.what();
+ return;
+ }
+ }
+
+ std::optional<armem::grasping::arondto::KnownGraspInfo> Reader::queryKnownGraspInfo(const armem::wm::Memory& memory, const armem::Time&)
+ {
+ // clang-format off
+ const armem::wm::CoreSegment& s = memory
+ .getCoreSegment(properties.coreSegmentName);
+ // clang-format on
+
+ const armem::wm::EntityInstance* instance = nullptr;
+ s.forEachInstance([&instance](const wm::EntityInstance& i)
+ { instance = &i; });
+ if (instance == nullptr)
+ {
+ ARMARX_WARNING << "No entity snapshots found";
+ return std::nullopt;
+ }
+ return armem::grasping::arondto::KnownGraspInfo::FromAron(instance->data());
+ }
+
+ std::optional<armarx::armem::grasping::arondto::KnownGraspInfo> Reader::queryKnownGraspInfoByEntityName(const std::string& entityName, const armem::Time& timestamp)
+ {
+ // Query all entities from all provider.
+ armem::client::query::Builder qb;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(properties.coreSegmentName)
+ .providerSegments().all()
+ .entities().withName(entityName)
+ .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]] */
+ {
+ return std::nullopt;
+ }
+
+ return queryKnownGraspInfo(qResult.memory, timestamp);
+ }
+
+} // namespace armarx::armem::attachment
diff --git a/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.h b/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.h
new file mode 100644
index 0000000000000000000000000000000000000000..945670d82d8b2b130da2e04b3aad0ace5977efee
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.h
@@ -0,0 +1,65 @@
+/*
+ * 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 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+#include <optional>
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
+
+#include <RobotAPI/libraries/armem_grasping/aron/KnownGraspCandidate.aron.generated.h>
+
+
+namespace armarx::armem::grasping::known_grasps
+{
+ class Reader
+ {
+ public:
+ Reader(armem::client::MemoryNameSystem& memoryNameSystem);
+ virtual ~Reader() = default;
+
+ void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
+ void connect();
+
+ std::optional<armem::grasping::arondto::KnownGraspInfo> queryKnownGraspInfoByEntityName(const std::string&, const armem::Time&);
+ std::optional<armem::grasping::arondto::KnownGraspInfo> queryKnownGraspInfo(const armem::wm::Memory& memory, const armem::Time&);
+
+ private:
+
+ struct Properties
+ {
+ std::string memoryName = "Grasp";
+ std::string coreSegmentName = "KnownGraspCandidate";
+ } properties;
+
+ const std::string propertyPrefix = "mem.grasping.knowngrasps.";
+
+ armem::client::MemoryNameSystem& memoryNameSystem;
+ armem::client::Reader memoryReader;
+ std::mutex memoryWriterMutex;
+ };
+
+
+} // namespace armarx::armem::attachment
diff --git a/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5b8aaafea2d04db384f510d23788b16d94fe34db
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
@@ -0,0 +1,144 @@
+#include "KnownGraspProviderSegment.h"
+
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
+
+namespace armarx::armem::grasping::segment
+{
+ KnownGraspProviderSegment::KnownGraspProviderSegment(armem::server::MemoryToIceAdapter& iceMemory) :
+ Base(iceMemory, PROVIDER_SEGMENT_NAME, CORE_SEGMENT_NAME)
+ {
+
+ }
+
+ void KnownGraspProviderSegment::init()
+ {
+ Base::init();
+
+ loadMemory();
+ }
+
+ std::optional<arondto::KnownGraspInfo> KnownGraspProviderSegment::knownGraspInfoFromObjectInfo(const ObjectInfo& info)
+ {
+ std::string objectClassName = info.className();
+ auto fileLocInfo = info.file(".xml", "_Grasps");
+ std::filesystem::path graspFilePath = fileLocInfo.absolutePath;
+
+ if (std::filesystem::is_regular_file(graspFilePath))
+ {
+ auto reader = RapidXmlReader::FromFile(graspFilePath);
+ RapidXmlReaderNode root = reader->getRoot();
+
+ std::string objectClassNameXML = root.attribute_value("name");
+ ARMARX_CHECK_EQUAL(objectClassName, objectClassNameXML);
+
+ arondto::KnownGraspInfo ret;
+ ret.correspondingObject.memoryName = "Object";
+ ret.correspondingObject.coreSegmentName = "Class";
+ ret.correspondingObject.providerSegmentName = "PriorKnowledgeData";
+ ret.correspondingObject.entityName = info.idStr();
+ ret.xml.package = fileLocInfo.package;
+ ret.xml.path = fileLocInfo.relativePath;
+
+ for (const auto& graspSetNode : root.nodes())
+ {
+ if (graspSetNode.name() != "GraspSet")
+ {
+ continue;
+ }
+
+ arondto::KnownGraspSet retGraspSet;
+
+ retGraspSet.name = graspSetNode.attribute_value("name");
+ //std::string robotType = graspSetNode.attribute_value("RobotType");
+ //std::string endEffector = graspSetNode.attribute_value("EndEffector");
+
+ for (const auto& graspNode : graspSetNode.nodes())
+ {
+ if (graspNode.name() != "Grasp")
+ {
+ continue;
+ }
+
+ arondto::KnownGrasp retGrasp;
+
+ retGrasp.name = graspNode.attribute_value("name");
+ retGrasp.quality = std::stof(graspNode.attribute_value("quality"));
+ retGrasp.creator = graspNode.attribute_value("Creation");
+
+ ARMARX_CHECK_EQUAL(graspNode.nodes().size(), 1);
+ RapidXmlReaderNode transformNode = graspNode.nodes()[0];
+
+ ARMARX_CHECK_EQUAL(transformNode.nodes().size(), 1);
+ RapidXmlReaderNode matrixNode = transformNode.nodes()[0];
+
+ ARMARX_CHECK_EQUAL(matrixNode.nodes().size(), 4);
+ RapidXmlReaderNode row0 = matrixNode.nodes()[0];
+ RapidXmlReaderNode row1 = matrixNode.nodes()[1];
+ RapidXmlReaderNode row2 = matrixNode.nodes()[2];
+ RapidXmlReaderNode row3 = matrixNode.nodes()[3];
+
+ retGrasp.pose(0, 0) = std::stof(row0.attribute_value("c1"));
+ retGrasp.pose(0, 1) = std::stof(row0.attribute_value("c2"));
+ retGrasp.pose(0, 2) = std::stof(row0.attribute_value("c3"));
+ retGrasp.pose(0, 3) = std::stof(row0.attribute_value("c4"));
+ retGrasp.pose(1, 0) = std::stof(row1.attribute_value("c1"));
+ retGrasp.pose(1, 1) = std::stof(row1.attribute_value("c2"));
+ retGrasp.pose(1, 2) = std::stof(row1.attribute_value("c3"));
+ retGrasp.pose(1, 3) = std::stof(row1.attribute_value("c4"));
+ retGrasp.pose(2, 0) = std::stof(row2.attribute_value("c1"));
+ retGrasp.pose(2, 1) = std::stof(row2.attribute_value("c2"));
+ retGrasp.pose(2, 2) = std::stof(row2.attribute_value("c3"));
+ retGrasp.pose(2, 3) = std::stof(row2.attribute_value("c4"));
+ retGrasp.pose(3, 0) = std::stof(row3.attribute_value("c1"));
+ retGrasp.pose(3, 1) = std::stof(row3.attribute_value("c2"));
+ retGrasp.pose(3, 2) = std::stof(row3.attribute_value("c3"));
+ retGrasp.pose(3, 3) = std::stof(row3.attribute_value("c4"));
+
+ ARMARX_INFO << "Found grasp '" << retGrasp.name << "' in set '" << retGraspSet.name << "' for obj '" << objectClassName << "' with pose \n" << retGrasp.pose;
+
+ retGraspSet.grasps[retGrasp.name] = retGrasp;
+ }
+ ret.graspSets[retGraspSet.name] = retGraspSet;
+ }
+ return ret;
+ }
+ return std::nullopt;
+ }
+
+ void KnownGraspProviderSegment::loadMemory()
+ {
+ // load data from prior knowledge
+ ObjectFinder objectFinder;
+ const auto now = armem::Time::now();
+
+ const bool checkPaths = false;
+ std::vector<ObjectInfo> infos = objectFinder.findAllObjects(checkPaths);
+
+ const MemoryID providerID = segmentPtr->id().withProviderSegmentName(objectFinder.getPackageName());
+ ARMARX_INFO << "Checking up to " << infos.size() << " object classes from '" << objectFinder.getPackageName() << "' ...";
+
+ Commit commit;
+ for (ObjectInfo& info : infos)
+ {
+ info.setLogError(false);
+ if (auto knownGraspCandidate = knownGraspInfoFromObjectInfo(info); knownGraspCandidate)
+ {
+ EntityUpdate& update = commit.add();
+ update.entityID = providerID.withEntityName(info.id().str());
+ update.entityID.timestamp = update.timeArrived = update.timeCreated = update.timeSent = now;
+
+ update.instancesData =
+ {
+ knownGraspCandidate->toAron()
+ };
+ }
+ }
+ ARMARX_INFO << "Loaded " << commit.updates.size() << " grasp candidates from object classes from '" << objectFinder.getPackageName() << "'.";
+ auto result = iceMemory.commit(commit);
+ if (!result.allSuccess())
+ {
+ ARMARX_WARNING << "Got errors for commit: " << result.allErrorMessages();
+ }
+ }
+}
diff --git a/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.h b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.h
new file mode 100644
index 0000000000000000000000000000000000000000..de92372581076450341fb53e4628c4dd7ae4ba7c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.h
@@ -0,0 +1,27 @@
+# pragma once
+
+#include <RobotAPI/libraries/armem/server/segment/SpecializedSegment.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+#include <RobotAPI/libraries/armem_grasping/aron/KnownGraspCandidate.aron.generated.h>
+
+namespace armarx::armem::grasping::segment
+{
+ class KnownGraspProviderSegment : public armem::server::segment::SpecializedProviderSegment
+ {
+ using Base = armem::server::segment::SpecializedProviderSegment;
+
+ public:
+ KnownGraspProviderSegment(armem::server::MemoryToIceAdapter& iceMemory);
+
+ void init() override;
+
+ private:
+ void loadMemory();
+ std::optional<arondto::KnownGraspInfo> knownGraspInfoFromObjectInfo(const ObjectInfo&);
+
+ public:
+ static const constexpr char* CORE_SEGMENT_NAME = "KnownGraspCandidate";
+ static const constexpr char* PROVIDER_SEGMENT_NAME = "PriorKnowledgeData";
+ };
+}
diff --git a/source/RobotAPI/libraries/armem_gui/ActionsMenuBuilder.cpp b/source/RobotAPI/libraries/armem_gui/ActionsMenuBuilder.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b8bf2121617d084537b456fb741fad34fcbf8e26
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/ActionsMenuBuilder.cpp
@@ -0,0 +1,52 @@
+#include "ActionsMenuBuilder.h"
+
+#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
+
+
+namespace armarx::armem::gui
+{
+ ActionsMenuBuilder::ActionsMenuBuilder(
+ MemoryID memoryID,
+ QWidget* parent,
+ std::function<void(const MemoryID&, const actions::ActionPath&)> func) :
+ memoryID(std::move(memoryID)), parent(parent), func(std::move(func))
+ {
+ }
+
+ QMenu*
+ ActionsMenuBuilder::buildActionsMenu(actions::data::GetActionsOutput& actionsOutput)
+ {
+ auto* menu = new QMenu("Actions", parent);
+
+ for (const auto& entry : actions::Menu::fromIce(actionsOutput.menu).entries)
+ {
+ addMenuEntry(menu, {}, entry);
+ }
+
+ return menu;
+ }
+
+ void
+ ActionsMenuBuilder::addMenuEntry( // NOLINT (clangd complains about the recursion here)
+ QMenu* menu,
+ actions::ActionPath path,
+ const actions::MenuEntry& entry)
+ {
+ path.push_back(entry.id);
+ if (entry.entries.empty())
+ {
+ menu->addAction(QString::fromStdString(entry.text),
+ parent,
+ [this, path]() { func(memoryID, path); });
+ }
+ else
+ {
+ QMenu* qSubmenu = menu->addMenu(QString::fromStdString(entry.text));
+ for (const auto& subEntry : entry.entries)
+ {
+ addMenuEntry(qSubmenu, path, subEntry);
+ }
+ }
+ }
+
+} // namespace armarx::armem::gui
diff --git a/source/RobotAPI/libraries/armem_gui/ActionsMenuBuilder.h b/source/RobotAPI/libraries/armem_gui/ActionsMenuBuilder.h
new file mode 100644
index 0000000000000000000000000000000000000000..4d1261f65cecf4cfd4afd77867ad4e2b9af7ebac
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/ActionsMenuBuilder.h
@@ -0,0 +1,28 @@
+#pragma once
+
+#include <QMenu>
+
+#include <RobotAPI/interface/armem/actions.h>
+#include <RobotAPI/libraries/armem/core/actions.h>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+
+namespace armarx::armem::gui
+{
+ class ActionsMenuBuilder
+ {
+ public:
+ ActionsMenuBuilder(MemoryID memoryID,
+ QWidget* parent,
+ std::function<void(const MemoryID&, const actions::ActionPath&)> func);
+
+ QMenu* buildActionsMenu(actions::data::GetActionsOutput& actionsOutput);
+
+ private:
+ void
+ addMenuEntry(QMenu* menu, actions::ActionPath path, const actions::MenuEntry& entry);
+
+ MemoryID memoryID;
+ QWidget* parent;
+ std::function<void(const MemoryID&, const actions::ActionPath&)> func;
+ };
+} // namespace armarx::armem::gui
diff --git a/source/RobotAPI/libraries/armem_gui/CMakeLists.txt b/source/RobotAPI/libraries/armem_gui/CMakeLists.txt
index 5a774f498c13b372fa2b188bba2d6b1508fc3094..b92bcbfbce86f7bc65bee7a9f186c73d14d0c9b5 100644
--- a/source/RobotAPI/libraries/armem_gui/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_gui/CMakeLists.txt
@@ -21,6 +21,7 @@ set(SOURCES
gui_utils.cpp
lifecycle.cpp
+ ActionsMenuBuilder.cpp
MemoryViewer.cpp
PeriodicUpdateWidget.cpp
@@ -60,6 +61,7 @@ set(HEADERS
gui_utils.h
lifecycle.h
+ ActionsMenuBuilder.h
MemoryViewer.h
PeriodicUpdateWidget.h
TreeWidgetBuilder.h
diff --git a/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp b/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
index ddb264ec45b8b36ea0867e453c68a93a136e88bb..318329d8eb16a348338132ad276e819e631b1cdb 100644
--- a/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
+++ b/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
@@ -1,45 +1,47 @@
#include "MemoryViewer.h"
-#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
-
-#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
-#include <RobotAPI/libraries/armem_gui/gui_utils.h>
-
-#include <RobotAPI/libraries/armem/server/query_proc/wm/wm.h>
-#include <RobotAPI/libraries/armem/server/query_proc/ltm/disk/ltm.h>
-
-#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
-
-#include <ArmarXCore/core/ManagedIceObject.h>
-#include <ArmarXCore/core/time/TimeUtil.h>
-#include <ArmarXCore/observers/variant/Variant.h>
-
-#include <SimoxUtility/algorithm/get_map_keys_values.h>
-
#include <QApplication>
#include <QBoxLayout>
#include <QCheckBox>
#include <QClipboard>
#include <QDialog>
#include <QGroupBox>
-#include <QMenu>
#include <QLabel>
#include <QLayout>
+#include <QMenu>
#include <QSettings>
#include <QTimer>
-#include <iomanip>
+#include <Ice/Exception.h>
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/observers/variant/Variant.h>
+
+#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
+
+#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
+#include <RobotAPI/interface/armem/actions.h>
+#include <RobotAPI/interface/armem/memory.h>
+#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
+#include <RobotAPI/libraries/armem/server/query_proc/ltm/disk/ltm.h>
+#include <RobotAPI/libraries/armem/server/query_proc/wm/wm.h>
+#include <RobotAPI/libraries/armem_gui/ActionsMenuBuilder.h>
+#include <RobotAPI/libraries/armem_gui/gui_utils.h>
namespace armarx::armem::gui
{
- MemoryViewer::MemoryViewer(
- QBoxLayout* updateWidgetLayout,
- QGroupBox* memoryGroupBox, QLayout* memoryGroupBoxParentLayout,
- QGroupBox* instanceGroupBox, QLayout* instanceGroupBoxParentLayout,
- QBoxLayout* diskControlWidgetLayout,
- QLabel* statusLabel
- )
+ MemoryViewer::MemoryViewer(QBoxLayout* updateWidgetLayout,
+ QGroupBox* memoryGroupBox,
+ QLayout* memoryGroupBoxParentLayout,
+ QGroupBox* instanceGroupBox,
+ QLayout* instanceGroupBoxParentLayout,
+ QBoxLayout* diskControlWidgetLayout,
+ QLabel* statusLabel)
{
Logging::setTag("MemoryViewer");
@@ -47,19 +49,17 @@ namespace armarx::armem::gui
this->statusLabel->clear();
statusLabel->setContextMenuPolicy(Qt::CustomContextMenu);
- connect(statusLabel, &QLabel::customContextMenuRequested, [statusLabel](const QPoint & pos)
- {
- QMenu menu(statusLabel);
- menu.addAction("Copy to clipboard", [statusLabel]()
- {
- QApplication::clipboard()->setText(statusLabel->text());
- });
- menu.addAction("Clear status", [statusLabel]()
- {
- statusLabel->clear();
- });
- menu.exec(statusLabel->mapToGlobal(pos));
- });
+ connect(statusLabel,
+ &QLabel::customContextMenuRequested,
+ [statusLabel](const QPoint& pos)
+ {
+ QMenu menu(statusLabel);
+ menu.addAction("Copy to clipboard",
+ [statusLabel]()
+ { QApplication::clipboard()->setText(statusLabel->text()); });
+ menu.addAction("Clear status", [statusLabel]() { statusLabel->clear(); });
+ menu.exec(statusLabel->mapToGlobal(pos));
+ });
// Update timer
@@ -68,7 +68,7 @@ namespace armarx::armem::gui
updateWidgetLayout->insertWidget(0, updateWidget);
processQueryResultTimer = new QTimer(this);
- processQueryResultTimer->setInterval(1000 / 60); // Keep this stable.
+ processQueryResultTimer->setInterval(1000 / 60); // Keep this stable.
processQueryResultTimer->start();
// Memory View
@@ -91,8 +91,14 @@ namespace armarx::armem::gui
}
// Connections
- connect(diskControl, &armem::gui::disk::ControlWidget::requestedStoreOnDisk, this, &This::storeOnDisk);
- connect(diskControl, &armem::gui::disk::ControlWidget::requestedLoadFromDisk, this, &This::loadFromDisk);
+ connect(diskControl,
+ &armem::gui::disk::ControlWidget::requestedStoreOnDisk,
+ this,
+ &This::storeOnDisk);
+ connect(diskControl,
+ &armem::gui::disk::ControlWidget::requestedLoadFromDisk,
+ this,
+ &This::loadFromDisk);
connect(this, &This::connected, this, &This::startQueries);
connect(updateWidget, &armem::gui::PeriodicUpdateWidget::update, this, &This::startQueries);
@@ -102,21 +108,43 @@ namespace armarx::armem::gui
connect(memoryGroup->commitWidget(), &armem::gui::CommitWidget::commit, this, &This::commit);
connect(this, &This::memoryDataChanged, this, &This::updateMemoryTree);
- connect(memoryGroup->tree(), &armem::gui::MemoryTreeWidget::selectedItemChanged, this, &This::updateInstanceTree);
-
- connect(memoryGroup->tree(), &armem::gui::MemoryTreeWidget::updated, this, &This::memoryTreeUpdated);
- connect(instanceGroup, &armem::gui::InstanceGroupBox::viewUpdated, this, &This::instanceTreeUpdated);
- connect(instanceGroup->view, &armem::gui::InstanceView::memoryIdResolutionRequested, this, &This::resolveMemoryID);
+ connect(memoryGroup->tree(),
+ &armem::gui::MemoryTreeWidget::selectedItemChanged,
+ this,
+ &This::updateInstanceTree);
+
+ connect(memoryGroup->tree(),
+ &armem::gui::MemoryTreeWidget::updated,
+ this,
+ &This::memoryTreeUpdated);
+ connect(memoryGroup->tree(),
+ &armem::gui::MemoryTreeWidget::actionsMenuRequested,
+ this,
+ &This::showActionsMenu);
+ connect(instanceGroup,
+ &armem::gui::InstanceGroupBox::viewUpdated,
+ this,
+ &This::instanceTreeUpdated);
+ connect(instanceGroup->view,
+ &armem::gui::InstanceView::memoryIdResolutionRequested,
+ this,
+ &This::resolveMemoryID);
+ connect(instanceGroup->view,
+ &armem::gui::InstanceView::actionsMenuRequested,
+ this,
+ &This::showActionsMenu);
}
- void MemoryViewer::setLogTag(const std::string& _tag) // Leading _ silences a warning
+ void
+ MemoryViewer::setLogTag(const std::string& _tag) // Leading _ silences a warning
{
Logging::setTag(_tag);
}
- void MemoryViewer::onInit(ManagedIceObject& component)
+ void
+ MemoryViewer::onInit(ManagedIceObject& component)
{
if (mnsName.size() > 0)
{
@@ -131,7 +159,8 @@ namespace armarx::armem::gui
}
- void MemoryViewer::onConnect(ManagedIceObject& component)
+ void
+ MemoryViewer::onConnect(ManagedIceObject& component)
{
if (not mnsName.empty())
{
@@ -154,7 +183,8 @@ namespace armarx::armem::gui
}
- void MemoryViewer::onDisconnect(ManagedIceObject&)
+ void
+ MemoryViewer::onDisconnect(ManagedIceObject&)
{
updateWidget->stopTimer();
@@ -162,14 +192,15 @@ namespace armarx::armem::gui
}
- const armem::wm::Memory* MemoryViewer::getSingleMemoryData(const std::string& memoryName)
+ const armem::wm::Memory*
+ MemoryViewer::getSingleMemoryData(const std::string& memoryName)
{
auto it = memoryData.find(memoryName);
if (it == memoryData.end())
{
std::stringstream ss;
- ss << "Memory name '" << memoryName << "' is unknown. Known are: "
- << simox::alg::get_keys(memoryData);
+ ss << "Memory name '" << memoryName
+ << "' is unknown. Known are: " << simox::alg::get_keys(memoryData);
statusLabel->setText(QString::fromStdString(ss.str()));
return nullptr;
}
@@ -185,7 +216,8 @@ namespace armarx::armem::gui
}
- void MemoryViewer::storeInLTM()
+ void
+ MemoryViewer::storeInLTM()
{
TIMING_START(MemoryStore);
@@ -244,7 +276,8 @@ namespace armarx::armem::gui
}
- void MemoryViewer::storeOnDisk(QString directory)
+ void
+ MemoryViewer::storeOnDisk(QString directory)
{
TIMING_START(MemoryExport)
@@ -256,10 +289,12 @@ namespace armarx::armem::gui
}
- void MemoryViewer::loadFromDisk(QString directory)
+ void
+ MemoryViewer::loadFromDisk(QString directory)
{
std::string status;
- std::map<std::string, wm::Memory> data = diskControl->loadFromDisk(directory, memoryGroup->queryWidget()->queryInput(), &status);
+ std::map<std::string, wm::Memory> data =
+ diskControl->loadFromDisk(directory, memoryGroup->queryWidget()->queryInput(), &status);
memoryWriters = mns.getAllWriters(true);
for (auto& [name, memory] : data)
@@ -281,16 +316,19 @@ namespace armarx::armem::gui
}
- void MemoryViewer::startQueries()
+ void
+ MemoryViewer::startQueries()
{
memoryReaders = mns.getAllReaders(true);
startDueQueries(runningQueries, memoryReaders);
}
- void MemoryViewer::processQueryResults()
+ void
+ MemoryViewer::processQueryResults()
{
- const std::map<std::string, client::QueryResult> results = collectQueryResults(runningQueries, memoryReaders);
+ const std::map<std::string, client::QueryResult> results =
+ collectQueryResults(runningQueries, memoryReaders);
int errorCount = 0;
applyQueryResults(results, &errorCount);
@@ -300,7 +338,8 @@ namespace armarx::armem::gui
}
- void MemoryViewer::updateStatusLabel(int errorCount)
+ void
+ MemoryViewer::updateStatusLabel(int errorCount)
{
// Code to output status label information
if (statusLabel and errorCount > 0)
@@ -317,9 +356,9 @@ namespace armarx::armem::gui
}
- void MemoryViewer::startDueQueries(
- std::map<std::string, Ice::AsyncResultPtr>& queries,
- const std::map<std::string, armem::client::Reader>& readers)
+ void
+ MemoryViewer::startDueQueries(std::map<std::string, Ice::AsyncResultPtr>& queries,
+ const std::map<std::string, armem::client::Reader>& readers)
{
armem::client::QueryInput input = memoryGroup->queryWidget()->queryInput();
@@ -334,9 +373,8 @@ namespace armarx::armem::gui
std::map<std::string, client::QueryResult>
- MemoryViewer::collectQueryResults(
- std::map<std::string, Ice::AsyncResultPtr>& queries,
- const std::map<std::string, client::Reader>& readers)
+ MemoryViewer::collectQueryResults(std::map<std::string, Ice::AsyncResultPtr>& queries,
+ const std::map<std::string, client::Reader>& readers)
{
TIMING_START(tCollectQueryResults)
@@ -352,7 +390,8 @@ namespace armarx::armem::gui
{
try
{
- results[name] = client::QueryResult::fromIce(jt->second.memoryPrx->end_query(queryPromise));
+ results[name] = client::QueryResult::fromIce(
+ jt->second.memoryPrx->end_query(queryPromise));
}
catch (const Ice::ConnectionRefusedException&)
{
@@ -366,27 +405,29 @@ namespace armarx::armem::gui
}
else
{
- ++it; // Uncompleted => Keep.
+ ++it; // Uncompleted => Keep.
}
}
TIMING_END_STREAM(tCollectQueryResults, ARMARX_VERBOSE)
if (debugObserver)
{
- debugObserver->begin_setDebugChannel(Logging::tag.tagName,
- {
- { "t Collect Query Results [ms]", new Variant(tCollectQueryResults.toMilliSecondsDouble()) },
- { "# Collected Query Results", new Variant(static_cast<int>(results.size())) },
- });
+ debugObserver->begin_setDebugChannel(
+ Logging::tag.tagName,
+ {
+ {"t Collect Query Results [ms]",
+ new Variant(tCollectQueryResults.toMilliSecondsDouble())},
+ {"# Collected Query Results", new Variant(static_cast<int>(results.size()))},
+ });
}
return results;
}
- void MemoryViewer::applyQueryResults(
- const std::map<std::string, client::QueryResult>& results,
- int* outErrorCount)
+ void
+ MemoryViewer::applyQueryResults(const std::map<std::string, client::QueryResult>& results,
+ int* outErrorCount)
{
TIMING_START(tProcessQueryResults)
for (const auto& [name, result] : results)
@@ -397,7 +438,8 @@ namespace armarx::armem::gui
}
else
{
- ARMARX_WARNING << "Querying memory server '" << name << "' produced an error: \n" << result.errorMessage;
+ ARMARX_WARNING << "Querying memory server '" << name << "' produced an error: \n"
+ << result.errorMessage;
if (outErrorCount)
{
outErrorCount++;
@@ -422,16 +464,19 @@ namespace armarx::armem::gui
TIMING_END_STREAM(tProcessQueryResults, ARMARX_VERBOSE)
if (debugObserver)
{
- debugObserver->begin_setDebugChannel(Logging::tag.tagName,
- {
- { "t Process Query Results [ms]", new Variant(tProcessQueryResults.toMilliSecondsDouble()) },
- { "# Processed Query Results", new Variant(static_cast<int>(results.size())) },
- });
+ debugObserver->begin_setDebugChannel(
+ Logging::tag.tagName,
+ {
+ {"t Process Query Results [ms]",
+ new Variant(tProcessQueryResults.toMilliSecondsDouble())},
+ {"# Processed Query Results", new Variant(static_cast<int>(results.size()))},
+ });
}
}
- void MemoryViewer::updateInstanceTree(const armem::MemoryID& selectedID)
+ void
+ MemoryViewer::updateInstanceTree(const armem::MemoryID& selectedID)
{
const armem::wm::Memory* data = getSingleMemoryData(selectedID.memoryName);
if (data)
@@ -481,11 +526,12 @@ namespace armarx::armem::gui
}
- void MemoryViewer::resolveMemoryID(const MemoryID& id)
+ void
+ MemoryViewer::resolveMemoryID(const MemoryID& id)
{
// ARMARX_IMPORTANT << "Resolving memory ID: " << id;
- auto handleError = [this](const std::string & msg)
+ auto handleError = [this](const std::string& msg)
{
statusLabel->setText(QString::fromStdString(msg));
ARMARX_WARNING << msg;
@@ -548,7 +594,8 @@ namespace armarx::armem::gui
}
- void MemoryViewer::updateMemoryTree()
+ void
+ MemoryViewer::updateMemoryTree()
{
std::map<std::string, const armem::wm::Memory*> convMap;
for (auto& [name, data] : memoryData)
@@ -564,7 +611,9 @@ namespace armarx::armem::gui
{
try
{
- debugObserver->setDebugDatafield(Logging::tag.tagName, "GUI Update [ms]", new Variant(GuiUpdate.toMilliSecondsDouble()));
+ debugObserver->setDebugDatafield(Logging::tag.tagName,
+ "GUI Update [ms]",
+ new Variant(GuiUpdate.toMilliSecondsDouble()));
}
catch (const Ice::Exception&)
{
@@ -574,28 +623,140 @@ namespace armarx::armem::gui
}
+ void
+ MemoryViewer::showActionsMenu(const MemoryID& memoryID,
+ QWidget* parent,
+ const QPoint& pos,
+ QMenu* menu)
+ {
+ // Called if we have to stop because of an error.
+ auto showMenu = [menu, pos]()
+ {
+ if (menu)
+ menu->exec(pos);
+ };
+
+ mns::dto::MemoryServerInterfaces prx;
+ try
+ {
+ prx = mns.resolveServer(memoryID);
+ }
+ catch (const error::CouldNotResolveMemoryServer& e)
+ {
+ statusLabel->setText(
+ QString::fromStdString(
+ e.makeMsg(memoryID, "Could not resolve memory server.")));
+ showMenu();
+ return;
+ }
+
+ if (!prx.actions)
+ {
+ std::stringstream ss;
+ ss << "Memory server " << memoryID
+ << " does not support actions or is offline.";
+ statusLabel->setText(QString::fromStdString(ss.str()));
+ showMenu();
+ return;
+ }
+
+ actions::GetActionsOutputSeq result;
+ try
+ {
+ result = prx.actions->getActions({{toIce<data::MemoryID>(memoryID)}});
+ }
+ catch (const Ice::LocalException& e)
+ {
+ std::stringstream ss;
+ ss << "Could not get actions for " << memoryID << ".";
+ statusLabel->setText(QString::fromStdString(ss.str()));
+ showMenu();
+ return;
+ }
+
+ if (result.size() == 0)
+ {
+ showMenu();
+ return;
+ }
+ auto builder = ActionsMenuBuilder(
+ memoryID,
+ parent,
+ [this, prx](const MemoryID& memoryID, const actions::ActionPath& path)
+ {
+ actions::data::ExecuteActionOutputSeq result;
+ try
+ {
+ result = prx.actions->executeActions(
+ {{toIce<armem::data::MemoryID>(memoryID), path}});
+ }
+ catch (const Ice::LocalException& e)
+ {
+ std::stringstream ss;
+ ss << "Failed to execute action: " << e.what();
+ statusLabel->setText(QString::fromStdString(ss.str()));
+ }
+
+ for (const auto& [success, errorMessage] : result)
+ {
+ if (not success)
+ {
+ std::stringstream ss;
+ ss << "Failed to execute action: " << errorMessage;
+ statusLabel->setText(QString::fromStdString(ss.str()));
+ ARMARX_WARNING << ss.str();
+ }
+ }
+ });
+
+ QMenu* actionsMenu = builder.buildActionsMenu(result[0]);
+ if (menu == nullptr)
+ {
+ actionsMenu->exec(pos);
+ }
+ else
+ {
+ menu->addMenu(actionsMenu);
+ menu->exec(pos);
+ }
+ }
+
+
const static std::string CONFIG_KEY_MEMORY = "MemoryViewer.MemoryNameSystem";
const static std::string CONFIG_KEY_DEBUG_OBSERVER = "MemoryViewer.DebugObserverName";
- void MemoryViewer::loadSettings(QSettings* settings)
+ void
+ MemoryViewer::loadSettings(QSettings* settings)
{
- mnsName = settings->value(QString::fromStdString(CONFIG_KEY_MEMORY), "MemoryNameSystem").toString().toStdString();
- debugObserverName = settings->value(QString::fromStdString(CONFIG_KEY_DEBUG_OBSERVER), "DebugObserver").toString().toStdString();
+ mnsName = settings->value(QString::fromStdString(CONFIG_KEY_MEMORY), "MemoryNameSystem")
+ .toString()
+ .toStdString();
+ debugObserverName =
+ settings->value(QString::fromStdString(CONFIG_KEY_DEBUG_OBSERVER), "DebugObserver")
+ .toString()
+ .toStdString();
}
- void MemoryViewer::saveSettings(QSettings* settings)
+ void
+ MemoryViewer::saveSettings(QSettings* settings)
{
- settings->setValue(QString::fromStdString(CONFIG_KEY_MEMORY), QString::fromStdString(mnsName));
- settings->setValue(QString::fromStdString(CONFIG_KEY_DEBUG_OBSERVER), QString::fromStdString(debugObserverName));
+ settings->setValue(QString::fromStdString(CONFIG_KEY_MEMORY),
+ QString::fromStdString(mnsName));
+ settings->setValue(QString::fromStdString(CONFIG_KEY_DEBUG_OBSERVER),
+ QString::fromStdString(debugObserverName));
}
- void MemoryViewer::writeConfigDialog(SimpleConfigDialog* dialog)
+ void
+ MemoryViewer::writeConfigDialog(SimpleConfigDialog* dialog)
{
- dialog->addProxyFinder<armarx::armem::mns::MemoryNameSystemInterfacePrx>({CONFIG_KEY_MEMORY, "MemoryNameSystem", "MemoryNameSystem"});
- dialog->addProxyFinder<armarx::DebugObserverInterfacePrx>({CONFIG_KEY_DEBUG_OBSERVER, "Debug Observer", "DebugObserver"});
+ dialog->addProxyFinder<armarx::armem::mns::MemoryNameSystemInterfacePrx>(
+ {CONFIG_KEY_MEMORY, "MemoryNameSystem", "MemoryNameSystem"});
+ dialog->addProxyFinder<armarx::DebugObserverInterfacePrx>(
+ {CONFIG_KEY_DEBUG_OBSERVER, "Debug Observer", "DebugObserver"});
}
- void MemoryViewer::readConfigDialog(SimpleConfigDialog* dialog)
+ void
+ MemoryViewer::readConfigDialog(SimpleConfigDialog* dialog)
{
mnsName = dialog->getProxyName(CONFIG_KEY_MEMORY);
if (mnsName.empty())
@@ -605,4 +766,4 @@ namespace armarx::armem::gui
debugObserverName = dialog->getProxyName(CONFIG_KEY_DEBUG_OBSERVER);
}
-}
+} // namespace armarx::armem::gui
diff --git a/source/RobotAPI/libraries/armem_gui/MemoryViewer.h b/source/RobotAPI/libraries/armem_gui/MemoryViewer.h
index b4ea2897516a60e01ff13709ab41d7e5df413964..51cda4cb797911c3afcd1afaf43657b145b9a140 100644
--- a/source/RobotAPI/libraries/armem_gui/MemoryViewer.h
+++ b/source/RobotAPI/libraries/armem_gui/MemoryViewer.h
@@ -7,6 +7,7 @@
#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <RobotAPI/interface/armem/actions.h>
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
@@ -71,6 +72,9 @@ namespace armarx::armem::gui
void resolveMemoryID(const MemoryID& id);
+ void showActionsMenu(const MemoryID& memoryID, QWidget* parent,
+ const QPoint& pos, QMenu* menu);
+
// Disk Control
void storeOnDisk(QString directory);
void loadFromDisk(QString directory);
@@ -98,7 +102,6 @@ namespace armarx::armem::gui
void updateMemoryTree();
-
signals:
void memoryDataChanged();
@@ -130,7 +133,6 @@ namespace armarx::armem::gui
int* outErrorCount = nullptr);
-
public:
std::string mnsName;
diff --git a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp
index 6cdbcab8351b7597bd5dc0dab8e1ed07408f5ad9..3bbb2ee55a87be3216386a0f847b40829b4bb785 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp
@@ -253,13 +253,13 @@ namespace armarx::armem::gui::instance
return; // No item => no context menu.
}
- QMenu menu(this);
+ QMenu* menu = new QMenu(this);
if (item == this->treeItemInstanceID && currentInstance.has_value())
{
if (QAction* action = makeActionCopyMemoryID(currentInstance->id()))
{
- menu.addAction(action);
+ menu->addAction(action);
}
}
else if (item == this->treeItemData && currentInstance.has_value())
@@ -267,11 +267,10 @@ namespace armarx::armem::gui::instance
auto actions = makeActionsCopyDataToClipboard();
for (const auto& action : actions)
{
- if (!action)
+ if (action)
{
- continue;
+ menu->addAction(action);
}
- menu.addAction(action);
}
}
else if (item->parent() == nullptr)
@@ -288,7 +287,7 @@ namespace armarx::armem::gui::instance
if (const std::optional<aron::Path> path = getElementPath(item))
{
QAction* viewAction = new QAction("Show image");
- menu.addAction(viewAction);
+ menu->addAction(viewAction);
connect(viewAction, &QAction::triggered, [this, path]()
{
this->showImageView(path.value());
@@ -330,11 +329,11 @@ namespace armarx::armem::gui::instance
{
if (QAction* action = makeActionCopyMemoryID(id.value()))
{
- menu.addAction(action);
+ menu->addAction(action);
}
if (QAction* action = makeActionResolveMemoryID(id.value()))
{
- menu.addAction(action);
+ menu->addAction(action);
}
}
}
@@ -346,17 +345,20 @@ namespace armarx::armem::gui::instance
auto actions = makeActionsCopyDataToClipboard(elementPath.value());
for (const auto& action : actions)
{
- if (!action)
+ if (action)
{
- continue;
+ menu->addAction(action);
}
- menu.addAction(action);
}
}
- if (menu.actions().size() > 0)
+ if (menu->actions().size() > 0)
{
- menu.exec(tree->mapToGlobal(pos));
+ emit actionsMenuRequested(currentInstance->id(), this, tree->mapToGlobal(pos), menu);
+ }
+ else
+ {
+ emit actionsMenuRequested(currentInstance->id(), this, tree->mapToGlobal(pos), nullptr);
}
}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.h b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.h
index 289e67caff9749d8a3e8012ea2c2798507b255aa..982d277db1febb68070290d3c86a3f06c602b990 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.h
@@ -3,6 +3,7 @@
#include <optional>
#include <deque>
+#include <QMenu>
#include <QWidget>
#include <QGroupBox>
@@ -55,6 +56,8 @@ namespace armarx::armem::gui::instance
void updated();
void instanceSelected(const MemoryID& id);
void memoryIdResolutionRequested(const MemoryID& id);
+ void actionsMenuRequested(const MemoryID& memoryID, QWidget* parent,
+ const QPoint& pos, QMenu* menu);
private slots:
diff --git a/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp b/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp
index 0ece75cfb419df323a3269a7baacb8d3e4fbba6e..858c932bdba6825696a99de538bbdf02284c159c 100644
--- a/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp
@@ -3,10 +3,12 @@
#include <RobotAPI/libraries/armem_gui/instance/sanitize_typename.h>
#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
+#include <RobotAPI/interface/armem/actions.h>
#include <SimoxUtility/algorithm/string.h>
#include <QHeaderView>
+#include <QMenu>
namespace armarx::armem::gui::memory
@@ -45,6 +47,9 @@ namespace armarx::armem::gui::memory
connect(this, &This::entitySelected, this, &This::itemSelected);
connect(this, &This::snapshotSelected, this, &This::itemSelected);
connect(this, &This::instanceSelected, this, &This::itemSelected);
+
+ setContextMenuPolicy(Qt::CustomContextMenu);
+ connect(this, &QTreeWidget::customContextMenuRequested, this, &This::prepareTreeContextMenu);
}
void TreeWidget::initBuilders()
@@ -269,6 +274,18 @@ namespace armarx::armem::gui::memory
(void) data, (void) dataItem;
}
+ void TreeWidget::prepareTreeContextMenu(const QPoint& pos)
+ {
+ const QTreeWidgetItem* item = this->itemAt(pos);
+ if (item == nullptr)
+ {
+ return;
+ }
+
+ MemoryID memoryID(item->data(int(Columns::ID), Qt::UserRole).toString().toStdString());
+ emit actionsMenuRequested(memoryID, this, mapToGlobal(pos), nullptr);
+ }
+
template <class MemoryItemT>
QTreeWidgetItem* TreeWidget::makeItem(const std::string& key, const MemoryItemT& memoryItem)
{
diff --git a/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.h b/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.h
index 75f56c9ace248e58daf3f7ee3ac69baa12b0edfe..1ad975e5f29e741df801c55906d0ff364e607996 100644
--- a/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.h
+++ b/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.h
@@ -44,6 +44,9 @@ namespace armarx::armem::gui::memory
void snapshotSelected(const MemoryID& id);
void instanceSelected(const MemoryID& id);
+ void actionsMenuRequested(const MemoryID& memoryID, QWidget* parent,
+ const QPoint& pos, QMenu* menu);
+
private slots:
@@ -65,6 +68,7 @@ namespace armarx::armem::gui::memory
void updateChildren(const armem::wm::EntitySnapshot& snapshot, QTreeWidgetItem* snapshotItem);
void updateChildren(const armem::wm::EntityInstance& data, QTreeWidgetItem* parent);
+ void prepareTreeContextMenu(const QPoint& pos);
template <class MemoryItemT>
QTreeWidgetItem* makeItem(const std::string& key, const MemoryItemT& memoryItem);
diff --git a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
index 88fd8cf80f190ee13489c8180e4c4a53f652caff..74928ae14dc40cc72b49c6a81570525336c48bfa 100644
--- a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
@@ -44,6 +44,8 @@ armarx_add_library(
client/attachment/Reader.h
client/attachment/Writer.h
+
+ client/instance/ObjectReader.h
SOURCES
aron_conversions.cpp
@@ -58,6 +60,8 @@ armarx_add_library(
client/attachment/Reader.cpp
client/attachment/Writer.cpp
+
+ client/instance/ObjectReader.cpp
ARON_FILES
aron/ObjectClass.xml
diff --git a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..49dbdcf6140717e63ceec39e8cbf7d0f9b91dd77
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp
@@ -0,0 +1,138 @@
+#include "ObjectReader.h"
+
+#include <mutex>
+#include <optional>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/PackagePath.h>
+
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/util/util.h>
+#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+
+
+namespace armarx::armem::obj::instance
+{
+ Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem) :
+ memoryNameSystem(memoryNameSystem)
+ {}
+
+ void Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ {
+ ARMARX_DEBUG << "Reader: registerPropertyDefinitions";
+
+ const std::string prefix = propertyPrefix;
+
+ def->optional(properties.memoryName, prefix + "MemoryName");
+
+ def->optional(properties.coreSegmentName,
+ prefix + "CoreSegment",
+ "Name of the memory core segment to use for object instances.");
+ }
+
+
+ void Reader::connect()
+ {
+ // Wait for the memory to become available and add it as dependency.
+ ARMARX_IMPORTANT << "Reader: Waiting for memory '" << properties.memoryName << "' ...";
+ try
+ {
+ memoryReader = memoryNameSystem.useReader(properties.memoryName);
+ ARMARX_IMPORTANT << "Reader: Connected to memory '" << properties.memoryName << "'";
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
+ {
+ ARMARX_ERROR << e.what();
+ return;
+ }
+ }
+
+ std::optional<armarx::armem::arondto::ObjectInstance> Reader::queryObject(const armem::wm::Memory& memory, const armem::Time& timestamp)
+ {
+ // clang-format off
+ const armem::wm::CoreSegment& s = memory
+ .getCoreSegment(properties.coreSegmentName);
+ // clang-format on
+
+ // What to do with timestamp?
+ const armem::wm::EntityInstance* instance = nullptr;
+ s.forEachInstance([&instance](const wm::EntityInstance& i)
+ { instance = &i; });
+ if (instance == nullptr)
+ {
+ return std::nullopt;
+ }
+ return armem::arondto::ObjectInstance::FromAron(instance->data());
+ }
+
+ std::optional<armarx::armem::arondto::ObjectInstance> Reader::queryObjectByEntityID(const std::string& entityName, const armem::Time& timestamp)
+ {
+ // Query all entities from all provider.
+ armem::client::query::Builder qb;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(properties.coreSegmentName)
+ .providerSegments().all()
+ .entities().withName(entityName)
+ .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]] */
+ {
+ return std::nullopt;
+ }
+
+ return queryObject(qResult.memory, timestamp);
+ }
+
+ std::optional<armarx::armem::arondto::ObjectInstance> Reader::queryObjectByObjectID(const std::string& objectId, const armem::Time& timestamp)
+ {
+ // Query all entities from all provider.
+ armem::client::query::Builder qb;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(properties.coreSegmentName)
+ .providerSegments().all()
+ .entities().all()
+ .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]] */
+ {
+ return std::nullopt;
+ }
+
+ // clang-format off
+ const armem::wm::CoreSegment& s = qResult.memory
+ .getCoreSegment(properties.coreSegmentName);
+ // clang-format on
+
+ const armem::wm::EntityInstance* instance = nullptr;
+ s.forEachInstance([&instance, &objectId](const wm::EntityInstance& i)
+ { if (simox::alg::starts_with(i.id().entityName, objectId)) instance = &i; });
+ if (instance == nullptr)
+ {
+ return std::nullopt;
+ }
+ return armem::arondto::ObjectInstance::FromAron(instance->data());
+ }
+
+} // namespace armarx::armem::attachment
diff --git a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h
new file mode 100644
index 0000000000000000000000000000000000000000..8db6a1cab642e103dd6c25105b607c47651ccb15
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h
@@ -0,0 +1,70 @@
+/*
+ * 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 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+#include <optional>
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem_objects/types.h>
+
+#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
+
+
+namespace armarx::armem::obj::instance
+{
+ class Reader
+ {
+ public:
+ Reader(armem::client::MemoryNameSystem& memoryNameSystem);
+ virtual ~Reader() = default;
+
+ void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
+ void connect();
+
+
+ std::optional<armem::arondto::ObjectInstance> queryObject(const armem::wm::Memory& memory, const armem::Time&);
+ std::optional<armem::arondto::ObjectInstance> queryObjectByEntityID(const std::string& entityName, const armem::Time&);
+ std::optional<armem::arondto::ObjectInstance> queryObjectByObjectID(const std::string& objectId, const armem::Time&);
+
+
+ private:
+
+
+ struct Properties
+ {
+ std::string memoryName = "Object";
+ std::string coreSegmentName = "Instance";
+ } properties;
+
+ const std::string propertyPrefix = "mem.obj.instance.";
+
+ armem::client::MemoryNameSystem& memoryNameSystem;
+ armem::client::Reader memoryReader;
+ std::mutex memoryWriterMutex;
+ };
+
+
+} // namespace armarx::armem::attachment
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index c86bc7e6fe2afe0bd1a9f0e3703a1587c9c641f9..91a9196beec405f386854d9f45f02ef5cd5bf1e7 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -788,7 +788,8 @@ namespace armarx::armem::server::obj::instance
if (not result.has_value())
{
- ARMARX_WARNING << "No provider segment for classID " << classID.str() << " found";
+ ARMARX_WARNING << deactivateSpam(120)
+ << "No provider segment for classID " << classID.str() << " found";
}
return result;
}
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 4428f3d967085e9d1f1a8e1951c5524d1097b2e7..bdeeb0bd659f42333abe3450ac319d17e91847b0 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -174,7 +174,7 @@ namespace armarx::armem::robot_state
std::optional<robot::RobotState::JointMap>
RobotReader::queryJointState(const robot::RobotDescription& description,
- const armem::Time& timestamp) const
+ const armem::Time& timestamp) const // Why timestamp?!?!
{
// TODO(fabian.reister): how to deal with multiple providers?
diff --git a/source/RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.cpp b/source/RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.cpp
index dce0a7f051c96ead6968faef643ed261735abb9d..57151c646f1206a45e689714c37dfac3508aeb88 100644
--- a/source/RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.cpp
+++ b/source/RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.cpp
@@ -40,7 +40,7 @@ namespace armarx::skills::segment
skills::manager::dto::SkillExecutionInfo info;
info.providerName = request.providerName;
info.skillName = request.skillName;
- info.params = params->toAronDictPtr();
+ info.params = aron::data::Dict::ToAronDictDTO(params);
return info;
}
@@ -58,9 +58,7 @@ namespace armarx::skills::segment
auto aron = request.toAron();
- aron::data::DictPtr aron_params = nullptr;
- if (info.params) aron_params = std::make_shared<aron::data::Dict>(info.params);
-
+ aron::data::DictPtr aron_params = aron::data::Dict::FromAronDictDTO(info.params);
aron->addElement("params", aron_params); // todo add as any type
armem::MemoryID skillExecutionMemID = id();
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/Variant.h b/source/RobotAPI/libraries/aron/core/data/variant/Variant.h
index 04c39ea84332665c56269b7e8d16c17aacd83aa4..aee8c55fc02d07395e67ff7fd907a8fb5b207999 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/Variant.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/Variant.h
@@ -144,9 +144,6 @@ namespace armarx::aron::data
/// convert the variant to the ice representation
virtual data::dto::GenericDataPtr toAronDTO() const = 0;
- /// set a variant from a std string
- virtual void fromString(const std::string& setter) = 0;
-
protected:
const data::Descriptor descriptor;
const Path path;
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.cpp b/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.cpp
index c5a8b08ab2b68ded38ca90859c77a0168fb3887d..7de89a06a92c1da3e84046c3af98c2d10d602515 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.cpp
@@ -87,6 +87,10 @@ namespace armarx::aron::data
// static methods
NDArrayPtr NDArray::FromNDArrayDTO(const data::dto::NDArrayPtr& aron)
{
+ if (!aron)
+ {
+ return nullptr;
+ }
return std::make_shared<NDArray>(aron);
}
@@ -156,11 +160,6 @@ namespace armarx::aron::data
}
// virtual implementations
- void NDArray::fromString(const std::string& setter)
- {
- // TODO!
- }
-
std::string NDArray::getShortName() const
{
return "NDArray";
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h b/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h
index 92dbed05e7433e90fd9725690413687bda908c31..31677060acad93fe7fbbbe949b4439d434392931 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h
@@ -90,8 +90,6 @@ namespace armarx::aron::data
return ret;
}
- void fromString(const std::string& setter) override;
-
virtual std::string getShortName() const override;
virtual std::string getFullName() const override;
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 ef895067b7b299f89be4e031b5d86d69cc8d3182..0e7de7dae17c7aaa92d956a8dd56a27a7f9bbba3 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp
@@ -90,17 +90,21 @@ namespace armarx::aron::data
}
// static methods
- DictPtr Dict::FromAronDictPtr(const data::dto::DictPtr& aron)
+ DictPtr Dict::FromAronDictDTO(const data::dto::DictPtr& aron)
{
+ if (!aron)
+ {
+ return nullptr;
+ }
return std::make_shared<Dict>(aron);
}
- data::dto::DictPtr Dict::ToAronDictPtr(const DictPtr& navigator)
+ data::dto::DictPtr Dict::ToAronDictDTO(const DictPtr& navigator)
{
- return navigator ? navigator->toAronDictPtr() : nullptr;
+ return navigator ? navigator->toAronDictDTO() : nullptr;
}
- data::dto::DictPtr Dict::toAronDictPtr() const
+ data::dto::DictPtr Dict::toAronDictDTO() const
{
return aron;
}
@@ -186,11 +190,6 @@ namespace armarx::aron::data
}
// virtual implementations
- void Dict::fromString(const std::string& setter)
- {
- // TODO!
- }
-
std::string Dict::getShortName() const
{
return "Dict";
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.h b/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.h
index 99d7a368293c3d3613937a0316c208261bfa3caf..3c8f5a74d3e87cb7ae90bab620c58435817e39c7 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.h
@@ -53,11 +53,11 @@ namespace armarx::aron::data
bool operator==(const DictPtr&) const override;
VariantPtr operator[](const std::string&) const;
- static PointerType FromAronDictPtr(const data::dto::DictPtr& aron);
- static data::dto::DictPtr ToAronDictPtr(const PointerType& navigator);
+ static PointerType FromAronDictDTO(const data::dto::DictPtr& aron);
+ static data::dto::DictPtr ToAronDictDTO(const PointerType& navigator);
// public member functions
- data::dto::DictPtr toAronDictPtr() const;
+ data::dto::DictPtr toAronDictDTO() const;
std::vector<std::string> getAllKeys() const;
std::string getAllKeysAsString() const;
@@ -82,8 +82,6 @@ namespace armarx::aron::data
return ret;
}
- void fromString(const std::string& setter) override;
-
std::string getShortName() const override;
std::string getFullName() const override;
std::vector<VariantPtr> getChildren() const override;
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 0d4dffe9bc20479e12c03797964dc4ccba4a42f0..bb86d4f1baff4013c62c788a7f6ddf0f99f80a1c 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/container/List.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/container/List.cpp
@@ -93,14 +93,18 @@ namespace armarx::aron::data
}
// static methods
- ListPtr List::FromAronListPtr(const data::dto::ListPtr& aron)
+ ListPtr List::FromAronListDTO(const data::dto::ListPtr& aron)
{
+ if (!aron)
+ {
+ return nullptr;
+ }
return std::make_shared<List>(aron);
}
- data::dto::ListPtr List::ToAronListPtr(const ListPtr& navigator)
+ data::dto::ListPtr List::ToAronListDTO(const ListPtr& navigator)
{
- return navigator ? navigator->toAronListPtr() : nullptr;
+ return navigator ? navigator->toAronListDTO() : nullptr;
}
// public member functions
@@ -183,17 +187,12 @@ namespace armarx::aron::data
aron->elements.clear();
}
- data::dto::ListPtr List::toAronListPtr() const
+ data::dto::ListPtr List::toAronListDTO() const
{
return aron;
}
// virtual implementations
- void List::fromString(const std::string& setter)
- {
- // TODO!
- }
-
std::string List::getShortName() const
{
return "List";
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/container/List.h b/source/RobotAPI/libraries/aron/core/data/variant/container/List.h
index cab6a38e5fd1ff8bd85a996fe3336a7ddff16504..b766ce5aae83416ae632b7e0ff27c30ade72c0e0 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/container/List.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/container/List.h
@@ -54,13 +54,13 @@ namespace armarx::aron::data
bool operator==(const ListPtr&) const override;
// static methods
- static PointerType FromAronListPtr(const data::dto::ListPtr& aron);
- static data::dto::ListPtr ToAronListPtr(const PointerType& navigator);
+ static PointerType FromAronListDTO(const data::dto::ListPtr& aron);
+ static data::dto::ListPtr ToAronListDTO(const PointerType& navigator);
// public member functions
DictPtr getAsDict() const;
- data::dto::ListPtr toAronListPtr() const;
+ data::dto::ListPtr toAronListDTO() const;
void addElement(const VariantPtr&);
void setElement(unsigned int, const VariantPtr&);
@@ -83,8 +83,6 @@ namespace armarx::aron::data
return ret;
}
- void fromString(const std::string& setter) override;
-
std::string getShortName() const override;
std::string getFullName() const override;
std::vector<VariantPtr> getChildren() const override;
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/detail/PrimitiveVariant.h b/source/RobotAPI/libraries/aron/core/data/variant/detail/PrimitiveVariant.h
index b7356be2074a27ba2ac4285ffafd2a400fcf1864..849d146d5c398966f4328f03ffd1f0d286488877 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/detail/PrimitiveVariant.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/detail/PrimitiveVariant.h
@@ -71,6 +71,9 @@ namespace armarx::aron::data::detail
return this->aron->value == x;
}*/
+ /// set a primitive from a std string
+ virtual void fromString(const std::string& setter) = 0;
+
// virtual implementations
VariantPtr navigateAbsolute(const Path &path) const override
{
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.cpp
index 59f8ffa1602955de4f00c60346b3c0b480d24683..7c5e526822769e56b61e3b0099509274f6ae9224 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.cpp
@@ -44,7 +44,7 @@ namespace armarx::aron::data
/* operators */
bool Bool::operator==(const Bool& other) const
{
- const auto& otherAron = other.toAronBoolPtr();
+ const auto& otherAron = other.toAronBoolDTO();
if (aron->value != otherAron->value)
{
return false;
@@ -62,18 +62,22 @@ namespace armarx::aron::data
/* static methods */
- BoolPtr Bool::FromAronBoolPtr(const data::dto::AronBoolPtr& aron)
+ BoolPtr Bool::FromAronBoolDTO(const data::dto::AronBoolPtr& aron)
{
+ if (!aron)
+ {
+ return nullptr;
+ }
return std::make_shared<Bool>(aron);
}
- data::dto::AronBoolPtr Bool::ToAronBoolPtr(const BoolPtr& navigator)
+ data::dto::AronBoolPtr Bool::ToAronBoolDTO(const BoolPtr& navigator)
{
- return navigator ? navigator->toAronBoolPtr() : nullptr;
+ return navigator ? navigator->toAronBoolDTO() : nullptr;
}
/* public member functions */
- data::dto::AronBoolPtr Bool::toAronBoolPtr() const
+ data::dto::AronBoolPtr Bool::toAronBoolDTO() const
{
return aron;
}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.h
index f76f9396ce4a640e18b9c16d46b9d6383cbee8ba..65cfc31a7bec67f63e8f9bf5101f697c7d61e2c7 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.h
@@ -52,11 +52,11 @@ namespace armarx::aron::data
bool operator==(const BoolPtr&) const override;
// static methods
- static BoolPtr FromAronBoolPtr(const data::dto::AronBoolPtr& aron);
- static data::dto::AronBoolPtr ToAronBoolPtr(const BoolPtr& navigator);
+ static BoolPtr FromAronBoolDTO(const data::dto::AronBoolPtr& aron);
+ static data::dto::AronBoolPtr ToAronBoolDTO(const BoolPtr& navigator);
// class methods
- data::dto::AronBoolPtr toAronBoolPtr() const;
+ data::dto::AronBoolPtr toAronBoolDTO() const;
/* virtual implementations */
void fromString(const std::string& setter) override;
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.cpp
index e96cd84855419855414946958162f3370a7adba8..729e5bda480e8a21d14e16be1f454a02d1f7cf9b 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.cpp
@@ -48,7 +48,7 @@ namespace armarx::aron::data
/* operators */
bool Double::operator==(const Double& other) const
{
- const auto& otherAron = other.toAronDoublePtr();
+ const auto& otherAron = other.toAronDoubleDTO();
if (this->aron->value != otherAron->value)
{
return false;
@@ -65,19 +65,23 @@ namespace armarx::aron::data
}
/* static methods */
- DoublePtr Double::FromAronDoublePtr(const data::dto::AronDoublePtr& aron)
+ DoublePtr Double::FromAronDoubleDTO(const data::dto::AronDoublePtr& aron)
{
+ if (!aron)
+ {
+ return nullptr;
+ }
return std::make_shared<Double>(aron);
}
- data::dto::AronDoublePtr Double::ToAronDoublePtr(const DoublePtr& navigator)
+ data::dto::AronDoublePtr Double::ToAronDoubleDTO(const DoublePtr& navigator)
{
- return navigator ? navigator->toAronDoublePtr() : nullptr;
+ return navigator ? navigator->toAronDoubleDTO() : nullptr;
}
/* public member functions */
- data::dto::AronDoublePtr Double::toAronDoublePtr() const
+ data::dto::AronDoublePtr Double::toAronDoubleDTO() const
{
return aron;
}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.h
index fdaba984aa6583523062995451ce062db3a56123..b02571f1dd7cef61635849b21edddf943469c7cd 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.h
@@ -52,11 +52,11 @@ namespace armarx::aron::data
bool operator==(const DoublePtr&) const override;
/* static methods */
- static DoublePtr FromAronDoublePtr(const data::dto::AronDoublePtr& aron);
- static data::dto::AronDoublePtr ToAronDoublePtr(const DoublePtr& navigator);
+ static DoublePtr FromAronDoubleDTO(const data::dto::AronDoublePtr& aron);
+ static data::dto::AronDoublePtr ToAronDoubleDTO(const DoublePtr& navigator);
/* public member functions */
- data::dto::AronDoublePtr toAronDoublePtr() const;
+ data::dto::AronDoublePtr toAronDoubleDTO() const;
/* virtual implementations */
void fromString(const std::string& setter) override;
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.cpp
index 35fd408454f77d91c3e45a260ead10d2a23ae037..f638882eaf80af2a6a4cba0f9d1fffe43cd6e304 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.cpp
@@ -44,7 +44,7 @@ namespace armarx::aron::data
/* operators */
bool Float::operator==(const Float& other) const
{
- const auto& otherAron = other.toAronFloatPtr();
+ const auto& otherAron = other.toAronFloatDTO();
if (this->aron->value != otherAron->value)
{
return false;
@@ -62,18 +62,22 @@ namespace armarx::aron::data
/* static methods */
- FloatPtr Float::FromAronFloatPtr(const data::dto::AronFloatPtr& aron)
+ FloatPtr Float::FromAronFloatDTO(const data::dto::AronFloatPtr& aron)
{
+ if (!aron)
+ {
+ return nullptr;
+ }
return std::make_shared<Float>(aron);
}
- data::dto::AronFloatPtr Float::ToAronFloatPtr(const FloatPtr& navigator)
+ data::dto::AronFloatPtr Float::ToAronFloatDTO(const FloatPtr& navigator)
{
- return navigator ? navigator->toAronFloatPtr() : nullptr;
+ return navigator ? navigator->toAronFloatDTO() : nullptr;
}
/* public member functions */
- data::dto::AronFloatPtr Float::toAronFloatPtr() const
+ data::dto::AronFloatPtr Float::toAronFloatDTO() const
{
return aron;
}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.h
index 52f3f41cfc0ef98bc13fa47a17c5a160d95ae992..d7b44e56784ef2a6a1c8b35ebb3b7b3841448426 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.h
@@ -52,11 +52,11 @@ namespace armarx::aron::data
bool operator==(const FloatPtr&) const override;
/* static methods */
- static FloatPtr FromAronFloatPtr(const data::dto::AronFloatPtr& aron);
- static data::dto::AronFloatPtr ToAronFloatPtr(const FloatPtr& navigator);
+ static FloatPtr FromAronFloatDTO(const data::dto::AronFloatPtr& aron);
+ static data::dto::AronFloatPtr ToAronFloatDTO(const FloatPtr& navigator);
/* public member functions */
- data::dto::AronFloatPtr toAronFloatPtr() const;
+ data::dto::AronFloatPtr toAronFloatDTO() const;
/* virtual implementations */
void fromString(const std::string& setter) override;
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.cpp
index ba774f9e5b498f2546b231f795e644a79d39ec35..b5f0d39196b750084876ad85a49f9f77cdf23112 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.cpp
@@ -47,7 +47,7 @@ namespace armarx::aron::data
/* operators */
bool Int::operator==(const Int& other) const
{
- const auto& otherAron = other.toAronIntPtr();
+ const auto& otherAron = other.toAronIntDTO();
if (this->aron->value != otherAron->value)
{
return false;
@@ -65,18 +65,22 @@ namespace armarx::aron::data
/* static methods */
- IntPtr Int::FromAronIntPtr(const data::dto::AronIntPtr& aron)
+ IntPtr Int::FromAronIntDTO(const data::dto::AronIntPtr& aron)
{
+ if (!aron)
+ {
+ return nullptr;
+ }
return std::make_shared<Int>(aron);
}
- data::dto::AronIntPtr Int::ToAronIntPtr(const IntPtr& navigator)
+ data::dto::AronIntPtr Int::ToAronIntDTO(const IntPtr& navigator)
{
- return navigator ? navigator->toAronIntPtr() : nullptr;
+ return navigator ? navigator->toAronIntDTO() : nullptr;
}
/* public member functions */
- data::dto::AronIntPtr Int::toAronIntPtr() const
+ data::dto::AronIntPtr Int::toAronIntDTO() const
{
return aron;
}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.h
index 7cf20562d95eeadd431eb27c35b19de39d57e02f..9a803a035d0be45cd7f38241a937f67179c2bfa2 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.h
@@ -53,11 +53,11 @@ namespace armarx::aron::data
bool operator==(const IntPtr&) const override;
/* static methods */
- static IntPtr FromAronIntPtr(const data::dto::AronIntPtr& aron);
- static data::dto::AronIntPtr ToAronIntPtr(const IntPtr& navigator);
+ static IntPtr FromAronIntDTO(const data::dto::AronIntPtr& aron);
+ static data::dto::AronIntPtr ToAronIntDTO(const IntPtr& navigator);
/* public member functions */
- data::dto::AronIntPtr toAronIntPtr() const;
+ data::dto::AronIntPtr toAronIntDTO() const;
/* virtual implementations */
void fromString(const std::string& setter) override;
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.cpp
index 4a65a39333df0f16e02e5db7eb9b5d6cbb036bb8..1885fc750b5cee8a7b477627167972c37e514db9 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.cpp
@@ -48,7 +48,7 @@ namespace armarx::aron::data
/* operators */
bool Long::operator==(const Long& other) const
{
- const auto& otherAron = other.toAronLongPtr();
+ const auto& otherAron = other.toAronLongDTO();
if (this->aron->value != otherAron->value)
{
return false;
@@ -66,18 +66,22 @@ namespace armarx::aron::data
/* static methods */
- LongPtr Long::FromAronLongPtr(const data::dto::AronLongPtr& aron)
+ LongPtr Long::FromAronLongDTO(const data::dto::AronLongPtr& aron)
{
+ if (!aron)
+ {
+ return nullptr;
+ }
return std::make_shared<Long>(aron);
}
- data::dto::AronLongPtr Long::ToAronLongPtr(const LongPtr& navigator)
+ data::dto::AronLongPtr Long::ToAronLongDTO(const LongPtr& navigator)
{
- return navigator ? navigator->toAronLongPtr() : nullptr;
+ return navigator ? navigator->toAronLongDTO() : nullptr;
}
/* public member functions */
- data::dto::AronLongPtr Long::toAronLongPtr() const
+ data::dto::AronLongPtr Long::toAronLongDTO() const
{
return aron;
}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.h
index e8136ed7b3e08a1015c3cc8013105ef1ebb39f2c..82b9481be9f957271735d55686659767b60c53ec 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.h
@@ -53,11 +53,11 @@ namespace armarx::aron::data
bool operator==(const LongPtr&) const override;
/* static methods */
- static LongPtr FromAronLongPtr(const data::dto::AronLongPtr& aron);
- static data::dto::AronLongPtr ToAronLongPtr(const LongPtr& navigator);
+ static LongPtr FromAronLongDTO(const data::dto::AronLongPtr& aron);
+ static data::dto::AronLongPtr ToAronLongDTO(const LongPtr& navigator);
/* public member functions */
- data::dto::AronLongPtr toAronLongPtr() const;
+ data::dto::AronLongPtr toAronLongDTO() const;
/* virtual implementations */
void fromString(const std::string& setter) override;
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.cpp
index 4ca2ff4d854a4ac028792aa166be41172640f992..b896221b06d79f3e9cee12b2d84ec9156216dc7f 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.cpp
@@ -44,7 +44,7 @@ namespace armarx::aron::data
/* operators */
bool String::operator==(const String& other) const
{
- const auto& otherAron = other.toAronStringPtr();
+ const auto& otherAron = other.toAronStringDTO();
if (this->aron->value != otherAron->value)
{
return false;
@@ -62,18 +62,22 @@ namespace armarx::aron::data
/* static methods */
- StringPtr String::FromAronStringPtr(const data::dto::AronStringPtr& aron)
+ StringPtr String::FromAronStringDTO(const data::dto::AronStringPtr& aron)
{
+ if (!aron)
+ {
+ return nullptr;
+ }
return std::make_shared<String>(aron);
}
- data::dto::AronStringPtr String::ToAronStringPtr(const StringPtr& navigator)
+ data::dto::AronStringPtr String::ToAronStringDTO(const StringPtr& navigator)
{
- return navigator ? navigator->toAronStringPtr() : nullptr;
+ return navigator ? navigator->toAronStringDTO() : nullptr;
}
/* public member functions */
- data::dto::AronStringPtr String::toAronStringPtr() const
+ data::dto::AronStringPtr String::toAronStringDTO() const
{
return aron;
}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.h
index b8ec3ec038c459f37a907e3526ba7cdd034a9f1f..33e9ef6ea7d160bc5dab7e64cc043cc7948c1e34 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.h
@@ -52,11 +52,11 @@ namespace armarx::aron::data
bool operator==(const StringPtr&) const override;
/* static methods */
- static StringPtr FromAronStringPtr(const data::dto::AronStringPtr& aron);
- static data::dto::AronStringPtr ToAronStringPtr(const StringPtr& navigator);
+ static StringPtr FromAronStringDTO(const data::dto::AronStringPtr& aron);
+ static data::dto::AronStringPtr ToAronStringDTO(const StringPtr& navigator);
/* public member functions */
- data::dto::AronStringPtr toAronStringPtr() const;
+ data::dto::AronStringPtr toAronStringDTO() const;
/* virtual implementations */
void fromString(const std::string& setter) override;
diff --git a/source/RobotAPI/libraries/aron/core/test/aronRandomizedTest.cpp b/source/RobotAPI/libraries/aron/core/test/aronRandomizedTest.cpp
index 77a9ad525476d265e7919cf112abf761552670dd..33bb9c3208e99bf053c881556ff3a576e1b2948a 100644
--- a/source/RobotAPI/libraries/aron/core/test/aronRandomizedTest.cpp
+++ b/source/RobotAPI/libraries/aron/core/test/aronRandomizedTest.cpp
@@ -107,7 +107,7 @@ void test_toAronType(T& in, T& out)
{
type::ObjectPtr in_type_nav = in.toAronType();
- type::dto::AronObjectPtr in_type = in_type_nav->toObjectDTO();
+ type::dto::AronObjectPtr in_type = in_type_nav->toAronObjectDTO();
BOOST_CHECK(in_type);
armarx::aron::type::writer::NlohmannJSONWriter json_writer_in;
@@ -120,7 +120,7 @@ void test_toAronType(T& in, T& out)
{
type::ObjectPtr out_type_nav = out.toAronType();
- type::dto::AronObjectPtr out_type = out_type_nav->toObjectDTO();
+ type::dto::AronObjectPtr out_type = out_type_nav->toAronObjectDTO();
BOOST_CHECK(out_type);
armarx::aron::type::writer::NlohmannJSONWriter json_writer_out;
@@ -147,7 +147,7 @@ void test_toAron(T& in, T& out)
}
BOOST_TEST_INFO("getting in aron");
- data::dto::DictPtr in_aron = in_aron_nav->toAronDictPtr();
+ data::dto::DictPtr in_aron = in_aron_nav->toAronDictDTO();
BOOST_CHECK(in_aron);
//BOOST_TEST_MESSAGE("in_aron_nav: \n" << converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(in_aron_nav).dump(2));
@@ -162,7 +162,7 @@ void test_toAron(T& in, T& out)
out_aron_nav = out.toAron();
BOOST_CHECK(*in_aron_nav == *out_aron_nav);
- data::dto::DictPtr out_aron = out_aron_nav->toAronDictPtr();
+ data::dto::DictPtr out_aron = out_aron_nav->toAronDictDTO();
BOOST_CHECK(out_aron);
}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/Variant.h b/source/RobotAPI/libraries/aron/core/type/variant/Variant.h
index aba2fdd514cc43a18281e349de0a63e378ad8264..ab996582c630a9083bafb45bc7ec19755e59bb67 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/Variant.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/Variant.h
@@ -115,9 +115,6 @@ namespace armarx::aron::type
/// get the full name of this specific type
virtual std::string getFullName() const = 0;
- /// the default string to set the corresponding data object from string
- virtual std::string getDefaultFromString() const = 0;
-
/// get all child elements
virtual std::vector<VariantPtr> getChildren() const = 0;
virtual size_t childrenSize() const = 0;
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/container/Dict.cpp b/source/RobotAPI/libraries/aron/core/type/variant/container/Dict.cpp
index c58669203698b39693ce57fd7e1b8efb4bfc03f8..3919bf8447c76e594ca0873277b24626ae4d4ffb 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/container/Dict.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/container/Dict.cpp
@@ -80,15 +80,6 @@ namespace armarx::aron::type
return "armarx::aron::type::Dict<" + acceptedType->getFullName() + ">";
}
- std::string Dict::getDefaultFromString() const
- {
- std::stringstream ss;
- ss << "{" << "\n";
- ss << "\t" << "\"key\": " << acceptedType->getDefaultFromString() << "\n";
- ss << "}";
- return ss.str();
- }
-
VariantPtr Dict::navigateAbsolute(const Path& path) const
{
if (!path.hasElement())
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/container/Dict.h b/source/RobotAPI/libraries/aron/core/type/variant/container/Dict.h
index 0c2e11ee5c58391e0ba2d0a349d5fe1d27071a71..4dfdd35e238b39b729d67c583ec9378fc19f5c0e 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/container/Dict.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/container/Dict.h
@@ -56,7 +56,6 @@ namespace armarx::aron::type
std::string getShortName() const override;
std::string getFullName() const override;
- std::string getDefaultFromString() const override;
std::vector<VariantPtr> getChildren() const override;
size_t childrenSize() const override;
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/container/List.cpp b/source/RobotAPI/libraries/aron/core/type/variant/container/List.cpp
index d019192030891998cbb89c5de371357e6c0f6344..807d64fedf211b1d14949ece0409ab2baea952bb 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/container/List.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/container/List.cpp
@@ -80,15 +80,6 @@ namespace armarx::aron::type
return "armarx::aron::type::List<" + acceptedType->getFullName() + ">";
}
- std::string List::getDefaultFromString() const
- {
- std::stringstream ss;
- ss << "[" << "\n";
- ss << "\t" << acceptedType->getDefaultFromString() << "\n";
- ss << "]";
- return ss.str();
- }
-
VariantPtr List::navigateAbsolute(const Path& path) const
{
if (!path.hasElement())
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/container/List.h b/source/RobotAPI/libraries/aron/core/type/variant/container/List.h
index c739d069449fcbdcc599c0862f44148d806293d0..532146e010bf8217c4669649ee2da9771c0623ec 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/container/List.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/container/List.h
@@ -55,7 +55,6 @@ namespace armarx::aron::type
std::string getShortName() const override;
std::string getFullName() const override;
- std::string getDefaultFromString() const override;
std::vector<VariantPtr> getChildren() const override;
size_t childrenSize() const override;
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/container/Object.cpp b/source/RobotAPI/libraries/aron/core/type/variant/container/Object.cpp
index ee9ab1195b302832d096e64e970f6811a840091a..b3bf456051a2bc19095c6b050f88347c72cfbcbf 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/container/Object.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/container/Object.cpp
@@ -54,6 +54,24 @@ namespace armarx::aron::type
}
}
+ ObjectPtr Object::FromAronObjectDTO(const type::dto::AronObjectPtr& aron)
+ {
+ if (!aron)
+ {
+ return nullptr;
+ }
+ return std::make_shared<Object>(*aron);
+ }
+
+ type::dto::AronObjectPtr Object::ToAronObjectDTO(const ObjectPtr& aron)
+ {
+ if (!aron)
+ {
+ return nullptr;
+ }
+ return aron->toAronObjectDTO();
+ }
+
bool Object::checkObjectName(const std::string& s) const
{
if (s.empty())
@@ -117,7 +135,7 @@ namespace armarx::aron::type
void Object::setExtends(const std::shared_ptr<Object>& p)
{
ARMARX_CHECK_NOT_NULL(p);
- type::dto::AronObjectPtr ex = p->toObjectDTO();
+ type::dto::AronObjectPtr ex = p->toAronObjectDTO();
ARMARX_CHECK_NOT_NULL(ex);
extends = p;
}
@@ -154,7 +172,7 @@ namespace armarx::aron::type
return ret;
}
- type::dto::AronObjectPtr Object::toObjectDTO() const
+ type::dto::AronObjectPtr Object::toAronObjectDTO() const
{
// TODO: Shall we allow empty objects? For now yes!
//if(acceptedTypeNavigators.empty())
@@ -197,13 +215,6 @@ namespace armarx::aron::type
return "armarx::aron::type::Object<" + this->aron->objectName + (extends ? (" : " + extends->getFullName()) : "") + ">";
}
- std::string Object::getDefaultFromString() const
- {
- std::stringstream ss;
- ss << "TODO!";
- return ss.str();
- }
-
VariantPtr Object::navigateAbsolute(const Path& path) const
{
if (!path.hasElement())
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/container/Object.h b/source/RobotAPI/libraries/aron/core/type/variant/container/Object.h
index 15a3273a0d7077af1d2ab313e45e13598b598229..094d1f532a357128d329fe52d673c9f7ec248960 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/container/Object.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/container/Object.h
@@ -48,6 +48,9 @@ namespace armarx::aron::type
Object(const std::string&, const std::map<std::string, VariantPtr>& = {}, const Path& = Path());
Object(const type::dto::AronObject&, const Path& = Path());
+ static ObjectPtr FromAronObjectDTO(const type::dto::AronObjectPtr&);
+ static type::dto::AronObjectPtr ToAronObjectDTO(const ObjectPtr&);
+
// public member functions
bool checkObjectName(const std::string&) const;
@@ -65,14 +68,13 @@ namespace armarx::aron::type
std::vector<std::string> getAllKeys() const;
- type::dto::AronObjectPtr toObjectDTO() const;
+ type::dto::AronObjectPtr toAronObjectDTO() const;
// virtual implementations
VariantPtr navigateAbsolute(const Path& path) const override;
std::string getShortName() const override;
std::string getFullName() const override;
- std::string getDefaultFromString() const override;
std::vector<VariantPtr> getChildren() const override;
size_t childrenSize() const override;
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/container/Pair.cpp b/source/RobotAPI/libraries/aron/core/type/variant/container/Pair.cpp
index fb938e302a9fba89bd3be70c1643645db7b7e987..48e0c6f4dea325fc7e58ecc0cede68cbb8138854 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/container/Pair.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/container/Pair.cpp
@@ -115,13 +115,6 @@ namespace armarx::aron::type
return "armarx::aron::type::Pair<" + acceptedType1->getFullName() + ", " + acceptedType2->getFullName() + ">";
}
- std::string Pair::getDefaultFromString() const
- {
- std::stringstream ss;
- ss << "TODO!";
- return ss.str();
- }
-
VariantPtr Pair::navigateAbsolute(const Path& path) const
{
if (!path.hasElement())
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/container/Pair.h b/source/RobotAPI/libraries/aron/core/type/variant/container/Pair.h
index 9798548d0c95fea7a9568563d4191077cbaec90c..a63f6819406ce2b328e97a4026aeae21127d7b09 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/container/Pair.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/container/Pair.h
@@ -59,7 +59,6 @@ namespace armarx::aron::type
std::string getShortName() const override;
std::string getFullName() const override;
- std::string getDefaultFromString() const override;
std::vector<VariantPtr> getChildren() const override;
size_t childrenSize() const override;
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/container/Tuple.cpp b/source/RobotAPI/libraries/aron/core/type/variant/container/Tuple.cpp
index c06d9d1c8ad61053240658f1786ddd8faf75e46a..9fdca45e77972d2e58066e4fe47b0d9b492f9f44 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/container/Tuple.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/container/Tuple.cpp
@@ -107,13 +107,6 @@ namespace armarx::aron::type
return "armarx::aron::type::Tuple<" + simox::alg::to_string(names, ", ") + ">";
}
- std::string Tuple::getDefaultFromString() const
- {
- std::stringstream ss;
- ss << "TODO!";
- return ss.str();
- }
-
VariantPtr Tuple::navigateAbsolute(const Path& path) const
{
if (!path.hasElement())
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/container/Tuple.h b/source/RobotAPI/libraries/aron/core/type/variant/container/Tuple.h
index d17df06b74a59b83176424cbbdf6e23a4692d956..d464f83269d5074e1ec6e759119b433c76de0f7d 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/container/Tuple.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/container/Tuple.h
@@ -56,7 +56,6 @@ namespace armarx::aron::type
std::string getShortName() const override;
std::string getFullName() const override;
- std::string getDefaultFromString() const override;
std::vector<VariantPtr> getChildren() const override;
size_t childrenSize() const override;
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/enum/IntEnum.cpp b/source/RobotAPI/libraries/aron/core/type/variant/enum/IntEnum.cpp
index d425e0af8bf802dc1aebc6f0ae4563bf626eb329..734509d9e3272271c0ca3baece597c6e5b1b8d7a 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/enum/IntEnum.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/enum/IntEnum.cpp
@@ -111,10 +111,5 @@ namespace armarx::aron::type
{
return "armarx::aron::type::IntEnum";
}
-
- std::string IntEnum::getDefaultFromString() const
- {
- return "0";
- }
}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/enum/IntEnum.h b/source/RobotAPI/libraries/aron/core/type/variant/enum/IntEnum.h
index 1fd5661e3fde44db8d94d71db13383308e90aeab..cf652d23cfb225d1ba4efde9c7d48af7c0e34966 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/enum/IntEnum.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/enum/IntEnum.h
@@ -64,6 +64,5 @@ namespace armarx::aron::type
// virtual implementations
std::string getShortName() const override;
std::string getFullName() const override;
- std::string getDefaultFromString() const override;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Image.cpp b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Image.cpp
index 4ea9d7c9333cc8bc646cc12a52bc713bf5a6bf68..987a85ce26779811ecba5296eb4743c1022b49f4 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Image.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Image.cpp
@@ -77,10 +77,5 @@ namespace armarx::aron::type
{
return "armarx::aron::type::Image<" + Pixeltype2String.at(this->aron->pixelType) + ">";
}
-
- std::string Image::getDefaultFromString() const
- {
- return "TODO!";
- }
}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Image.h b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Image.h
index e6bdee312e17b0800827301b26b983d1481b5f84..0b9f9dd0992c79187368a7136b7fa957a42cacae 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Image.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Image.h
@@ -53,7 +53,6 @@ namespace armarx::aron::type
// virtual implementations
std::string getShortName() const override;
std::string getFullName() const override;
- std::string getDefaultFromString() const override;
static const std::map<image::PixelType, std::string> Pixeltype2String;
static const std::map<std::string, image::PixelType> String2Pixeltype;
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Matrix.cpp b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Matrix.cpp
index f471b23989280e2d479efbcc0910a69dec2dca8b..62eabd986890eb92cd5bf9b050c887ff985da6d6 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Matrix.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Matrix.cpp
@@ -104,10 +104,5 @@ namespace armarx::aron::type
{
return "armarx::aron::type::Matrix<" + std::to_string(this->aron->rows) + ", " + std::to_string(this->aron->cols) + ", " + Elementtype2String.at(this->aron->elementType) + ">";
}
-
- std::string Matrix::getDefaultFromString() const
- {
- return "TODO!";
- }
}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Matrix.h b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Matrix.h
index fa2a7cabe82c931d7b87ce377dd589edee132cc1..da9fd537dbcf62f9e17e54b9c42815009d408e8a 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Matrix.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Matrix.h
@@ -57,7 +57,6 @@ namespace armarx::aron::type
// virtual implementations
std::string getShortName() const override;
std::string getFullName() const override;
- std::string getDefaultFromString() const override;
static const std::map<matrix::ElementType, std::string> Elementtype2String;
static const std::map<std::string, matrix::ElementType> String2Elementtype;
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/NDArray.cpp b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/NDArray.cpp
index e30a71b2cdf544bf368918554a105b0c9cc14c71..cbbf517ba53d9e3d50342298b1c34b2652662216 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/NDArray.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/NDArray.cpp
@@ -77,10 +77,5 @@ namespace armarx::aron::type
{
return "armarx::aron::type::NDArray";
}
-
- std::string NDArray::getDefaultFromString() const
- {
- return "TODO!";
- }
}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/NDArray.h b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/NDArray.h
index b17f6f7ea68db4182db6ac83eddb96956e639dff..d04d218eb5ce19a1d53b8d1851b00d3a9ba7925a 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/NDArray.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/NDArray.h
@@ -56,6 +56,5 @@ namespace armarx::aron::type
// virtual implementations
std::string getShortName() const override;
std::string getFullName() const override;
- std::string getDefaultFromString() const override;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Orientation.cpp b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Orientation.cpp
index 58b7f948c6b43465cd05671f0d1393d92ad1702c..031fd6f73e7d2f924d8723107325cb16c3b57d46 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Orientation.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Orientation.cpp
@@ -52,10 +52,5 @@ namespace armarx::aron::type
{
return "armarx::aron::type::Orientation";
}
-
- std::string Orientation::getDefaultFromString() const
- {
- return "TODO!";
- }
}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Orientation.h b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Orientation.h
index 3a8107a261f2f6804e1e8f0a80de13acbef03a89..b4bebb8ca5e40d782681e88bed6f9bad30afd128 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Orientation.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Orientation.h
@@ -50,6 +50,5 @@ namespace armarx::aron::type
// virtual implementations
std::string getShortName() const override;
std::string getFullName() const override;
- std::string getDefaultFromString() const override;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/PointCloud.cpp b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/PointCloud.cpp
index 8278b017d810244005e21b3c537d965e22136046..10a3473021b616c78cc7e88b2e45dc94226663bb 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/PointCloud.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/PointCloud.cpp
@@ -75,10 +75,5 @@ namespace armarx::aron::type
{
return "armarx::aron::type::PointCloud<" + Voxeltype2String.at(this->aron->voxelType) + ">";
}
-
- std::string PointCloud::getDefaultFromString() const
- {
- return "TODO!";
- }
}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/PointCloud.h b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/PointCloud.h
index bec4627de7ecf9fa2b2fa707243773b4bf04f281..37d3aacdbed11c396cdc7351f8e7077705f2e2a6 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/PointCloud.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/PointCloud.h
@@ -54,7 +54,6 @@ namespace armarx::aron::type
// virtual implementations
std::string getShortName() const override;
std::string getFullName() const override;
- std::string getDefaultFromString() const override;
static const std::map<pointcloud::VoxelType, std::string> Voxeltype2String;
static const std::map<std::string, pointcloud::VoxelType> String2Voxeltype;
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Pose.cpp b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Pose.cpp
index 8ac9dc88699886788a58125198028d1abb81d363..c6965c12ba714ef01242c4e1420a9579ec11aa4e 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Pose.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Pose.cpp
@@ -52,15 +52,5 @@ namespace armarx::aron::type
{
return "armarx::aron::type::Pose";
}
-
- std::string Pose::getDefaultFromString() const
- {
- std::stringstream ss;
- ss << "0, 0, 0, 0" << "\n";
- ss << "0, 0, 0, 0" << "\n";
- ss << "0, 0, 0, 0" << "\n";
- ss << "0, 0, 0, 0";
- return ss.str();
- }
}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Pose.h b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Pose.h
index 20037341ddb6b7928f907efbe5b48a4100c11df7..fa2c1f184799004ab64f875cfe3cb749d743b6c9 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Pose.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Pose.h
@@ -50,6 +50,5 @@ namespace armarx::aron::type
// virtual implementations
std::string getShortName() const override;
std::string getFullName() const override;
- std::string getDefaultFromString() const override;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Position.cpp b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Position.cpp
index 8a169dbf223c7431bf08860e982d85200ee23b0f..68effb684d71d5745e02df03e4a396ac00cc2551 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Position.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Position.cpp
@@ -52,10 +52,5 @@ namespace armarx::aron::type
{
return "armarx::aron::type::Position";
}
-
- std::string Position::getDefaultFromString() const
- {
- return "[0, 0, 0]";
- }
}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Position.h b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Position.h
index 1ed5a040c521c942e56d412e6e5504d686b5120e..427f0f43a6f5c32dd7b7d16950cbc301de3f309a 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Position.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Position.h
@@ -50,6 +50,5 @@ namespace armarx::aron::type
// virtual implementations
std::string getShortName() const override;
std::string getFullName() const override;
- std::string getDefaultFromString() const override;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Quaternion.cpp b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Quaternion.cpp
index 4d439844fbefcdfda9567efc91eb5fdd651b85b8..5457ee84421cc570dda07881070f6f9249359b73 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Quaternion.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Quaternion.cpp
@@ -71,10 +71,5 @@ namespace armarx::aron::type
{
return "armarx::aron::type::Quaternion<" + Elementtype2String.at(this->aron->elementType) + ">";
}
-
- std::string Quaternion::getDefaultFromString() const
- {
- return "[0, 0, 0]";
- }
}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Quaternion.h b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Quaternion.h
index 77f181907a092db2959738608da2aae133a2f932..f660702be8c84613877d164763914bcf4cd57bc9 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Quaternion.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Quaternion.h
@@ -53,7 +53,6 @@ namespace armarx::aron::type
// virtual implementations
virtual std::string getShortName() const override;
virtual std::string getFullName() const override;
- std::string getDefaultFromString() const override;
static const std::map<quaternion::ElementType, std::string> Elementtype2String;
static const std::map<std::string, quaternion::ElementType> String2Elementtype;
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Bool.cpp b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Bool.cpp
index 4ab368a3472a5cadf07319d89ad23f55864dce0e..9eca559042cb6d38e575399b604e51f194f8aec7 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Bool.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Bool.cpp
@@ -57,9 +57,4 @@ namespace armarx::aron::type
{
return "armarx::aron::type::Bool";
}
-
- std::string Bool::getDefaultFromString() const
- {
- return "true|false";
- }
}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Bool.h b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Bool.h
index 104849a7146979ec9c54cadd98aafc8de7e3488f..eac4aeaea068387ba065a98376a64212a2fc2307 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Bool.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Bool.h
@@ -47,6 +47,5 @@ namespace armarx::aron::type
/* virtual implementations */
virtual std::string getShortName() const override;
virtual std::string getFullName() const override;
- std::string getDefaultFromString() const override;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Double.cpp b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Double.cpp
index 9e2f558b3cb913b0bc71e3b78d2f3c2bae074a97..256e0a61fa70bb46f1580553de206f16ef2fbddd 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Double.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Double.cpp
@@ -57,9 +57,4 @@ namespace armarx::aron::type
{
return "armarx::aron::type::Double";
}
-
- std::string Double::getDefaultFromString() const
- {
- return "0.0";
- }
}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Double.h b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Double.h
index 998a2cdf7a2063b94067416d002583a0f2937557..17aa98ad764ac8081cfcd2e23aad38f388749d18 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Double.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Double.h
@@ -47,6 +47,5 @@ namespace armarx::aron::type
/* virtual implementations */
virtual std::string getShortName() const override;
virtual std::string getFullName() const override;
- std::string getDefaultFromString() const override;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Float.cpp b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Float.cpp
index f98587a18166c76f70d70d081b0de7d2ce1db2d1..3d08b5bdaae4aa689761bb181aee1d688e0dd8a2 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Float.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Float.cpp
@@ -57,9 +57,4 @@ namespace armarx::aron::type
{
return "armarx::aron::type::Float";
}
-
- std::string Float::getDefaultFromString() const
- {
- return "0.0";
- }
}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Float.h b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Float.h
index 77ff6e4a39f234a369646463fa19f3c8d5324efa..2dedfb1739b6f000baddc8bb13306781a3d34a99 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Float.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Float.h
@@ -47,6 +47,5 @@ namespace armarx::aron::type
/* virtual implementations */
virtual std::string getShortName() const override;
virtual std::string getFullName() const override;
- std::string getDefaultFromString() const override;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Int.cpp b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Int.cpp
index 511512cfda190fa912898990e1d8849a25f025ef..b371f53ddf1f82275d334e0752367e01fec57c93 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Int.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Int.cpp
@@ -57,9 +57,4 @@ namespace armarx::aron::type
{
return "armarx::aron::type::Int";
}
-
- std::string Int::getDefaultFromString() const
- {
- return "0";
- }
}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Int.h b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Int.h
index 6a20a43a458c99649a6974aa4952ded887c27ebd..240cd4a50df8687f8cdba2ff15c040d21ec27fbe 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Int.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Int.h
@@ -47,6 +47,5 @@ namespace armarx::aron::type
/* virtual implementations */
virtual std::string getShortName() const override;
virtual std::string getFullName() const override;
- std::string getDefaultFromString() const override;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Long.cpp b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Long.cpp
index 9d945e717e5cc5bb155415694b370dafbab40273..2654a773d6f00f2552e7246007294baa161de1bf 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Long.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Long.cpp
@@ -57,9 +57,4 @@ namespace armarx::aron::type
{
return "armarx::aron::type::Long";
}
-
- std::string Long::getDefaultFromString() const
- {
- return "0";
- }
}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Long.h b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Long.h
index eca05833e02c6732bb63aa68eefd473604dd49a1..710df1a421e5ff684ac85b70839ff7cff513378e 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Long.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Long.h
@@ -47,6 +47,5 @@ namespace armarx::aron::type
/* virtual implementations */
virtual std::string getShortName() const override;
virtual std::string getFullName() const override;
- std::string getDefaultFromString() const override;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/primitive/String.cpp b/source/RobotAPI/libraries/aron/core/type/variant/primitive/String.cpp
index 1ac6a3c0bc490be4959d145b58d414aeef1f71f4..1cced6991fa157d23638241f1e012c2c0e7a4589 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/primitive/String.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/primitive/String.cpp
@@ -58,9 +58,4 @@ namespace armarx::aron::type
{
return "armarx::aron::type::String";
}
-
- std::string String::getDefaultFromString() const
- {
- return "";
- }
}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/primitive/String.h b/source/RobotAPI/libraries/aron/core/type/variant/primitive/String.h
index b5da234237a140b681e47445487a9f2ce46cdbb5..54dedab7b228d2262097d0cbe59675c4f721eb39 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/primitive/String.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/primitive/String.h
@@ -47,6 +47,5 @@ namespace armarx::aron::type
/* virtual implementations */
virtual std::string getShortName() const override;
virtual std::string getFullName() const override;
- std::string getDefaultFromString() const override;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Time.cpp b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Time.cpp
index 1a0e19d856c85ece58c792b31df8ef1a87c10ae6..76ba1e55c0c2d7d65c12760e6c66ef725ffbbd5c 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Time.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Time.cpp
@@ -58,9 +58,4 @@ namespace armarx::aron::type
{
return "armarx::aron::type::Time";
}
-
- std::string Time::getDefaultFromString() const
- {
- return "0";
- }
}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Time.h b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Time.h
index d460e792132d6cb759b4457c82f40cceb1ef6aa3..bd92b965d9c280979732d1f8d54389153a13573f 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Time.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Time.h
@@ -47,6 +47,5 @@ namespace armarx::aron::type
/* virtual implementations */
virtual std::string getShortName() const override;
virtual std::string getFullName() const override;
- std::string getDefaultFromString() const override;
};
}
diff --git a/source/RobotAPI/libraries/core/FramedPose.cpp b/source/RobotAPI/libraries/core/FramedPose.cpp
index 3399214dc4957654f5e8cdefa76a99bdee29640f..e84ad2fc735ae6341a573f54ad4103e6bbb1bb9a 100644
--- a/source/RobotAPI/libraries/core/FramedPose.cpp
+++ b/source/RobotAPI/libraries/core/FramedPose.cpp
@@ -26,6 +26,8 @@
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+#include <ArmarXCore/observers/AbstractObjectSerializer.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <VirtualRobot/Robot.h>
@@ -33,6 +35,10 @@
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/RobotConfig.h>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+
using namespace Eigen;
@@ -273,6 +279,31 @@ namespace armarx
}
}
+ Ice::ObjectPtr FramedDirection::ice_clone() const
+ {
+ return this->clone();
+ }
+
+ VariantDataClassPtr FramedDirection::clone(const Ice::Current& c) const
+ {
+ return new FramedDirection(*this);
+ }
+
+ VariantTypeId FramedDirection::getType(const Ice::Current& c) const
+ {
+ return VariantType::FramedDirection;
+ }
+
+ bool FramedDirection::validate(const Ice::Current& c)
+ {
+ return true;
+ }
+
+ std::ostream& operator<<(std::ostream& stream, const FramedDirection& rhs)
+ {
+ stream << "FramedDirection: " << std::endl << rhs.output() << std::endl;
+ return stream;
+ }
FramedPose::FramedPose() :
@@ -692,6 +723,51 @@ namespace armarx
}
}
+ FramedPosition::FramedPosition(const FramedPosition& other) :
+ Shared(other),
+ armarx::Serializable(other),
+ Vector3Base(other.x, other.y, other.z),
+ FramedPositionBase(other.x, other.y, other.z, other.frame, other.agent),
+ Vector3(other.x, other.y, other.z)
+ {
+ }
+
+ FramedPosition& FramedPosition::operator=(const FramedPosition& other)
+ {
+ x = other.x;
+ y = other.y;
+ z = other.z;
+ frame = other.frame;
+ agent = other.agent;
+ return *this;
+ }
+
+ Ice::ObjectPtr FramedPosition::ice_clone() const
+ {
+ return this->clone();
+ }
+
+ VariantDataClassPtr FramedPosition::clone(const Ice::Current& c) const
+ {
+ return new FramedPosition(*this);
+ }
+
+ VariantTypeId FramedPosition::getType(const Ice::Current& c) const
+ {
+ return VariantType::FramedPosition;
+ }
+
+ bool FramedPosition::validate(const Ice::Current& c)
+ {
+ return true;
+ }
+
+ std::ostream& operator<<(std::ostream& stream, const FramedPosition& rhs)
+ {
+ stream << "FramedPosition: " << std::endl << rhs.output() << std::endl;
+ return stream;
+ }
+
FramedOrientation::FramedOrientation() :
Quaternion()
@@ -890,6 +966,32 @@ namespace armarx
}
}
+ Ice::ObjectPtr FramedOrientation::ice_clone() const
+ {
+ return this->clone();
+ }
+
+ VariantDataClassPtr FramedOrientation::clone(const Ice::Current& c) const
+ {
+ return new FramedOrientation(*this);
+ }
+
+ VariantTypeId FramedOrientation::getType(const Ice::Current& c) const
+ {
+ return VariantType::FramedOrientation;
+ }
+
+ bool FramedOrientation::validate(const Ice::Current& c)
+ {
+ return true;
+ }
+
+ std::ostream& operator<<(std::ostream& stream, const FramedOrientation& rhs)
+ {
+ stream << "FramedOrientation: " << std::endl << rhs.output() << std::endl;
+ return stream;
+ }
+
VirtualRobot::LinkedCoordinate FramedPose::createLinkedCoordinate(const VirtualRobot::RobotPtr& virtualRobot, const FramedPositionPtr& position, const FramedOrientationPtr& orientation)
{
@@ -963,5 +1065,31 @@ namespace armarx
return toFrame(referenceRobot, newFrame)->toEigen();
}
+ Ice::ObjectPtr FramedPose::ice_clone() const
+ {
+ return this->clone();
+ }
+
+ VariantDataClassPtr FramedPose::clone(const Ice::Current& c) const
+ {
+ return new FramedPose(*this);
+ }
+
+ VariantTypeId FramedPose::getType(const Ice::Current& c) const
+ {
+ return VariantType::FramedPose;
+ }
+
+ bool FramedPose::validate(const Ice::Current& c)
+ {
+ return true;
+ }
+
+ std::ostream& operator<<(std::ostream& stream, const FramedPose& rhs)
+ {
+ stream << "FramedPose: " << std::endl << rhs.output() << std::endl;
+ return stream;
+ }
+
}
diff --git a/source/RobotAPI/libraries/core/FramedPose.h b/source/RobotAPI/libraries/core/FramedPose.h
index 27a04fccb69d3b8bf89d91a4e33c45d0d0b36885..e0ca6fdaa8c6aa51880ade434d232929f15d769e 100644
--- a/source/RobotAPI/libraries/core/FramedPose.h
+++ b/source/RobotAPI/libraries/core/FramedPose.h
@@ -25,21 +25,11 @@
#pragma once
#include "Pose.h"
+#include <VirtualRobot/VirtualRobot.h>
#include <RobotAPI/interface/core/FramedPoseBase.h>
-#include <RobotAPI/interface/core/RobotState.h>
-
-#include <ArmarXCore/observers/variant/Variant.h>
-#include <ArmarXCore/observers/AbstractObjectSerializer.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include <VirtualRobot/VirtualRobot.h>
-#include <VirtualRobot/LinkedCoordinate.h>
-#include <Eigen/Core>
-#include <Eigen/Geometry>
-#include <sstream>
namespace armarx::VariantType
{
@@ -50,8 +40,21 @@ namespace armarx::VariantType
const VariantTypeId FramedOrientation = Variant::addTypeName("::armarx::FramedOrientationBase");
}
+namespace VirtualRobot
+{
+ class LinkedCoordinate;
+}
+namespace IceProxy::armarx
+{
+ class SharedRobotInterface;
+}
+namespace Eigen
+{
+ using Matrix6f = Matrix<Ice::Float, 6, 6>;
+}
namespace armarx
{
+ using SharedRobotInterfacePrx = ::IceInternal::ProxyHandle< ::IceProxy::armarx::SharedRobotInterface> ;
/**
* @ingroup RobotAPI-FramedPose
* Variable of the global coordinate system. use this if you are specifying a global pose.
@@ -104,29 +107,13 @@ namespace armarx
Eigen::Vector3f toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const;
// inherited from VariantDataClass
- Ice::ObjectPtr ice_clone() const override
- {
- return this->clone();
- }
- VariantDataClassPtr clone(const Ice::Current& c = Ice::emptyCurrent) const override
- {
- return new FramedDirection(*this);
- }
+ Ice::ObjectPtr ice_clone() const override;
+ VariantDataClassPtr clone(const Ice::Current& c = Ice::emptyCurrent) const override;
std::string output(const Ice::Current& c = Ice::emptyCurrent) const override;
- VariantTypeId getType(const Ice::Current& c = Ice::emptyCurrent) const override
- {
- return VariantType::FramedDirection;
- }
- bool validate(const Ice::Current& c = Ice::emptyCurrent) override
- {
- return true;
- }
-
- friend std::ostream& operator<<(std::ostream& stream, const FramedDirection& rhs)
- {
- stream << "FramedDirection: " << std::endl << rhs.output() << std::endl;
- return stream;
- }
+ VariantTypeId getType(const Ice::Current& c = Ice::emptyCurrent) const override;
+ bool validate(const Ice::Current& c = Ice::emptyCurrent) override;
+
+ friend std::ostream& operator<<(std::ostream& stream, const FramedDirection& rhs);
public: // serialization
void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) const override;
@@ -154,24 +141,9 @@ namespace armarx
FramedPosition(const Eigen::Vector3f&, const std::string& frame, const std::string& agent);
FramedPosition(const Eigen::Matrix4f&, const std::string& frame, const std::string& agent);
//FramedPosition(const Vector3BasePtr pos, const std::string &frame ); // this doesnt work for unknown reasons
- FramedPosition(const FramedPosition& other):
- Shared(other),
- armarx::Serializable(other),
- Vector3Base(other.x, other.y, other.z),
- FramedPositionBase(other.x, other.y, other.z, other.frame, other.agent),
- Vector3(other.x, other.y, other.z)
- {
- }
-
- FramedPosition& operator=(const armarx::FramedPosition& other)
- {
- x = other.x;
- y = other.y;
- z = other.z;
- frame = other.frame;
- agent = other.agent;
- return *this;
- }
+ FramedPosition(const FramedPosition& other);
+
+ FramedPosition& operator=(const armarx::FramedPosition& other);
std::string getFrame() const;
@@ -189,29 +161,13 @@ namespace armarx
Eigen::Vector3f toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const;
// inherited from VariantDataClass
- Ice::ObjectPtr ice_clone() const override
- {
- return this->clone();
- }
- VariantDataClassPtr clone(const Ice::Current& c = Ice::emptyCurrent) const override
- {
- return new FramedPosition(*this);
- }
+ Ice::ObjectPtr ice_clone() const override;
+ VariantDataClassPtr clone(const Ice::Current& c = Ice::emptyCurrent) const override;
std::string output(const Ice::Current& c = Ice::emptyCurrent) const override;
- VariantTypeId getType(const Ice::Current& c = Ice::emptyCurrent) const override
- {
- return VariantType::FramedPosition;
- }
- bool validate(const Ice::Current& c = Ice::emptyCurrent) override
- {
- return true;
- }
-
- friend std::ostream& operator<<(std::ostream& stream, const FramedPosition& rhs)
- {
- stream << "FramedPosition: " << std::endl << rhs.output() << std::endl;
- return stream;
- };
+ VariantTypeId getType(const Ice::Current& c = Ice::emptyCurrent) const override;
+ bool validate(const Ice::Current& c = Ice::emptyCurrent) override;
+
+ friend std::ostream& operator<<(std::ostream& stream, const FramedPosition& rhs);
public: // serialization
void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) const override;
@@ -243,23 +199,11 @@ namespace armarx
std::string getFrame()const ;
// inherited from VariantDataClass
- Ice::ObjectPtr ice_clone() const override
- {
- return this->clone();
- }
- VariantDataClassPtr clone(const Ice::Current& c = Ice::emptyCurrent) const override
- {
- return new FramedOrientation(*this);
- }
+ Ice::ObjectPtr ice_clone() const override;
+ VariantDataClassPtr clone(const Ice::Current& c = Ice::emptyCurrent) const override;
std::string output(const Ice::Current& c = Ice::emptyCurrent) const override;
- VariantTypeId getType(const Ice::Current& c = Ice::emptyCurrent) const override
- {
- return VariantType::FramedOrientation;
- }
- bool validate(const Ice::Current& c = Ice::emptyCurrent) override
- {
- return true;
- }
+ VariantTypeId getType(const Ice::Current& c = Ice::emptyCurrent) const override;
+ bool validate(const Ice::Current& c = Ice::emptyCurrent) override;
void changeFrame(const SharedRobotInterfacePrx& referenceRobot, const std::string& newFrame);
void changeFrame(const VirtualRobot::RobotPtr& referenceRobot, const std::string& newFrame);
@@ -274,11 +218,7 @@ namespace armarx
Eigen::Matrix3f toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const;
Eigen::Matrix3f toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const;
- friend std::ostream& operator<<(std::ostream& stream, const FramedOrientation& rhs)
- {
- stream << "FramedOrientation: " << std::endl << rhs.output() << std::endl;
- return stream;
- };
+ friend std::ostream& operator<<(std::ostream& stream, const FramedOrientation& rhs);
public: // serialization
void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) const override;
@@ -312,27 +252,15 @@ namespace armarx
std::string getFrame() const;
// inherited from VariantDataClass
- Ice::ObjectPtr ice_clone() const override
- {
- return this->clone();
- }
+ Ice::ObjectPtr ice_clone() const override;
- VariantDataClassPtr clone(const Ice::Current& c = Ice::emptyCurrent) const override
- {
- return new FramedPose(*this);
- }
+ VariantDataClassPtr clone(const Ice::Current& c = Ice::emptyCurrent) const override;
std::string output(const Ice::Current& c = Ice::emptyCurrent) const override;
- VariantTypeId getType(const Ice::Current& c = Ice::emptyCurrent) const override
- {
- return VariantType::FramedPose;
- }
+ VariantTypeId getType(const Ice::Current& c = Ice::emptyCurrent) const override;
- bool validate(const Ice::Current& c = Ice::emptyCurrent) override
- {
- return true;
- }
+ bool validate(const Ice::Current& c = Ice::emptyCurrent) override;
void changeFrame(const SharedRobotInterfacePrx& referenceRobot, const std::string& newFrame);
void changeFrame(const VirtualRobot::RobotPtr& referenceRobot, const std::string& newFrame);
@@ -351,11 +279,7 @@ namespace armarx
Eigen::Matrix4f toFrameEigen(const SharedRobotInterfacePrx& referenceRobot, const std::string& newFrame) const;
Eigen::Matrix4f toFrameEigen(const VirtualRobot::RobotPtr& referenceRobot, const std::string& newFrame) const;
- friend std::ostream& operator<<(std::ostream& stream, const FramedPose& rhs)
- {
- stream << "FramedPose: " << std::endl << rhs.output() << std::endl;
- return stream;
- }
+ friend std::ostream& operator<<(std::ostream& stream, const FramedPose& rhs);
FramedPositionPtr getPosition() const;
FramedOrientationPtr getOrientation() const;
diff --git a/source/RobotAPI/libraries/core/LinkedPose.cpp b/source/RobotAPI/libraries/core/LinkedPose.cpp
index 4a4d285d97a3001b2d5d3eb1ab504604809806c6..1f4a2b6fe5f66815fb87eb53d60bcf876e6a39ed 100644
--- a/source/RobotAPI/libraries/core/LinkedPose.cpp
+++ b/source/RobotAPI/libraries/core/LinkedPose.cpp
@@ -26,6 +26,7 @@
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <Eigen/Geometry>
#include <Eigen/Core>
diff --git a/source/RobotAPI/libraries/core/Pose.cpp b/source/RobotAPI/libraries/core/Pose.cpp
index 39968afc088d5ed82cad8461cfd89cb96a75e321..f837f87117f5c8934ab5430af9491277010cb4e9 100644
--- a/source/RobotAPI/libraries/core/Pose.cpp
+++ b/source/RobotAPI/libraries/core/Pose.cpp
@@ -28,7 +28,12 @@
#include <VirtualRobot/LinkedCoordinate.h>
#include <VirtualRobot/LinkedCoordinate.h>
#include <VirtualRobot/VirtualRobot.h>
+#include <ArmarXCore/observers/AbstractObjectSerializer.h>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <sstream>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
template class ::IceInternal::Handle<::armarx::Pose>;
diff --git a/source/RobotAPI/libraries/core/Pose.h b/source/RobotAPI/libraries/core/Pose.h
index 16b70957d4a336a57d0eeab072525c2dce08effc..268b6ba846022f0586fbdfafb5cf8d2999665c55 100644
--- a/source/RobotAPI/libraries/core/Pose.h
+++ b/source/RobotAPI/libraries/core/Pose.h
@@ -27,13 +27,10 @@
#include <RobotAPI/interface/core/PoseBase.h>
#include <ArmarXCore/observers/variant/Variant.h>
-#include <ArmarXCore/observers/AbstractObjectSerializer.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
-#include <sstream>
-
namespace armarx::VariantType
{
// variant types
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
index 60577ef2dfb7b01eaf648a1a86008f7a969061dd..4f0f563e7960efbb4cc8055f2c198803105b841e 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
@@ -34,6 +34,7 @@
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <ArmarXCore/core/logging/Logging.h>
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobotNode.cpp b/source/RobotAPI/libraries/core/remoterobot/RemoteRobotNode.cpp
index e6cc65c23c13e154382042424db8483d879a89b4..28bb76573e925f3f6135ba0f3d75be2aa060d9f4 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobotNode.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobotNode.cpp
@@ -27,7 +27,7 @@
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/interface/core/BasicTypes.h>
-#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <VirtualRobot/VirtualRobot.h>
diff --git a/source/RobotAPI/libraries/skills/provider/Skill.cpp b/source/RobotAPI/libraries/skills/provider/Skill.cpp
index 5cac4402ff144b92bde79a2464a50f7b48e8b4ad..fa68245928b32e12f1f948a786aa8b8af2b9f983 100644
--- a/source/RobotAPI/libraries/skills/provider/Skill.cpp
+++ b/source/RobotAPI/libraries/skills/provider/Skill.cpp
@@ -4,34 +4,85 @@ namespace armarx
{
namespace skills
{
- Skill::Skill()
+ Skill::Skill(const SkillDescription& desc):
+ description(desc)
{
// replace constructor if you want to have a specific logging tag
- Logging::setTag("armarx::skills::Skill");
+ Logging::setTag("armarx::skills::" + description.skillName);
}
- Skill::Status Skill::execute(const aron::data::DictPtr& params, const CallbackT& callback)
+ std::thread Skill::installTimeoutCondition()
+ {
+ // start timeout check
+ return std::thread{ [&](){
+ if (description.timeoutMs <= 0) return;
+ long skillStartedAt = IceUtil::Time::now().toMilliSeconds();
+ while(running)
+ {
+ auto now = IceUtil::Time::now().toMilliSeconds();
+ if ((now - skillStartedAt) >= description.timeoutMs) notifyTimeoutReached();
+ std::this_thread::sleep_for(std::chrono::milliseconds(100));
+ }
+ }
+ };
+ }
+
+ void Skill::notifyTimeoutReached()
+ {
+ timeoutReached = true;
+ _notifyTimeoutReached();
+ }
+
+ void Skill::notifyStopped()
+ {
+ stopped = true;
+ _notifyStopped();
+ }
+
+ void Skill::_notifyTimeoutReached()
+ {
+
+ }
+
+ void Skill::_notifyStopped()
{
- (void) params;
- ARMARX_WARNING_S << "You have to override this method!";
- return Status::Succeeded;
}
void Skill::reset()
{
stopped = false;
timeoutReached = false;
+
+ _reset();
}
- void Skill::notifyStopped()
+ void Skill::_reset()
{
- stopped = true;
+
}
- void Skill::notifyTimeoutReached()
+ Skill::Status Skill::execute(const aron::data::DictPtr& params, const CallbackT& callback)
{
- timeoutReached = true;
+ running = true;
+ started = IceUtil::Time::now().toMilliSeconds();
+
+ auto timeoutCheck = installTimeoutCondition();
+
+ auto ret = _execute(params, callback);
+
+ started = 0;
+ running = false;
+ timeoutCheck.join(); // safely wait for the timeoutcheck to return
+ return ret;
+ }
+
+ Skill::Status Skill::_execute(const aron::data::DictPtr& params, const CallbackT& callback)
+ {
+ (void) params;
+
+ ARMARX_WARNING_S << "You have to override this method!";
+ return Status::Succeeded;
}
}
diff --git a/source/RobotAPI/libraries/skills/provider/Skill.h b/source/RobotAPI/libraries/skills/provider/Skill.h
index 27a2c6089bdab93ad11a77fbcb8c9c25a50eebdf..a4bf8ba687d332e2598c728e7f77be053d93e884 100644
--- a/source/RobotAPI/libraries/skills/provider/Skill.h
+++ b/source/RobotAPI/libraries/skills/provider/Skill.h
@@ -13,6 +13,8 @@
#include <RobotAPI/interface/skills/SkillManagerInterface.h>
#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+#include "SkillDescription.h"
+
namespace armarx
{
namespace skills
@@ -26,22 +28,45 @@ namespace armarx
{
Succeeded,
TimeoutReached,
+ Stopped,
Failed
};
- Skill();
+ Skill() = delete;
+ Skill(const SkillDescription&);
virtual ~Skill() = default;
- virtual void notifyStopped();
- virtual void notifyTimeoutReached();
+ /// Execute Skill
+ Status execute(const aron::data::DictPtr& params, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; });
+
+ /// Reset all parameters before starting a skill.
+ void reset();
+
+ /// Set the notification bools
+ void notifyTimeoutReached();
+ void notifyStopped();
/// Override this method with the actual implementation. The callback is for status updates to the calling instance
- virtual Status execute(const aron::data::DictPtr& params, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; });
+ virtual Status _execute(const aron::data::DictPtr& params, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; });
+
+ /// Override if you have special members that needs to be resetted
+ virtual void _reset();
- /// Reset all parameters before starting a skill. Override if you have special members that needs to be resetted
- virtual void reset();
+ /// Override these methods if you want to do something special when notification comes
+ virtual void _notifyTimeoutReached();
+ virtual void _notifyStopped();
+
+ private:
+ /// install a condition as a seperate thread
+ std::thread installTimeoutCondition();
+
+ public:
+ const SkillDescription description;
+ std::atomic_bool running = false;
+ long started = 0;
protected:
+ /// Use conditions during runtime this way
std::atomic_bool stopped = false;
std::atomic_bool timeoutReached = false;
};
diff --git a/source/RobotAPI/libraries/skills/provider/SkillDescription.cpp b/source/RobotAPI/libraries/skills/provider/SkillDescription.cpp
index 844249f0e46f5075d044ed0bb4bb8cffc75b0846..5295057bec7d1ace202d9ac1245306542848d6f9 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillDescription.cpp
+++ b/source/RobotAPI/libraries/skills/provider/SkillDescription.cpp
@@ -7,10 +7,7 @@ namespace armarx
provider::dto::SkillDescription SkillDescription::toIce() const
{
provider::dto::SkillDescription ret;
- if (acceptedType)
- {
- ret.acceptedType = acceptedType->toObjectDTO();
- }
+ ret.acceptedType = aron::type::Object::ToAronObjectDTO(acceptedType);
ret.description = description;
ret.skillName = skillName;
ret.targets = targets;
diff --git a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
index 30bbcc86a9764465dedaa63b314aa3da4ae8c451..c471f140cc312952e960a922ea26be50996eb260 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
@@ -52,35 +52,40 @@ namespace armarx
addPlugin(plugin);
}
- void SkillProviderComponentPluginUser::addSkill(const std::shared_ptr<skills::Skill>& skill, const skills::SkillDescription& desc)
+ void SkillProviderComponentPluginUser::addSkill(const std::shared_ptr<skills::Skill>& skill)
{
+ if (!skill)
+ {
+ return;
+ }
+
std::lock_guard l(skillsMutex);
- std::string skillName = desc.skillName;
+ std::string skillName = skill->description.skillName;
if (skillImplementations.find(skillName) != skillImplementations.end())
{
- ARMARX_WARNING << "Try to add a skill '" + skillName + "' which already exists in list. Ignore this skill.";
+ ARMARX_WARNING << "Try to add a skill '" + skillName + "' which already exists in list. Ignoring this skill.";
return;
}
- ARMARX_DEBUG << "Adding skill " << skillName << " to list";
- skills::detail::SkillImplementationWrapper s(desc, skill);
+ ARMARX_INFO << "Adding skill " << skillName;
+ skills::detail::SkillImplementationWrapper s(skill);
skillImplementations.insert({skillName, s});
}
void SkillProviderComponentPluginUser::addSkill(const skills::helper::LambdaSkill::FunT& f, const skills::SkillDescription& desc)
{
- auto lambda = std::make_shared<skills::helper::LambdaSkill>(f);
- addSkill(lambda, desc);
+ auto lambda = std::make_shared<skills::helper::LambdaSkill>(f, desc);
+ addSkill(lambda);
}
skills::provider::dto::SkillDescriptionMap SkillProviderComponentPluginUser::getSkills(const Ice::Current ¤t)
{
std::lock_guard l(skillsMutex);
skills::provider::dto::SkillDescriptionMap skillDesciptions;
- for (const auto& [key, skillImplementation] : skillImplementations)
+ for (const auto& [key, skillWrapper] : skillImplementations)
{
- skillDesciptions.insert({key, skillImplementation.description.toIce()});
+ skillDesciptions.insert({key, skillWrapper.skill->description.toIce()});
}
return skillDesciptions;
}
@@ -88,10 +93,10 @@ namespace armarx
skills::provider::dto::SkillStatusUpdate SkillProviderComponentPluginUser::getSkillExecutionStatus(const std::string& skill, const Ice::Current ¤t)
{
std::lock_guard l(skillsMutex);
- auto& skillImplementation = skillImplementations.at(skill);
+ auto& skillWrapper = skillImplementations.at(skill);
- std::lock_guard l2(skillImplementation.statusInfo.skillStatusMutex);
- return skillImplementation.statusInfo.statusUpdate.toIce();
+ std::lock_guard l2(skillWrapper.skillStatusMutex);
+ return skillWrapper.statusUpdate.toIce();
}
void SkillProviderComponentPluginUser::executeSkill(const skills::provider::dto::SkillExecutionInfo& info, const Ice::Current ¤t)
@@ -100,38 +105,21 @@ namespace armarx
std::string skillName = info.skillName;
ARMARX_CHECK_EXPRESSION(skillImplementations.count(skillName) > 0);
- auto& impl = skillImplementations.at(skillName);
-
- if (impl.task.joinable())
+ auto& wrapper = skillImplementations.at(skillName);
+ if (wrapper.task.joinable())
{
- impl.task.join();
- }
- if (impl.timeoutCheck.joinable())
- {
- impl.timeoutCheck.join();
- }
- if (impl.stoppedCheck.joinable())
- {
- impl.stoppedCheck.join();
+ wrapper.task.join();
}
// update input params
- impl.reset();
- impl.statusInfo.statusUpdate.usedCallbackInterface = info.callbackInterface;
- impl.statusInfo.statusUpdate.skillName = info.skillName;
- impl.statusInfo.statusUpdate.providerName = getName();
- if (info.params)
- {
- impl.statusInfo.statusUpdate.usedParameterization = std::make_shared<aron::data::Dict>(info.params);
- }
-
- // make sure thread is started
- impl.statusInfo.started = true;
-
- // recreate thread and execute skill
- impl.timeoutCheck = std::thread{ [&](){ impl.constantlyCheckForTimeoutReached();}};
- impl.stoppedCheck = std::thread{ [&](){ impl.constantlyCheckForStopped();}};
- impl.task = std::thread{ [&] { impl.execute();}};
+ wrapper.reset();
+ wrapper.statusUpdate.usedCallbackInterface = info.callbackInterface;
+ wrapper.statusUpdate.skillName = info.skillName;
+ wrapper.statusUpdate.providerName = getName();
+ wrapper.statusUpdate.usedParameterization = aron::data::Dict::FromAronDictDTO(info.params);
+
+ // recreate thread and execute skill.
+ wrapper.task = std::thread{ [&] { wrapper.execute();}};
}
skills::provider::dto::SkillStatusUpdate SkillProviderComponentPluginUser::abortSkill(const std::string& skillName, const Ice::Current ¤t)
@@ -139,33 +127,16 @@ namespace armarx
std::lock_guard l(skillsMutex);
ARMARX_CHECK_EXPRESSION(skillImplementations.count(skillName) > 0);
- auto& impl = skillImplementations.at(skillName);
- auto& statusInfo = impl.statusInfo;
+ auto& wrapper = skillImplementations.at(skillName);
- std::lock_guard l2(statusInfo.skillStatusMutex);
- auto& statusUpdate = statusInfo.statusUpdate;
-
- impl.statusInfo.stopped = true;
- impl.statusInfo.started = false;
- if (impl.task.joinable())
- {
- impl.task.join();
- }
- if (impl.timeoutCheck.joinable())
- {
- impl.timeoutCheck.join();
- }
- if (impl.stoppedCheck.joinable())
- {
- impl.stoppedCheck.join();
- }
+ std::lock_guard l2(wrapper.skillStatusMutex);
- if (statusUpdate.status != skills::provider::dto::Execution::Status::Succeeded &&
- statusUpdate.status != skills::provider::dto::Execution::Status::Failed)
+ if (wrapper.skill->running)
{
- impl.statusInfo.statusUpdate.status = skills::provider::dto::Execution::Status::Aborted;
+ wrapper.skill->notifyStopped();
+ wrapper.task.join();
}
- return statusUpdate.toIce();
+ return wrapper.statusUpdate.toIce();
}
}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
index 14b3ce55ec587254aa8d2627231266453ac9d4f3..7e370346cbd8d49f4ec2ac96e638ab85ea23fcea 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
+++ b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
@@ -54,7 +54,7 @@ namespace armarx
protected:
void addSkill(const skills::helper::LambdaSkill::FunT&, const skills::SkillDescription&);
- void addSkill(const std::shared_ptr<skills::Skill>&, const skills::SkillDescription&);
+ void addSkill(const std::shared_ptr<skills::Skill>&);
private:
armarx::plugins::SkillProviderComponentPlugin* plugin = nullptr;
diff --git a/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.cpp b/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.cpp
index f19d037dc3c24993f2a87d651121e2855128041e..cfc9f6fd218acc22e618e5ab264078af053f71da 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.cpp
+++ b/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.cpp
@@ -9,13 +9,10 @@ namespace armarx
provider::dto::SkillStatusUpdate ret;
ret.providerName = providerName;
ret.skillName = skillName;
- if (data)
- {
- ret.data = data->toAronDictPtr();
- }
+ ret.data = aron::data::Dict::ToAronDictDTO(data);
ret.status = status;
ret.usedCallbackInterface = usedCallbackInterface;
- ret.usedParams = usedParameterization->toAronDictPtr();
+ ret.usedParams = aron::data::Dict::ToAronDictDTO(usedParameterization);
return ret;
}
}
diff --git a/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h b/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h
index e3606ef526d18f1aa326c6ca60009b0b47de691d..a1b617a49700fc57f97e904c79de2882d3b6b690 100644
--- a/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h
+++ b/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h
@@ -2,6 +2,11 @@
#include "Skill.h"
+#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
+
+// Debug
+#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
+
namespace armarx
{
namespace skills
@@ -13,8 +18,14 @@ namespace armarx
using Skill::Skill;
virtual ~SpecializedSkill() = default;
+ /// returns the accepted type of the skill
+ static armarx::aron::type::ObjectPtr GetAcceptedType()
+ {
+ return AronT::toAronType();
+ }
+
/// Override this method with the actual implementation. The callback is for status updates to the calling instance
- virtual Status execute(const AronT& params, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; })
+ virtual Status _execute(const AronT& params, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; })
{
(void) params;
@@ -23,12 +34,13 @@ namespace armarx
}
/// Do not use anymore
- Status execute(const aron::data::DictPtr& params, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; }) final
+ Status _execute(const aron::data::DictPtr& params, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; }) final
{
AronT p;
+ //ARMARX_IMPORTANT << aron::converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(params).dump(2);
p.fromAron(params);
- return execute(p, callback);
+ return _execute(p, callback);
}
};
}
diff --git a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
index 4160489c075261afaf522f8c9b311ea349f7ebb6..9f9c573b80df17e055baa37737f3f1dc86e4997a 100644
--- a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
+++ b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
@@ -4,126 +4,96 @@ namespace armarx
{
namespace skills::detail
{
- void SkillImplementationWrapper::constantlyCheckForTimeoutReached() const
- {
- while(statusInfo.started)
- {
- if (isTimeoutReached())
- {
- skill->notifyTimeoutReached();
- break; // only notify once?
- }
- }
- std::this_thread::sleep_for(std::chrono::milliseconds(100));
- }
-
- void SkillImplementationWrapper::constantlyCheckForStopped() const
- {
- while(statusInfo.started)
- {
- if (statusInfo.stopped)
- {
- skill->notifyStopped();
- break; // only notify once?
- }
- }
- std::this_thread::sleep_for(std::chrono::milliseconds(100));
- }
-
void SkillImplementationWrapper::reset()
{
- std::lock_guard l(statusInfo.skillStatusMutex);
- statusInfo.started = false;
- statusInfo.stopped = false;
- statusInfo.statusUpdate.status = skills::provider::dto::Execution::Status::Idle;
- statusInfo.statusUpdate.data = nullptr;
- skillStarted = IceUtil::Time::now().toMilliSeconds();
+ ARMARX_CHECK_NOT_NULL(skill);
+ std::lock_guard l(skillStatusMutex);
+ statusUpdate.status = skills::provider::dto::Execution::Status::Idle;
+ statusUpdate.data = nullptr;
skill->reset();
}
void SkillImplementationWrapper::execute()
{
- ARMARX_INFO_S << "Executing skill: " << description.skillName;
- statusInfo.started = true;
+ ARMARX_CHECK_NOT_NULL(skill);
+ std::lock_guard l(executingMutex);
+
+ const std::string skillName = skill->description.skillName;
+ ARMARX_INFO_S << "Executing skill: " << skillName;
// get params and setup variables
- auto& aron_params = statusInfo.statusUpdate.usedParameterization;
- if (not(aron_params) && description.acceptedType)
- {
- throw armarx::LocalException("The Skill '" + description.skillName + "' requires a type but no params are NULL.");
- }
+ auto& aron_params = statusUpdate.usedParameterization;
try
{
- // set scheduled
+ // Check params
+ if (not(aron_params) && skill->description.acceptedType)
{
- std::lock_guard l(statusInfo.skillStatusMutex);
- statusInfo.statusUpdate.status = skills::provider::dto::Execution::Status::Scheduled;
-
- // do callback
- updateStatusCallback();
+ throw armarx::LocalException("The Skill '" + skillName + "' requires a type but no params are NULL.");
}
-
- // execute
+ // set scheduled
{
- std::lock_guard l(statusInfo.skillStatusMutex);
- statusInfo.statusUpdate.status = skills::provider::dto::Execution::Status::Running;
- updateStatusCallback();
+ std::lock_guard l(skillStatusMutex);
+ statusUpdate.status = skills::provider::dto::Execution::Status::Scheduled;
+ updateStatus();
}
- auto result = skill->execute(aron_params, [&](const aron::data::DictPtr& update)
- {
- statusInfo.statusUpdate.data = update;
- updateStatusCallback();
- }
- );
- if (result == skills::Skill::Status::Failed)
+ // execute
{
- throw armarx::LocalException("The Skill '"+description.skillName+"' failed during execution.");
+ std::lock_guard l(skillStatusMutex);
+ statusUpdate.status = skills::provider::dto::Execution::Status::Running;
+ updateStatus();
}
- if (result == skills::Skill::Status::TimeoutReached)
+
+ auto execution_callback = [&](const aron::data::DictPtr& update)
{
- throw armarx::LocalException("The Skill '"+description.skillName+"' reached timeout during execution.");
- }
+ statusUpdate.data = update;
+ updateStatus();
+ };
+ auto result = skill->execute(aron_params, execution_callback);
+ switch (result)
{
- std::lock_guard l(statusInfo.skillStatusMutex);
- statusInfo.statusUpdate.status = skills::provider::dto::Execution::Status::Succeeded;
- updateStatusCallback();
+ case skills::Skill::Status::Failed:
+ throw armarx::LocalException("The Skill '" + skillName + "' failed during execution.");
+ break;
+ case skills::Skill::Status::TimeoutReached:
+ throw armarx::LocalException("The Skill '" + skillName + "' reached timeout during execution.");
+ break;
+ case skills::Skill::Status::Stopped:
+ {
+ std::lock_guard l(skillStatusMutex);
+ statusUpdate.status = skills::provider::dto::Execution::Status::Aborted;
+ updateStatus();
+ break;
+ }
+ default:
+ {
+ std::lock_guard l(skillStatusMutex);
+ statusUpdate.status = skills::provider::dto::Execution::Status::Succeeded;
+ updateStatus();
+ }
}
}
catch (const std::exception& ex)
{
- ARMARX_WARNING_S << "Skill " << description.skillName << " died with exception:\n" << ex.what();
-
- std::lock_guard l(statusInfo.skillStatusMutex);
- statusInfo.statusUpdate.status = skills::provider::dto::Execution::Status::Failed;
- updateStatusCallback();
- }
-
- statusInfo.started = false;
- }
+ ARMARX_WARNING_S << "Skill " << skillName << " died with exception:\n" << ex.what();
- bool SkillImplementationWrapper::isTimeoutReached() const
- {
- if (description.timeoutMs < 0)
- {
- return false;
+ std::lock_guard l(skillStatusMutex);
+ statusUpdate.status = skills::provider::dto::Execution::Status::Failed;
+ updateStatus();
}
-
- auto now = IceUtil::Time::now().toMilliSeconds();
- return (now - skillStarted) >= description.timeoutMs;
}
- void SkillImplementationWrapper::updateStatusCallback() const
+ void SkillImplementationWrapper::updateStatus() const
{
- auto& callbackInterface = statusInfo.statusUpdate.usedCallbackInterface;
+ auto& callbackInterface = statusUpdate.usedCallbackInterface;
if (callbackInterface)
{
- callbackInterface->updateStatusForSkill(statusInfo.statusUpdate.toIce());
+ callbackInterface->updateStatusForSkill(statusUpdate.toIce());
}
}
}
diff --git a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h
index 24a8637b44f634734e8ffba20809ecadf9a493ce..35910a968a66026b5eff1399b133484adef1437a 100644
--- a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h
+++ b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h
@@ -16,47 +16,32 @@ namespace armarx
{
public:
// fixed values. Do not change after skill instantiation
- const SkillDescription description;
const std::shared_ptr<Skill> skill;
// Current execution status. Changes during execution
// The status also holds the used parameterization
- struct StatusInfo
- {
- mutable std::mutex skillStatusMutex;
- std::atomic_bool started;
- std::atomic_bool stopped;
- SkillStatusUpdate statusUpdate;
- } statusInfo;
+ mutable std::mutex skillStatusMutex;
+ SkillStatusUpdate statusUpdate;
// Task information. task is recreated every time the skill restarts
+ mutable std::mutex executingMutex;
std::thread task;
- std::thread timeoutCheck;
- std::thread stoppedCheck;
- SkillImplementationWrapper(const skills::SkillDescription& desc, const std::shared_ptr<skills::Skill> skill) :
- description(desc), skill(skill)
+ SkillImplementationWrapper(const std::shared_ptr<skills::Skill> skill) :
+ skill(skill)
{
reset();
}
SkillImplementationWrapper(const SkillImplementationWrapper& s) :
- SkillImplementationWrapper(s.description, s.skill)
+ SkillImplementationWrapper(s.skill)
{}
void execute();
void reset();
- // checks for interrupts
- void constantlyCheckForTimeoutReached() const;
- void constantlyCheckForStopped() const;
-
protected:
- bool isTimeoutReached() const;
- void updateStatusCallback() const;
-
- private:
- long skillStarted = 0;
+ void updateStatus() const;
};
}
}
diff --git a/source/RobotAPI/libraries/skills/provider/helper/LambdaSkillImplementation.cpp b/source/RobotAPI/libraries/skills/provider/helper/LambdaSkillImplementation.cpp
index 9b761b403f51552235b690d601137ff1c11a002b..59d51cd16b66a4799f25e0503d2c987bea7ab4cc 100644
--- a/source/RobotAPI/libraries/skills/provider/helper/LambdaSkillImplementation.cpp
+++ b/source/RobotAPI/libraries/skills/provider/helper/LambdaSkillImplementation.cpp
@@ -5,7 +5,7 @@ namespace armarx
namespace skills::helper
{
- Skill::Status LambdaSkill::execute(const aron::data::DictPtr& data, const CallbackT& callback)
+ Skill::Status LambdaSkill::_execute(const aron::data::DictPtr& data, const CallbackT& callback)
{
(void) callback;
bool res = fun(data);
diff --git a/source/RobotAPI/libraries/skills/provider/helper/LambdaSkillImplementation.h b/source/RobotAPI/libraries/skills/provider/helper/LambdaSkillImplementation.h
index bb422b1bda029935f54ac94bd7cca0289ac799e0..27e1ab0aea43127441d7b4798207433bd7549783 100644
--- a/source/RobotAPI/libraries/skills/provider/helper/LambdaSkillImplementation.h
+++ b/source/RobotAPI/libraries/skills/provider/helper/LambdaSkillImplementation.h
@@ -12,9 +12,13 @@ namespace armarx
using FunT = std::function<bool(const aron::data::DictPtr&)>;
LambdaSkill() = delete;
- LambdaSkill(const FunT& f) : fun(f) {};
+ LambdaSkill(const FunT& f, const SkillDescription& desc) :
+ Skill(desc),
+ fun(f)
+ {};
- Skill::Status execute(const aron::data::DictPtr& data, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; }) override;
+ protected:
+ Skill::Status _execute(const aron::data::DictPtr& data, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; }) override;
private:
FunT fun;