diff --git a/source/RobotAPI/components/armem/client/GraspProviderExample/CMakeLists.txt b/source/RobotAPI/components/armem/client/GraspProviderExample/CMakeLists.txt index 551bac20677cd24f02ea46397ab791856094db04..18ea059105a516dbafffaba07b3d4c322dabdc11 100644 --- a/source/RobotAPI/components/armem/client/GraspProviderExample/CMakeLists.txt +++ b/source/RobotAPI/components/armem/client/GraspProviderExample/CMakeLists.txt @@ -7,6 +7,7 @@ set(COMPONENT_LIBS ArmarXCore ArmarXCoreInterfaces # for DebugObserverInterface ArmarXGuiComponentPlugins RobotAPICore RobotAPIInterfaces armem + GraspingUtility ${IVT_LIBRARIES} ) diff --git a/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.cpp b/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.cpp index 3a94f3b17dc21c5801b88f42f4ee5ae0b6a43099..2d74eb7599039d92beda16667e976ba594f14d84 100644 --- a/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.cpp +++ b/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.cpp @@ -5,6 +5,8 @@ #include <ArmarXCore/core/exceptions/local/ExpressionException.h> #include <ArmarXCore/core/time/CycleUtil.h> +#include <RobotAPI/libraries/core/Pose.h> + #include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h> #include <RobotAPI/libraries/armem/client/query/Builder.h> #include <RobotAPI/libraries/armem/client/query/query_fns.h> @@ -38,6 +40,11 @@ namespace armarx return "GraspProviderExample"; } + GraspProviderExample::GraspProviderExample() : writer(*this) + { + + } + void GraspProviderExample::onInitComponent() { @@ -47,7 +54,7 @@ namespace armarx void GraspProviderExample::onConnectComponent() { - + /* // Wait for the memory to become available and add it as dependency. ARMARX_IMPORTANT << "Waiting for memory '" << memoryName << "' ..."; auto result = useMemory(memoryName); @@ -60,7 +67,8 @@ namespace armarx providerID = addProviderSegment(); // Construct the entity ID. entityID = providerID.withEntityName("grasp_entity"); - + */ + writer.connect(); task = new RunningTask<GraspProviderExample>(this, &GraspProviderExample::run); task->start(); @@ -81,7 +89,7 @@ namespace armarx void GraspProviderExample::run() { ARMARX_IMPORTANT << "Running example."; - + /* armem::MemoryID snapshotID = commitSingleSnapshot(entityID); if (true) { @@ -91,12 +99,24 @@ namespace armarx { commitGraspCandidate(); } - + */ CycleUtil c(1000); int i = 0; + + //grasping::BoundingBox bb = grasping::BoundingBox(Vector3BasePtr(toIce(Eigen::Vector3f())), Vector3BasePtr(toIce(Eigen::Vector3f())), Vector3BasePtr(toIce(Eigen::Vector3f())), Vector3BasePtr(toIce(Eigen::Vector3f()))); + //grasping::GraspCandidateSourceInfo info = grasping::GraspCandidateSourceInfo(PoseBasePtr(toIce(Eigen::Matrix4f())), "string", 5, &bb); + + while (!task->isStopped() && i++ < 100) { - commitSingleSnapshot(entityID); + armarx::grasping::GraspCandidate candidate = armarx::grasping::GraspCandidate(); + candidate.groupNr = 5; + candidate.approachVector = Vector3BasePtr(toIce(Eigen::Vector3f())); + candidate.graspPose = PoseBasePtr(toIce(Eigen::Matrix4f())); + candidate.providerName = "Example"; + candidate.robotPose = PoseBasePtr(toIce(Eigen::Matrix4f())); + + writer.storeGraspCandidate(candidate, IceUtil::Time::now().toMilliSeconds()); c.waitForCycleDuration(); } } diff --git a/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.h b/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.h index 6c8790a6272339add9a7e09ec7ce7c4b10e4d79e..1dc18a24bf451e392ec52b9405d0ea27fe89e5e0 100644 --- a/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.h +++ b/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.h @@ -1,6 +1,7 @@ #include <ArmarXCore/core/Component.h> #include <ArmarXCore/interface/observers/ObserverInterface.h> #include <ArmarXCore/util/tasks.h> +#include <RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.h> #include <RobotAPI/libraries/armem/client/ComponentPlugin.h> @@ -24,19 +25,18 @@ namespace armarx class GraspProviderExample : virtual public armarx::Component, - virtual public armarx::armem::client::ComponentPluginUser + virtual public armarx::armem::ClientWriterComponentPluginUser { public: /// @see armarx::ManagedIceObject::getDefaultName() std::string getDefaultName() const override; - + GraspProviderExample(); protected: armarx::PropertyDefinitionsPtr createPropertyDefinitions() override; - void onInitComponent() override; void onConnectComponent() override; void onDisconnectComponent() override; @@ -67,7 +67,7 @@ namespace armarx armarx::DebugObserverInterfacePrx debugObserver; std::string memoryName = "Grasp"; - + armarx::armem::GraspCandidateWriter writer; }; }