LocalizationQuery.cpp
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::Observers
17 * @author Kai Welke (welke at kit dot edu), David Schiebener (david dot schiebener at kit dot edu)
18 * @copyright 2012 Humanoids Group, HIS, KIT
19 * @license http://www.gnu.org/licenses/gpl-2.0.txt
20 * GNU General Public License
21 */
22 
23 // core
24 #include "LocalizationQuery.h"
29 
30 namespace memoryx
31 {
32  LocalizationQuery::LocalizationQuery(const std::string& queryName, const std::string& objectClassName, int cycleTimeMS, int priority)
33  {
34  this->queryName = queryName;
35  this->className = objectClassName;
36  this->cycleTimeMS = cycleTimeMS;
37  this->priority = priority;
38  }
39 
40  std::vector<LocalizationJobPtr> LocalizationQuery::createJobs(const ObjectClassMemorySegmentBasePtr& objectClassSegment)
41  {
42  std::scoped_lock lock(jobsMutex);
43 
44  std::vector<LocalizationJobPtr> jobs;
45 
46  ObjectClassList objectClasses = objectClassSegment->getClassWithSubclasses(className);
47 
48  // extract classes which are recognizable and fill in recognition method map
49  std::multimap<std::string, std::string> recognitionMethodMap;
50  std::set<std::string> recognitionMethods;
51 
52  for (ObjectClassList::iterator iter = objectClasses.begin() ; iter != objectClasses.end() ; iter++)
53  {
54  ObjectClassPtr objectClass = ObjectClassPtr::dynamicCast(*iter);
55 
56  if (getLocalizableObjectClass(objectClass))
57  {
58  EntityWrappers::ObjectRecognitionWrapperPtr recognitionWrapper = objectClass->addWrapper(new EntityWrappers::ObjectRecognitionWrapper());
59  std::string recognitionMethod = recognitionWrapper->getRecognitionMethod();
60 
61  recognitionMethodMap.insert(std::make_pair(recognitionMethod, objectClass->getName()));
62  recognitionMethods.insert(recognitionMethod);
63  }
64  }
65 
66  // create one job for each recognition method
67  for (std::set<std::string>::iterator iter = recognitionMethods.begin() ; iter != recognitionMethods.end() ; iter++)
68  {
69  std::string recognitionMethod = *iter;
70 
71  std::pair<std::multimap<std::string, std::string>::iterator, std::multimap<std::string, std::string>::iterator> classesForMethod;
72  classesForMethod = recognitionMethodMap.equal_range(recognitionMethod);
73 
74  std::vector<std::string> objectClassNames;
75 
76  for (std::multimap<std::string, std::string>::iterator iter = classesForMethod.first; iter != classesForMethod.second ; iter++)
77  {
78  objectClassNames.push_back(iter->second);
79  }
80 
81  // create job
83 
84  if (cycleTimeMS <= 0)
85  {
86  job = new LocalizationStrategyOnce();
87  }
88  else
89  {
90  job = new LocalizationStrategyRepeated(cycleTimeMS);
91  }
92 
93  job->init(this, recognitionMethod, objectClassNames);
94 
95  jobsFinished[recognitionMethod] = false;
96  jobs.push_back(job);
97  }
98 
99 
100 
101  return jobs;
102  }
103 
105  {
106  std::scoped_lock lock(jobsMutex);
107 
108  if (jobsFinished.size() == 0)
109  {
110  return false;
111  }
112 
113  bool finished = true;
114 
115  for (std::map<std::string, bool>::iterator iter = jobsFinished.begin() ; iter != jobsFinished.end() ; iter++)
116  {
117  finished &= iter->second;
118  }
119 
120  return finished;
121  }
122 
123  void LocalizationQuery::setJobFinished(std::string recognitionMethod)
124  {
125  std::scoped_lock lock(jobsMutex);
126  jobsFinished[recognitionMethod] = true;
127 
128  }
129 
130  ObjectClassPtr LocalizationQuery::getLocalizableObjectClass(const EntityBasePtr& entity)
131  {
132  // check if this is an entity of type objectClass
133  ObjectClassPtr objectClass = ObjectClassPtr::dynamicCast(entity);
134 
135  if (objectClass)
136  {
137  EntityWrappers::ObjectRecognitionWrapperPtr objectRecognitionWrapper = objectClass->addWrapper(new EntityWrappers::ObjectRecognitionWrapper());
138  std::string recognitionMethod = objectRecognitionWrapper->getRecognitionMethod();
139 
140  // check if a recognition method is defined
141  if ((recognitionMethod != "") && (recognitionMethod != "<none>"))
142  {
143  return objectClass;
144  }
145  else
146  {
147  if (objectClass->getName() != "all")
148  {
149  ARMARX_WARNING_S << "Recognition method for object " << objectClass->getName() << " is undefined";
150  }
151 
152  return NULL;
153  }
154  }
155  else
156  {
157  ARMARX_WARNING_S << "Entity " << entity->getName() << " could not be casted to an ObjectClassPtr";
158  return NULL;
159  }
160  }
161 }
memoryx::LocalizationJobPtr
IceUtil::Handle< LocalizationJob > LocalizationJobPtr
Definition: LocalizationJob.h:67
LocalizationQuery.h
memoryx
VirtualRobot headers.
Definition: CommonPlacesTester.cpp:48
ObjectRecognitionWrapper.h
memoryx::EntityWrappers::ObjectRecognitionWrapperPtr
IceInternal::Handle< ObjectRecognitionWrapper > ObjectRecognitionWrapperPtr
Definition: ObjectRecognitionWrapper.h:56
IceInternal::Handle< ObjectClass >
ARMARX_WARNING_S
#define ARMARX_WARNING_S
Definition: Logging.h:206
LocalizationStrategyOnce.h
ObjectClassMemorySegment.h
memoryx::EntityWrappers::ObjectRecognitionWrapper
Definition: ObjectRecognitionWrapper.h:40
memoryx::ObjectClassPtr
IceInternal::Handle< ObjectClass > ObjectClassPtr
Definition: ObjectClass.h:35
LocalizationStrategyRepeated.h
memoryx::LocalizationQuery::getFinished
bool getFinished()
getFinished indicates if there are localization jobs in the queue which have not finished yet
Definition: LocalizationQuery.cpp:104