do_filter_iteration.hpp
Go to the documentation of this file.
1 
18 #pragma once
19 
20 #include <vector>
21 
22 #include <boost/shared_ptr.hpp>
23 
24 #include "model/robot_state.hpp"
25 #include "model/ptu_tuple.hpp"
26 
27 #include "filter/filter_basic.hpp"
28 
29 
30 namespace directSearchWS {
31 
33 
34 private:
35  std::vector<FilterBasicPtr> filtersToApply;
37 
38 public:
39  DoFilterIteration(const RobotStatePtrVecPtr &posesToExplorePtr) : filtersToApply(), posesToExplorePtr(posesToExplorePtr) { }
40 
41  void addFilter(const FilterBasicPtr &filter) {
42  filtersToApply.push_back(filter);
43  }
44 
45  bool doIteration() {
46  std::vector<FilterBasicPtr> filteredFiltersToApply;
47  for (const FilterBasicPtr &filter_ptr : filtersToApply) {
48  if (filter_ptr->needIteration()) {
49  filteredFiltersToApply.push_back(filter_ptr);
50  }
51  }
52 
53  // Iterate over all poses and ptu_tuples and apply filters
54  RobotStatePtrVec::iterator posesToExploreIter = posesToExplorePtr->begin();
55  while (posesToExploreIter != posesToExplorePtr->end()) {
56  PtuTuplePtrVec::iterator ptuTuplePtrIter = posesToExploreIter->get()->getPtuTuplePtrVecPtr()->begin();
57  while (ptuTuplePtrIter != posesToExploreIter->get()->getPtuTuplePtrVecPtr()->end()) {
58  bool shouldDelete = false;
59  for (const FilterBasicPtr &filter_ptr : filteredFiltersToApply) {
60  shouldDelete |= filter_ptr->shouldPtuTupleBeDeleted(posesToExploreIter, ptuTuplePtrIter);
61  if (shouldDelete) {
62  break;
63  }
64  }
65 
66  if (shouldDelete) {
67  ptuTuplePtrIter = posesToExploreIter->get()->getPtuTuplePtrVecPtr()->erase(ptuTuplePtrIter);
68  } else {
69  ++ptuTuplePtrIter;
70  }
71  }
72  if (posesToExploreIter->get()->getPtuListSize() == 0) {
73  posesToExploreIter = posesToExplorePtr->erase(posesToExploreIter);
74  } else {
75  ++posesToExploreIter;
76  }
77  }
78 
79  bool wasSuccessful = true;
80  for (const FilterBasicPtr &filter_ptr : filteredFiltersToApply) {
81  wasSuccessful &= filter_ptr->wasIterationSuccessful();
82  }
83  return wasSuccessful;
84  }
85 
86 };
87 
89 
90 }
91 
92 
93 
void addFilter(const FilterBasicPtr &filter)
boost::shared_ptr< DoFilterIteration > DoFilterIterationPtr
const RobotStatePtrVecPtr & posesToExplorePtr
std::vector< FilterBasicPtr > filtersToApply
DoFilterIteration(const RobotStatePtrVecPtr &posesToExplorePtr)


asr_direct_search_manager
Author(s): Borella Jocelyn, Karrenbauer Oliver, Mei├čner Pascal
autogenerated on Wed Jan 8 2020 03:15:41