direct_search_handler.cpp
Go to the documentation of this file.
1 
19 
20 namespace directSearchWS {
21 
23  nextCameraPosePtr(), nextRobotPosePtr(), nextPtuPtr(), nextFilteredSearchedObjectTypesAndIds(), remainingPosesDistances(0.0),
24  isSameRobotPoseAsBefore(false), isRobotPoseChanged(true), isNoPoseLeft(false), arePosesFromDemonstrationLeft(false), poseHelperPtr(PoseHelper::getInstance()) {
25 }
26 
28 
32  posesToExplorePtr->clear();
33  nextCameraPosePtr.reset();
34  nextRobotPosePtr.reset();
35  nextPtuPtr.reset();
39  isRobotPoseChanged = true;
40  isNoPoseLeft = false;
42  return true;
43 }
44 
45 bool DirectSearchHandler::getNextRobotState(const SearchedObjectTypesAndIds &searchedObjectTypesAndIds) {
46  if (posesToExplorePtr->empty()) {
47  ROS_INFO("Get next robot state called, when no poses were left");
48  isNoPoseLeft = true;
49  return true;
50  } else {
51  isNoPoseLeft = false;
52  }
53 
54  nextRobotPosePtr = posesToExplorePtr->front()->getRobotPosePtr();
56 
57  if (posesToExplorePtr->front()->getPtuListSize() == 0) {
58  ROS_ERROR("It is not allowed to have a robot_pose with no PTU config left!");
59  return false;
60  }
61 
62  nextPtuPtr = posesToExplorePtr->front()->getTopPtuTuplePtr();
63  posesToExplorePtr->front()->eraseFrontPTUTuplePtr();
64  nextCameraPosePtr = nextPtuPtr->getCameraPosePtr();
65 
66  nextFilteredSearchedObjectTypesAndIds = searchedObjectTypesAndIds;
67  // Erase the object_types in the next view which were already searched there
69 
70  if (posesToExplorePtr->front()->getPtuListSize() == 0) {
71  ROS_INFO("Get next robot state called, when only one PTU tuple was left");
72  posesToExplorePtr->erase(posesToExplorePtr->begin());
73  isRobotPoseChanged = true;
74  if (!posesToExplorePtr->empty()) {
75  remainingPosesDistances -= poseHelperPtr->calculateDistance(*nextRobotPosePtr, *posesToExplorePtr->front()->getRobotPosePtr());
76  arePosesFromDemonstrationLeft = posesToExplorePtr->front()->getTopPtuTuplePtr()->getRatedViewport().rating > 0;
77  } else {
80  }
81  return true;
82  } else {
83  ROS_INFO("Get next robot state called, when more than one PTU tuple was left");
84  arePosesFromDemonstrationLeft = posesToExplorePtr->front()->getTopPtuTuplePtr()->getRatedViewport().rating > 0;
85  isRobotPoseChanged = false;
86  return true;
87  }
88 }
89 
90 bool DirectSearchHandler::backToInitial(const SearchedObjectTypesAndIds &searchedObjectTypesAndIds) {
91  isRobotPoseChanged = true;
92  double distance = DBL_MAX;
93  const geometry_msgs::Pose &robotPose = poseHelperPtr->getCurrentRobotPose();
94  RobotStatePtrVec::iterator saved_it = posesToExplorePtr->begin();
95  for (RobotStatePtrVec::iterator it = posesToExplorePtr->begin(); it != posesToExplorePtr->end(); ++it) {
96  double loop_distance = poseHelperPtr->calculateDistance(robotPose, *it->get()->getRobotPosePtr());
97  if (loop_distance < distance) {
98  distance = loop_distance;
99  saved_it = it;
100  }
101  }
102  if (saved_it != posesToExplorePtr->begin()) {
103  ROS_INFO("Poses to explore have been reordered.");
104  RobotStatePtrVecPtr buffer_vectorPtr = RobotStatePtrVecPtr(new RobotStatePtrVec());
105  buffer_vectorPtr->insert(buffer_vectorPtr->end(),saved_it, posesToExplorePtr->end());
106  buffer_vectorPtr->insert(buffer_vectorPtr->end(),posesToExplorePtr->begin(), saved_it);
107  posesToExplorePtr->clear();
108  posesToExplorePtr = buffer_vectorPtr;
109  }
110 
111  return true;
112 }
113 
116  if (posesToExplorePtr->size() <= 1) {
117  return;
118  }
119  for (RobotStatePtrVec::iterator it = posesToExplorePtr->begin(); it != posesToExplorePtr->end() - 1; ++it) {
120  remainingPosesDistances += poseHelperPtr->calculateDistance(*it->get()->getRobotPosePtr(), *(it+1)->get()->getRobotPosePtr());
121  }
122  ROS_INFO_STREAM("new calculated remainingPosesDistances: " << remainingPosesDistances);
123 }
124 
125 
126 void filterSearchedObjectTypes(SearchedObjectTypes &searched_object_types_to_filter, const SearchedObjectTypes &filter_searched_object_types) {
127  for (const std::string &filter_searched_object_type : filter_searched_object_types) {
128  bool isFound = false;
129  do {
130  SearchedObjectTypes::iterator iter = std::find(searched_object_types_to_filter.begin(), searched_object_types_to_filter.end(), filter_searched_object_type);
131  if (iter != searched_object_types_to_filter.end()) {
132  searched_object_types_to_filter.erase(iter);
133  isFound = true;
134  } else {
135  isFound = false;
136  }
137  } while (isFound);
138  }
139 }
140 
141 void filterSearchedObjectTypesAndIds(SearchedObjectTypesAndIds &searched_object_types_and_ids_to_filter, const SearchedObjectTypes &filter_searched_object_types) {
142  for (const std::string &filter_searched_object_type : filter_searched_object_types) {
143  SearchedObjectTypesAndIds::iterator searched_object_type_and_id_to_filter_iter = searched_object_types_and_ids_to_filter.begin();
144  while (searched_object_type_and_id_to_filter_iter != searched_object_types_and_ids_to_filter.end()) {
145  if (searched_object_type_and_id_to_filter_iter->type == filter_searched_object_type) {
146  searched_object_type_and_id_to_filter_iter = searched_object_types_and_ids_to_filter.erase(searched_object_type_and_id_to_filter_iter);
147  } else {
148  ++searched_object_type_and_id_to_filter_iter;
149  }
150  }
151  }
152 }
153 
155  const SearchedObjectTypes &second_searched_object_types) {
156  SearchedObjectTypes resultSearchedObjectTypes;
157 
158  SearchedObjectTypes copySearchecObjectTypes1(first_searched_object_types);
159  SearchedObjectTypes copySearchecObjectTypes2(second_searched_object_types);
160  std::sort(copySearchecObjectTypes1.begin(), copySearchecObjectTypes1.end());
161  std::sort(copySearchecObjectTypes2.begin(), copySearchecObjectTypes2.end());
162 
163  std::set_intersection(copySearchecObjectTypes1.begin(), copySearchecObjectTypes1.end(),
164  copySearchecObjectTypes2.begin(), copySearchecObjectTypes2.end(),
165  std::back_inserter(resultSearchedObjectTypes));
166 
167  return resultSearchedObjectTypes;
168 }
169 
172 
173  checkParameterFromOtherNode<std::string>(nh, "rp_ism_node", "dbfilename");
174 
175  checkParameterFromOtherNode<double>(nh, "asr_flir_ptu_driver", "pan_min_angle");
176  checkParameterFromOtherNode<double>(nh, "asr_flir_ptu_driver", "pan_max_angle");
177  checkParameterFromOtherNode<double>(nh, "asr_flir_ptu_driver", "tilt_min_angle");
178  checkParameterFromOtherNode<double>(nh, "asr_flir_ptu_driver", "tilt_max_angle");
179 }
180 
181 template <typename T>
182 void checkParameterFromOtherNode(ros::NodeHandle &nh, const std::string &node_name, const std::string &param_name) {
183  std::string full_name = "/" + node_name + "/" + param_name;
184  if (nh.hasParam(full_name)) {
185  T param;
186  nh.getParam(full_name, param);
187  nh.setParam(param_name, param);
188  ROS_INFO_STREAM("Param: " << full_name << ": " << param);
189  }
190 }
191 
192 }
193 
virtual bool getNextRobotState(const SearchedObjectTypesAndIds &searchedObjectTypesAndIds)
void filterSearchedObjectTypes(SearchedObjectTypes &searched_object_types_to_filter, const SearchedObjectTypes &filter_searched_object_types)
bool param(const std::string &param_name, T &param_val, const T &default_val)
virtual bool backToInitial(const SearchedObjectTypesAndIds &searchedObjectTypesAndIds)
boost::shared_ptr< RobotStatePtrVec > RobotStatePtrVecPtr
Definition: robot_state.hpp:52
std::vector< std::string > SearchedObjectTypes
Definition: ptu_tuple.hpp:27
ROSCPP_DECL const std::string & getName()
TFSIMD_FORCE_INLINE tfScalar distance(const Vector3 &v) const
void checkParameterFromOtherNode(ros::NodeHandle &nh, const std::string &node_name, const std::string &param_name)
std::vector< asr_msgs::AsrTypeAndId > SearchedObjectTypesAndIds
void filterSearchedObjectTypesAndIds(SearchedObjectTypesAndIds &searched_object_types_and_ids_to_filter, const SearchedObjectTypes &filter_searched_object_types)
#define ROS_INFO(...)
SearchedObjectTypesAndIds nextFilteredSearchedObjectTypesAndIds
std::vector< RobotStatePtr > RobotStatePtrVec
Definition: robot_state.hpp:51
#define ROS_INFO_STREAM(args)
bool getParam(const std::string &key, std::string &s) const
bool hasParam(const std::string &key) const
SearchedObjectTypes getIntersectionOfSearchObjectTypes(const SearchedObjectTypes &first_searched_object_types, const SearchedObjectTypes &second_searched_object_types)
#define ROS_ERROR(...)
void setParam(const std::string &key, const XmlRpc::XmlRpcValue &v) const


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