recording_manager.cpp
Go to the documentation of this file.
1 
18 #include "recording_manager.hpp"
19 
20 namespace directSearchWS {
21 
22 RecordingManager::RecordingManager(std::string recordFilePath) : DirectSearchManager(), recordFilePath(recordFilePath) {
24 
25  n.getParam("filterMinimumNumberOfDeletedNormals", filterMinimumNumberOfDeletedNormalsParam);
26  ROS_INFO_STREAM("Param: filterMinimumNumberOfDeletedNormals: " << filterMinimumNumberOfDeletedNormalsParam);
27  n.getParam("filterIsPositionAllowed", filterIsPositionAllowedParam);
28  ROS_INFO_STREAM("Param: filterIsPositionAllowed: " << filterIsPositionAllowedParam);
29  n.getParam("concatApproxEqualsPoses", concatApproxEqualsPosesParam);
30  ROS_INFO_STREAM("Param: concatApproxEqualsPoses: " << concatApproxEqualsPosesParam);
31 
32  n.getParam("concatRobotPosePositionDistanceThreshold", concatRobotPosePositionDistanceThreshold);
33  ROS_INFO_STREAM("Param: concatRobotPosePositionDistanceThreshold: " << concatRobotPosePositionDistanceThreshold);
34  n.getParam("concatRobotPoseOrientationRadDistanceThreshold", concatRobotPoseOrientationRadDistanceThreshold);
35  ROS_INFO_STREAM("Param: concatRobotPoseOrientationRadDistanceThreshold: " << concatRobotPoseOrientationRadDistanceThreshold);
36 
37  n.getParam("reorderPosesByTSP", reorderPosesByTSPParam);
38  ROS_INFO_STREAM("Param: reorderPosesByTSP: " << reorderPosesByTSPParam);
40 }
41 
43 
45  bool success = DirectSearchManager::resetHandler();
46  success &= initHandler();
47  return success;
48 }
49 
52  DoFilterIteration doFilterIteration(posesToExplorePtr);
53 
54  // TODO: A nice feature would be to filter poses where the frustum was empty while recording.
55 
56  FilterBasicPtr filterMinimumNumberOfDeletedNormalsPtr =
58  doFilterIteration.addFilter(filterMinimumNumberOfDeletedNormalsPtr);
59 
61  FilterBasicPtr filterIsPositionAllowedPtr =
63  doFilterIteration.addFilter(filterIsPositionAllowedPtr);
64  }
65 
66  success &= doFilterIteration.doIteration();
67 
69  // start concat in seperat iteration
70  DoFilterIteration concatFilterIteration(posesToExplorePtr);
71  FilterBasicPtr concatApproxEqualePosesPtr = ConcatApproxEqualePosesPtr(
73  concatFilterIteration.addFilter(concatApproxEqualePosesPtr);
74  success &= concatFilterIteration.doIteration();
75  }
76 
77  ROS_DEBUG_STREAM("Number of filtered posesToExplore: " << posesToExplorePtr->size());
78 
79  sortHelperPtr->nearest_neighbour_and_update_distance(remainingPosesDistances);
80  sortHelperPtr->two_opt_and_update_distance(remainingPosesDistances);
81  std::reverse(posesToExplorePtr->begin(), posesToExplorePtr->end());
82 
85  }
86 
87  return success;
88 }
89 
90 bool RecordingManager::getNextRobotState(const SearchedObjectTypesAndIds &searchedObjectTypesAndIds) {
91  bool success = DirectSearchManager::getNextRobotState(searchedObjectTypesAndIds);
93  sortHelperPtr->two_opt_and_update_distance(remainingPosesDistances);
94  }
95  return success;
96 }
97 
98 bool RecordingManager::backToInitial(const SearchedObjectTypesAndIds &searchedObjectTypesAndIds) {
99  bool success = DirectSearchManager::backToInitial(searchedObjectTypesAndIds);
100  if (!reorderPosesByNBVParam) {
101  sortHelperPtr->two_opt_and_update_distance(remainingPosesDistances);
102  }
103  return success;
104 }
105 
106 }
107 
boost::shared_ptr< FilterMinimumNumberOfDeletedNormals > FilterMinimumNumberOfDeletedNormalsPtr
boost::shared_ptr< ConcatApproxEqualePoses > ConcatApproxEqualePosesPtr
void addFilter(const FilterBasicPtr &filter)
bool getNextRobotState(const SearchedObjectTypesAndIds &searchedObjectTypesAndIds)
ROSCPP_DECL const std::string & getName()
FilterPosesDependingOnAlreadyFoundObjectTypesPtr filterPosesDependingOnAlreadyFoundObjectTypesPtr
boost::shared_ptr< FilterIsPositionAllowed > FilterIsPositionAllowedPtr
virtual bool backToInitial(const SearchedObjectTypesAndIds &searchedObjectTypesAndIds)
std::vector< asr_msgs::AsrTypeAndId > SearchedObjectTypesAndIds
#define ROS_DEBUG_STREAM(args)
#define ROS_INFO_STREAM(args)
RecordingManager(std::string recordFilePath)
boost::shared_ptr< SortHelper > SortHelperPtr
Definition: sort_helper.hpp:47
bool getParam(const std::string &key, std::string &s) const
bool backToInitial(const SearchedObjectTypesAndIds &searchedObjectTypesAndIds)
bool parsePosesToExploreFromXML(const std::string &path)
virtual bool getNextRobotState(const SearchedObjectTypesAndIds &searchedObjectTypesAndIds)


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