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)
