From 697bbf08587d26f18ef0195f422a063180e4b29b Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Mon, 3 Aug 2020 10:32:21 +0200
Subject: [PATCH] Add ObjectPoseProviderExample
---
source/RobotAPI/components/CMakeLists.txt | 1 +
.../ObjectPoseProviderExample/CMakeLists.txt | 37 +++++
.../ObjectPoseProviderExample.cpp | 131 ++++++++++++++++++
.../ObjectPoseProviderExample.h | 120 ++++++++++++++++
.../RequestedObjects.cpp | 20 +++
.../RequestedObjects.h | 24 ++++
.../test/CMakeLists.txt | 5 +
.../test/ObjectPoseProviderExampleTest.cpp | 37 +++++
8 files changed, 375 insertions(+)
create mode 100644 source/RobotAPI/components/ObjectPoseProviderExample/CMakeLists.txt
create mode 100644 source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp
create mode 100644 source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.h
create mode 100644 source/RobotAPI/components/ObjectPoseProviderExample/RequestedObjects.cpp
create mode 100644 source/RobotAPI/components/ObjectPoseProviderExample/RequestedObjects.h
create mode 100644 source/RobotAPI/components/ObjectPoseProviderExample/test/CMakeLists.txt
create mode 100644 source/RobotAPI/components/ObjectPoseProviderExample/test/ObjectPoseProviderExampleTest.cpp
diff --git a/source/RobotAPI/components/CMakeLists.txt b/source/RobotAPI/components/CMakeLists.txt
index 3a49ac17f..e4cce02b6 100644
--- a/source/RobotAPI/components/CMakeLists.txt
+++ b/source/RobotAPI/components/CMakeLists.txt
@@ -15,6 +15,7 @@ add_subdirectory(KITProstheticHandUnit)
add_subdirectory(MultiHandUnit)
add_subdirectory(NaturalIKTest)
add_subdirectory(ObjectPoseObserver)
+add_subdirectory(ObjectPoseProviderExample)
add_subdirectory(RobotHealth)
add_subdirectory(RobotNameService)
add_subdirectory(RobotState)
diff --git a/source/RobotAPI/components/ObjectPoseProviderExample/CMakeLists.txt b/source/RobotAPI/components/ObjectPoseProviderExample/CMakeLists.txt
new file mode 100644
index 000000000..3179669cd
--- /dev/null
+++ b/source/RobotAPI/components/ObjectPoseProviderExample/CMakeLists.txt
@@ -0,0 +1,37 @@
+armarx_component_set_name("ObjectPoseProviderExample")
+
+
+set(COMPONENT_LIBS
+ ArmarXCore ArmarXCoreInterfaces # for DebugObserverInterface
+ # RobotAPICore RobotAPIInterfaces
+ # RobotAPIComponentPlugins # for ArViz and other plugins
+ RobotAPIArmarXObjects ObjectPoseObserver
+)
+
+set(SOURCES
+ ObjectPoseProviderExample.cpp
+
+ RequestedObjects.cpp
+)
+set(HEADERS
+ ObjectPoseProviderExample.h
+
+ RequestedObjects.h
+)
+
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
+#find_package(MyLib QUIET)
+#armarx_build_if(MyLib_FOUND "MyLib not available")
+# all target_include_directories must be guarded by if(Xyz_FOUND)
+# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
+#if(MyLib_FOUND)
+# target_include_directories(ObjectPoseProviderExample PUBLIC ${MyLib_INCLUDE_DIRS})
+#endif()
+
+# add unit tests
+add_subdirectory(test)
+
+#generate the application
+armarx_generate_and_add_component_executable()
diff --git a/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp b/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp
new file mode 100644
index 000000000..c33f59a10
--- /dev/null
+++ b/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp
@@ -0,0 +1,131 @@
+/*
+ * 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::ArmarXObjects::ObjectPoseProviderExample
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "ObjectPoseProviderExample.h"
+
+#include <ArmarXCore/core/time/CycleUtil.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+#include <RobotAPI/libraries/core/Pose.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+
+
+namespace armarx
+{
+ ObjectPoseProviderExamplePropertyDefinitions::ObjectPoseProviderExamplePropertyDefinitions(std::string prefix) :
+ armarx::ComponentPropertyDefinitions(prefix)
+ {
+ }
+
+ armarx::PropertyDefinitionsPtr ObjectPoseProviderExample::createPropertyDefinitions()
+ {
+ armarx::PropertyDefinitionsPtr defs = new ObjectPoseProviderExamplePropertyDefinitions(getConfigIdentifier());
+
+ defs->topic(debugObserver);
+
+ defs->optional(initialObjectIDs, "Objects", "Object IDs of objects to be tracked.")
+ .map("Dataset1/Name1, Dataset2/Name2", {});
+
+ return defs;
+ }
+
+ std::string ObjectPoseProviderExample::getDefaultName() const
+ {
+ return "ObjectPoseProviderExample";
+ }
+
+ objpose::ProviderInfo ObjectPoseProviderExample::getProviderInfo(const Ice::Current&)
+ {
+ objpose::ProviderInfo info;
+
+ return info;
+ }
+
+ void ObjectPoseProviderExample::requestObjects(
+ const objpose::ObjectIDSeq& objectIDs, Ice::Long relativeTimeoutMS, const Ice::Current&)
+ {
+ std::scoped_lock lock(requestedObjectsMutex);
+ requestedObjects.requestObjects(objectIDs, relativeTimeoutMS);
+ }
+
+
+ void ObjectPoseProviderExample::onInitComponent()
+ {
+ }
+
+
+ void ObjectPoseProviderExample::onConnectComponent()
+ {
+ poseEstimationTask = new SimpleRunningTask<>([this]()
+ {
+ this->poseEstimationTaskRun();
+ });
+ poseEstimationTask->start();
+ }
+
+
+ void ObjectPoseProviderExample::onDisconnectComponent()
+ {
+ }
+
+ void ObjectPoseProviderExample::onExitComponent()
+ {
+ }
+
+ void ObjectPoseProviderExample::poseEstimationTaskRun()
+ {
+ CycleUtil cycle(50);
+
+ std::vector<ObjectID> requested;
+ for (const auto& initial : initialObjectIDs)
+ {
+ ObjectID id(initial);
+ ARMARX_INFO << "Estimating pose of: " << id;
+ requested.push_back(id);
+ }
+
+ while (poseEstimationTask && poseEstimationTask->isRunning())
+ {
+ armarx::objpose::data::ProvidedObjectPoseSeq poses;
+ for (const ObjectID& id : requested)
+ {
+ armarx::objpose::data::ProvidedObjectPose& pose = poses.emplace_back();
+ pose.providerName = getName();
+ pose.objectType = objpose::ObjectTypeEnum::KnownObject;
+
+ pose.objectID.dataset = id.dataset();
+ pose.objectID.className = id.className();
+ pose.objectID.instanceName = id.instanceName();
+
+ Eigen::Matrix4f p = p.Identity();
+ pose.objectPose = new Pose(p);
+ pose.objectPoseFrame = armarx::GlobalFrame;
+
+ pose.confidence = 1.0;
+ pose.timestampMicroSeconds = TimeUtil::GetTime().toMicroSeconds();
+ }
+ ARMARX_INFO << "Reportnig " << poses.size() << " object poses";
+ objectPoseTopic->reportObjectPoses(getName(), poses);
+ }
+ }
+
+}
diff --git a/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.h b/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.h
new file mode 100644
index 000000000..22ab16ef7
--- /dev/null
+++ b/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.h
@@ -0,0 +1,120 @@
+/*
+ * 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::ArmarXObjects::ObjectPoseProviderExample
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+
+#include <ArmarXCore/core/Component.h>
+
+#include <ArmarXCore/interface/observers/ObserverInterface.h>
+// #include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+
+
+// Include the ProviderPlugin
+#include <RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseProviderPlugin.h>
+
+
+#include <mutex>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+#include "RequestedObjects.h"
+
+
+namespace armarx
+{
+ /**
+ * @class ObjectPoseProviderExamplePropertyDefinitions
+ * @brief Property definitions of `ObjectPoseProviderExample`.
+ */
+ class ObjectPoseProviderExamplePropertyDefinitions :
+ public armarx::ComponentPropertyDefinitions
+ {
+ public:
+ ObjectPoseProviderExamplePropertyDefinitions(std::string prefix);
+ };
+
+
+
+ /**
+ * @defgroup Component-ObjectPoseProviderExample ObjectPoseProviderExample
+ * @ingroup RobotAPI-Components
+ * A description of the component ObjectPoseProviderExample.
+ *
+ * @class ObjectPoseProviderExample
+ * @ingroup Component-ObjectPoseProviderExample
+ * @brief Brief description of class ObjectPoseProviderExample.
+ *
+ * Detailed description of class ObjectPoseProviderExample.
+ */
+ class ObjectPoseProviderExample :
+ virtual public armarx::Component
+ // Derive from the provider plugin.
+ , virtual public armarx::ObjectPoseProviderPluginUser
+ {
+ public:
+
+ /// @see armarx::ManagedIceObject::getDefaultName()
+ std::string getDefaultName() const override;
+
+
+ // Implement the ObjectPoseProvider interface
+ public:
+ objpose::ProviderInfo getProviderInfo(const Ice::Current& = Ice::emptyCurrent) override;
+ void requestObjects(const objpose::ObjectIDSeq& objectIDs, Ice::Long relativeTimeoutMS, const Ice::Current&) override;
+
+
+ protected:
+
+ /// @see armarx::ManagedIceObject::onInitComponent()
+ void onInitComponent() override;
+
+ /// @see armarx::ManagedIceObject::onConnectComponent()
+ void onConnectComponent() override;
+
+ /// @see armarx::ManagedIceObject::onDisconnectComponent()
+ void onDisconnectComponent() override;
+
+ /// @see armarx::ManagedIceObject::onExitComponent()
+ void onExitComponent() override;
+
+ /// @see PropertyUser::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+
+ private:
+
+ void poseEstimationTaskRun();
+
+
+ private:
+
+ std::mutex requestedObjectsMutex;
+ objpose::RequestedObjects requestedObjects;
+
+ std::vector<std::string> initialObjectIDs;
+
+ armarx::SimpleRunningTask<>::pointer_type poseEstimationTask;
+
+ /// Debug observer. Used to visualize e.g. time series.
+ armarx::DebugObserverInterfacePrx debugObserver;
+
+ };
+}
diff --git a/source/RobotAPI/components/ObjectPoseProviderExample/RequestedObjects.cpp b/source/RobotAPI/components/ObjectPoseProviderExample/RequestedObjects.cpp
new file mode 100644
index 000000000..80d426264
--- /dev/null
+++ b/source/RobotAPI/components/ObjectPoseProviderExample/RequestedObjects.cpp
@@ -0,0 +1,20 @@
+#include "RequestedObjects.h"
+
+
+namespace armarx::objpose
+{
+ RequestedObjects::RequestedObjects()
+ {
+
+ }
+
+ void RequestedObjects::requestObjects(const std::vector<armarx::ObjectID>& objectIDs, long relativeTimeOutMS)
+ {
+
+ }
+ void RequestedObjects::requestObjects(const std::vector<armarx::objpose::ObjectID>& objectIDs, long relativeTimeOutMS)
+ {
+
+ }
+}
+
diff --git a/source/RobotAPI/components/ObjectPoseProviderExample/RequestedObjects.h b/source/RobotAPI/components/ObjectPoseProviderExample/RequestedObjects.h
new file mode 100644
index 000000000..2b8095bed
--- /dev/null
+++ b/source/RobotAPI/components/ObjectPoseProviderExample/RequestedObjects.h
@@ -0,0 +1,24 @@
+#pragma once
+
+#include <vector>
+
+
+#include <RobotAPI/interface/objectpose/object_pose_types.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+
+
+
+namespace armarx::objpose
+{
+ class RequestedObjects
+ {
+ public:
+ RequestedObjects();
+
+ void requestObjects(const std::vector<armarx::ObjectID>& objectIDs, long relativeTimeOutMS);
+ void requestObjects(const std::vector<armarx::objpose::ObjectID>& objectIDs, long relativeTimeOutMS);
+
+
+
+ };
+}
diff --git a/source/RobotAPI/components/ObjectPoseProviderExample/test/CMakeLists.txt b/source/RobotAPI/components/ObjectPoseProviderExample/test/CMakeLists.txt
new file mode 100644
index 000000000..e65a2f31b
--- /dev/null
+++ b/source/RobotAPI/components/ObjectPoseProviderExample/test/CMakeLists.txt
@@ -0,0 +1,5 @@
+
+# Libs required for the tests
+SET(LIBS ${LIBS} ArmarXCore ${ARMARX_COMPONENT_LIB_NAME})
+
+armarx_add_test(ObjectPoseProviderExampleTest ObjectPoseProviderExampleTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/components/ObjectPoseProviderExample/test/ObjectPoseProviderExampleTest.cpp b/source/RobotAPI/components/ObjectPoseProviderExample/test/ObjectPoseProviderExampleTest.cpp
new file mode 100644
index 000000000..d173c24a7
--- /dev/null
+++ b/source/RobotAPI/components/ObjectPoseProviderExample/test/ObjectPoseProviderExampleTest.cpp
@@ -0,0 +1,37 @@
+/*
+ * 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::ArmarXObjects::ObjectPoseProviderExample
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE RobotAPI::ArmarXObjects::ObjectPoseProviderExample
+
+#define ARMARX_BOOST_TEST
+
+#include <RobotAPI/Test.h>
+#include <RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.h>
+
+#include <iostream>
+
+BOOST_AUTO_TEST_CASE(testExample)
+{
+ armarx::ObjectPoseProviderExample instance;
+
+ BOOST_CHECK_EQUAL(true, true);
+}
--
GitLab