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