concat_approx_equale_poses.hpp
Go to the documentation of this file.
1 
18 #pragma once
19 
20 #include "filter/filter_basic.hpp"
22 #include "helper/pose_helper.hpp"
23 
24 namespace directSearchWS {
25 
27 
28 private:
36 
38 
39 
40 public:
41  ConcatApproxEqualePoses(const RobotStatePtrVecPtr &posesToExplorePtr, double positionDistanceThreshold, double orientationRadDistanceThreshold,
42  bool checkOnlyNext = false, bool shouldDelete = true, bool shouldConcat = true) :
43  FilterBasic(posesToExplorePtr), concatCount(0), deleteCount(0), positionDistanceThreshold(positionDistanceThreshold),
44  orientationRadDistanceThreshold(orientationRadDistanceThreshold), checkOnlyNext(checkOnlyNext), shouldDelete(shouldDelete), shouldConcat(shouldConcat),
45  poseHelperPtr(PoseHelper::getInstance()) {
46 
47  }
49 
50  bool needIteration() {
51  concatCount = 0;
52  deleteCount = 0;
53  return true;
54  }
55 
57  if (shouldConcat) {
58  ROS_INFO_STREAM("Number of concatenated ptu_tuple by ConcatApproxEqualePoses: " << concatCount << " (number is not representative)");
59  }
60  if (shouldDelete) {
61  ROS_INFO_STREAM("Number of deleted ptu_tuple by ConcatApproxEqualePoses: " << deleteCount);
62  }
63  return true;
64  }
65 
66  bool shouldPtuTupleBeDeleted(const RobotStatePtrVec::iterator &posesToExploreIter, const PtuTuplePtrVec::iterator &ptuTuplePtrIter) {
67  RobotStatePtrVec::iterator robotEnd = posesToExplorePtr->end();
68  if (checkOnlyNext && posesToExploreIter + 1 != posesToExplorePtr->end()) {
69  robotEnd = posesToExploreIter + 2;
70  }
71 
72  // Check if robot_poses are approx_equale and concate to one robot_state with multiple ptuTuples
73  for (RobotStatePtrVec::iterator posesToExploreCompareIter = posesToExploreIter + 1; posesToExploreCompareIter != robotEnd; ++posesToExploreCompareIter) {
74  // If poses are approx_equale push current ptu_tuple in other pose and delete/concat it
75  if (poseHelperPtr->checkPosesAreApproxEquale(*posesToExploreIter->get()->getRobotPosePtr(),
76  *posesToExploreCompareIter->get()->getRobotPosePtr(), positionDistanceThreshold, orientationRadDistanceThreshold)) {
77  // Only push back if the PTU-tuple does not exists in the other part
78  bool isEquale = false;
79  PtuTuplePtrVec::iterator ptuTuplePtrCompareIter = posesToExploreCompareIter->get()->getPtuTuplePtrVecPtr()->begin();
80  for (;ptuTuplePtrCompareIter != posesToExploreCompareIter->get()->getPtuTuplePtrVecPtr()->end(); ++ptuTuplePtrCompareIter) {
81  if (ptuTuplePtrCompareIter->get()->getPan() == ptuTuplePtrIter->get()->getPan()
82  && ptuTuplePtrCompareIter->get()->getTilt() == ptuTuplePtrIter->get()->getTilt()) {
83  //ROS_DEBUG("Two poses are approx_- and ptu_configs are equale -> one ptu_config will be removed");
84  isEquale = true;
85  break;
86  }
87  }
88  if (isEquale) {
89  if (shouldDelete) {
90  ++deleteCount;
91  SearchedObjectTypes firstSearchObjectTypes = ptuTuplePtrCompareIter->get()->getAlreadySearchedObjectTypes();
92  SearchedObjectTypes secondSearchObjectTypes = ptuTuplePtrIter->get()->getAlreadySearchedObjectTypes();
93  ptuTuplePtrCompareIter->get()->resetAlreadySearchedObjectTypes();
94  SearchedObjectTypes intersectionSearchObjectTypes = getIntersectionOfSearchObjectTypes(firstSearchObjectTypes, secondSearchObjectTypes);
95  ptuTuplePtrCompareIter->get()->addAlreadySearchedObjectTypes(intersectionSearchObjectTypes);
96  } else {
97  continue;
98  }
99  } else {
100  if (shouldConcat) {
101  posesToExploreCompareIter->get()->getPtuTuplePtrVecPtr()->insert(posesToExploreCompareIter->get()->getPtuTuplePtrVecPtr()->end() - 1, *ptuTuplePtrIter);
102  ++concatCount;
103  } else {
104  continue;
105  }
106  }
107 
108  return true;
109  }
110  }
111  return false;
112  }
113 
114 };
115 
117 
118 }
119 
120 
121 
122 
boost::shared_ptr< ConcatApproxEqualePoses > ConcatApproxEqualePosesPtr
bool shouldPtuTupleBeDeleted(const RobotStatePtrVec::iterator &posesToExploreIter, const PtuTuplePtrVec::iterator &ptuTuplePtrIter)
std::vector< std::string > SearchedObjectTypes
Definition: ptu_tuple.hpp:27
const RobotStatePtrVecPtr & posesToExplorePtr
#define ROS_INFO_STREAM(args)
SearchedObjectTypes getIntersectionOfSearchObjectTypes(const SearchedObjectTypes &first_searched_object_types, const SearchedObjectTypes &second_searched_object_types)
ConcatApproxEqualePoses(const RobotStatePtrVecPtr &posesToExplorePtr, double positionDistanceThreshold, double orientationRadDistanceThreshold, bool checkOnlyNext=false, bool shouldDelete=true, bool shouldConcat=true)


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