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)]
