direct_search_manager.cpp
Go to the documentation of this file.
1 
19 
20 namespace directSearchWS {
21 
22 DirectSearchManager::DirectSearchManager(): DirectSearchHandler(), currentPan(0.0), currentTilt(0.0), needSortByNBV(true), lastSearchedObjectTypes() {
24  getViewportListServiceClient = n.serviceClient<asr_world_model::GetViewportList>("/env/asr_world_model/get_viewport_list");
25 
26  ptuDriverStateSubscriber = n.subscribe("/asr_flir_ptu_driver/state", 1000, &DirectSearchManager::ptuDriverStateCallback, this);
27  setInitRobotStateServiceClient = n.serviceClient<asr_next_best_view::SetInitRobotState>("/nbv/set_init_robot_state");
28  setAttributedPointCloudServiceClient = n.serviceClient<asr_next_best_view::SetAttributedPointCloud>("/nbv/set_point_cloud");
29  rateViewportsServiceClient = n.serviceClient<asr_next_best_view::RateViewports>("/nbv/rate_viewports");
30 
31  n.getParam("reorderPosesByNBV", reorderPosesByNBVParam);
32  ROS_INFO_STREAM("Param: reorderPosesByNBV: " << reorderPosesByNBVParam);
33 
34  n.getParam("dbfilename", dbfilenameParam);
35  ROS_INFO_STREAM("Param: dbfilename: " << dbfilenameParam);
36 
41 }
42 
44 
45 bool DirectSearchManager::parsePosesToExploreFromXML(const std::string &path) {
46  //read config file.
47  std::ifstream ConfigFile(path.c_str(),std::ifstream::in);
48  if (!ConfigFile) {
49  ROS_ERROR_STREAM("Unable to locate " << path << ". Please make sure the launch file is correct and start again");
50  return false;
51  }
52  std::vector<char> buffer((std::istreambuf_iterator<char>(ConfigFile)), std::istreambuf_iterator<char>());
53  buffer.push_back('\0');
54  // Parse the buffer using the xml file parsing library into doc
55  rapidxml::xml_document<> doc;
56  doc.parse<0>(&buffer[0]);
57 
58  // Find our root node
59  rapidxml::xml_node<> *root_node = doc.first_node("root");
60  for (rapidxml::xml_node<> *state_node = root_node->first_node("state"); state_node; state_node = state_node->next_sibling()) {
61  const PosePtr &goal_camera_pose_ptr = parsePose(state_node, "goal_camera_pose");
62  const PosePtr &goal_robot_pose_ptr = parsePose(state_node, "goal_robot_pose");
63 
64  rapidxml::xml_node<> *goal_ptu_position_node = state_node->first_node("goal_ptu_position");
65  int goal_ptu_position_pan = boost::lexical_cast<int>((goal_ptu_position_node->first_attribute("pan")->value()));
66  int goal_ptu_position_tilt = boost::lexical_cast<int>((goal_ptu_position_node->first_attribute("tilt")->value()));
67 
68  int deactivated_object_normals_count = 0;
69  if (state_node->first_node("deactivated_object_normals_count") != nullptr) {
70  deactivated_object_normals_count = boost::lexical_cast<int>((state_node->first_node("deactivated_object_normals_count")->first_attribute("count")->value()));
71  }
72 
73  PtuTuplePtrVecPtr ptu_tuple_ptr_vecotr_ptr = PtuTuplePtrVecPtr(new PtuTuplePtrVec());
74  ptu_tuple_ptr_vecotr_ptr->push_back(PtuTuplePtr(new PtuTuple(goal_ptu_position_pan, goal_ptu_position_tilt, goal_camera_pose_ptr, deactivated_object_normals_count)));
75 
76  posesToExplorePtr->push_back(RobotStatePtr(new RobotState(goal_robot_pose_ptr, ptu_tuple_ptr_vecotr_ptr)));
77  }
78  // ROS_DEBUG_STREAM("Parsed posesToExplore:\n" << posesToExplorePtr);
79  ROS_DEBUG_STREAM("Number of parsed posesToExplore: " << posesToExplorePtr->size());
80  return true;
81 }
82 
83 PosePtr DirectSearchManager::parsePose(const rapidxml::xml_node<> *node, const char *poseName) const {
84  PosePtr pose_ptr = PosePtr(new geometry_msgs::Pose());
85  rapidxml::xml_node<> *pose_node = node->first_node(poseName);
86 
87  rapidxml::xml_node<> *position_node = pose_node->first_node("position");
88  pose_ptr->position.x = boost::lexical_cast<double>((position_node->first_attribute("x")->value()));
89  pose_ptr->position.y = boost::lexical_cast<double>((position_node->first_attribute("y")->value()));
90  pose_ptr->position.z = boost::lexical_cast<double>((position_node->first_attribute("z")->value()));
91 
92  rapidxml::xml_node<> *orientation_node = pose_node->first_node("orientation");
93  pose_ptr->orientation.x = boost::lexical_cast<double>((orientation_node->first_attribute("x")->value()));
94  pose_ptr->orientation.y = boost::lexical_cast<double>((orientation_node->first_attribute("y")->value()));
95  pose_ptr->orientation.z = boost::lexical_cast<double>((orientation_node->first_attribute("z")->value()));
96  pose_ptr->orientation.w = boost::lexical_cast<double>((orientation_node->first_attribute("w")->value()));
97  return pose_ptr;
98 }
99 
100 
102  bool success = DirectSearchHandler::resetHandler();
104  n.getParam("dbfilename", dbfilenameParam);
105  ROS_INFO_STREAM("Param: dbfilename: " << dbfilenameParam);
106  lastSearchedObjectTypes.clear();
107  needSortByNBV = true;
108  return success;
109 }
110 
112  SearchedObjectTypes searchedObjectTypes;
113  for (const asr_msgs::AsrTypeAndId &searchedObjectType : searchedObjectTypesAndIds) {
114  searchedObjectTypes.push_back(searchedObjectType.type);
115  }
116  if (checkSearchedObjectTypesAreEquale(lastSearchedObjectTypes, searchedObjectTypes) && !searchedObjectTypes.empty()) {
117  ROS_INFO("Same searched object types -> nothing to update");
118  } else {
119  ROS_INFO("Different searched object types -> update");
120  // Check if some poses are not needed anymore, because the searchedObjectTypes were already searched there
121  filterPosesDependingOnAlreadyFoundObjectTypesPtr->updateSearchedObjectTypes(searchedObjectTypes);
123 
125  if (!setInitialRobotState()) {
126  ROS_ERROR("Service call to setInitialRobotState was not successful");
127  return false;
128  }
129 
130  if (!setPointCloudInNBV(searchedObjectTypesAndIds)) {
131  ROS_ERROR("Service call to setPointCloudInNBV was not successful");
132  return false;
133  }
134  }
135  }
136  lastSearchedObjectTypes = searchedObjectTypes;
137 
138  bool success = true;
140  success &= reorderPosesByNBV(searchedObjectTypes);
142  }
143 
144  if (reorderPosesByNBVParam && !posesToExplorePtr->empty()) {
145  ROS_DEBUG_STREAM("nextRatedViewort:\n" <<posesToExplorePtr->front()->getTopPtuTuplePtr()->getRatedViewport());
146 
147  if (needSortByNBV && posesToExplorePtr->front()->getTopPtuTuplePtr()->getRatedViewport().rating == 0) {
148  // reorder poses to current position, if there is no improve from nbv anymore
149  success &= DirectSearchHandler::backToInitial(searchedObjectTypesAndIds);
150  needSortByNBV = false;
151  }
152  }
153  success &= DirectSearchHandler::getNextRobotState(searchedObjectTypesAndIds);
154  return success;
155 }
156 
157 bool DirectSearchManager::getViewportsFromWorldModel(std::vector<asr_msgs::AsrViewport> &viewports) {
158  asr_world_model::GetViewportList getViewportServiceCall;
159  getViewportServiceCall.request.object_type = "all";
160  bool success = getViewportListServiceClient.call(getViewportServiceCall);
161  viewports = getViewportServiceCall.response.viewport_list;
162  return success;
163 }
164 
165 bool DirectSearchManager::backToInitial(const SearchedObjectTypesAndIds &searchedObjectTypesAndIds) {
166  needSortByNBV = true;
167  std::vector<asr_msgs::AsrViewport> viewports;
168  bool success = getViewportsFromWorldModel(viewports);
169  ROS_INFO_STREAM("Got viewports from world_model: " << viewports.size());
170  // Iterate over all viewports from world_model and update the already_found_object_types in the ptu_tuples
171  for (const asr_msgs::AsrViewport &viewport : viewports) {
172  for (const RobotStatePtr &robotStatePtr : *posesToExplorePtr) {
173  for (const PtuTuplePtr &ptuTuplePtr : *robotStatePtr->getPtuTuplePtrVecPtr()) {
174  if (poseHelperPtr->checkViewCenterPointsAreApproxEquale(viewport.pose, *ptuTuplePtr->getCameraPosePtr())) {
175  ptuTuplePtr->addAlreadySearchedObjectTypes(viewport.object_type_name_list);
176  }
177  }
178  }
179  }
180 
181  success &= DirectSearchHandler::backToInitial(searchedObjectTypesAndIds);
182 
184  if (!setInitialRobotState()) {
185  ROS_ERROR("Service call to setInitialRobotState was not successful");
186  return false;
187  }
188 
189  if (!setPointCloudInNBV(searchedObjectTypesAndIds)) {
190  ROS_ERROR("Service call to setPointCloudInNBV was not successful");
191  return false;
192  }
193 
194  SearchedObjectTypes searchedObjectTypes;
195  for (const asr_msgs::AsrTypeAndId &searchedObjectType : searchedObjectTypesAndIds) {
196  searchedObjectTypes.push_back(searchedObjectType.type);
197  }
198  success &= reorderPosesByNBV(searchedObjectTypes);
200 
201  if (!posesToExplorePtr->empty()) {
202  arePosesFromDemonstrationLeft = posesToExplorePtr->front()->getTopPtuTuplePtr()->getRatedViewport().rating > 0;
203  }
204  } else {
206  }
207 
208  return success;
209 }
210 
212  bool success = true;
214 
215  asr_next_best_view::RateViewports rateViewports;
216  rateViewports.request.current_pose = poseHelperPtr->getCurrentCameraPose();
217  rateViewports.request.object_type_name_list = searchedObjectTypes;
218  rateViewports.request.use_object_type_to_rate = false;
219 
220  RobotStatePtrVecPtr robotStatesToReorder = RobotStatePtrVecPtr(new RobotStatePtrVec());
221 
222  const int orginalNumberOfPoses = posesToExplorePtr->size();
223  ROS_DEBUG_STREAM("Initial number of poses to explore: " << orginalNumberOfPoses);
224 
225  for (const RobotStatePtr &robotStatePtr : *posesToExplorePtr) {
226  for (const PtuTuplePtr &ptuTuplePtr : *robotStatePtr->getPtuTuplePtrVecPtr()) {
227  rateViewports.request.viewports.push_back(*ptuTuplePtr->getCameraPosePtr());
228 
229  PtuTuplePtrVecPtr tempPtuTuplePtrVecPtr = PtuTuplePtrVecPtr(new PtuTuplePtrVec());
230  // do not copy ratedViewport
231  tempPtuTuplePtrVecPtr->push_back(PtuTuplePtr(new PtuTuple(ptuTuplePtr->getPan(),
232  ptuTuplePtr->getTilt(),
233  ptuTuplePtr->getCameraPosePtr(),
234  ptuTuplePtr->getDeactivatedObjectNormalsCount(),
235  ptuTuplePtr->getAlreadySearchedObjectTypes())));
236  RobotStatePtr robotStatePtrCopy = RobotStatePtr(new RobotState(robotStatePtr->getRobotPosePtr(), tempPtuTuplePtrVecPtr));
237  robotStatesToReorder->push_back(robotStatePtrCopy);
238  }
239  }
240 
241  ROS_DEBUG_STREAM("Number of poses to explore with only one PTU config per each: " << robotStatesToReorder->size());
242 
243  if (!rateViewportsServiceClient.call(rateViewports)) {
244  ROS_ERROR("Service call to rateViewportsServiceClient was not successful");
245  return false;
246  }
247 
248  const std::vector<asr_next_best_view::RatedViewport> &ratedViewports = rateViewports.response.sortedRatedViewports;
249 
250  ROS_ASSERT_MSG(ratedViewports.size() == robotStatesToReorder->size(),
251  "The number of returned viewports from RateViewports were not equale, expect: %zu actual: %zu",
252  robotStatesToReorder->size(), ratedViewports.size());
253 
254  int numberOfPosesWhereRatingIsNot0 = 0;
255 
256  RobotStatePtrVecPtr reorderedRobotStates = RobotStatePtrVecPtr(new RobotStatePtrVec());
257  for (unsigned int currentIndex = 0; currentIndex < ratedViewports.size(); ++currentIndex) {
258  const asr_next_best_view::RatedViewport &currentRatedViewport = ratedViewports[currentIndex];
259  ROS_ASSERT_MSG(poseHelperPtr->checkPosesAreApproxEquale(currentRatedViewport.pose,
260  *(*robotStatesToReorder)[currentRatedViewport.oldIdx]->getTopPtuTuplePtr()->getCameraPosePtr(),
261  0.0001, 0.0001),
262  "The Pose of currentRatedViewport does not fit to my Pose at oldIdx, for currentIndexOfCurrentRatedViewport: %u currentRatedViewport.oldIndex: %u",
263  currentIndex, currentRatedViewport.oldIdx);
264 
265  const RobotStatePtr &robotStatePtrToInsert = (*robotStatesToReorder)[currentRatedViewport.oldIdx];
266 
267  if (currentRatedViewport.rating != 0.0) {
268  ++numberOfPosesWhereRatingIsNot0;
269 
270  // Split robotState in two. In first run look for expected objects and in second for the remaining
271  PtuTuplePtrVecPtr copyPtuTuplePtrVecPtr = PtuTuplePtrVecPtr(new PtuTuplePtrVec());
272  copyPtuTuplePtrVecPtr->push_back(PtuTuplePtr(new PtuTuple(robotStatePtrToInsert->getTopPtuTuplePtr()->getPan(),
273  robotStatePtrToInsert->getTopPtuTuplePtr()->getTilt(),
274  robotStatePtrToInsert->getTopPtuTuplePtr()->getCameraPosePtr(),
275  robotStatePtrToInsert->getTopPtuTuplePtr()->getDeactivatedObjectNormalsCount(),
276  robotStatePtrToInsert->getTopPtuTuplePtr()->getAlreadySearchedObjectTypes())));
277  RobotStatePtr robotStatePtrToInsertCopy = RobotStatePtr(new RobotState(robotStatePtrToInsert->getRobotPosePtr(), copyPtuTuplePtrVecPtr));
278  robotStatePtrToInsertCopy->getTopPtuTuplePtr()->setRatedViewport(currentRatedViewport);
279 
280 
281  // alreadySearchedObjectTypesForFirstRun = searchedObjectTypes \ currentRatedViewport.object_type_name_list
282  // -> nextFilteredSearchedObjectTypes = searchedObjectTypes \ alreadySearchedObjectTypesForFirstRun = currentRatedViewport.object_type_name_list
283  SearchedObjectTypes alreadySearchedObjectTypesForFirstRun =
284  getSearchedObjectTypesFromAWithoutB(searchedObjectTypes, currentRatedViewport.object_type_name_list);
285  robotStatePtrToInsertCopy->getTopPtuTuplePtr()->addAlreadySearchedObjectTypes(alreadySearchedObjectTypesForFirstRun);
286  reorderedRobotStates->push_back(robotStatePtrToInsertCopy);
287 
288  robotStatePtrToInsert->getTopPtuTuplePtr()->addAlreadySearchedObjectTypes(currentRatedViewport.object_type_name_list);
289  robotStatePtrToInsert->getTopPtuTuplePtr()->setRatedViewport(asr_next_best_view::RatedViewport());
290  }
291  }
292  ROS_DEBUG_STREAM("numberOfPosesWhereRatingIsNot0: " << numberOfPosesWhereRatingIsNot0);
293 
294  ROS_DEBUG_STREAM("Number of new poses to explore: " << reorderedRobotStates->size());
295  reorderedRobotStates->insert(reorderedRobotStates->end(), robotStatesToReorder->begin(), robotStatesToReorder->end());
296  ROS_DEBUG_STREAM("Number of all poses to explore: " << reorderedRobotStates->size());
297 
298  ROS_DEBUG_STREAM("Check if some poses are not needed anymore, because they have no objects to search for");
299  success &= filterPosesDependingOnAlreadyFoundObjectTypes(reorderedRobotStates, searchedObjectTypes);
300  ROS_DEBUG_STREAM("Number of poses to explore after filterPosesDependingOnAlreadyFoundObjectTypes: " << reorderedRobotStates->size());
301 
302  ROS_DEBUG_STREAM("Concat poses again");
303  DoFilterIteration concatFilterIteration(reorderedRobotStates);
304  FilterBasicPtr concatApproxEqualePosesPtr =
305  ConcatApproxEqualePosesPtr(new ConcatApproxEqualePoses(reorderedRobotStates, 0.0001, 0.0001, true, false));
306  concatFilterIteration.addFilter(concatApproxEqualePosesPtr);
307  success &= concatFilterIteration.doIteration();
308 
309  posesToExplorePtr = reorderedRobotStates;
310  ROS_DEBUG_STREAM("Number of poses to explore after concate again: " << posesToExplorePtr->size());
311 
312  // With one swap there can be at max two new poses
313  int numberOfNewPoses = static_cast<int>(posesToExplorePtr->size()) - orginalNumberOfPoses;
314  ROS_DEBUG_STREAM("Number of new poses after reorder: " << numberOfNewPoses);
315  return success;
316 }
317 
319  ROS_DEBUG_STREAM("deleteAllRedundantPoses");
320  DoFilterIteration concatFilterIterationForDeleteRedundant(robotStates);
321  FilterBasicPtr deleteApproxEqualePosesPtr =
322  ConcatApproxEqualePosesPtr(new ConcatApproxEqualePoses(robotStates, 0.0001, 0.0001, false, true, false));
323  concatFilterIterationForDeleteRedundant.addFilter(deleteApproxEqualePosesPtr);
324  return concatFilterIterationForDeleteRedundant.doIteration();
325 }
326 
328  ROS_DEBUG_STREAM("filterPosesDependingOnAlreadyFoundObjectTypes");
329  DoFilterIteration doFilterIterationForFilterPosesDependingOnAlreadyFoundObjectTypes = DoFilterIteration(robotStates);
332  filterPosesDependingOnAlreadyFoundObjectTypes->updateSearchedObjectTypes(searchedObjectTypes);
333  doFilterIterationForFilterPosesDependingOnAlreadyFoundObjectTypes.addFilter(filterPosesDependingOnAlreadyFoundObjectTypes);
334  return doFilterIterationForFilterPosesDependingOnAlreadyFoundObjectTypes.doIteration();
335 }
336 
338  asr_next_best_view::SetInitRobotState setInitRobotState;
339  setInitRobotState.request.robotState.pan = currentPan;
340  setInitRobotState.request.robotState.tilt = currentTilt;
341 
342  geometry_msgs::Pose currentRobotPose = poseHelperPtr->getCurrentRobotPose();
343  setInitRobotState.request.robotState.x = currentRobotPose.position.x;
344  setInitRobotState.request.robotState.y = currentRobotPose.position.y;
345 
346  tf::Quaternion quat;
347  tf::quaternionMsgToTF(currentRobotPose.orientation, quat);
348  setInitRobotState.request.robotState.rotation = quat.getAngle();
349 
350  return setInitRobotStateServiceClient.call(setInitRobotState);
351 }
352 
353 void DirectSearchManager::ptuDriverStateCallback(const sensor_msgs::JointState::ConstPtr &ptuState) {
354  currentPan = ptuState->position[0];
355  currentTilt = ptuState->position[1];
356 }
357 
359  asr_next_best_view::SetAttributedPointCloud setAttributePointCloud;
360  bool success = getViewportsFromWorldModel(setAttributePointCloud.request.viewports_to_filter);
361 
362  ISM::TableHelperPtr table_helper = ISM::TableHelperPtr(new ISM::TableHelper(dbfilenameParam));
363 
364  const std::vector<int> &setIds = table_helper->getSetIds();
365  for (const int &setId : setIds) {
366  const ISM::ObjectSetPtr &objectSetPtr = table_helper->getRecordedObjectSet(setId);
367  for (const ISM::ObjectPtr &object : objectSetPtr->objects) {
368  bool shouldAdd = false;
369  // only add objects and ids which we are looking for
370  for (const asr_msgs::AsrTypeAndId &searchedObjectTypeAndId : searchedObjectTypesAndIds) {
371  if (searchedObjectTypeAndId.type == object->type && searchedObjectTypeAndId.identifier == object->observedId) {
372  shouldAdd = true;
373  break;
374  }
375  }
376  if (shouldAdd) {
377  asr_msgs::AsrAttributedPoint pointCloudPoint;
378  pointCloudPoint.type = object->type;
379  pointCloudPoint.identifier = object->observedId;
380  pointCloudPoint.pose.position.x = object->pose->point->eigen[0];
381  pointCloudPoint.pose.position.y = object->pose->point->eigen[1];
382  pointCloudPoint.pose.position.z = object->pose->point->eigen[2];
383  pointCloudPoint.pose.orientation.w = object->pose->quat->eigen.w();
384  pointCloudPoint.pose.orientation.x = object->pose->quat->eigen.x();
385  pointCloudPoint.pose.orientation.y = object->pose->quat->eigen.y();
386  pointCloudPoint.pose.orientation.z = object->pose->quat->eigen.z();
387  setAttributePointCloud.request.point_cloud.elements.push_back(pointCloudPoint);
388  }
389  }
390  }
391 
392  success &= setAttributedPointCloudServiceClient.call(setAttributePointCloud);
393  success &= setAttributePointCloud.response.is_valid;
394  return success;
395 }
396 
397 
398 bool checkSearchedObjectTypesAreEquale(const SearchedObjectTypes &searchecObjectTypes1, const SearchedObjectTypes &searchecObjectTypes2) {
399  bool equale = searchecObjectTypes1.size() == searchecObjectTypes2.size();
400  if (equale) {
401  SearchedObjectTypes copySearchecObjectTypes1(searchecObjectTypes1);
402  SearchedObjectTypes copySearchecObjectTypes2(searchecObjectTypes2);
403  std::sort(copySearchecObjectTypes1.begin(), copySearchecObjectTypes1.end());
404  std::sort(copySearchecObjectTypes2.begin(), copySearchecObjectTypes2.end());
405  equale = std::equal(copySearchecObjectTypes1.begin(), copySearchecObjectTypes1.begin() + copySearchecObjectTypes1.size(), copySearchecObjectTypes2.begin());
406  }
407  return equale;
408 }
409 
411  // aWithoutB = a \ b
412  SearchedObjectTypes aWithoutB = a;
413  filterSearchedObjectTypes(aWithoutB, b);
414  return aWithoutB;
415 }
416 
417 }
418 
419 
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
virtual bool getNextRobotState(const SearchedObjectTypesAndIds &searchedObjectTypesAndIds)
void filterSearchedObjectTypes(SearchedObjectTypes &searched_object_types_to_filter, const SearchedObjectTypes &filter_searched_object_types)
tfScalar getAngle() const
boost::shared_ptr< geometry_msgs::Pose > PosePtr
Definition: ptu_tuple.hpp:28
virtual bool backToInitial(const SearchedObjectTypesAndIds &searchedObjectTypesAndIds)
boost::shared_ptr< RobotStatePtrVec > RobotStatePtrVecPtr
Definition: robot_state.hpp:52
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
bool setPointCloudInNBV(const SearchedObjectTypesAndIds &searchedObjectTypesAndIds)
boost::shared_ptr< ConcatApproxEqualePoses > ConcatApproxEqualePosesPtr
void addFilter(const FilterBasicPtr &filter)
bool call(MReq &req, MRes &res)
std::vector< std::string > SearchedObjectTypes
Definition: ptu_tuple.hpp:27
ROSCPP_DECL const std::string & getName()
boost::shared_ptr< PtuTuple > PtuTuplePtr
Definition: ptu_tuple.hpp:69
std::vector< PtuTuplePtr > PtuTuplePtrVec
Definition: ptu_tuple.hpp:70
FilterPosesDependingOnAlreadyFoundObjectTypesPtr filterPosesDependingOnAlreadyFoundObjectTypesPtr
bool reorderPosesByNBV(const SearchedObjectTypes &searchedObjectTypes)
bool filterPosesDependingOnAlreadyFoundObjectTypes(const RobotStatePtrVecPtr &robotStates, const SearchedObjectTypes &searchedObjectTypes) const
bool deleteAllRedundantPoses(const RobotStatePtrVecPtr &robotStates) const
boost::shared_ptr< DoFilterIteration > DoFilterIterationPtr
virtual bool backToInitial(const SearchedObjectTypesAndIds &searchedObjectTypesAndIds)
bool checkSearchedObjectTypesAreEquale(const SearchedObjectTypes &searchecObjectTypes1, const SearchedObjectTypes &searchecObjectTypes2)
std::vector< asr_msgs::AsrTypeAndId > SearchedObjectTypesAndIds
static void quaternionMsgToTF(const geometry_msgs::Quaternion &msg, Quaternion &bt)
#define ROS_INFO(...)
#define ROS_ASSERT_MSG(cond,...)
void ptuDriverStateCallback(const sensor_msgs::JointState::ConstPtr &ptuState)
std::vector< RobotStatePtr > RobotStatePtrVec
Definition: robot_state.hpp:51
PosePtr parsePose(const rapidxml::xml_node<> *node, const char *poseName) const
#define ROS_DEBUG_STREAM(args)
bool getViewportsFromWorldModel(std::vector< asr_msgs::AsrViewport > &viewports)
boost::shared_ptr< PtuTuplePtrVec > PtuTuplePtrVecPtr
Definition: ptu_tuple.hpp:71
#define ROS_INFO_STREAM(args)
boost::shared_ptr< FilterPosesDependingOnAlreadyFoundObjectTypes > FilterPosesDependingOnAlreadyFoundObjectTypesPtr
bool getParam(const std::string &key, std::string &s) const
boost::shared_ptr< RobotState > RobotStatePtr
Definition: robot_state.hpp:50
SearchedObjectTypes getSearchedObjectTypesFromAWithoutB(const SearchedObjectTypes &a, const SearchedObjectTypes &b)
#define ROS_ERROR_STREAM(args)
bool parsePosesToExploreFromXML(const std::string &path)
#define ROS_ERROR(...)
virtual bool getNextRobotState(const SearchedObjectTypesAndIds &searchedObjectTypesAndIds)
DoFilterIterationPtr doFilterIterationPtrForPosesDependingOnAlreadyFoundObjectTypes


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