diff --git a/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt b/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt
index 375f518122a5b764afc1cb69714c2cc44a15af7a..dfdb915ea0797f13b4f1253b04324b23e387c377 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt
+++ b/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt
@@ -2,24 +2,11 @@ armarx_component_set_name("ObjectPoseObserver")
set(COMPONENT_LIBS
- # ArmarXCore
- ArmarXCore ArmarXCoreInterfaces
- # ArmarXGui
- ArmarXGuiComponentPlugins
- # RobotAPI
- RobotAPI::ComponentPlugins
- RobotAPI::armem_objects
-
- # This project
- ${PROJECT_NAME}Interfaces
)
set(SOURCES
- ObjectPoseObserver.cpp
)
set(HEADERS
- ObjectPoseObserver.h
-
# Legacy headers.
plugins/ObjectPoseProviderPlugin.h
plugins/ObjectPoseClientPlugin.h
@@ -29,7 +16,7 @@ armarx_add_component("${SOURCES}" "${HEADERS}")
# add unit tests
-add_subdirectory(test)
+# add_subdirectory(test)
# generate the application
-armarx_generate_and_add_component_executable()
+# armarx_generate_and_add_component_executable()
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
deleted file mode 100644
index 44f7f9f8155baec73e4039195d659abb9f61d075..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
+++ /dev/null
@@ -1,519 +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::ArmarXObjects::ObjectPoseObserver
- * @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 "ObjectPoseObserver.h"
-
-#include <RobotAPI/libraries/core/Pose.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
-#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
-#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
-
-#include <ArmarXCore/core/time/CycleUtil.h>
-#include <ArmarXCore/observers/variant/Variant.h>
-
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/RobotConfig.h>
-
-#include <SimoxUtility/algorithm/get_map_keys_values.h>
-#include <SimoxUtility/meta/EnumNames.hpp>
-
-
-namespace armarx
-{
-
- armarx::PropertyDefinitionsPtr ObjectPoseObserver::createPropertyDefinitions()
- {
- armarx::PropertyDefinitionsPtr defs(new ComponentPropertyDefinitions(getConfigIdentifier()));
-
- defs->defineOptionalProperty<std::string>("ObjectPoseTopicName", "ObjectPoseTopic", "Name of the Object Pose Topic.");
- defs->defineOptionalProperty<std::string>("KinematicUnitObserverName", "KinematicUnitObserver", "Name of the kinematic unit observer.");
- defs->topic(debugObserver);
-
- calibration.defineProperties(defs, "calibration.");
- data.defineProperties(defs);
- robotHead.defineProperties(defs, "head.");
- visu.defineProperties(defs, "visu.");
-
- return defs;
- }
-
- ObjectPoseObserver::ObjectPoseObserver() :
- data(memory, iceMemory)
- {
- }
-
- std::string ObjectPoseObserver::getDefaultName() const
- {
- return "ObjectPoseObserver";
- }
-
- void ObjectPoseObserver::onInitComponent()
- {
- data.setTag(getName());
- data.decay.setTag(getName());
- robotHead.setTag(getName());
- visu.setTag(getName());
-
- data.init();
-
- usingTopicFromProperty("ObjectPoseTopicName");
- }
-
- void ObjectPoseObserver::onConnectComponent()
- {
- // onConnect can be called multiple times, but addRobot will fail if called more than once with the same ID
- // So we need to always make sure to guard a call to addRobot
- if (!RobotState::hasRobot("robot"))
- {
- data.robot = RobotState::addRobot("robot", VirtualRobot::RobotIO::RobotDescription::eStructure);
- }
- data.robotStateComponent = getRobotStateComponent();
-
- getProxyFromProperty(robotHead.kinematicUnitObserver, "KinematicUnitObserverName", false, "", false);
- robotHead.debugObserver = debugObserver;
- robotHead.fetchDatafields();
-
- visu.arviz = arviz;
- if (!visu.updateTask)
- {
- visu.updateTask = new SimpleRunningTask<>([this]()
- {
- this->visualizeRun();
- });
- visu.updateTask->start();
- }
-
- createRemoteGuiTab();
- RemoteGui_startRunningTask();
- }
-
- void ObjectPoseObserver::onDisconnectComponent()
- {
- }
-
- void ObjectPoseObserver::onExitComponent()
- {
- }
-
-
- void ObjectPoseObserver::reportProviderAvailable(const std::string& providerName, const objpose::ProviderInfo& info, const Ice::Current&)
- {
- updateProviderInfo(providerName, info);
- }
-
-
- void ObjectPoseObserver::reportObjectPoses(
- const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& providedPoses, const Ice::Current&)
- {
- ARMARX_VERBOSE << "Received object " << providedPoses.size() << " poses from provider '" << providerName << "'.";
- updateObjectPoses(providerName, providedPoses);
- }
-
-
- void ObjectPoseObserver::updateProviderInfo(const std::string& providerName, const objpose::ProviderInfo& info)
- {
- if (!info.proxy)
- {
- ARMARX_WARNING << "Received availability signal by provider '" << providerName << "' "
- << "with invalid provider proxy.\nIgnoring provider '" << providerName << "'.";
- return;
- }
- {
- std::scoped_lock lock(dataMutex);
- std::stringstream ss;
- for (const auto& id : info.supportedObjects)
- {
- ss << "- " << id << "\n";
- }
- ARMARX_VERBOSE << "Provider '" << providerName << "' available.\n"
- << "Supported objects: \n" << ss.str();
- data.providers[providerName] = info;
- }
- }
-
-
- void ObjectPoseObserver::updateObjectPoses(
- const std::string& providerName,
- const objpose::data::ProvidedObjectPoseSeq& providedPoses)
- {
- TIMING_START(tReportObjectPoses);
-
- RobotHeadMovement::Discard discard;
- {
- std::scoped_lock lock(robotHeadMutex);
- discard = robotHead.getDiscard();
- }
- if (debugObserver)
- {
- StringVariantBaseMap map;
- map["Discarding All Updates"] = new Variant(discard.all ? 1.f : 0.f);
- if (discard.all)
- {
- map["Proportion Updated Poses"] = new Variant(0.f);
- }
- debugObserver->setDebugChannel(getName(), map);
- }
-
- if (discard.all)
- {
- return;
- }
-
- {
- std::scoped_lock lock(dataMutex);
- RobotState::synchronizeLocalClone(data.robot);
-
- if (data.robot->hasRobotNode(calibration.robotNode))
- {
- VirtualRobot::RobotNodePtr robotNode = data.robot->getRobotNode(calibration.robotNode);
- float value = robotNode->getJointValue();
- robotNode->setJointValue(value + calibration.offset);
- }
-
- TIMING_START(tCommitObjectPoses);
- Data::CommitStats stats =
- data.commitObjectPoses(providerName, providedPoses, discard.updatesUntil);
- TIMING_END_STREAM(tCommitObjectPoses, ARMARX_VERBOSE);
-
- if (debugObserver)
- {
- debugObserver->setDebugChannel(getName(),
- {
- { "Discarding All Updates", new Variant(discard.all ? 1 : 0) },
- { "Proportion Updated Poses", new Variant(static_cast<float>(stats.numUpdated) / providedPoses.size()) }
- });
- }
-
- handleProviderUpdate(providerName);
-
- TIMING_END_STREAM(tReportObjectPoses, ARMARX_VERBOSE);
- if (debugObserver)
- {
- debugObserver->setDebugChannel(getName(),
- {
- { "t ReportObjectPoses [ms]", new Variant(tReportObjectPoses.toMilliSecondsDouble()) },
- { "t MemorySetObjectPoses [ms]", new Variant(tCommitObjectPoses.toMilliSecondsDouble()) },
- });
- }
- }
- }
-
-
- void ObjectPoseObserver::handleProviderUpdate(const std::string& providerName)
- {
- // Initialized to 0 on first access.
- if (data.providers.count(providerName) == 0)
- {
- data.providers[providerName] = objpose::ProviderInfo();
- }
- }
-
-
- objpose::data::ObjectPoseSeq ObjectPoseObserver::getObjectPoses(const Ice::Current&)
- {
- TIMING_START(tGetObjectPoses);
-
- TIMING_START(tGetObjectPosesLock);
- std::scoped_lock lock(dataMutex);
- TIMING_END_STREAM(tGetObjectPosesLock, ARMARX_VERBOSE);
-
- const IceUtil::Time now = TimeUtil::GetTime();
- const objpose::data::ObjectPoseSeq result = objpose::toIce(data.getObjectPoses(now));
-
- TIMING_END_STREAM(tGetObjectPoses, ARMARX_VERBOSE);
-
- if (debugObserver)
- {
- debugObserver->setDebugChannel(getName(),
- {
- { "t GetObjectPoses() [ms]", new Variant(tGetObjectPoses.toMilliSecondsDouble()) },
- { "t GetObjectPoses() lock [ms]", new Variant(tGetObjectPosesLock.toMilliSecondsDouble()) }
- });
- }
-
- return result;
- }
-
- objpose::data::ObjectPoseSeq ObjectPoseObserver::getObjectPosesByProvider(const std::string& providerName, const Ice::Current&)
- {
- TIMING_START(GetObjectPoses);
-
- TIMING_START(GetObjectPosesLock);
- std::scoped_lock lock(dataMutex);
- TIMING_END_STREAM(GetObjectPosesLock, ARMARX_VERBOSE);
-
- const IceUtil::Time now = TimeUtil::GetTime();
- const objpose::data::ObjectPoseSeq result = objpose::toIce(data.getObjectPosesByProvider(providerName, now));
-
- TIMING_END_STREAM(GetObjectPoses, ARMARX_VERBOSE);
-
- if (debugObserver)
- {
- debugObserver->setDebugChannel(getName(),
- {
- { "t GetObjectPosesByProvider() [ms]", new Variant(GetObjectPoses.toMilliSecondsDouble()) },
- { "t GetObjectPosesByProvider() lock [ms]", new Variant(GetObjectPosesLock.toMilliSecondsDouble()) }
- });
- }
-
- return result;
- }
-
-
- objpose::observer::RequestObjectsOutput ObjectPoseObserver::requestObjects(
- const objpose::observer::RequestObjectsInput& input, const Ice::Current&)
- {
- std::map<std::string, objpose::provider::RequestObjectsInput> providerRequests;
- std::map<std::string, objpose::ObjectPoseProviderPrx> proxies;
-
- objpose::observer::RequestObjectsOutput output;
-
- auto updateProxy = [&](const std::string & providerName)
- {
- if (proxies.count(providerName) == 0)
- {
- if (auto it = data.providers.find(providerName); it != data.providers.end())
- {
- proxies[providerName] = it->second.proxy;
- }
- else
- {
- ARMARX_ERROR << "No proxy for provider ' " << providerName << "'.";
- proxies[providerName] = nullptr;
- }
- }
- };
-
- if (input.provider.size() > 0)
- {
- providerRequests[input.provider] = input.request;
- updateProxy(input.provider);
- }
- else
- {
- std::scoped_lock lock(dataMutex);
- for (const auto& objectID : input.request.objectIDs)
- {
- bool found = true;
- for (const auto& [providerName, info] : data.providers)
- {
- // ToDo: optimize look up.
- if (std::find(info.supportedObjects.begin(), info.supportedObjects.end(), objectID) != info.supportedObjects.end())
- {
- providerRequests[providerName].objectIDs.push_back(objectID);
- updateProxy(providerName);
- break;
- }
- }
- if (!found)
- {
- ARMARX_ERROR << "Did not find a provider for " << objectID << ".";
- output.results[objectID].providerName = "";
- }
- }
- }
-
- for (const auto& [providerName, request] : providerRequests)
- {
- if (objpose::ObjectPoseProviderPrx proxy = proxies.at(providerName); proxy)
- {
- ARMARX_INFO << "Requesting " << request.objectIDs.size() << " objects by provider '"
- << providerName << "' for " << request.relativeTimeoutMS << " ms.";
- objpose::provider::RequestObjectsOutput providerOutput = proxy->requestObjects(request);
-
- int successful = 0;
- for (const auto& [objectID, result] : providerOutput.results)
- {
- objpose::observer::ObjectRequestResult& res = output.results[objectID];
- res.providerName = providerName;
- res.result = result;
- successful += int(result.success);
- }
- ARMARX_INFO << successful << " of " << request.objectIDs.size() << " object requests successful.";
- }
- }
- return output;
- }
-
- objpose::ProviderInfoMap ObjectPoseObserver::getAvailableProvidersInfo(const Ice::Current&)
- {
- std::scoped_lock lock(dataMutex);
- return data.providers;
- }
-
- Ice::StringSeq ObjectPoseObserver::getAvailableProviderNames(const Ice::Current&)
- {
- std::scoped_lock lock(dataMutex);
- return simox::alg::get_keys(data.providers);
- }
-
- objpose::ProviderInfo ObjectPoseObserver::getProviderInfo(const std::string& providerName, const Ice::Current&)
- {
- std::scoped_lock lock(dataMutex);
- return data.getProviderInfo(providerName);
- }
-
- bool ObjectPoseObserver::hasProvider(const std::string& providerName, const Ice::Current&)
- {
- std::scoped_lock lock(dataMutex);
- return data.providers.count(providerName) > 0;
- }
-
-
- objpose::AttachObjectToRobotNodeOutput ObjectPoseObserver::attachObjectToRobotNode(
- const objpose::AttachObjectToRobotNodeInput& input, const Ice::Current&)
- {
- std::scoped_lock lock(dataMutex);
- return data.attachObjectToRobotNode(input);
- }
-
- objpose::DetachObjectFromRobotNodeOutput ObjectPoseObserver::detachObjectFromRobotNode(
- const objpose::DetachObjectFromRobotNodeInput& input, const Ice::Current&)
- {
- std::scoped_lock lock(dataMutex);
- return data.detachObjectFromRobotNode(input);
- }
-
- objpose::DetachAllObjectsFromRobotNodesOutput ObjectPoseObserver::detachAllObjectsFromRobotNodes(
- const objpose::DetachAllObjectsFromRobotNodesInput& input, const Ice::Current&)
- {
- std::scoped_lock lock(dataMutex);
- return data.detachAllObjectsFromRobotNodes(input);
- }
-
-
- objpose::AgentFramesSeq ObjectPoseObserver::getAttachableFrames(const Ice::Current&)
- {
- std::scoped_lock lock(dataMutex);
-
- objpose::AgentFramesSeq output;
- std::vector<VirtualRobot::RobotPtr> agents = { data.robot };
- for (VirtualRobot::RobotPtr agent : agents)
- {
- objpose::AgentFrames& frames = output.emplace_back();
- frames.agent = agent->getName();
- frames.frames = agent->getRobotNodeNames();
- }
- return output;
- }
-
- objpose::SignalHeadMovementOutput
- ObjectPoseObserver::signalHeadMovement(const objpose::SignalHeadMovementInput& input, const Ice::Current&)
- {
- std::scoped_lock lock(robotHeadMutex);
- return robotHead.signalHeadMovement(input);
- }
-
-
- void ObjectPoseObserver::visualizeRun()
- {
- CycleUtil cycle(static_cast<int>(1000 / visu.frequencyHz));
- while (visu.updateTask && !visu.updateTask->isStopped())
- {
- {
- std::scoped_lock lock(visuMutex);
-
- if (visu.enabled)
- {
- TIMING_START(Visu);
-
- objpose::ObjectPoseSeq objectPoses;
- ObjectFinder objectFinder;
- visu.minConfidence = -1;
- {
- std::scoped_lock lock(dataMutex);
-
- const IceUtil::Time now = TimeUtil::GetTime();
- objectPoses = data.getObjectPoses(now);
- objectFinder = data.objectFinder;
- if (data.decay.enabled)
- {
- visu.minConfidence = data.decay.removeObjectsBelowConfidence;
- }
- }
- const std::vector<viz::Layer> layers = visu.visualizeCommit(objectPoses, objectFinder);
- arviz.commit(layers);
-
- TIMING_END_STREAM(Visu, ARMARX_VERBOSE);
-
- if (debugObserver)
- {
- debugObserver->setDebugChannel(getName(),
- {
- { "t Visualize [ms]", new Variant(Visu.toMilliSecondsDouble()) },
- });
- }
- }
- }
- cycle.waitForCycleDuration();
- }
- }
-
-
- objpose::ObjectPoseProviderPrx ObjectPoseObserver::getProviderProxy(const std::string& providerName)
- {
- return getProxy<objpose::ObjectPoseProviderPrx>(providerName, false, "", false);
- }
-
- void ObjectPoseObserver::createRemoteGuiTab()
- {
- using namespace armarx::RemoteGui::Client;
-
- tab.visu.setup(this->visu);
- tab.data.setup(this->data);
- tab.decay.setup(this->data.decay);
- tab.robotHead.setup(this->robotHead);
-
- VBoxLayout root =
- {
- tab.visu.group, tab.data.group, tab.decay.group, tab.robotHead.group,
- VSpacer()
- };
- RemoteGui_createTab(getName(), root, &tab);
- }
-
- void ObjectPoseObserver::RemoteGui_update()
- {
- // Non-atomic variables need to be guarded by a mutex if accessed by multiple threads
- {
- std::scoped_lock lock(visuMutex);
- tab.visu.update(this->visu);
- }
- {
- std::scoped_lock lock(dataMutex);
- tab.data.update(this->data);
- tab.decay.update(this->data.decay);
- }
- {
- std::scoped_lock lock(robotHeadMutex);
- tab.robotHead.update(this->robotHead);
- }
- }
-
- void ObjectPoseObserver::Calibration::defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix)
- {
- defs->optional(robotNode, prefix + "robotNode", "Robot node which can be calibrated.");
- defs->optional(offset, prefix + "offset", "Offset for the node to be calibrated.");
- }
-
-}
-
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
deleted file mode 100644
index 0c53032886b712a0cf478a571589409b653beb33..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
+++ /dev/null
@@ -1,185 +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::ArmarXObjects::ObjectPoseObserver
- * @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 <mutex>
-
-#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
-
-#include <RobotAPI/interface/objectpose/ObjectPoseObserverInterface.h>
-#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
-#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
-#include <RobotAPI/libraries/armem/server/ComponentPlugin.h>
-
-#include <RobotAPI/libraries/armem_objects/Data.h>
-#include <RobotAPI/libraries/armem_objects/Decay.h>
-#include <RobotAPI/libraries/armem_objects/Visu.h>
-#include <RobotAPI/libraries/armem_objects/RobotHeadMovement.h>
-
-
-#define ICE_CURRENT_ARG const Ice::Current& = Ice::emptyCurrent
-
-
-namespace armarx
-{
-
- /**
- * @defgroup Component-ObjectPoseObserver ObjectPoseObserver
- * @ingroup RobotAPI-Components
- * A description of the component ObjectPoseObserver.
- *
- * @class ObjectPoseObserver
- * @ingroup Component-ObjectPoseObserver
- * @brief Brief description of class ObjectPoseObserver.
- *
- * Detailed description of class ObjectPoseObserver.
- */
- class ObjectPoseObserver :
- virtual public Component
- , virtual public armarx::objpose::ObjectPoseObserverInterface
- , virtual public armarx::armem::server::ComponentPluginUser
- , virtual public armarx::RobotStateComponentPluginUser
- , virtual public armarx::LightweightRemoteGuiComponentPluginUser
- , virtual public armarx::ArVizComponentPluginUser
- {
- public:
- using RobotState = armarx::RobotStateComponentPluginUser;
-
- ObjectPoseObserver();
-
-
- /// @see armarx::ManagedIceObject::getDefaultName()
- std::string getDefaultName() const override;
-
-
- // ObjectPoseTopic interface
- public:
- void reportProviderAvailable(const std::string& providerName, const objpose::ProviderInfo& info, ICE_CURRENT_ARG) override;
- void reportObjectPoses(const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& objectPoses, ICE_CURRENT_ARG) override;
-
- // ObjectPoseObserverInterface interface
- public:
-
- // OBJECT POSES
-
- objpose::data::ObjectPoseSeq getObjectPoses(ICE_CURRENT_ARG) override;
- objpose::data::ObjectPoseSeq getObjectPosesByProvider(const std::string& providerName, ICE_CURRENT_ARG) override;
-
- // PROVIDER INFORMATION
-
- bool hasProvider(const std::string& providerName, ICE_CURRENT_ARG) override;
- objpose::ProviderInfo getProviderInfo(const std::string& providerName, ICE_CURRENT_ARG) override;
- Ice::StringSeq getAvailableProviderNames(ICE_CURRENT_ARG) override;
- objpose::ProviderInfoMap getAvailableProvidersInfo(ICE_CURRENT_ARG) override;
-
-
- // REQUESTING
-
- objpose::observer::RequestObjectsOutput requestObjects(const objpose::observer::RequestObjectsInput& input, ICE_CURRENT_ARG) override;
-
- // ATTACHING
-
- objpose::AttachObjectToRobotNodeOutput attachObjectToRobotNode(const objpose::AttachObjectToRobotNodeInput& input, ICE_CURRENT_ARG) override;
- objpose::DetachObjectFromRobotNodeOutput detachObjectFromRobotNode(const objpose::DetachObjectFromRobotNodeInput& input, ICE_CURRENT_ARG) override;
- objpose::DetachAllObjectsFromRobotNodesOutput detachAllObjectsFromRobotNodes(const objpose::DetachAllObjectsFromRobotNodesInput& input, ICE_CURRENT_ARG) override;
-
- objpose::AgentFramesSeq getAttachableFrames(ICE_CURRENT_ARG) override;
-
- // HEAD MOVEMENT SIGNALS
-
- objpose::SignalHeadMovementOutput signalHeadMovement(const objpose::SignalHeadMovementInput& input, ICE_CURRENT_ARG) override;
-
-
-
- // Remote GUI
- void createRemoteGuiTab();
- void RemoteGui_update() override;
-
-
- protected:
-
- armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
-
- void onInitComponent() override;
- void onConnectComponent() override;
-
- void onDisconnectComponent() override;
- void onExitComponent() override;
-
-
- private:
-
- void updateProviderInfo(const std::string& providerName, const objpose::ProviderInfo& info);
-
- void updateObjectPoses(const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& providedPoses);
- void handleProviderUpdate(const std::string& providerName);
-
- objpose::ObjectPoseProviderPrx getProviderProxy(const std::string& providerName);
-
-
-
- // Visualization
-
- void visualizeRun();
-
-
- private:
-
- DebugObserverInterfacePrx debugObserver;
-
- using Data = objpose::observer::Data;
- Data data;
- std::mutex& dataMutex = memoryMutex;
-
- using RobotHeadMovement = objpose::observer::RobotHeadMovement;
- RobotHeadMovement robotHead;
- std::mutex robotHeadMutex;
-
- objpose::observer::Visu visu;
- std::mutex visuMutex;
-
-
- struct Calibration
- {
- std::string robotNode = "Neck_2_Pitch";
- float offset = 0.0f;
-
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "calibration.");
- };
- Calibration calibration;
-
-
- struct RemoteGuiTab : RemoteGui::Client::Tab
- {
- objpose::observer::Visu::RemoteGui visu;
- objpose::observer::Data::RemoteGui data;
- objpose::observer::Decay::RemoteGui decay;
- objpose::observer::RobotHeadMovement::RemoteGui robotHead;
- };
- RemoteGuiTab tab;
-
- };
-
-}
-
-#undef ICE_CURRENT_ARG
diff --git a/source/RobotAPI/components/ObjectPoseObserver/test/CMakeLists.txt b/source/RobotAPI/components/ObjectPoseObserver/test/CMakeLists.txt
deleted file mode 100644
index 878921745f8b8cfd10cc67cbcc594d2199c84ea3..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/ObjectPoseObserver/test/CMakeLists.txt
+++ /dev/null
@@ -1,5 +0,0 @@
-
-# Libs required for the tests
-SET(LIBS ${LIBS} ArmarXCore ObjectPoseObserver)
-
-armarx_add_test(ObjectPoseObserverTest ObjectPoseObserverTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/components/ObjectPoseObserver/test/ObjectPoseObserverTest.cpp b/source/RobotAPI/components/ObjectPoseObserver/test/ObjectPoseObserverTest.cpp
deleted file mode 100644
index 903d51bfb830771b2b70b0ef4bdacc414ecee3ed..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/ObjectPoseObserver/test/ObjectPoseObserverTest.cpp
+++ /dev/null
@@ -1,68 +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::ArmarXObjects::ObjectPoseObserver
- * @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::ObjectPoseObserver
-
-#define ARMARX_BOOST_TEST
-
-#include <RobotAPI/Test.h>
-#include <RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h>
-
-#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
-#include <RobotAPI/libraries/core/Pose.h>
-
-#include <iostream>
-
-using namespace armarx;
-
-
-BOOST_AUTO_TEST_CASE(test_from_to_OOBB)
-{
- Eigen::Vector3f pos(-100, -200, -300);
- Eigen::Matrix3f ori = Eigen::AngleAxisf(1.0, Eigen::Vector3f(1, 2, 3).normalized()).toRotationMatrix();
- Eigen::Vector3f extents(40, 50, 60);
-
- armarx::objpose::Box box;
- box.position = new Vector3(pos);
- box.orientation = new Quaternion(ori);
- box.extents = new Vector3(extents);
-
-
- const float prec = 1e-3;
-
- simox::OrientedBoxf oobb;
- armarx::objpose::fromIce(box, oobb);
- ARMARX_CHECK_LESS_EQUAL((oobb.center() - pos).norm(), prec);
- ARMARX_CHECK(oobb.rotation().isApprox(ori, prec));
- ARMARX_CHECK_LESS_EQUAL((oobb.dimensions() - extents).norm(), prec);
-
- armarx::objpose::Box boxOut;
- armarx::objpose::toIce(boxOut, oobb);
-
- Eigen::Vector3f posOut = Vector3Ptr::dynamicCast(boxOut.position)->toEigen();
- Eigen::Matrix3f oriOut = QuaternionPtr::dynamicCast(boxOut.orientation)->toEigen();
- Eigen::Vector3f extentsOut = Vector3Ptr::dynamicCast(boxOut.extents)->toEigen();
-
- ARMARX_CHECK_LESS_EQUAL((posOut - pos).norm(), prec);
- ARMARX_CHECK(oriOut.isApprox(ori, prec));
- ARMARX_CHECK_LESS_EQUAL((extentsOut - extents).norm(), prec);
-}