def handle_next_location(self, req): print "Next probable location service: Recieved request for next probable search location in %s" % req.room ret = NextProbableLocationResponse() # Look through all the location with the same probabilities and get the minimum nexts = self._cols[req.room].next_same() min_param = rospy.get_param(PARAM_NAME + req.room + '/' + nexts[0][0]) ret.location = nexts[0][0] if len(nexts) > 1: make_plan = None if self._make_plan: try: rospy.wait_for_service('/move_base/make_plan', 0.1) make_plan = rospy.ServiceProxy('/move_base/make_plan', GetPlan) except rospy.ROSException: pass # make_plan is already set to None robot_pose = Pose() robot_pose.position.x = 0 robot_pose.position.y = 0 robot_pose.position.z = 0 robot_pose = transform_pose(robot_pose, '/base_link', '/map', timeout=3) # Get robot's pose in /map coordinates robot_tuple = (robot_pose.position.x, robot_pose.position.y) pose_tuple = (min_param[1], min_param[2]) min_dist = ofb_utils.distance(robot_tuple, pose_tuple, make_plan) for name, _ in nexts[1:]: param = rospy.get_param(PARAM_NAME + req.room + '/' + name) pose_tuple = (param[1], param[2]) d = ofb_utils.distance(robot_tuple, pose_tuple, make_plan) if d < min_dist: min_dist = d min_param = param ret.location = name # All the locations in nexts have the same probabilty self._cols[req.room].swap_elements(nexts[0], (ret.location, nexts[0][1])) pose = Pose() pose.position = Point(min_param[1], min_param[2], 0) pose.orientation = Quaternion(*quaternion_from_euler(0, 0, min_param[3])) ret.loc_position = pose return ret
def _create_distance_matrix(self): n = self._n make_plan = None if self._with_plan: try: rospy.wait_for_service('/move_base/make_plan', 0.1) make_plan = rospy.ServiceProxy('/move_base/make_plan', GetPlan) except ROSException: pass # We already have make_plan set to None # i is the row, j the column #self._dist_mat = [[(self._distance(i, j, make_plan) if i != j else -1) for j in range(n)] for i in range(n)] self._dist_mat = [[ofb_utils.distance(self._nodes[i], self._nodes[j], make_plan) for j in range(n) if j < i] for i in range(n)]