def cancel_exploration(self):
self.intersections_requested = True
if self.is_exploring:
result = self.goal_cancel_srv(CancelExplorationRequest(req=1))
self.is_exploring = False
pu.log_msg(self.robot_id, "Canceling exploration ...",
self.debug_mode)
def move_result_callback(self, data):
id_0 = "robot_{}_{}_{}".format(self.robot_id, self.goal_count - 1,
TO_FRONTIER)
if data.status:
if data.status.status == ABORTED:
if data.status.goal_id.id == id_0:
if self.move_attempt < MAX_ATTEMPTS:
self.rotate_robot()
self.move_robot_to_goal(self.frontier_point,
TO_FRONTIER)
self.move_attempt += 1
else:
self.failed_points.append(self.frontier_point)
rv_ps = [
p for p in self.frontier_points
if p not in self.failed_points
]
if rv_ps:
self.frontier_point = rv_ps[-1]
self.move_robot_to_goal(self.frontier_point,
TO_FRONTIER)
else: # start exploration anyways
self.initiate_exploration()
elif data.status.status == SUCCEEDED:
if data.status.goal_id.id == id_0:
pu.log_msg(
self.robot_id,
"Arrived at Frontier: {}".format(self.frontier_point),
self.debug_mode)
self.initiate_exploration()
def move_status_callback(self, data):
id_4 = "robot_{}_{}_{}".format(self.robot_id, self.goal_count - 1, FROM_EXPLORATION)
id_1 = "robot_{}_{}_{}".format(self.robot_id, self.goal_count - 1, TO_RENDEZVOUS)
id_0 = "robot_{}_{}_{}".format(self.robot_id, self.goal_count - 1, TO_FRONTIER)
if data.status_list:
goal_status = data.status_list[0]
if goal_status.goal_id.id:
if goal_status.goal_id.id == id_4 or goal_status.goal_id.id == id_1:
if not self.robot_stopped and goal_status.status == ACTIVE or goal_status.status == ABORTED:
close_devices = self.get_close_devices()
if self.parent_robot_id in close_devices:
self.move_to_stop()
self.robot_stopped = True
pose = self.get_robot_pose()
self.rendezvous_point = pose
if goal_status.goal_id.id == id_1:
self.push_messages_to_receiver([self.parent_robot_id])
else:
pu.log_msg(self.robot_id, "Back from exploration.. stopped: there's connectivity",
self.debug_mode)
# self.move_to_stop()
self.share_data_with_parent()
elif goal_status.goal_id.id == id_0:
if goal_status.status == ABORTED:
self.move_to_stop()
self.initiate_exploration()
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 feedback_motion_cb(self, feedback):
if self.current_state == self.MOVE_TO_LEAF:
self.prev_pose.append(self.get_robot_pose())
if (np.isclose(self.last_distance, feedback.distance) or \
np.isclose(feedback.distance, self.last_distance)):
self.same_location_counter += 1
if self.same_location_counter > 5: # TODO parameter:
self.client_motion.cancel_goal()
else:
self.last_distance = feedback.distance
self.same_location_counter = 0
cpose = self.get_robot_pose()
if self.graph.should_communicate(cpose):
# self.current_state = self.COMMUNICATE
pu.log_msg(self.robot_id, "communicate", self.debug_mode)
cp = Pose()
cp.position.x = cpose[INDEX_FOR_X]
cp.position.x = cpose[INDEX_FOR_X]
self.intersec_pub.publish(cp)
if rospy.Time.now(
) - self.graph.latest_map.header.stamp > rospy.Duration(
10): # TODO parameter:
self.graph.generate_graph()
if not self.graph.latest_map.is_frontier(
self.prev_goal_grid, self.goal_grid,
self.graph.min_range_radius):
self.client_motion.cancel_goal()
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 start_exploration_action(self, new_point, direction=0):
if not self.frontier_point:
self.previous_frontier_point = copy.deepcopy(self.frontier_point)
self.frontier_point = new_point
self.move_robot_to_goal(self.frontier_point, TO_FRONTIER)
pu.log_msg(self.robot_id,
"Going to frontier {} ..".format(self.frontier_point),
self.debug_mode)
def explore_feedback_callback(self, data):
# self.feedback_count += 1
# if self.feedback_count == 1:
if not self.is_exploring:
self.is_exploring = True
self.session_id = None
self.intersections_requested = False
pu.log_msg(self.robot_id, "Received goal feedback",
self.debug_mode)
def initiate_exploration(self):
result = self.start_exploration()
if result:
pu.log_msg(self.robot_id, "Starting Exploration ...", self.debug_mode)
self.exploration_start_time = rospy.Time.now()
self.is_exploring = True
self.heading_back = 0
else:
pu.log_msg(self.robot_id, "Failed to start exploration...", self.debug_mode)
def cancel_exploration(self):
if self.exploration_id:
pu.log_msg(self.robot_id, "Cancelling exploration...",
self.debug_mode)
goal_id = GoalID()
goal_id.id = self.exploration_id
self.cancel_explore_pub.publish(goal_id)
else:
pu.log_msg(self.robot_id, "Exploration ID not set...",
self.debug_mode)
def move_back_to_base_station(self):
self.cancel_exploration()
self.heading_back = 1
self.is_exploring = False
robot_pose = self.get_robot_pose()
pu.log_msg(self.robot_id, "Heading back to RV point: {}".format(self.rendezvous_point), self.debug_mode)
point = self.get_closest_point(robot_pose, self.rendezvous_points)
if point:
self.rendezvous_point = point
self.move_robot_to_goal(self.rendezvous_point, FROM_EXPLORATION)
def spin(self):
r = rospy.Rate(0.1)
while not rospy.is_shutdown():
try:
self.update_base_points()
pu.log_msg(self.robot_id, "Is exploring: {}".format(self.is_exploring), self.debug_mode)
if self.is_exploring:
self.check_if_time_to_getback_to_rendezvous()
except Exception as e:
pu.log_msg(self.robot_id, "Got Error: {}".format(e), 1-self.debug_mode)
r.sleep()
def start_gvg_exploration(self, edge):
parent_id = self.get_id()
leaf_id = self.get_id()
parent_ids = {leaf_id: parent_id, parent_id: parent_id}
id_pose = {parent_id: edge[0], leaf_id: edge[1]}
all_visited_poses = {edge[0]: parent_id, edge[1]: leaf_id}
# -------------------------------- DFS starts here ---------------
pivot_node = {parent_id: edge[0]}
parent = {leaf_id: parent_id}
visited = [parent_id]
pivot_id = pivot_node.keys()[0]
while not rospy.is_shutdown():
if self.cancel_request:
break
S = [pivot_id]
self.fetch_new_graph()
self.localize_nodes(id_pose, pivot_node)
while len(S) > 0:
u = S.pop()
pivot_node = {u: id_pose[u]}
leaf_pose = id_pose[u]
self.move_to_frontier(leaf_pose,
theta=pu.theta(id_pose[parent_ids[u]],
leaf_pose))
start_time = rospy.Time.now().to_sec()
self.fetch_new_graph()
self.localize_nodes(id_pose, pivot_node)
leaves = self.get_leaves(id_pose[u], id_pose[parent_ids[u]])
if not leaves:
if id_pose[u] not in all_visited_poses:
S.append(u)
else:
pivot_node = {parent_ids[u]: id_pose[parent_ids[u]]}
else:
best_leaf = self.get_best_leaf(leaves)
leaf_parent = leaves[best_leaf]
v_id = self.get_id()
p_id = self.get_id()
id_pose[v_id] = best_leaf
id_pose[p_id] = leaf_parent
parent_ids[v_id] = p_id
S.append(v_id)
parent[v_id] = u
visited.append(u)
all_visited_poses[leaf_pose] = u
end_time = rospy.Time.now().to_sec()
gvg_time = end_time - start_time
self.explore_computation.append({
'time': start_time,
'gvg_compute': gvg_time
})
pu.log_msg(self.robot_id, "Returned from DFS...", self.debug_mode)
sleep(1)
def shared_data_handler(self, data):
pu.log_msg(self.robot_id, "Received request from robot..",
self.debug_mode)
received_buff_data = data.req_data
sender_id = received_buff_data.msg_header.header.frame_id
self.update_share_time(sender_id)
buff_data = self.create_buff_data(sender_id, is_alert=0)
self.process_data(sender_id, received_buff_data)
# thread = Thread(target=self.process_data, args=(sender_id, received_buff_data,))
# thread.start()
return SharedDataResponse(res_data=buff_data)
def home_alert_callback(self, data):
pu.log_msg(self.robot_id, 'Home Alert received from {}'.format(data.robot_id), self.debug_mode)
received_data = data.req_data
sender_id = received_data.msg_header.header.frame_id
self.update_share_time(sender_id)
self.process_data(sender_id,received_data)
# thread = Thread(target=self.process_data, args=(sender_id, received_data,))
# thread.start()
buff_data = self.create_buff_data(sender_id, is_home_alert=1)
self.delete_data_for_id(sender_id)
return HomeAlertResponse(res_data=buff_data)
def exploration_callback(self, data):
if data.status_list:
goal_status = data.status_list[0]
gid = goal_status.goal_id.id
if gid and self.exploration_id != gid:
self.exploration_id = gid
status = goal_status.status
if status == ABORTED:
pu.log_msg(self.robot_id, "Robot stopped share data..",
self.debug_mode)
self.move_to_stop()
self.start_exploration()
def start_exploration_action(self, new_point, direction=0):
if direction == TO_RENDEZVOUS:
self.rendezvous_point = copy.deepcopy(new_point)
self.move_robot_to_goal(self.rendezvous_point, TO_RENDEZVOUS)
pu.log_msg(self.robot_id, "Going to RV point: {}".format(self.rendezvous_point), self.debug_mode)
elif direction == TO_FRONTIER:
if not self.frontier_point:
self.previous_frontier_point = copy.deepcopy(self.frontier_point)
self.frontier_point = new_point
# self.frontier_point=(45.0,13.0)
self.move_robot_to_goal(self.frontier_point, TO_FRONTIER)
pu.log_msg(self.robot_id, "Going to frontier: {}".format(self.frontier_point), self.debug_mode)
def initial_action_handler(self, leaf):
pu.log_msg(self.robot_id, "GVGExplore received new goal",
self.debug_mode)
self.graph.generate_graph()
# if not self.current_pose:
# self.current_pose = self.get_robot_pose()
self.prev_goal_grid = self.graph.latest_map.pose_to_grid(
self.current_pose)
self.goal_grid = self.graph.latest_map.pose_to_grid(
np.array([leaf.position.x, leaf.position.y]))
self.current_state = self.MOVE_TO_LEAF
self.move_robot_to_goal(np.array([leaf.position.x, leaf.position.y]))
def robots_karto_out_callback(self, data):
if data.robot_id - 1 == self.robot_id:
pu.log_msg(
self.robot_id,
"ROBOT received a message Robot is saving a karto message: {}".
format(data.robot_id), self.debug_mode)
for rid in self.candidate_robots:
self.add_to_file(rid, [data])
if self.is_initial_data_sharing:
self.push_messages_to_receiver(self.candidate_robots,
None,
initiator=1)
self.is_initial_data_sharing = False
def process_alert_data(self, received_data):
for sender_id, received_buff_data in received_data.items():
# received_buff_data = data.res_data
sender_id = received_buff_data.msg_header.header.frame_id
heading_back = received_buff_data.heading_back
if heading_back:
if self.heading_back:
self.heading_back = 0 # then you've shared your data and you're all set. continue with exploration
self.base_points = copy.deepcopy(self.robot_points)
self.process_data(sender_id,received_buff_data)
# thread = Thread(target=self.process_data, args=(sender_id, received_buff_data,))
# thread.start()
pu.log_msg(self.robot_id, "Processed alert data", self.debug_mode)
def spin(self):
r = rospy.Rate(0.1)
while not rospy.is_shutdown():
try:
pu.log_msg(
self.robot_id, "Is exploring: {}, Session ID: {}".format(
self.is_exploring, self.session_id), self.debug_mode)
if self.is_exploring:
self.check_data_sharing_status()
except Exception as e:
pu.log_msg(self.robot_id, "Throwing error: {}".format(e),
self.debug_mode)
r.sleep()
def process_data(self, sender_id, buff_data):
# self.lock.acquire()
try:
data_vals = buff_data.data
self.last_map_update_time = rospy.Time.now().to_sec()
counter = 0
for scan in data_vals:
if self.save_message(scan):
self.karto_pub.publish(scan)
counter += 1
except Exception as e:
pu.log_msg(self.robot_id, "Error processing data: {}".format(e),
self.debug_mode)
def create_buff_data(self, rid, is_alert=0):
message_data = self.load_data_for_id(rid)
pu.log_msg(self.robot_id, "Contains message data", self.debug_mode)
buffered_data = BufferedData()
buffered_data.msg_header.header.stamp = rospy.Time.now()
buffered_data.msg_header.header.frame_id = '{}'.format(self.robot_id)
buffered_data.msg_header.sender_id = str(self.robot_id)
buffered_data.msg_header.receiver_id = str(rid)
buffered_data.msg_header.topic = 'received_data'
buffered_data.secs = []
buffered_data.data = message_data
buffered_data.alert_flag = is_alert
return buffered_data
def buffered_data_callback(self, buff_data):
sender_id = buff_data.msg_header.header.frame_id
if sender_id in self.candidate_robots:
if self.initial_receipt:
self.push_messages_to_receiver([sender_id])
self.process_data(sender_id, buff_data)
# thread = Thread(target=self.process_data, args=(sender_id, buff_data,))
# thread.start()
pu.log_msg(
self.robot_id, "Initial data from {}: {} files".format(
sender_id, len(buff_data.data)), self.debug_mode)
self.initial_data_count += 1
if self.initial_data_count == len(self.candidate_robots):
self.initial_receipt = False
if self.i_have_least_id():
self.wait_for_map_update()
pu.log_msg(
self.robot_id,
"Computing frontier points Active".format(
self.robot_id), self.debug_mode)
self.request_and_share_frontiers()
else:
pu.log_msg(self.robot_id,
"Waiting for frontier points...",
self.debug_mode)
else:
pu.log_msg(self.robot_id, "Already initiated...",
self.debug_mode)
pass
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 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 spin(self):
r = rospy.Rate(0.1)
while not rospy.is_shutdown():
try:
# if self.is_initial_data_sharing:
# if len(self.get_close_devices()) == len(self.candidate_robots): # For real robot. # TODO
# pu.log_msg(self.robot_id,"Sending initial data to all robots...",self.debug_mode)
# self.push_messages_to_receiver(self.candidate_robots, None, initiator=1)
# self.is_initial_data_sharing = False
pu.log_msg(
self.robot_id, "Is exploring: {}, Session ID: {}".format(
self.is_exploring, self.session_id), self.debug_mode)
except Exception as e:
pu.log_msg(self.robot_id, "Throwing error: {}".format(e),
self.debug_mode)
r.sleep()
def get_robot_pose(self):
robot_pose = None
while not robot_pose:
if True:
self.listener.waitForTransform(
"robot_{}/map".format(self.robot_id),
"robot_{}/base_link".format(self.robot_id), rospy.Time(),
rospy.Duration(4.0))
(robot_loc_val, rot) = self.listener.lookupTransform(
"robot_{}/map".format(self.robot_id),
"robot_{}/base_link".format(self.robot_id), rospy.Time(0))
robot_pose = (math.floor(robot_loc_val[0]),
math.floor(robot_loc_val[1]), robot_loc_val[2])
pu.log_msg(self.robot_id, "Robot pose: {}".format(robot_pose),
self.debug_mode)
#except: # TODO: catch proper exceptions.
# pass
return robot_pose
def frontier_point_handler(self, request):
count = request.count
frontier_points = []
self.generate_graph()
free_points = list(self.latest_map.get_explored_region())
orig_pose = self.get_robot_pose()
potential_fps = []
for ap in free_points:
npoint = self.transform_pose(orig_pose, ap)
prev_cell = self.latest_map.pose_to_grid(ap)
next_cell = self.latest_map.pose_to_grid(npoint)
if tuple(prev_cell) == tuple(next_cell):
pu.log_msg(
self.robot_id,
"SIMILAR POINTS: Prev point: {}, Next point: {}".format(
ap, npoint), self.debug_mode)
continue
try:
if self.latest_map.is_frontier(prev_cell, next_cell,
self.lidar_scan_radius):
potential_fps.append(ap)
except:
pu.log_msg(
self.robot_id,
"MEM ERROR Prev: {}, Mem Erro Next: {}".format(
prev_cell, next_cell), self.debug_mode)
pu.log_msg(self.robot_id, "Got an error whe checking frontier",
1 - self.debug_mode)
if potential_fps:
pu.log_msg(self.robot_id,
'Returned points: {}'.format(len(potential_fps)),
self.debug_mode)
hull, boundary_points = graham_scan(potential_fps, count, False)
for ap in boundary_points:
pu.log_msg(self.robot_id,
"Found frontier point: {}".format(ap),
self.debug_mode)
pose = Pose()
pose.position.x = ap[INDEX_FOR_X]
pose.position.y = ap[INDEX_FOR_Y]
frontier_points.append(pose)
# pu.log_msg(self.robot_id,"Returned frontier points: {}".format(frontier_points),self.debug_mode)
return FrontierPointResponse(frontiers=frontier_points)
def compute_and_share_auction_points(self, auction_feedback,
frontier_points):
all_robots = list(auction_feedback)
pu.log_msg(self.robot_id, "All robots: {}".format(auction_feedback),
self.debug_mode)
taken_poses = []
for i in all_robots:
pose_i = (auction_feedback[i][1].position.x,
auction_feedback[i][1].position.y)
conflicts = []
for j in all_robots:
if i != j:
pose_j = (auction_feedback[j][1].position.x,
auction_feedback[j][1].position.y)
if pose_i == pose_j:
conflicts.append((i, j))
rpose = auction_feedback[i][1]
frontier = self.create_frontier(
i, (rpose.position.x, rpose.position.y))
# pu.log_msg(self.robot_id, "Sharing point with {}".format(i),self.debug_mode)
res = self.allocation_pub[i](
SharedFrontierRequest(frontier=frontier))
pu.log_msg(
self.robot_id,
"Shared a frontier point with robot {}: {}".format(i, res),
self.debug_mode)
taken_poses.append(pose_i)
for c in conflicts:
conflicting_robot_id = c[1]
feedback_for_j = self.all_feedbacks[conflicting_robot_id]
rob_poses = feedback_for_j.poses
rob_distances = feedback_for_j.distances
remaining_poses = {}
for k in range(len(rob_poses)):
p = (rob_poses[k].position.x, rob_poses[k].position.y)
if p != pose_i and p not in taken_poses:
remaining_poses[rob_distances[k]] = rob_poses[k]
if remaining_poses:
next_closest_dist = min(list(remaining_poses))
auction_feedback[conflicting_robot_id] = (
next_closest_dist, remaining_poses[next_closest_dist])
return taken_poses