def create_probability_route(userdata):
furn_prob_map = rospy.get_param(FURN_PROB_PARAM)
n = len(userdata.in_nodes)
route = range(n)
# Get a list of tuples (probability, index_of_name)
prob_index = zip(reduce(lambda acc, x: acc + [furn_prob_map[x]], userdata.in_furniture_name_list, []), route)
prob_index.sort(key=lambda x: x[0], reverse=True) # Sort by probability
robot_pose = Pose()
robot_pose.position.x = 0
robot_pose.position.y = 0
robot_pose.position.z = 0
# Get robot's pose in /map coordinates
robot_pose = transform_pose(robot_pose, '/base_link', '/map', timeout=3)
actual = (robot_pose.position.x, robot_pose.position.y)
route[0] = prob_index[0][1]
for i in xrange(1, n):
route[i] = prob_index[i][1]
if prob_index[i][0] == 0: # If probability is 0 we don't look at it.
del route[i]
del userdata.in_furniture_name_list[i]
elif prob_index[i][0] == prob_index[i-1][0]: # If two things have the same probability, get the closer one
di_r = ofb_utils.euclidean_distance(userdata.in_nodes[route[i]], actual)
di1_r = ofb_utils.euclidean_distance(userdata.in_nodes[route[i-1]], actual)
if di_r < di1_r: # The index at i is nearer than the one at i-1
route[i] = route[i-1]
route[i-1] = prob_index[i][1]
userdata.out_route = route
userdata.out_furniture_list = userdata.in_furniture_name_list
return succeeded
def check_gather_data(userdata):
furn_names = userdata.in_furniture_response[0]
furn_poses = userdata.in_furniture_response[1]
for i in xrange(len(furn_names)):
i_pose = (furn_poses[i].pose.position.x, furn_poses[i].pose.position.y)
for n, p in userdata.in_furniture_info:
p_pose = (p.pose.position.x, p.pose.position.y)
if (furn_names[i] == n) and (ofb_utils.euclidean_distance(p_pose, i_pose) <= distance_treshold):
break
else: # The loop ended without breaking
furn_poses[i].pose = transform_pose(furn_poses[i].pose, furn_poses[i].header.frame_id, '/base_link', timeout=3)
furn_poses[i].header.frame_id = '/base_link'
userdata.in_furniture_info.append((furn_names[i], furn_poses[i]))
userdata.out_furniture_info = userdata.in_furniture_info
return succeeded
def check_gather_data(userdata):
table_data = userdata.in_tabletop_response
table_orient = table_data.pose.pose.orientation
if table_orient.w < 0.95 or table_orient.x > 0.05 or table_orient.y > 0.05 or table_orient.z > 0.05:
# The surface is not horizontal, so we don't care about it.
print 'It was not a table...'
return succeeded
table_pose = ((table_data.x_min+table_data.x_max)/2, (table_data.y_min+table_data.y_max)/2)
for ti in userdata.in_tabletop_info:
tp = ((ti.x_min+ti.x_max)/2, (ti.y_min+ti.y_max)/2)
if ofb_utils.euclidean_distance(tp, table_pose) <= distance_treshold:
break
else: # The loop ended without breaking
userdata.in_tabletop_info.append(userdata.in_tabletop_response)
userdata.out_tabletop_info = userdata.in_tabletop_info
return succeeded
def check_ttop_data(userdata):
table_data = userdata.in_tabletop_data
for t in table_data:
z = t.pose.pose.position.z
q = Quaternion(*quaternion_from_euler(0, 0, 0))
table_pose = Pose(Point(t.x_min-dist_to_table, (t.y_min+t.y_max)/2, z), q)
src_frame = t.pose.header.frame_id
# src_frame should be either base_link or base_footprint
table_pose = transform_pose(table_pose, src_frame, DEST_FRAME, timeout=3)
tuple_pose = (table_pose.position.x, table_pose.position.y)
for tp in userdata.in_pose_list:
if ofb_utils.euclidean_distance(tp, tuple_pose) <= distance_treshold:
break
else: # The loop ended without breaking
userdata.in_pose_list.append(tuple_pose)
userdata.out_pose_list = userdata.in_pose_list
userdata.in_orientation_list.append(table_pose.orientation) # FIXME table_pose? Should be more_less equal...
userdata.out_orientation_list = userdata.in_orientation_list
return succeeded
def check_furn_data(userdata):
furniture_data = userdata.in_furniture_data
for f_name, f_pose in furniture_data:
q = Quaternion(*quaternion_from_euler(0, 0, 0))
f_pose.pose.position.x -= dist_to_furniture
furniture_pose = Pose(f_pose.pose.position, q)
src_frame = f_pose.header.frame_id
# src_frame should be either base_link or base_footprint
furniture_pose = transform_pose(furniture_pose, src_frame, DEST_FRAME, timeout=3)
tuple_pose = (furniture_pose.position.x, furniture_pose.position.y)
for n, tp in zip(userdata.in_name_list, userdata.in_pose_list):
if (f_name == n) and (ofb_utils.euclidean_distance(tp, tuple_pose) <= distance_treshold):
break
else: # The loop ended without breaking
userdata.in_pose_list.append(tuple_pose)
userdata.out_pose_list = userdata.in_pose_list
userdata.in_orientation_list.append(furniture_pose.orientation)
userdata.out_orientation_list = userdata.in_orientation_list
userdata.in_name_list.append(f_name)
userdata.out_name_list = userdata.in_name_list
return succeeded
def visible(self, g, q):
dist_mtrs = ofb_utils.euclidean_distance(g, q)*self._resolution
if dist_mtrs > self.MAX_DIST_MTRS or dist_mtrs < self.MIN_DIST_MTRS:
return False
return self.bresenham_supercover_visibility(g[0], g[1], q[0], q[1])
