def get_radius(self, node, parent): radius = 1.0 radii = [] e = (parent, node) e1 = (node, parent) if e in self.edges: radii.append(pu.D(self.edges[e][0], self.edges[e][1])) if e1 in self.edges: radii.append(pu.D(self.edges[e1][0], self.edges[e1][1])) if radii: radius = max(radii) return radius
def shared_point_handler(self, auction_data): pu.log_msg(self.robot_id, "Received auction", self.debug_mode) data = auction_data.req_data sender_id = data.msg_header.header.frame_id poses = data.poses if not poses: pu.log_msg(self.robot_id, "No poses received. Proceeding to my next frontier", self.debug_mode) self.start_exploration_action(self.frontier_point) return SharedPointResponse(auction_accepted=1, res_data=None) received_points = [] distances = [] robot_pose = self.get_robot_pose() robot_pose = pu.scale_up(robot_pose, self.graph_scale) for p in poses: received_points.append(p) point = (p.position.x, p.position.y, self.get_elevation((p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w))) distance = pu.D(robot_pose, point) distances.append(distance) auction = Auction() auction.msg_header.header.frame_id = '{}'.format(self.robot_id) auction.msg_header.header.stamp = rospy.Time.now() auction.msg_header.sender_id = str(self.robot_id) auction.msg_header.receiver_id = str(sender_id) auction.msg_header.topic = 'auction_feedback' auction.poses = received_points auction.distances = distances auction.session_id = data.session_id return SharedPointResponse(auction_accepted=1, res_data=auction)
def localize_nodes(self, id_pose, pivot_node): all_nodes = list(self.adj_list) pivot_key = list(pivot_node.keys())[0] pivot_pose = pivot_node[pivot_key] node_dist = {} for n in all_nodes: n_dist = pu.D(pivot_pose, n) node_dist[n_dist] = n new_n = node_dist[min(node_dist)] pivot_node[pivot_key] = new_n parent = {new_n: None} visited = [] S = deque([new_n]) node_dist = {} while len(S) > 0: u = S.popleft() if u != pivot_node.values()[0]: self.compute_distance(id_pose, node_dist, u) neighbors = self.adj_list[u] for v in neighbors: if v not in visited: S.append(v) parent[v] = u visited.append(u) node_keys = list(id_pose.keys()) for k in node_keys: min_dist = min(node_dist[k].keys()) n = node_dist[k][min_dist] id_pose[k] = n # update poses of parent nodes
def move_result_callback(self, data): id_0 = "robot_{}_{}_explore".format(self.robot_id, self.goal_count - 1) if data.status: if data.status.status == pu.ABORTED: pu.log_msg(self.robot_id, "Motion failed", self.debug_mode) rospy.logerr(data) self.move_to_stop() self.motion_failed = True self.has_arrived = True if self.moving_to_frontier: self.moving_to_frontier = False self.current_point = self.get_robot_pose() elif data.status.status == pu.SUCCEEDED: self.has_arrived = True if self.moving_to_frontier: self.moving_to_frontier = False if not self.prev_explored: self.prev_explored = self.current_point self.traveled_distance.append({ 'time': rospy.Time.now().to_sec(), 'traved_distance': pu.D(self.prev_explored, self.current_point) }) self.prev_explored = self.current_point
def get_closest_point(self, pose, points): dists = {} chosen_point = None for p in points: dists[pu.D(pose, p)] = p if dists: chosen_point = dists[min(dists.keys())] return chosen_point
def compute_distance(self, id_pose, node_dist, u): for k, node in id_pose.items(): d = pu.D(node, u) if self.line_has_obstacles(u, node): d = INF if k not in node_dist: node_dist[k] = {d: u} else: node_dist[k][d] = u
def already_explored(self, v, parent_nodes): is_visited = False dists = {} for node_id, val in parent_nodes.items(): dists[pu.D(v, val)] = node_id closest_distance = min(dists.keys()) if closest_distance < self.lidar_scan_radius: is_visited = True return is_visited
def compute_aux_nodes(self, u, obs): ax_nodes = [u] q1 = obs[0] q2 = obs[1] width = pu.D(q1, q2) if width >= self.lidar_scan_radius: req_poses = width / self.lidar_scan_radius ax_nodes += pu.line_points(obs[0], obs[1], req_poses) return ax_nodes
def shared_point_handler(self, auction_data): pu.log_msg(self.robot_id, "Received auction", self.debug_mode) data = auction_data.req_data session_id = data.session_id if self.is_sender or not self.session_id or session_id != self.session_id: return SharedPointResponse(auction_accepted=0) pu.log_msg(self.robot_id, "creating auction response", self.debug_mode) sender_id = data.msg_header.sender_id poses = data.poses if not poses and self.frontier_ridge: pu.log_msg(self.robot_id, "No poses received. Proceeding to my next frontier", self.debug_mode) self.start_exploration_action(self.frontier_ridge) return SharedPointResponse(auction_accepted=1, res_data=None) received_points = [] distances = [] pu.log_msg(self.robot_id, "Getting robot pose", self.debug_mode) robot_pose = self.get_robot_pose() pu.log_msg(self.robot_id, "Received robot pose: {}".format(robot_pose), self.debug_mode) p_in_sender = PoseStamped() p_in_sender.header = data.msg_header.header for p_from_sender in poses: p_in_sender.pose = p_from_sender p = self.listener.transformPose( "robot_{}/map".format(self.robot_id), p_in_sender) received_points.append(p) point = (p.pose.position.x, p.pose.position.y) #self.get_elevation((p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w))) distance = pu.D(robot_pose, point) distances.append(distance) auction = Auction() auction.msg_header.header.frame_id = data.msg_header.header.frame_id auction.msg_header.header.stamp = rospy.Time.now() auction.msg_header.sender_id = str(self.robot_id) auction.msg_header.receiver_id = str(sender_id) auction.msg_header.topic = 'auction_feedback' auction.poses = poses auction.distances = distances auction.session_id = data.session_id # start waiting for location after bidding self.time_after_bidding = rospy.Time.now().to_sec() self.waiting_for_frontier_point = True # reset waiting for auction flags self.waiting_for_auction = False self.auction_waiting_time = rospy.Time.now().to_sec() pu.log_msg(self.robot_id, "sending auction data back", self.debug_mode) return SharedPointResponse(auction_accepted=1, res_data=auction)
def compute_next_frontier(self, taken_poses, frontier_points): ridge = None robot_pose = self.get_robot_pose() dist_dict = {} for point in frontier_points: dist_dict[pu.D(robot_pose, point)] = point while not ridge and dist_dict: min_dist = min(list(dist_dict)) closest = dist_dict[min_dist] if closest not in taken_poses: ridge = frontier_points[closest] break del dist_dict[min_dist] return ridge
def goal_callback(self, data): rospy.logerr('Received a new pose: {}'.format(data.pose.position)) if len(self.previous_point) == 0: self.previous_point = self.get_robot_pose() goal = [0.0] * 2 goal[pu.INDEX_FOR_X] = data.pose.position.x goal[pu.INDEX_FOR_X] = data.pose.position.y self.traveled_distance.append({ 'time': rospy.Time.now().to_sec(), 'traved_distance': pu.D(self.previous_point, goal) }) self.previous_point = goal
def shared_frontier_handler(self, req): data = req.frontier pu.log_msg(self.robot_id, "Received new frontier point", self.debug_mode) self.frontier_ridge = data.frontiers[data.frontier_id] new_point = [0.0] * 2 new_point[pu.INDEX_FOR_X] = self.frontier_ridge.position.x new_point[pu.INDEX_FOR_Y] = self.frontier_ridge.position.y new_point = pu.scale_down(new_point, self.graph_scale) self.frontier_point = new_point robot_pose = self.get_robot_pose() self.frontier_data.append( {'time': rospy.Time.now().to_sec(), 'distance_to_frontier': pu.D(robot_pose, new_point)}) self.move_robot_to_goal(self.frontier_point, TO_FRONTIER) pu.log_msg(self.robot_id, "Received allocated points", self.debug_mode) return SharedFrontierResponse(success=1)
def localize_parent_nodes(self, parent_nodes, all_visited_nodes): all_nodes = list(self.adj_list) node_keys = list(parent_nodes) node_dist = {} for n in all_nodes: for node_id, val in parent_nodes.items(): n_dist = pu.D(val, n) if node_id not in node_dist: node_dist[node_id] = {n_dist: n} else: node_dist[node_id][n_dist] = n for k in node_keys: n_dists = node_dist[k].keys() new_n = node_dist[k][min(n_dists)] parent_nodes[k] = new_n all_visited_nodes[new_n] = None
def move_robot_to_goal(self, goal, theta=0): if len(self.previous_point) == 0: self.previous_point = self.get_robot_pose() pu.log_msg(self.robot_id, "Prev: {}, Current: {}".format(self.previous_point, goal), 1) self.traveled_distance.append({ 'time': rospy.Time.now().to_sec(), 'traved_distance': pu.D(self.previous_point, goal) }) self.previous_point = goal self.current_point = goal move = MoveToPosition2DGoal() frame_id = 'robot_{}/map'.format(self.robot_id) # TODO check. move.header.frame_id = frame_id move.target_pose.x = goal[INDEX_FOR_X] move.target_pose.y = goal[INDEX_FOR_Y] move.target_pose.theta = theta move.target_distance = self.target_distance move.target_angle = self.target_angle self.last_distance = -1 self.client_motion.send_goal(move, feedback_cb=self.feedback_motion_cb) self.prev_pose.append(self.current_pose) if self.client_motion.wait_for_result(): state = self.client_motion.get_state() if self.current_state == self.MOVE_TO: if state == GoalStatus.SUCCEEDED: self.prev_pose += self.path_to_leaf[1:] self.current_pose = goal else: self.current_pose = self.get_robot_pose() self.prev_pose += self.path_to_leaf[1:pu.get_closest_point( self.current_pose, np.array(self.path_to_leaf))[0] + 1] self.current_state = self.DECISION elif self.current_state == self.MOVE_TO_LEAF: self.current_state = self.DECISION self.current_pose = self.get_robot_pose() # publish to feedback pose = Pose() pose.position.x = self.current_pose[INDEX_FOR_X] pose.position.y = self.current_pose[INDEX_FOR_Y] self.goal_feedback_pub.publish(pose)
def request_and_share_frontiers(self, direction=0): frontier_point_response = self.fetch_frontier_points(FrontierPointRequest(count=len(self.candidate_robots) + 1)) frontier_points = frontier_point_response.frontiers robot_assignments = {} if frontier_points: rpose=self.get_robot_pose() distances=[pu.D(rpose,(p.position.x,p.position.y)) for p in frontier_points] bgraph = {self.robot_id: [distances[i] for i in range(len(frontier_points))]} auction = self.create_auction(frontier_points) pu.log_msg(self.robot_id, "session devices: {}".format(self.candidate_robots), self.debug_mode) for rid in self.candidate_robots: pu.log_msg(self.robot_id, "Action Request to Robot {}".format(rid), self.debug_mode) auction_response = self.shared_point_srv_map[rid](SharedPointRequest(req_data=auction)) if auction_response.auction_accepted == 1: data = auction_response.res_data bgraph[int(rid)] = [data.distances[i] for i in range(len(data.distances))] robot_assignments = self.hungarian_assignment(bgraph, frontier_points) else: pu.log_msg(self.robot_id, "All robots are busy..", self.debug_mode)
def fetch_rendezvous_points_handler(self, data): count = data.count rospy.logerr('Rendezvous request received') self.generate_graph() rendezvous_points = [] origin = self.get_robot_pose() free_points = [] all_poses = self.latest_map.get_explored_region() for pos in all_poses: if pu.D(origin, pos) <= self.comm_range: free_points.append(pos) if free_points: hull, boundary_points = graham_scan(free_points, count, False) for ap in boundary_points: pose = Pose() pose.position.x = ap[INDEX_FOR_X] pose.position.y = ap[INDEX_FOR_Y] rendezvous_points.append(pose) if rendezvous_points: self.last_frontier_points = rendezvous_points return RendezvousPointsResponse(poses=self.last_frontier_points)
def shared_point_handler(self, auction_data): # pu.log_msg(self.robot_id, "Received auction", self.debug_mode) data = auction_data.req_data session_id = data.session_id if self.is_sender or not self.session_id or session_id != self.session_id: return SharedPointResponse(auction_accepted=0) sender_id = data.msg_header.header.frame_id poses = data.poses if not poses and self.frontier_ridge: # pu.log_msg(self.robot_id, "No poses received. Proceeding to my next frontier", self.debug_mode) self.start_exploration_action(self.frontier_ridge) return SharedPointResponse(auction_accepted=1, res_data=None) received_points = [] distances = [] robot_pose = self.get_robot_pose() for p in poses: received_points.append(p) point = (p.position.x, p.position.y, self.get_elevation((p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w))) distance = pu.D(robot_pose, point) distances.append(distance) auction = Auction() auction.msg_header.header.frame_id = '{}'.format(self.robot_id) auction.msg_header.header.stamp = rospy.Time.now() auction.msg_header.sender_id = str(self.robot_id) auction.msg_header.receiver_id = str(sender_id) auction.msg_header.topic = 'auction_feedback' auction.poses = received_points auction.distances = distances auction.session_id = data.session_id # start waiting for location after bidding self.time_after_bidding = rospy.Time.now().to_sec() self.waiting_for_frontier_point = True # reset waiting for auction flags self.waiting_for_auction = False self.auction_waiting_time = rospy.Time.now().to_sec() # pu.log_msg(self.robot_id, "sending data back", self.debug_mode) return SharedPointResponse(auction_accepted=1, res_data=auction)
def shared_frontier_handler(self, req): data = req.frontier pu.log_msg(self.robot_id, "Received new frontier point", self.debug_mode) if not self.is_sender and data.session_id == self.session_id: # reset waiting for frontier point flags self.waiting_for_frontier_point = False self.time_after_bidding = rospy.Time.now().to_sec() # ============ End here ================= self.frontier_ridge = data.ridge new_point = [0.0] * 2 new_point[pu.INDEX_FOR_X] = self.frontier_ridge.nodes[1].position.x new_point[pu.INDEX_FOR_Y] = self.frontier_ridge.nodes[1].position.y robot_pose = self.get_robot_pose() self.frontier_data.append({ 'time': rospy.Time.now().to_sec(), 'distance_to_frontier': pu.D(robot_pose, new_point) }) self.start_exploration_action(self.frontier_ridge) pu.log_msg(self.robot_id, "Received allocated points", self.debug_mode) return SharedFrontierResponse(success=1)