filter_is_position_allowed.hpp
Go to the documentation of this file.
1 
18 #pragma once
19 
20 #include "filter/filter_basic.hpp"
21 #include "asr_robot_model_services/IsPositionAllowed.h"
22 
23 namespace directSearchWS {
24 
26 
27 private:
30 
32 
33 public:
34  FilterIsPositionAllowed(const RobotStatePtrVecPtr &posesToExplorePtr) : FilterBasic(posesToExplorePtr), deleteCount(0), wasSuccessful(true) {
36  isPositionAllowedServiceClient = n.serviceClient<asr_robot_model_services::IsPositionAllowed>("/asr_robot_model_services/IsPositionAllowed");
37  }
38 
40 
41  bool needIteration() {
42  deleteCount = 0;
43  wasSuccessful = true;
44  isPositionAllowedServiceClient.waitForExistence();
45  return true;
46  }
47 
49  ROS_INFO_STREAM("Number of deleted ptuTuple by FilterIsPositionAllowed: " << deleteCount);
50  return wasSuccessful;
51  }
52 
53  bool shouldPtuTupleBeDeleted(const RobotStatePtrVec::iterator &posesToExploreIter, const PtuTuplePtrVec::iterator &ptuTuplePtrIter) {
54  bool shouldDelete = false;
55  asr_robot_model_services::IsPositionAllowed isPositionAllowedCall;
56  isPositionAllowedCall.request.targetPosition = posesToExploreIter->get()->getRobotPosePtr()->position;
57  if (isPositionAllowedServiceClient.call(isPositionAllowedCall)) {
58  shouldDelete = !isPositionAllowedCall.response.isAllowed;
59  } else {
60  ROS_ERROR_STREAM("Service IsPositionAllowed to robot_model_services was not successful -> no filter of FilterIsPositionAllowed");
61  wasSuccessful = false;
62  }
63 
64  if (shouldDelete) {
65  ++deleteCount;
66  }
67  return shouldDelete;
68  }
69 
70 };
71 
73 
74 }
75 
76 
77 
78 
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
bool call(MReq &req, MRes &res)
ROSCPP_DECL const std::string & getName()
const RobotStatePtrVecPtr & posesToExplorePtr
boost::shared_ptr< FilterIsPositionAllowed > FilterIsPositionAllowedPtr
bool waitForExistence(ros::Duration timeout=ros::Duration(-1))
bool shouldPtuTupleBeDeleted(const RobotStatePtrVec::iterator &posesToExploreIter, const PtuTuplePtrVec::iterator &ptuTuplePtrIter)
#define ROS_INFO_STREAM(args)
#define ROS_ERROR_STREAM(args)
FilterIsPositionAllowed(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