ObjectLocalizationMemoryUpdater.h
Go to the documentation of this file.
1 /*
2 * This file is part of ArmarX.
3 *
4 * ArmarX is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License version 2 as
6 * published by the Free Software Foundation.
7 *
8 * ArmarX is distributed in the hope that it will be useful, but
9 * WITHOUT ANY WARRANTY; without even the implied warranty of
10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11 * GNU General Public License for more details.
12 *
13 * You should have received a copy of the GNU General Public License
14 * along with this program. If not, see <http://www.gnu.org/licenses/>.
15 *
16 * @package MemoryX::Core
17 * @author Kai Welke <welke@kit.edu>
18 * @copyright 2012 Kai Welke
19 * @license http://www.gnu.org/licenses/gpl-2.0.txt
20 * GNU General Public License
21 */
22 
23 #pragma once
24 
25 #include <string>
27 
33 
35 #include <RobotAPI/interface/core/RobotState.h>
37 
40 
41 #include <RobotAPI/interface/units/TCPControlUnit.h>
42 
44 
45 
46 namespace memoryx
47 {
48  class ObjectLocalizationMemoryUpdater;
50 
52  {
55  {
56  defineOptionalProperty<std::string>("RobotStateComponentName", "RobotStateComponent", "Name of the RobotStateComponent");
57  defineOptionalProperty<std::string>("WorkingMemoryName", "WorkingMemory", "Name of the WorkingMemory component that should be used");
58  defineOptionalProperty<std::string>("LongtermMemoryName", "LongtermMemory", "Name of the LongtermMemory component that should be used");
59  defineOptionalProperty<std::string>("RobotStateObserverName", "RobotStateObserver", "Name of the RobotStateObserver that can be used for observing the head motion");
60  defineOptionalProperty<std::string>("GazeTCPName", "VirtualCentralGaze", "Name of the gaze tcp that can be used for observing the head motion");
61  defineOptionalProperty<std::string>("CommonPlacesLearnerName", "", "Name of the CommonPlacesLearner that will be trained when object instances are updated");
62  defineOptionalProperty<bool>("CheckFieldOfView", false, "Check if detected object is inside field of view.");
63  defineOptionalProperty<float>("ExistenceCertaintyReductionFactorWhenLocalizationFailed", 0.95f, "Existence certainty will be reduced by this factor when a localization returns without result");
64  defineOptionalProperty<float>("CameraOpeningAngle", 30.0f * M_PI / 180.0f, "Very conservative estimation of the camera opening angle, in radians");
65  defineOptionalProperty<float>("MaximalObjectDistance", 1500.0f, "Maximal distance an object may have from the camera to be localized");
66  defineOptionalProperty<std::string>("RobotNodeNameLeftCamera", "EyeLeftCamera", "Name of the robot node of the left camera");
67  defineOptionalProperty<std::string>("RobotNodeNameRightCamera", "EyeRightCamera", "Name of the robot node of the right camera");
68  defineOptionalProperty<float>("HeadMotionVelocityLimit", 100, "Maximal velocity of the head up to which localizations are executed. Speed is in mm/s at a the virtual gaze tcp");
69  defineOptionalProperty<std::string>("HandNodeNameLeft", "TCP L", "Name of the robot node for the left hand/TCP");
70  defineOptionalProperty<std::string>("HandNodeNameRight", "TCP R", "Name of the robot node for the right hand/TCP");
71 
72  defineOptionalProperty<std::string>("KBMReferenceNodeName", "Cameras", "Name of the robot node on which the KBM resides. Data should be provided wrt. to its coordinate frame (i.e., not be transformed by the robot model which introduces a new source of error).");
73  defineOptionalProperty<std::string>("KBMLeftArmNodeSetName", "LeftArm", "Name of the robot node set for the kinematic chain for the left arm.");
74  defineOptionalProperty<std::string>("KBMRightArmNodeSetName", "RightArm", "Name of the robot node set for the kinematic chain for the right arm.");
75  }
76  };
77 
78 
79  /**
80  * @class ObjectLocalizationMemoryUpdater
81  * @ingroup WorkingMemory
82  *
83  * The ObjectLocalizationMemoryUpdater triggers the localization of object classes based on the content of
84  * the object classes working memory segment and generates object instances in the object instances
85  * if objects have been localized.
86  *
87  * The localization is triggered by adding a LocalizationQuery to the corresponding ObjectClass entity
88  * via the ObjectRecognitionWrapper:
89  */
91  virtual public WorkingMemoryUpdater,
92  virtual public ObjectLocalizationMemoryUpdaterBase
93  {
94  public:
96  void setSegmentNames(std::string classSegmentName, std::string instanceSegmentName);
97  void setReferenceFrame(const std::string& referenceFrameName);
98 
100  {
102  }
103 
104  std::string getDefaultName() const override
105  {
106  return "ObjectLocalizationMemoryUpdater";
107  }
108 
109 
110  protected:
111  // inherited from managedIceObject
112  void onInitComponent() override;
113  void onConnectComponent() override;
114  void onDisconnectComponent() override;
115 
116  // inherited from WorkingMemoryListenerInterface
117  void reportEntityCreated(const std::string& segmentName, const EntityBasePtr& entity, const Ice::Current& c = Ice::emptyCurrent) override {}
118  void reportEntityUpdated(const std::string& segmentName, const EntityBasePtr& entityOld, const EntityBasePtr& entityNew, const Ice::Current& c = Ice::emptyCurrent) override;
119  void reportEntityRemoved(const std::string& segmentName, const EntityBasePtr& entity, const Ice::Current& c = Ice::emptyCurrent) override {}
120  void reportSnapshotLoaded(const std::string& segmentName, const Ice::Current& c = Ice::emptyCurrent) override { }
121  void reportSnapshotCompletelyLoaded(const Ice::Current& c = Ice::emptyCurrent) override { }
122  void reportMemoryCleared(const std::string& segmentName, const Ice::Current& c = Ice::emptyCurrent) override { }
123 
124  private:
125  // main scheduler
126  void runJobs();
127  void scheduleJobs();
128 
129 
130  // asynchronous localization
131  void startLocalization(const LocalizationJobContainerPtr& jobContainer);
132  void localizationFinished(const Ice::AsyncResultPtr& r);
133  /**
134  * @brief finishJobContainer stops all LocalizationJob instances associated with \p jobContainer and updates all pending queries.
135  * @param jobContainer
136  */
137  void finishJobContainer(LocalizationJobContainerPtr jobContainer);
138 
139  // running state handling of recognition methods
140  bool isRecognitionMethodRunning(const std::string& recognitionMethod);
141  void setRecognitionMethodRunningState(const std::string& recognitionMethod, bool running);
142 
143  // utility methods
144  ObjectLocalizerInterfacePrx getProxyCached(const std::string& recognitionMethod);
145  LocalizationQueryList getMissingQueries(const LocalizationQueryList& queriesBase, const LocalizationQueryList& queriesCompare);
146  void updateQueries(const LocalizationJobContainerPtr& jobContainer);
147  bool checkObjectVisibility(armarx::FramedPositionPtr pos);
148  void setMotionModel(ObjectInstancePtr& objectInstance);
149  void predictPoses();
150 
151  // working memory segments
152  std::string workingMemoryName;
153  std::string classSegmentName;
154  std::string instanceSegmentName;
155  std::string agentInstancesSegmentName;
156  ObjectInstanceMemorySegmentPtr objectInstanceSegment;
157  ObjectClassMemorySegmentBasePtr objectClassSegment;
158  AgentInstancesSegmentPtr agentInstancesSegment;
159  std::string referenceFrameName;
160  std::string agentName;
161 
162  // job scheduling
164  std::mutex jobsMutex;
165  std::mutex predictionMutex;
166  std::vector<LocalizationJobPtr> jobs;
167 
168  // recognition method running state
169  std::mutex recognitionMethodStateMutex;
170  std::map<std::string, bool> recognitionMethodRunning;
171 
172  // proxy cache
173  std::map<std::string, ObjectLocalizerInterfacePrx> knownObjectProxies;
174  armarx::RobotStateComponentInterfacePrx robotStateInterfacePrx;
175 
176  // observes the head motion speed
177  armarx::ObserverInterfacePrx robotStateObserverProxy;
178  armarx::DataFieldIdentifierPtr headTCPVelocityDatafieldID;
179  float headMotionVelocityLimit;
180 
181  // CommonPlacesLearner to train
182  memoryx::CommonPlacesLearnerInterfacePrx commonPlacesLearnerProxy;
183 
184  // opening angle of the used cameras
185  bool checkFieldOfView;
186  float cameraOpeningAngle;
187  std::string robotNodeNameLeftCamera, robotNodeNameRightCamera, handNodeNameLeft, handNodeNameRight;
188 
189  LongtermMemoryInterfacePrx longtermMemoryPrx;
190 
191  };
192 }
193 
194 namespace std
195 {
196  template<typename T>
197  ostream&
198  operator<<(ostream& str, const std::set<T>& set)
199  {
200  str << "Set(" << set.size() << "):\n";
201 
202  for (const auto& elem : set)
203  {
204  str << "\t" << elem << "\n";
205  }
206 
207  return str;
208  }
209 }
memoryx::ObjectLocalizationMemoryUpdater::reportEntityCreated
void reportEntityCreated(const std::string &segmentName, const EntityBasePtr &entity, const Ice::Current &c=Ice::emptyCurrent) override
Definition: ObjectLocalizationMemoryUpdater.h:117
memoryx::ObjectLocalizationMemoryUpdater::reportSnapshotCompletelyLoaded
void reportSnapshotCompletelyLoaded(const Ice::Current &c=Ice::emptyCurrent) override
Definition: ObjectLocalizationMemoryUpdater.h:121
str
std::string str(const T &t)
Definition: UserAssistedSegmenterGuiWidgetController.cpp:42
memoryx::ObjectLocalizationMemoryUpdaterProperties
Definition: ObjectLocalizationMemoryUpdater.h:51
armarx::PropertyDefinitionContainer::prefix
std::string prefix
Prefix of the properties such as namespace, domain, component name, etc.
Definition: PropertyDefinitionContainer.h:333
PeriodicTask.h
LocalizationQuery.h
memoryx
VirtualRobot headers.
Definition: CommonPlacesTester.cpp:48
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
DataFieldIdentifier.h
memoryx::ObjectLocalizationMemoryUpdater::reportSnapshotLoaded
void reportSnapshotLoaded(const std::string &segmentName, const Ice::Current &c=Ice::emptyCurrent) override
Definition: ObjectLocalizationMemoryUpdater.h:120
ObjectClass.h
IceInternal::Handle< ObjectLocalizationMemoryUpdater >
ObjectInstanceMemorySegment.h
memoryx::WorkingMemoryUpdater
Base class for all working memory updater processes.
Definition: WorkingMemoryUpdater.h:54
LocalizationJob.h
M_PI
#define M_PI
Definition: MathTools.h:17
memoryx::ObjectLocalizationMemoryUpdater::getDefaultName
std::string getDefaultName() const override
Retrieve default name of component.
Definition: ObjectLocalizationMemoryUpdater.h:104
WorkingMemoryUpdater.h
memoryx::ObjectLocalizationMemoryUpdater
Definition: ObjectLocalizationMemoryUpdater.h:90
AgentInstancesSegment.h
LocalizationJobContainer.h
memoryx::ObjectLocalizationMemoryUpdaterPtr
IceInternal::Handle< ObjectLocalizationMemoryUpdater > ObjectLocalizationMemoryUpdaterPtr
Definition: WorkingMemory.h:39
CommonPlacesLearner.h
std::operator<<
ARMARXCORE_IMPORT_EXPORT ostream & operator<<(ostream &stream, const armarx::RunningTaskIceBase &task)
memoryx::ObjectLocalizationMemoryUpdaterProperties::ObjectLocalizationMemoryUpdaterProperties
ObjectLocalizationMemoryUpdaterProperties(std::string prefix)
Definition: ObjectLocalizationMemoryUpdater.h:53
memoryx::ObjectLocalizationMemoryUpdater::createPropertyDefinitions
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: ObjectLocalizationMemoryUpdater.h:99
std
Definition: Application.h:66
set
set(LIBS ArmarXCoreInterfaces ${CMAKE_THREAD_LIBS_INIT} ${dl_LIBRARIES} ${rt_LIBRARIES} ${QT_LIBRARIES} ${Boost_LIBRARIES} BoostAssertionHandler ArmarXCPPUtility SimoxUtility) set(LIB_FILES ArmarXManager.cpp ArmarXMultipleObjectsScheduler.cpp ArmarXObjectScheduler.cpp ManagedIceObject.cpp ManagedIceObjectPlugin.cpp Component.cpp ComponentPlugin.cpp IceGridAdmin.cpp ArmarXObjectObserver.cpp IceManager.cpp PackagePath.cpp RemoteReferenceCount.cpp logging/LoggingUtil.cpp logging/Logging.cpp logging/LogSender.cpp logging/ArmarXLogBuf.cpp system/ArmarXDataPath.cpp system/DynamicLibrary.cpp system/ProcessWatcher.cpp system/FactoryCollectionBase.cpp system/cmake/CMakePackageFinder.cpp system/cmake/CMakePackageFinderCache.cpp system/cmake/ArmarXPackageToolInterface.cpp system/RemoteObjectNode.cpp services/sharedmemory/HardwareId.cpp services/tasks/RunningTask.cpp services/tasks/ThreadList.cpp services/tasks/ThreadPool.cpp services/profiler/Profiler.cpp services/profiler/FileLoggingStrategy.cpp services/profiler/IceLoggingStrategy.cpp application/Application.cpp application/ApplicationOptions.cpp application/ApplicationProcessFacet.cpp application/ApplicationNetworkStats.cpp application/properties/PropertyUser.cpp application/properties/Property.cpp application/properties/PropertyDefinition.cpp application/properties/PropertyDefinitionContainer.cpp application/properties/PropertyDefinitionHelpFormatter.cpp application/properties/PropertyDefinitionConfigFormatter.cpp application/properties/PropertyDefinitionBriefHelpFormatter.cpp application/properties/PropertyDefinitionXmlFormatter.cpp application/properties/PropertyDefinitionDoxygenFormatter.cpp application/properties/PropertyDefinitionDoxygenComponentPagesFormatter.cpp application/properties/PropertyDefinitionContainerBriefHelpFormatter.cpp application/properties/IceProperties.cpp exceptions/Exception.cpp exceptions/local/UnexpectedEnumValueException.cpp util/FileSystemPathBuilder.cpp util/StringHelpers.cpp util/IceReportSkipper.cpp util/Throttler.cpp util/distributed/AMDCallbackCollection.cpp util/distributed/RemoteHandle/ClientSideRemoteHandleControlBlock.cpp util/distributed/RemoteHandle/RemoteHandle.cpp util/distributed/RemoteHandle/RemoteHandleControlBlock.cpp time/ice_conversions.cpp time/json_conversions.cpp time/CallbackWaitLock.cpp time/Clock.cpp time/ClockType.cpp time/ClockTypeNames.cpp time/CycleUtil.cpp time/DateTime.cpp time/Duration.cpp time/Frequency.cpp time/LocalTimeServer.cpp time/Metronome.cpp time/ScopedStopWatch.cpp time/StopWatch.cpp time/Timer.cpp time/TimeKeeper.cpp time/TimeUtil.cpp csv/CsvWriter.cpp csv/CsvReader.cpp eigen/conversions.cpp eigen/ice_conversions.cpp) set(LIB_HEADERS ArmarXManager.h ArmarXDummyManager.h ArmarXMultipleObjectsScheduler.h ArmarXObjectObserver.h ArmarXObjectScheduler.h ArmarXFwd.h Component.h ComponentPlugin.h ComponentFactories.h CoreObjectFactories.h IceGridAdmin.h IceManager.h IceManagerImpl.h json_conversions.h ManagedIceObject.h ManagedIceObjectPlugin.h ManagedIceObjectImpl.h ManagedIceObjectDependency.h ManagedIceObjectRegistryInterface.h PackagePath.h RemoteReferenceCount.h system/ImportExport.h system/ImportExportComponent.h system/AbstractFactoryMethod.h system/FactoryCollectionBase.h system/Synchronization.h system/ArmarXDataPath.h system/DynamicLibrary.h system/ProcessWatcher.h system/ConditionSynchronization.h system/cmake/CMakePackageFinder.h system/cmake/CMakePackageFinderCache.h system/cmake/FindPackageX.cmake system/cmake/ArmarXPackageToolInterface.h system/RemoteObjectNode.h logging/LoggingUtil.h logging/LogSender.h logging/Logging.h logging/ArmarXLogBuf.h logging/SpamFilterData.h services/tasks/RunningTask.h services/tasks/PeriodicTask.h services/tasks/ThreadList.h services/tasks/TaskUtil.h services/tasks/ThreadPool.h services/sharedmemory/SharedMemoryProvider.h services/sharedmemory/SharedMemoryConsumer.h services/sharedmemory/IceSharedMemoryProvider.h services/sharedmemory/IceSharedMemoryConsumer.h services/sharedmemory/HardwareIdentifierProvider.h services/sharedmemory/HardwareId.h services/sharedmemory/exceptions/SharedMemoryExceptions.h services/profiler/Profiler.h services/profiler/LoggingStrategy.h services/profiler/FileLoggingStrategy.h services/profiler/IceLoggingStrategy.h application/Application.h application/ApplicationOptions.h application/ApplicationProcessFacet.h application/ApplicationNetworkStats.h application/properties/forward_declarations.h application/properties/Properties.h application/properties/Property.h application/properties/PluginEigen.h application/properties/PluginEnumNames.h application/properties/PluginCfgStruct.h application/properties/PluginAll.h application/properties/PropertyUser.h application/properties/PropertyDefinition.h application/properties/PropertyDefinition.hpp application/properties/PropertyDefinitionInterface.h application/properties/PropertyDefinitionContainer.h application/properties/PropertyDefinitionFormatter.h application/properties/PropertyDefinitionContainerFormatter.h application/properties/PropertyDefinitionConfigFormatter.h application/properties/PropertyDefinitionHelpFormatter.h application/properties/PropertyDefinitionBriefHelpFormatter.h application/properties/PropertyDefinitionXmlFormatter.h application/properties/PropertyDefinitionDoxygenFormatter.h application/properties/PropertyDefinitionDoxygenComponentPagesFormatter.h application/properties/PropertyDefinitionContainerBriefHelpFormatter.h application/properties/ProxyPropertyDefinition.h application/properties/IceProperties.h exceptions/Exception.h exceptions/LocalException.h exceptions/local/DynamicLibraryException.h exceptions/local/ExpressionException.h exceptions/local/FileIOException.h exceptions/local/InvalidPropertyValueException.h exceptions/local/MissingRequiredPropertyException.h exceptions/local/PropertyInheritanceCycleException.h exceptions/local/ProxyNotInitializedException.h exceptions/local/UnexpectedEnumValueException.h exceptions/local/UnmappedValueException.h exceptions/local/ValueRangeExceededException.h exceptions/user/NotImplementedYetException.h rapidxml/rapidxml.hpp rapidxml/rapidxml_print.hpp rapidxml/rapidxml_iterators.hpp rapidxml/rapidxml_utils.hpp rapidxml/wrapper/RapidXmlReader.h rapidxml/wrapper/RapidXmlWriter.h rapidxml/wrapper/DefaultRapidXmlReader.h rapidxml/wrapper/MultiNodeRapidXMLReader.h util/IceBlobToObject.h util/ObjectToIceBlob.h util/FileSystemPathBuilder.h util/FiniteStateMachine.h util/StringHelpers.h util/StringHelperTemplates.h util/algorithm.h util/OnScopeExit.h util/Predicates.h util/Preprocessor.h util/PropagateConst.h util/Registrar.h util/TemplateMetaProgramming.h util/TripleBuffer.h util/IceReportSkipper.h util/Throttler.h util/distributed/AMDCallbackCollection.h util/distributed/RemoteHandle/ClientSideRemoteHandleControlBlock.h util/distributed/RemoteHandle/RemoteHandle.h util/distributed/RemoteHandle/RemoteHandleControlBlock.h util/SimpleStatemachine.h time.h time_minimal.h time/forward_declarations.h time/ice_conversions.h time/json_conversions.h time/CallbackWaitLock.h time/Clock.h time/ClockType.h time/ClockTypeNames.h time/CycleUtil.h time/DateTime.h time/Duration.h time/Frequency.h time/LocalTimeServer.h time/Metronome.h time/ScopedStopWatch.h time/StopWatch.h time/Timer.h time/TimeUtil.h time/TimeKeeper.h csv/CsvWriter.h csv/CsvReader.h eigen/conversions.h eigen/ice_conversions.h ice_conversions.h ice_conversions/ice_conversions_boost_templates.h ice_conversions/ice_conversions_templates.h ice_conversions/ice_conversions_templates.tpp $
Definition: CMakeLists.txt:12
IceUtil::Handle< class PropertyDefinitionContainer >
memoryx::ObjectLocalizationMemoryUpdater::reportMemoryCleared
void reportMemoryCleared(const std::string &segmentName, const Ice::Current &c=Ice::emptyCurrent) override
Definition: ObjectLocalizationMemoryUpdater.h:122
memoryx::ObjectLocalizationMemoryUpdater::reportEntityRemoved
void reportEntityRemoved(const std::string &segmentName, const EntityBasePtr &entity, const Ice::Current &c=Ice::emptyCurrent) override
Definition: ObjectLocalizationMemoryUpdater.h:119
IceInternal::ProxyHandle<::IceProxy::armarx::RobotStateComponentInterface >
ImportExport.h
ARMARXCORE_IMPORT_EXPORT
#define ARMARXCORE_IMPORT_EXPORT
Definition: ImportExport.h:38
armarx::PropertyDefinitionsPtr
IceUtil::Handle< class PropertyDefinitionContainer > PropertyDefinitionsPtr
PropertyDefinitions smart pointer type.
Definition: forward_declarations.h:34
memoryx::WorkingMemoryUpdaterProperties
Definition: WorkingMemoryUpdater.h:37