class Map_To_Graph:
def __init__(self):
self.brushfire_cffi = Cffi()
self.topology = Topology()
self.routing = Routing()
self.coverage = Coverage()
# Origin is the translation between the (0,0) of the robot pose and the
# (0,0) of the map
self.origin = {}
self.origin['x'] = 0
self.origin['y'] = 0
self.resolution = 0.05
self.step = 0
# Load map's translation
if rospy.has_param('origin'):
translation = rospy.get_param('origin')
self.origin['x'] = translation[0]
self.origin['y'] = translation[1]
if rospy.has_param('resolution'):
self.resolution = rospy.get_param('resolution')
# Initilize sensor's specs
self.sensor_names = []
self.sensor_direction = []
self.sensor_fov = []
self.sensor_range = []
self.sensor_shape = []
self.sensor_reliability = []
self.min_distance = 0
self.navigation_target = 0
# Read sensor's specs
self.robot_radius = rospy.get_param('robot_radius')
self.sensor_names = rospy.get_param('sensor_names')
self.sensor_number = len(self.sensor_names)
self.navigation_target = rospy.get_param('navigation_target')
self.navigation_pattern = rospy.get_param('navigation_pattern')
self.fixed_step = rospy.get_param('fixed_step')
for name in self.sensor_names:
self.sensor_direction.append(
rospy.get_param(name + '/sensor_direction'))
self.sensor_fov.append(rospy.get_param(name + '/fov'))
self.sensor_range.append(rospy.get_param(name + '/range'))
self.sensor_shape.append(rospy.get_param(name + '/shape'))
self.sensor_reliability.append(
rospy.get_param(name + '/reliability'))
self.ogm_topic = '/map'
self.ogm = 0
self.ogm_raw = 0
self.ogm_width = 0
self.ogm_height = 0
self.ogm_header = 0
# Flag to wait ogm subscriber to finish
self.ogm_compute = False
self.gvd = 0
self.brush = 0
self.nodes = []
self.door_nodes = []
self.rooms = []
self.room_doors = []
self.room_type = []
self.room_sequence = []
self.entering_doors = {}
self.exiting_doors = {}
self.wall_follow_nodes = []
self.wall_follow_sequence = []
self.zig_zag_nodes = []
self.zig_zag_sequence = []
self.simple_nodes = []
self.simple_sequence = []
# Load nodes from json file
map_name = rospy.get_param('map_name')
filename = '/home/mal/catkin_ws/src/topology_finder/data/' + map_name + '.json'
with open(filename, 'r') as read_file:
self.data = json.load(read_file)
self.nodes = self.data['nodes']
self.door_nodes = self.data['doors']
self.rooms = self.data['rooms']
self.room_doors = self.data['roomDoors']
self.room_type = self.data['roomType']
if 'room_sequence' in self.data:
self.room_sequence = self.data['room_sequence']
if 'entering_doors' in self.data:
self.entering_doors = {
int(k): v
for k, v in self.data['entering_doors'].items()
}
if 'exiting_doors' in self.data:
self.exiting_doors = {
int(k): v
for k, v in self.data['exiting_doors'].items()
}
if 'wall_follow_nodes' in self.data:
self.wall_follow_nodes = self.data['wall_follow_nodes']
if 'wall_follow_sequence' in self.data:
self.wall_follow_sequence = self.data['wall_follow_sequence']
if 'zig_zag_nodes' in self.data:
self.zig_zag_nodes = self.data['zig_zag_nodes']
if 'zig_zag_sequence' in self.data:
self.zig_zag_sequence = self.data['zig_zag_sequence']
if 'simple_nodes' in self.data:
self.simple_nodes = self.data['simple_nodes']
if 'simple_sequence' in self.data:
self.simple_sequence = self.data['simple_sequence']
self.node_publisher = rospy.Publisher('/nodes', Marker, queue_size=100)
self.candidate_door_node_pub = rospy.Publisher('/nodes/candidateDoors',
Marker,
queue_size=100)
self.door_node_pub = rospy.Publisher('/nodes/doors',
Marker,
queue_size=100)
self.room_node_pub = rospy.Publisher('/nodes/rooms',
Marker,
queue_size=100)
def server_start(self):
rospy.init_node('map_to_graph')
rospy.loginfo('map_to_graph node initialized.')
# Read ogm
self.ogm_compute = True
rospy.Subscriber(self.ogm_topic, OccupancyGrid, self.read_ogm)
while self.ogm_compute:
pass
# Calculate brushfire field
self.brush = self.brushfire_cffi.obstacleBrushfireCffi(self.ogm)
# Calculate gvd from brushfire and ogm
self.gvd = self.topology.gvd(self.brush)
time.sleep(1)
# self.print_markers(self.nodes, [1., 0., 0.], self.node_publisher)
# self.print_markers(self.door_nodes, [0., 0., 1.], self.door_node_pub)
# Find sequence of rooms if not already exists
if self.room_sequence == []:
self.find_room_sequence(True)
if self.navigation_pattern == 'simple':
self.find_all_wall_nodes()
rospy.loginfo("Visualize node sequence")
self.visualise_node_sequence(self.simple_sequence)
self.simple_sequence, self.simple_nodes = self.a_priori_coverage(
self.simple_sequence, False)
# Compute length of sequence
i = 0
for temp_nodes in self.simple_nodes:
distances = cdist(np.array(temp_nodes), np.array(temp_nodes),
'euclidean')
cost = self.routing.route_length(distances,
range(len(temp_nodes)))
string = 'Length of zig zag nodes at room ' + str(
i) + ': ' + str(cost)
print(string)
i += 1
else:
self.find_best_path_wall_nodes()
self.visualise_node_sequence(self.wall_follow_sequence)
self.wall_follow_sequence, self.wall_follow_nodes = self.a_priori_coverage(
self.wall_follow_sequence)
# Save wall nodes to json
self.data['wall_follow_nodes'] = self.wall_follow_nodes
self.data['wall_follow_sequence'] = self.wall_follow_sequence
rospy.loginfo("Visualize node sequence")
self.visualise_node_sequence(self.wall_follow_sequence)
# Compute length of sequence
i = 0
for temp_nodes in self.wall_follow_nodes:
distances = cdist(np.array(temp_nodes), np.array(temp_nodes),
'euclidean')
cost = self.routing.route_length(distances,
range(len(temp_nodes)))
string = 'Length of wall follow nodes at room ' + str(
i) + ': ' + str(cost)
print(string)
i += 1
map_name = rospy.get_param('map_name')
filename = '/home/mal/catkin_ws/src/topology_finder/data/' + map_name + '.json'
with open(filename, 'w') as outfile:
data_to_json = json.dump(self.data, outfile)
print("5 secs sleep with wall_follow_sequence")
rospy.sleep(5)
self.zig_zag_sequence, self.zig_zag_nodes = self.add_zig_zag_nodes(
self.wall_follow_sequence)
self.zig_zag_sequence, self.zig_zag_nodes = self.a_priori_coverage(
self.zig_zag_sequence, False)
# Save zig zag nodes to json
self.data['zig_zag_nodes'] = self.zig_zag_nodes
self.data['zig_zag_sequence'] = self.zig_zag_sequence
rospy.loginfo("Visualize zig zag node sequence")
self.visualise_node_sequence(self.zig_zag_sequence)
# Compute length of sequence
i = 0
for temp_nodes in self.zig_zag_nodes:
distances = cdist(np.array(temp_nodes), np.array(temp_nodes),
'euclidean')
cost = self.routing.route_length(distances,
range(len(temp_nodes)))
string = 'Length of zig zag nodes at room ' + str(
i) + ': ' + str(cost)
print(string)
i += 1
map_name = rospy.get_param('map_name')
filename = '/home/mal/catkin_ws/src/topology_finder/data/' + map_name + '.json'
with open(filename, 'w') as outfile:
data_to_json = json.dump(self.data, outfile)
return
# Method to visualize sequence of nodes splited into rooms with different color
def visualise_node_sequence(self, rooms):
id = 0
for nodes in rooms:
rgb = [0, 0, 0]
i = 0
for node in nodes:
rgb[0] = i / len(nodes)
rgb[1] = min(2 * i, len(nodes)) / len(nodes)
rgb[2] = min(3 * i, len(nodes)) / len(nodes)
self.print_markers([node['position']], rgb, self.room_node_pub,
id)
rospy.sleep(0.01)
i += 1
id += 1
print("Printed entire room")
return
# Method to visualize room nodes splited into segments with different color in each one
def visualise_node_segments(self, room, id=0):
color_num = 0
color = [1., 0., 0.]
# threshold = 256/len(splited_node_route)
for segment in room:
self.print_markers(segment,
np.array(color_num) * color, self.room_node_pub,
id)
id += 1
color_num += 30 / 256
if color_num > 1:
color = (color[1:] + color[:1])
color_num -= 1
rospy.sleep(0.1)
return id
# Do uniform sampling across whole map and gather points near obstacles
def uniform_sampling(self):
# Uniform sampling on map
nodes = []
# Sampling step is half the sensor's range
min_range = int(min(self.sensor_range) / self.resolution)
max_range = int(max(self.sensor_range) / self.resolution)
step = int(1.5 * self.robot_radius / self.resolution)
step_list = []
while step <= max_range:
temp_nodes = []
step_list.append(step)
indx = np.where((self.brush > step) & (self.brush <= 2 * step)
& (self.brush <= max_range))
indx = zip(*indx)
for x, y in indx:
if not x % step and not y % step:
temp_nodes.append((x, y))
step *= 2
nodes.append(temp_nodes)
obstacles = np.zeros((self.ogm_width, self.ogm_height))
final_nodes = []
i = len(nodes) - 1
while i >= 0:
temp_nodes = nodes[i]
# self.print_markers(temp_nodes, [1., 0., 0.], self.node_publisher)
# rospy.sleep(2)
# Check if new nodes override previous ones and delete them
for point in temp_nodes:
x, y = point
indexes = self.brushfire_cffi.circularRayCastCoverageCffi(
point, self.ogm, max_range, 360, 0, 0, True)
if not len(indexes):
continue
if i == len(nodes) - 1:
obstacles[zip(*indexes)] = 1
final_nodes.append(point)
else:
temp = obstacles[zip(*indexes)]
p = len(np.where(temp > 0)[0]) / len(indexes)
if p < 0.9:
final_nodes.append((x, y))
obstacles[zip(*indexes)] = 1
i -= 1
return final_nodes, step_list
# Find sequence of rooms
def find_room_sequence(self, save_result):
rospy.loginfo("Finding room sequence...")
self.room_sequence = []
# Create graph
graph = Graph()
for room in self.room_doors:
if len(room) > 1:
for i in range(len(room)):
for j in range(i + 1, len(room)):
node_id_1 = self.door_nodes.index(room[i])
node_id_2 = self.door_nodes.index(room[j])
# Find brushfire distance (closest to real dist) between doors
dist = self.brushfire_cffi.pointToPointBrushfireCffi(tuple(room[i]), \
tuple(room[j]), self.ogm)
if dist < 0:
rospy.loginfo("Error computing door distance!")
return
graph.add_edge(node_id_1, node_id_2, dist)
graph.add_edge(node_id_2, node_id_1, dist)
# Calculate graph's distances between all nodes
doors_length = len(self.door_nodes)
if doors_length > 2:
distances = np.zeros((doors_length, doors_length),
dtype=np.float64)
for i in range(doors_length):
for j in range(i + 1, doors_length):
_, _, _, dist = find_path(graph, i, j)
distances[i][j] = dist
distances[j][i] = distances[i][j]
# Call hill climb algorithm
max_iterations = 500 * doors_length
# door_route, cost, iter = self.routing.hillclimb(distances, max_iterations)
door_route, cost, iter = self.routing.random_restart_hillclimb(
distances, max_iterations)
print('Optimal RRHC room sequence', door_route,
self.routing.route_length(distances, door_route))
# door_route, cost, iter = self.routing.anneal(distances, max_iterations, 1.0, 0.95)
# print('Optimal ANNEAL room sequence', door_route, self.routing.route_length(distances, door_route))
#Sub-optimal / greedy sequence
greedy_route = []
greedy_route.append(door_route[0])
for i in range(1, doors_length):
#find min
previous = greedy_route[i - 1]
unvisited = []
for j in range(doors_length):
if j not in greedy_route:
unvisited.append(j)
minimum = distances[previous, unvisited[0]]
min_idx = unvisited[0]
for idx in unvisited:
if distances[previous][idx] < minimum:
minimum = distances[previous][idx]
min_idx = idx
greedy_route.append(min_idx)
greedy_cost = self.routing.route_length(distances, greedy_route)
print('Greedy room sequence', greedy_route, greedy_cost)
elif doors_length == 2:
door_route = [0, 1]
_, _, _, dist = find_path(graph, 0, 1)
print('Only 2 doors with distance ', dist)
elif self.door_nodes != []:
door_route = [0]
print('One door found!')
else:
door_route = []
self.room_sequence = [0]
print('One room only!')
# Correspond door route to room route
for door in door_route:
for i in range(len(self.room_doors)):
if self.door_nodes[door] in self.room_doors[
i] and i not in self.room_sequence:
self.room_sequence.append(i)
enter = {}
leave = {}
if doors_length >= 2:
# Compute enter & exit doors of each room according to found room_sequence
room_i = 1 # Start from 2nd room
room_idx = self.room_sequence[room_i]
entered = False
# Simulate door/room sequence on the graph
for i in range(len(door_route)):
# Find path from current to next door
door_idx = door_route[i]
next_door_idx = door_route[(i + 1) % len(door_route)]
# print('Room', room_idx)
path = find_path(graph, door_idx, next_door_idx)
# print('Path',path.nodes)
if entered and len(
path.nodes
) > 1: # if already inside room, first node of path is already used
ii = 1
else:
ii = 0
# Transverse through path and check for corresponding rooms
while ii < len(path.nodes):
node_idx = path.nodes[ii]
door = self.door_nodes[node_idx]
if door in self.room_doors[room_idx]:
# print('Door',node_idx, door,'in room', room_idx)
if not entered:
enter[room_idx] = door
entered = not entered
if len(self.room_doors[room_idx]) == 1:
ii -= 1 # To revisit this door
else:
leave[room_idx] = door
entered = not entered
room_i += 1
room_idx = self.room_sequence[room_i % len(
self.room_sequence)]
# print('Room changed', room_idx)
ii -= 1 # To revisit this door
ii += 1
elif self.door_nodes != []:
for i in range(len(self.room_sequence)):
enter[i] = self.door_nodes[0]
leave[i] = self.door_nodes[0]
self.entering_doors = enter
self.exiting_doors = leave
# # Print results per room (blue - entering door, read - exiting door, green - room nodes)
# for room_idx in self.room_sequence:
# print('Room',room_idx, 'all doors',self.room_doors[room_idx])
# self.print_markers([enter[room_idx]], [0,0,1], self.node_publisher)
# self.print_markers([leave[room_idx]], [1,0,0], self.door_node_pub)
# self.print_markers(self.rooms[room_idx], [0,1,0], self.room_node_pub)
# rospy.sleep(4)
if save_result:
# Save room sequence
self.data['room_sequence'] = self.room_sequence
self.data['entering_doors'] = self.entering_doors
self.data['exiting_doors'] = self.exiting_doors
map_name = rospy.get_param('map_name')
filename = '/home/mal/catkin_ws/src/topology_finder/data/' + map_name + '.json'
with open(filename, 'w') as outfile:
data_to_json = json.dump(self.data, outfile)
return
# Find the yaw that maximizes the covered walls of a given node
def find_best_yaw(self, node, next_node, loop_threshold, obstacle_weight,
rotation_weight):
yaw_between_nodes = math.degrees(
math.atan2(next_node[1] - node[1], next_node[0] - node[0]))
yaw = []
steps = int(max(self.sensor_range) / self.resolution)
# Find reachable obstacles
obstacles = self.brushfire_cffi.inRangeObstacleBrushfireCffi(
node, self.ogm, steps, True)
if len(obstacles) == 0:
return yaw
# In case of many obstacles more poses are needed for full coverage
found = 0
while found < loop_threshold * len(obstacles) and len(obstacles):
best_evaluation = 0
best_yaw = 0
best_found = 0
best_covered_obstacles = []
for angle in range(-180, 180, 10):
pose = [node[0], node[1], angle]
all_covered_obstacles = self.coverage.checkNearbyObstacleCover(
pose)
covered_obstacles = []
for ob in all_covered_obstacles:
if ob in obstacles:
covered_obstacles.append(ob)
if not len(covered_obstacles):
continue
# Absolute yaw of current and next node in [0,180]
next_rotation = np.abs(angle - yaw_between_nodes)
while next_rotation >= 360:
next_rotation -= 360
if next_rotation > 180:
next_rotation = 360 - next_rotation
# Evaluate candidate angle
# Use Motor Schema with covered area and rotation to next node as parameters
evaluation = obstacle_weight * len(covered_obstacles) / len(
obstacles) + rotation_weight * (180 - next_rotation) / 180
if evaluation > best_evaluation:
best_evaluation = evaluation
best_yaw = angle
best_found = len(covered_obstacles)
best_covered_obstacles = covered_obstacles
found += best_found
# Check if obstacle is coverable
if best_evaluation:
yaw.append(best_yaw)
else:
break
# Speed up process bypassing deletions if all obstacles are covered
if found >= loop_threshold * len(obstacles):
break
else:
# Discard checked obstacles
for ob in best_covered_obstacles:
if ob in obstacles:
obstacles.remove(ob)
yaw = list(set(yaw))
return yaw
# Find the yaw that maximizes the covered walls of a given node
def find_yaw(self, node, next_node, obstacle_weight, rotation_weight):
yaw_between_nodes = math.degrees(
math.atan2(next_node[1] - node[1], next_node[0] - node[0]))
yaw = []
steps = int(max(self.sensor_range) / self.resolution)
node_brush = self.brush[node]
steps = max(steps, node_brush)
# Find reachable obstacles
obstacles = self.brushfire_cffi.inRangeObstacleBrushfireCffi(
node, self.ogm, steps, True)
if len(obstacles) == 0:
return yaw
# In case of many obstacles more poses are needed for full coverage
found = 0
best_evaluation = 0
best_yaw = 0
for angle in range(-180, 180, 10):
pose = [node[0], node[1], angle]
all_covered_obstacles = self.coverage.checkNearbyObstacleCover(
pose)
covered_obstacles = 0
for ob in all_covered_obstacles:
if ob in obstacles:
covered_obstacles += 1
if not covered_obstacles:
continue
# Absolute yaw of current and next node in [0,180]
next_rotation = np.abs(angle - yaw_between_nodes)
while next_rotation >= 360:
next_rotation -= 360
if next_rotation > 180:
next_rotation = 360 - next_rotation
# Evaluate candidate angle
# Use Motor Schema with covered area and rotation to next node as parameters
evaluation = obstacle_weight * covered_obstacles / len(
obstacles) + rotation_weight * (180 - next_rotation) / 180
if evaluation > best_evaluation:
best_evaluation = evaluation
best_yaw = angle
# Check if obstacle is coverable
if best_evaluation:
yaw.append(best_yaw)
return yaw
def find_weights(self):
if self.navigation_target == 'cover_first':
angles = []
for s in range(self.sensor_number):
theta = self.sensor_direction[s]
while theta < 0:
theta += 360
while theta >= 360:
theta -= 360
if theta > 180:
theta = 360 - theta
if theta > 90:
theta = 180 - theta
angles.append((90 - theta) / 90)
obstacle_weight = 1 + np.mean(angles)
rotation_weight = 2 - np.mean(angles)
elif self.navigation_target == 'fast_first':
angles = []
for s in range(len(self.sensor_number)):
theta = self.sensor_direction[s]
fov = self.sensor_fov[s] / 2
while theta < 0:
theta += 360
while theta >= 360:
theta -= 360
if theta > 180:
theta = 360 - theta
if theta > 90:
theta = 180 - theta
theta = min(theta + fov, 90)
angles.append((90 - theta) / 90)
obstacle_weight = 1 + np.mean(angles)
rotation_weight = 2 - np.mean(angles)
else:
obstacle_weight = 1
rotation_weight = 1
return obstacle_weight, rotation_weight
# Add zig zag nodes inside already computed node sequence
def add_zig_zag_nodes(self, wall_follow_sequence):
zig_zag_nodes = []
zig_zag_sequence = []
filled_ogm = self.topology.doorClosure(self.door_nodes, self.ogm,
self.brushfire_cffi)
j = 0
for room in wall_follow_sequence:
temp_nodes = []
temp_sequence = []
room_brush = self.brushfire_cffi.pointBrushfireCffi(
self.rooms[j], filled_ogm)
for i in range(len(room)):
if not i:
temp_nodes.append(room[i]['position'])
temp_sequence.append(room[i])
continue
node = room[i]['position']
previous_node = room[i - 1]['position']
x_final, y_final = 0, 0
if previous_node[0] == node[0] and np.abs(
previous_node[1] -
node[1]) and np.abs(previous_node[1] - node[1]) <= max(
self.sensor_range) / self.resolution:
dif = np.abs(previous_node[1] - node[1])
x1 = int(node[0] - dif / 2)
x2 = int(node[0] + dif / 2)
y = int(np.min([previous_node[1], node[1]]) + dif / 2)
if self.brush[x1, y] >= self.brush[x2, y]:
x_final = x1
y_final = y
yaw = math.degrees(
math.atan2(y - node[1], x1 - node[0]))
else:
x_final = x2
y_final = y
yaw = math.degrees(
math.atan2(y - node[1], x2 - node[0]))
elif previous_node[1] == node[1] and np.abs(
previous_node[0] -
node[0]) and np.abs(previous_node[0] - node[0]) <= max(
self.sensor_range) / self.resolution:
dif = np.abs(previous_node[0] - node[0])
y1 = int(node[1] - dif / 2)
y2 = int(node[1] + dif / 2)
x = int(np.min([previous_node[0], node[0]]) + dif / 2)
if self.brush[x, y1] >= self.brush[x, y2]:
x_final = x
y_final = y1
yaw = math.degrees(
math.atan2(y1 - node[1], x - node[0]))
else:
x_final = x
y_final = y2
yaw = math.degrees(
math.atan2(y2 - node[1], x - node[0]))
if room_brush[x_final][y_final] > 1:
temp_nodes.append((x_final, y_final))
temp_sequence.append({
'position': (x_final, y_final),
'yaw': yaw
})
temp_nodes.append(room[i]['position'])
temp_sequence.append(room[i])
zig_zag_nodes.append(temp_nodes)
zig_zag_sequence.append(temp_sequence)
j += 1
return zig_zag_sequence, zig_zag_nodes
# Find nodes for wall following coverage
def find_all_wall_nodes(self):
rospy.loginfo("Finding simple strategy nodes...")
self.simple_nodes = []
self.simple_sequence = []
obstacle_weight, rotation_weight = 1, 1
# Uniform sampling on map
nodes = []
step = int(self.fixed_step / self.resolution)
indx = np.where((self.brush > step) & (self.brush <= 2 * step))
indx = zip(*indx)
for x, y in indx:
if not x % step and not y % step:
nodes.append((x, y))
self.print_markers(nodes, [1., 0., 0.], self.node_publisher)
rospy.sleep(2)
# Find rooms of nodes
# Fill door holes with "wall"
filled_ogm = self.topology.doorClosure(self.door_nodes, self.ogm,
self.brushfire_cffi)
id = 0
for i in range(len(self.rooms)):
# Brushfire inside each room and gather brushed wall_follow_nodes
found_nodes = []
brush = self.brushfire_cffi.pointBrushfireCffi(
self.rooms[i], filled_ogm)
for n in nodes:
if brush[n] > 0:
found_nodes.append(n)
found_nodes.sort() # Optimize path finding
print("Found {} nodes in room {}.".format(len(found_nodes),
i)) # # DEBUG
nodes_length = len(found_nodes)
if nodes_length:
distances = cdist(np.array(found_nodes), np.array(found_nodes),
'euclidean')
print('Distances of nodes done.') # DEBUG:
found_nodes.insert(0, self.entering_doors[i])
found_nodes.append(self.exiting_doors[i])
# Call RR hillclimb to optimize path
max_iterations = 2000 * len(found_nodes)
distances = cdist(np.array(found_nodes), np.array(found_nodes),
'euclidean')
cost = self.routing.route_length(distances,
range(len(found_nodes)))
print('Route length after adding doors', cost)
# node_route, cost, iter = self.routing.hillclimb(distances, max_iterations)
fixed_edges = True
node_route, cost, iter = self.routing.random_restart_hillclimb(
distances, max_iterations, fixed_edges)
print('Second HC route length', cost, 'made iters', iter, 'from',
max_iterations)
found_nodes_with_yaw = []
k = 1 # DEBUG:
for n in range(len(found_nodes)):
print('Closest obstacles process: {}/{}'.format(
k, nodes_length))
# Find closest obstacle to get best yaw
x, y = found_nodes[node_route[n]]
x2, y2 = found_nodes[node_route[(n + 1) % len(node_route)]]
yaw = self.find_yaw((x, y), (x2, y2), obstacle_weight,
rotation_weight)
if yaw == []:
continue
for point in yaw:
temp_dict = {'position': (x, y), 'yaw': point}
found_nodes_with_yaw.append(temp_dict)
k += 1
self.simple_nodes.append(found_nodes)
self.simple_sequence.append(found_nodes_with_yaw)
# Save wall nodes to json
self.data['simple_nodes'] = self.simple_nodes
self.data['simple_sequence'] = self.simple_sequence
map_name = rospy.get_param('map_name')
filename = '/home/mal/catkin_ws/src/topology_finder/data/' + map_name + '.json'
with open(filename, 'w') as outfile:
data_to_json = json.dump(self.data, outfile)
return
# Find nodes for wall following coverage with optimal path
def find_best_path_wall_nodes(self):
rospy.loginfo("Finding wall follow nodes...")
self.wall_follow_nodes = []
self.wall_follow_sequence = []
loop_threshold = 0.6
obstacle_weight, rotation_weight = self.find_weights()
# Uniform sampling on map
nodes, self.step = self.uniform_sampling()
# self.print_markers(nodes, [1., 0., 0.], self.node_publisher)
rospy.sleep(1)
# Find rooms of nodes
# Fill door holes with "wall"
filled_ogm = self.topology.doorClosure(self.door_nodes, self.ogm,
self.brushfire_cffi)
id = 0
for i in range(0, len(self.rooms)):
# Brushfire inside each room and gather brushed wall_follow_nodes
found_nodes = []
brush = self.brushfire_cffi.pointBrushfireCffi(
self.rooms[i], filled_ogm)
for n in nodes:
if brush[n] > 0:
found_nodes.append(n)
found_nodes.sort() # Optimize path finding
print("Found {} nodes in room {}.".format(len(found_nodes),
i)) # # DEBUG
if len(found_nodes) == 0:
print("ERROR!")
continue
nodes_length = len(found_nodes)
distances = cdist(np.array(found_nodes), np.array(found_nodes),
'euclidean')
print('Distances of nodes done.') # DEBUG:
cost = self.routing.route_length(distances, range(nodes_length))
print('Sorted route length', cost)
# Call hill climb algorithm
max_iterations = 150 * nodes_length
node_route, cost, iter = self.routing.step_hillclimb(
distances, max_iterations, self.step)
print('Route of wall follow nodes found.')
print('First step HC route length', cost)
# Reorder according to step HC results
temp_route = []
for n in node_route:
temp_route.append(found_nodes[n])
# Add entering & exiting doors and rotate already found route
# so that 2nd point is the closest one to the entering door
first_route = []
enter = self.entering_doors[i]
first_route.append(enter)
closest_idx = cdist(np.array([enter]),
np.array(temp_route)).argmin()
for ii in range(len(temp_route)):
first_route.append(temp_route[(ii + closest_idx) %
nodes_length])
first_route.append(self.exiting_doors[i])
# Do another hillclimb to optimize path
max_iterations = 2000 * len(first_route)
distances = cdist(np.array(first_route), np.array(first_route),
'euclidean')
cost = self.routing.route_length(distances,
range(len(first_route)))
print('Route length after adding doors and rotating sequence',
cost)
fixed_edges = True
node_route, cost, iter = self.routing.random_restart_hillclimb(
distances, max_iterations, fixed_edges)
print('Second HC route length', cost, 'made iters', iter, 'from',
max_iterations)
final_route = []
for n in node_route:
final_route.append(first_route[n])
found_nodes_with_yaw = []
k = 1 # DEBUG:
for n in range(len(final_route)):
# print('Closest obstacles process: {}/{}'.format(k, nodes_length))
# Find closest obstacle to get best yaw
x, y = final_route[n]
x2, y2 = final_route[(n + 1) % len(final_route)]
# Find cover_first poses
yaw = self.find_best_yaw((x, y), (x2, y2), loop_threshold,
obstacle_weight, rotation_weight)
if yaw != []:
for point in yaw:
temp_dict = {'position': (x, y), 'yaw': point}
found_nodes_with_yaw.append(temp_dict)
k += 1
self.wall_follow_nodes.append(found_nodes)
self.wall_follow_sequence.append(found_nodes_with_yaw)
# Save wall nodes to json
self.data['wall_follow_best_yaw_weigths'] = {
'loop_threshold': loop_threshold,
'obstacle_weight': obstacle_weight,
'rotation_weight': rotation_weight
}
map_name = rospy.get_param('map_name')
filename = '/home/mal/catkin_ws/src/topology_finder/data/' + map_name + '.json'
with open(filename, 'w') as outfile:
data_to_json = json.dump(self.data, outfile)
return
# Do an a priori coverage with found order of nodes to eliminate the not needed
def a_priori_coverage(self, nodes, eliminate=True):
self.coverage.initCoverage()
new_sequence = []
new_nodes = []
for room in nodes:
# i = 0
new_room = []
new_room_nodes = []
for i in range(len(room)):
node = room[i]
x, y = node['position']
yaw = node['yaw']
if not i or i == len(room) - 1 or not eliminate:
self.coverage.updateCover([x, y, yaw],
publish=False,
track_specs=False)
updated = True
else:
updated = self.coverage.checkAndUpdateCover(
self.brush, [x, y, yaw], 0.85)
if updated:
new_room.append(node)
new_room_nodes.append(node['position'])
self.coverage.coverage_pub.publish(self.coverage.coverage_ogm)
new_sequence.append(new_room)
new_nodes.append(new_room_nodes)
rospy.loginfo(
"A-priori coverage done at room {0}, room nodes size changed from {1} to {2}"
.format(i, len(room), len(new_room)))
i += 1
near_obstacles = np.where(self.brush == 2)
near_obstacles_cover = self.coverage.coverage[near_obstacles]
covered_obstacles = len(np.where(near_obstacles_cover >= 80)[0])
rospy.loginfo("Estimated coverage percentage {}".format(
covered_obstacles / len(near_obstacles_cover)))
return new_sequence, new_nodes
def read_ogm(self, data):
# OGM is a 2D array of size width x height
# The values are from 0 to 100
# 0 is an unoccupied pixel
# 100 is an occupied pixel
# 50 or -1 is the unknown
self.ogm_raw = np.array(data.data)
self.ogm_width = data.info.width
self.ogm_height = data.info.height
self.ogm_header = data.header
self.ogm = np.zeros((data.info.width, data.info.height), \
dtype = np.int)
# Reshape ogm to a 2D array
for x in range(0, data.info.width):
for y in range(0, data.info.height):
self.ogm[x][y] = data.data[x + data.info.width * y]
# Initilize coverage OGM with same size width x height
self.coverage.coverage = np.zeros((self.ogm_width, self.ogm_height))
self.coverage.coverage_ogm.info = data.info
self.coverage.coverage_ogm.data = np.zeros(self.ogm_width *
self.ogm_height)
self.coverage.ogm = self.ogm
self.coverage.ogm_raw = self.ogm_raw
self.coverage.ogm_width = self.ogm_width
self.coverage.ogm_height = self.ogm_height
self.coverage.ogm_header = self.ogm_header
self.ogm_compute = False
return
# Method to publish points to rviz
def print_markers(self, nodes, color, publisher, id=0):
points = []
for point in nodes:
p = Point()
p.x = point[0] * self.resolution + self.origin['x']
p.y = point[1] * self.resolution + self.origin['y']
p.z = 0
points.append(p)
# Create Marker for nodes
marker = Marker()
marker.header.frame_id = "/map"
marker.id = id
marker.type = marker.POINTS
marker.action = marker.ADD
marker.points = points
marker.pose.orientation.w = 1.0
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.color.a = 1.0
marker.color.r = color[0]
marker.color.g = color[1]
marker.color.b = color[2]
# rospy.loginfo("Printing nodes!")
publisher.publish(marker)
return
class CoverageNumberSubscriber:
def __init__(self):
self.brushfire_cffi = Cffi()
# Origin is the translation between the (0,0) of the robot pose and the
# (0,0) of the map
self.origin = {}
self.origin['x'] = 0
self.origin['y'] = 0
self.resolution = 0
# Initilize sensor's specs
self.sensor_names = []
self.sensor_direction = []
self.sensor_fov = []
self.sensor_range = []
self.sensor_shape = []
self.sensor_reliability = []
# Read sensor's specs
self.sensor_names = rospy.get_param('sensor_names')
self.sensor_number = len(self.sensor_names)
for name in self.sensor_names:
self.sensor_direction.append(
rospy.get_param(name + '/sensor_direction'))
self.sensor_fov.append(rospy.get_param(name + '/fov'))
self.sensor_range.append(rospy.get_param(name + '/range'))
self.sensor_shape.append(rospy.get_param(name + '/shape'))
self.sensor_reliability.append(
rospy.get_param(name + '/reliability'))
# Load map's translation
translation = rospy.get_param('origin')
self.origin['x'] = translation[0]
self.origin['y'] = translation[1]
self.resolution = rospy.get_param('resolution')
# OGM related attributes
self.ogm_topic = '/map'
self.ogm = 0
self.ogm_raw = 0
self.ogm_width = 0
self.ogm_height = 0
self.ogm_header = 0
# Flags to wait subscribers to finish reading data
self.ogm_compute = True
self.readPose = False
# Holds the coverage information. This has the same size as the ogm
# If a cell has the value of 0 it is uncovered
# In the opposite case the cell's value will be 100
self.coverage = []
self.coverage_number = []
self.brush = 0
# coverage_ogm will be published in the coverage_topic
self.coverage_ogm = OccupancyGrid()
self.coverage_ogm.header.frame_id = "map"
self.coverage_topic = '/map/coverage'
self.coverage_number_ogm = OccupancyGrid()
self.coverage_number_ogm.header.frame_id = "map"
self.coverage_number_topic = '/map/coverage_number'
def server_start(self):
rospy.init_node('coverage__number_subscriber')
rospy.loginfo('Coverage Number Subscriber node initialized.')
# Read ogm
rospy.Subscriber(self.ogm_topic, OccupancyGrid, self.read_ogm)
while self.ogm_compute:
pass
# Calculate brushfire field
self.brush = self.brushfire_cffi.obstacleBrushfireCffi(self.ogm)
# Read coverage angles ogm
rospy.Subscriber(self.coverage_number_topic,
OccupancyGrid,
self.cov_callback,
queue_size=1)
rospy.spin()
return
def cov_callback(self, data):
# Reshape ogm to a 2D array
for x in range(0, self.ogm_width):
for y in range(0, self.ogm_height):
self.coverage_number[x][y] = data.data[x + self.ogm_width * y]
near_obstacles = np.where(self.brush == 2)
# Read coverage looks at obstacles
number = self.coverage_number[near_obstacles]
rospy.loginfo('Coverage Number Mean: {}, Std: {}'.format(
np.mean(number), np.std(number)))
# Count values and save to a dictionary
counted = Counter(number)
print(counted)
# plt.close()
#
# # Covert to dataframe
# df = pd.DataFrame(counted, index = counted.keys())
# df.drop(df.columns[1:], inplace = True)
# # Plot results
# df.plot(kind='bar')
# plt.title('Number of each obstacle\'s coverage.')
# print('Time of plot', rospy.get_time())
# plt.show()
return
def read_ogm(self, data):
# OGM is a 2D array of size width x height
# The values are from 0 to 100
# 0 is an unoccupied pixel
# 100 is an occupied pixel
# 50 or -1 is the unknown
rospy.loginfo("Coverage Subscriber node reading ogm.")
self.ogm_raw = np.array(data.data)
self.ogm_width = data.info.width
self.ogm_height = data.info.height
self.ogm_header = data.header
self.ogm = np.zeros((data.info.width, data.info.height), \
dtype = np.int)
# Reshape ogm to a 2D array
for x in range(0, data.info.width):
for y in range(0, data.info.height):
self.ogm[x][y] = data.data[x + data.info.width * y]
# Initilize coverage OGM with same size width x height
self.coverage = np.zeros((self.ogm_width, self.ogm_height))
self.coverage_ogm.info = data.info
self.coverage_ogm.data = np.zeros(self.ogm_width * self.ogm_height)
self.coverage_number = np.zeros((self.ogm_width, self.ogm_height))
rospy.loginfo("OGM read!")
self.ogm_compute = False
return
class CoverageSubscriber:
def __init__(self):
self.brushfire_cffi = Cffi()
# Origin is the translation between the (0,0) of the robot pose and the
# (0,0) of the map
self.origin = {}
self.origin['x'] = 0
self.origin['y'] = 0
self.resolution = 0
# Initilize sensor's specs
self.sensor_names = []
self.sensor_direction = []
self.sensor_fov = []
self.sensor_range = []
self.sensor_shape = []
self.sensor_reliability = []
# Read sensor's specs
self.sensor_names = rospy.get_param('sensor_names')
self.sensor_number = len(self.sensor_names)
for name in self.sensor_names:
self.sensor_direction.append(rospy.get_param(name + '/sensor_direction'))
self.sensor_fov.append(rospy.get_param(name + '/fov'))
self.sensor_range.append(rospy.get_param(name + '/range'))
self.sensor_shape.append(rospy.get_param(name + '/shape'))
self.sensor_reliability.append(rospy.get_param(name + '/reliability'))
# Compute bins to keep with which angle a sensor covers each point
max_fov = max(self.sensor_fov)/2 # compute for one half (symmetric with sensor_direction)
self.bins = []
# self.number_of_bins = 2 * int(math.ceil(max_fov/5)) # 5 degrees in each bin
# for i in range(self.number_of_bins):
# down = - self.number_of_bins/2 * 5 + i*5
# up = down + 5
# self.bins.append((down,up))
# Compute bins of negative and positive only
self.number_of_bins = 2
self.bins.append((-max_fov, 0))
self.bins.append((0, max_fov))
# Load map's translation
translation = rospy.get_param('origin')
self.origin['x'] = translation[0]
self.origin['y'] = translation[1]
self.resolution = rospy.get_param('resolution')
# OGM related attributes
self.ogm_topic = '/map'
self.ogm = 0
self.ogm_raw = 0
self.ogm_width = 0
self.ogm_height = 0
self.ogm_header = 0
# Flags to wait subscribers to finish reading data
self.ogm_compute = True
self.readPose = False
# Holds the coverage information. This has the same size as the ogm
# If a cell has the value of 0 it is uncovered
# In the opposite case the cell's value will be 100
self.coverage = []
self.coverage_number = []
self.coverage_angles = []
self.coverage_diverge = []
self.brush = 0
# coverage_ogm will be published in the coverage_topic
self.coverage_ogm = OccupancyGrid()
self.coverage_ogm.header.frame_id = "map"
self.coverage_topic = '/map/coverage'
self.coverage_diverge_ogm = OccupancyGrid()
self.coverage_diverge_ogm.header.frame_id = "map"
self.coverage_diverge_topic = '/map/coverage_angles/diverge'
self.coverage_angles_ogm = OccupancyGrid()
self.coverage_angles_ogm.header.frame_id = "map"
self.coverage_angles_topic = '/map/coverage_angles'
self.coverage_diverge_pub = rospy.Publisher(self.coverage_diverge_topic, \
OccupancyGrid, queue_size = 1)
def server_start(self):
rospy.init_node('coverage_subscriber')
rospy.loginfo('Coverage Subscriber node initialized.')
# Read ogm
rospy.Subscriber(self.ogm_topic, OccupancyGrid, self.read_ogm)
while self.ogm_compute:
pass
# Calculate brushfire field
self.brush = self.brushfire_cffi.obstacleBrushfireCffi(self.ogm)
# Read coverage angles ogm
rospy.Subscriber(self.coverage_angles_topic, OccupancyGrid, self.cov_callback, queue_size = 1)
rospy.spin()
return
def cov_callback(self, data):
# Reshape ogm to a 3D array
for a in range(self.number_of_bins):
for x in range(0, self.ogm_width):
for y in range(0, self.ogm_height):
self.coverage_angles[x][y][a] = data.data[x + self.ogm_width * y + self.ogm_width * self.ogm_height * a]
near_obstacles = np.where(self.brush == 2)
near_obstacles_looked = []
for x in range(0, self.ogm_width):
for y in range(0, self.ogm_height):
if self.brush[x][y] == 2:
left = self.coverage_angles[x][y][0]
right = self.coverage_angles[x][y][1]
if left + right:
near_obstacles_looked.append((x,y))
self.coverage_diverge[x][y] = np.abs(left-right)/(left + right)
self.coverage_diverge_ogm.data[x + self.ogm_width * y] = int(100 * self.coverage_diverge[x][y])
# Retrieve metrics
values = self.coverage_diverge[zip(*near_obstacles_looked)]
rospy.loginfo("Coverage Angle Divergence only of covered obstacles Mean: {}, Std: {}".format(np.mean(values), np.std(values)))
self.coverage_diverge_pub.publish(self.coverage_diverge_ogm)
#
# for a in range(self.number_of_bins):
# temp = self.coverage_angles[:,:,a].copy()
# total_looks = sum(temp[near_obstacles])
# string = 'Bin: ' + `self.bins[a]` + ' total: ' + `total_looks`
# rospy.loginfo(string)
return
def read_ogm(self, data):
# OGM is a 2D array of size width x height
# The values are from 0 to 100
# 0 is an unoccupied pixel
# 100 is an occupied pixel
# 50 or -1 is the unknown
rospy.loginfo("Coverage Subscriber node reading ogm.")
self.ogm_raw = np.array(data.data)
self.ogm_width = data.info.width
self.ogm_height = data.info.height
self.ogm_header = data.header
self.ogm = np.zeros((data.info.width, data.info.height), \
dtype = np.int)
# Reshape ogm to a 2D array
for x in range(0, data.info.width):
for y in range(0, data.info.height):
self.ogm[x][y] = data.data[x + data.info.width * y]
# Initilize coverage OGM with same size width x height
self.coverage = np.zeros((self.ogm_width, self.ogm_height))
self.coverage_ogm.info = data.info
self.coverage_ogm.data = np.zeros(self.ogm_width * self.ogm_height)
self.coverage_diverge = np.full((self.ogm_width, self.ogm_height), 100)
self.coverage_diverge_ogm.info = data.info
self.coverage_diverge_ogm.data = np.full(self.ogm_width * self.ogm_height, 100)
self.coverage_angles = np.zeros((self.ogm_width, self.ogm_height, self.number_of_bins))
rospy.loginfo("OGM read!")
self.ogm_compute = False
return
class Navigation:
def __init__(self):
# self.routing = Routing()
self.brushfire_cffi = Cffi()
self.topology = Topology()
# Origin is the translation between the (0,0) of the robot pose and the
# (0,0) of the map
self.origin = {}
self.origin['x'] = 0
self.origin['y'] = 0
self.resolution = 0
# Load map's translation
translation = rospy.get_param('origin')
self.origin['x'] = translation[0]
self.origin['y'] = translation[1]
self.resolution = rospy.get_param('resolution')
# Initilize sensor's specs
self.sensor_names = []
self.sensor_direction = []
self.sensor_fov = []
self.sensor_range = []
self.sensor_shape = []
self.sensor_reliability = []
# Read sensor's specs
self.sensor_names = rospy.get_param('sensor_names')
self.sensor_number = len(self.sensor_names)
for name in self.sensor_names:
self.sensor_direction.append(
rospy.get_param(name + '/sensor_direction'))
self.sensor_fov.append(rospy.get_param(name + '/fov'))
self.sensor_range.append(rospy.get_param(name + '/range'))
self.sensor_shape.append(rospy.get_param(name + '/shape'))
self.sensor_reliability.append(
rospy.get_param(name + '/reliability'))
# Flag to wait ogm subscriber to finish
self.ogm_compute = False
self.current_pose = Pose()
# OGM related attributes
self.ogm_topic = '/map'
self.ogm = 0
self.ogm_raw = 0
self.ogm_width = 0
self.ogm_height = 0
self.ogm_header = 0
self.gvd = 0
self.brush = 0
# Load path pattern for navigation
self.navigation_pattern = rospy.get_param('navigation_pattern')
# Attributes read from json file
self.nodes = []
self.door_nodes = []
self.rooms = []
self.room_doors = []
self.room_type = []
self.room_sequence = []
self.wall_follow_nodes = []
self.wall_follow_sequence = []
self.zig_zag_nodes = []
self.zig_zag_sequence = []
self.simple_nodes = []
self.simple_sequence = []
self.data = []
# Load nodes from json file
map_name = rospy.get_param('map_name')
filename = '/home/mal/catkin_ws/src/topology_finder/data/' + map_name + '.json'
with open(filename, 'r') as read_file:
self.data = json.load(read_file)
self.nodes = self.data['nodes']
self.door_nodes = self.data['doors']
self.rooms = self.data['rooms']
self.room_doors = self.data['roomDoors']
self.room_type = self.data['roomType']
self.room_sequence = self.data['room_sequence']
if 'wall_follow_nodes' in self.data:
self.wall_follow_nodes = self.data['wall_follow_nodes']
if 'wall_follow_sequence' in self.data:
self.wall_follow_sequence = self.data['wall_follow_sequence']
if 'zig_zag_nodes' in self.data:
self.zig_zag_nodes = self.data['zig_zag_nodes']
if 'zig_zag_sequence' in self.data:
self.zig_zag_sequence = self.data['zig_zag_sequence']
if 'simple_nodes' in self.data:
self.simple_nodes = self.data['simple_nodes']
if 'simple_sequence' in self.data:
self.simple_sequence = self.data['simple_sequence']
self.node_publisher = rospy.Publisher('/nodes', Marker, queue_size=100)
self.door_node_pub = rospy.Publisher('/nodes/doors',
Marker,
queue_size=100)
self.room_node_pub = rospy.Publisher('/nodes/rooms',
Marker,
queue_size=100)
self.odom_sub = rospy.Subscriber('/odom', Odometry, self.odom_callback)
self.move_base_client = actionlib.SimpleActionClient(
'move_base', MoveBaseAction)
# Cb to read robot's pose
def odom_callback(self, msg):
self.current_pose = msg.pose.pose
return
def server_start(self):
rospy.init_node('navigation')
rospy.loginfo('navigation node initialized.')
# Read ogm
self.ogm_compute = True
rospy.Subscriber(self.ogm_topic, OccupancyGrid, self.read_ogm)
while self.ogm_compute:
pass
# Calculate current x,y in map's frame
current_x = int((self.current_pose.position.x - self.origin['x']) /
self.resolution)
current_y = int((self.current_pose.position.y - self.origin['y']) /
self.resolution)
# Calculate brushfire field
self.brush = self.brushfire_cffi.obstacleBrushfireCffi(self.ogm)
# Calculate gvd from brushfire and ogm
self.gvd = self.topology.gvd(self.brush)
# Find current room
start = (int(current_x), int(current_y))
if self.gvd[start]:
gvd_nodes = [start]
else:
gvd_nodes = self.brushfire_cffi.pointToGvdBrushfireCffi(
start, self.ogm, self.gvd)
if not len(gvd_nodes):
rospy.loginfo("Error. No gvd in the neighborhood.")
return
neighbor_nodes = self.brushfire_cffi.gvdNeighborBrushfireCffi(
gvd_nodes[0], self.nodes, self.gvd)
current_room = -1
for p in neighbor_nodes:
node = list(p)
if node in self.door_nodes:
continue
for i in range(len(self.rooms)):
if node in self.rooms[i]:
current_room = i
break
if current_room != -1:
break
if current_room == -1:
rospy.loginfo("Problem finding current room! Exiting...")
return
current_room_index = self.room_sequence.index(current_room)
start_time = rospy.get_time()
if self.navigation_pattern == 'simple':
if self.simple_sequence == []:
rospy.loginfo(
"This navigation pattern has not been calculated for this map!"
)
return
for i in range(len(self.room_sequence)):
# print nodes
nodes = self.simple_nodes[current_room]
self.print_markers(nodes)
# find nodes of current room
nodes = self.simple_sequence[current_room]
# navigate to all nodes
for node in nodes:
result = self.goToGoal(node['position'], node['yaw'])
rospy.sleep(0.1)
current_room_index = (current_room_index + 1) % len(
self.room_sequence)
current_room = self.room_sequence[current_room_index]
elif self.navigation_pattern == 'wall_follow':
if self.wall_follow_sequence == []:
rospy.loginfo(
"This navigation pattern has not been calculated for this map!"
)
return
for i in range(len(self.room_sequence)):
# print nodes
nodes = self.wall_follow_nodes[current_room]
self.print_markers(nodes)
# find nodes of current room
nodes = self.wall_follow_sequence[current_room]
# navigate to all nodes
for node in nodes:
result = self.goToGoal(node['position'], node['yaw'])
rospy.sleep(0.1)
current_room_index = (current_room_index + 1) % len(
self.room_sequence)
current_room = self.room_sequence[current_room_index]
elif self.navigation_pattern == 'zig_zag':
if self.zig_zag_sequence == []:
rospy.loginfo(
"This navigation pattern has not been calculated for this map!"
)
return
for i in range(len(self.room_sequence)):
# print nodes
nodes = self.zig_zag_nodes[current_room]
self.print_markers(nodes)
# find nodes of current room
nodes = self.zig_zag_sequence[current_room]
# navigate to all nodes
for node in nodes:
result = self.goToGoal(node['position'], node['yaw'])
rospy.sleep(0.1)
current_room_index = (current_room_index + 1) % len(
self.room_sequence)
current_room = self.room_sequence[current_room_index]
finish_time = rospy.get_time()
print('Start time', start_time, 'Finish time', finish_time, 'Duration',
finish_time - start_time)
return
# Gets target pose, sends it to move_base and waits for the result
def goToGoal(self, target, yaw):
self.move_base_client.wait_for_server()
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = target[
0] * self.resolution + self.origin['x']
goal.target_pose.pose.position.y = target[
1] * self.resolution + self.origin['y']
quaternion = tf.transformations.quaternion_from_euler(0, 0, yaw)
goal.target_pose.pose.orientation.x = quaternion[0]
goal.target_pose.pose.orientation.y = quaternion[1]
goal.target_pose.pose.orientation.z = quaternion[2]
goal.target_pose.pose.orientation.w = quaternion[3]
self.move_base_client.send_goal(goal)
wait = self.move_base_client.wait_for_result()
if not wait:
rospy.logerr("Action server not available!")
rospy.signal_shutdown("Action server not available!")
else:
return self.move_base_client.get_result()
def read_ogm(self, data):
# OGM is a 2D array of size width x height
# The values are from 0 to 100
# 0 is an unoccupied pixel
# 100 is an occupied pixel
# 50 or -1 is the unknown
rospy.loginfo("Navigation node reading ogm.")
self.ogm_raw = np.array(data.data)
self.ogm_width = data.info.width
self.ogm_height = data.info.height
self.ogm_header = data.header
self.ogm = np.zeros((data.info.width, data.info.height), \
dtype = np.int)
# Reshape ogm to a 2D array
for x in range(0, data.info.width):
for y in range(0, data.info.height):
self.ogm[x][y] = data.data[x + data.info.width * y]
rospy.loginfo("OGM read!")
self.ogm_compute = False
return
def print_markers(self, nodes):
rospy.loginfo("Start collecting markers")
points = []
for point in nodes:
p = Point()
p.x = point[0] * self.resolution + self.origin['x']
p.y = point[1] * self.resolution + self.origin['y']
p.z = 0
points.append(p)
rospy.loginfo("Markers ready!")
# Create Marker for nodes
marker = Marker()
marker.header.frame_id = "/map"
marker.type = marker.POINTS
marker.action = marker.ADD
marker.points = points
marker.pose.orientation.w = 1.0
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.color.a = 1.0
marker.color.r = 1.0
rospy.loginfo("Printing nodes!")
self.node_publisher.publish(marker)
return
class Coverage:
def __init__(self):
self.brushfire_cffi = Cffi()
# Origin is the translation between the (0,0) of the robot pose and the
# (0,0) of the map
self.origin = {}
self.origin['x'] = 0
self.origin['y'] = 0
self.resolution = 0.05
# Initilize sensor's specs
self.sensor_names = []
self.sensor_direction = []
self.sensor_fov = []
self.sensor_range = []
self.sensor_shape = []
self.sensor_reliability = []
# Read sensor's specs
self.sensor_names = rospy.get_param('sensor_names')
self.sensor_number = len(self.sensor_names)
for name in self.sensor_names:
self.sensor_direction.append(rospy.get_param(name + '/sensor_direction'))
self.sensor_fov.append(rospy.get_param(name + '/fov'))
self.sensor_range.append(rospy.get_param(name + '/range'))
self.sensor_shape.append(rospy.get_param(name + '/shape'))
self.sensor_reliability.append(rospy.get_param(name + '/reliability'))
# Compute bins to keep with which angle a sensor covers each point
max_fov = max(self.sensor_fov)/2 # compute for one half (symmetric with sensor_direction)
# self.number_of_bins = 2 * int(math.ceil(max_fov/5)) # 5 degrees in each bin
self.bins = []
# for i in range(self.number_of_bins):
# down = - self.number_of_bins/2 * 5 + i*5
# up = down + 5
# self.bins.append((down,up))
# Compute bins of negative and positive only
self.number_of_bins = 2
self.bins.append((-max_fov, 0))
self.bins.append((0, max_fov))
# Load map's translation
if rospy.has_param('origin'):
translation = rospy.get_param('origin')
self.origin['x'] = translation[0]
self.origin['y'] = translation[1]
if rospy.has_param('resolution'):
self.resolution = rospy.get_param('resolution')
# OGM related attributes
self.ogm_topic = '/map'
self.ogm = 0
self.ogm_raw = 0
self.ogm_width = 0
self.ogm_height = 0
self.ogm_header = 0
# Flags to wait subscribers to finish reading data
self.ogm_compute = True
self.readPose = False
# Holds the coverage information. This has the same size as the ogm
# If a cell has the value of 0 it is uncovered
# In the opposite case the cell's value will be 100
self.coverage = []
self.coverage_number = []
self.coverage_angles = []
self.brush = 0
# coverage_ogm will be published in the coverage_topic
self.coverage_ogm = OccupancyGrid()
self.coverage_ogm.header.frame_id = "map"
self.coverage_topic = '/map/coverage'
self.coverage_number_ogm = OccupancyGrid()
self.coverage_number_ogm.header.frame_id = "map"
self.coverage_number_topic = '/map/coverage_number'
self.coverage_angles_ogm = OccupancyGrid()
self.coverage_angles_ogm.header.frame_id = "map"
self.coverage_angles_topic = '/map/coverage_angles'
self.current_pose = Pose()
# robot_pose is current_pose in map's frame (aka in pixels)
self.robot_pose = {}
self.robot_pose['x'] = 0
self.robot_pose['y'] = 0
self.robot_pose['th'] = 0
self.previous_robot_pose = {}
self.previous_robot_pose['x'] = 0
self.previous_robot_pose['y'] = 0
self.previous_robot_pose['th'] = 0
self.coverage_pub = rospy.Publisher(self.coverage_topic, \
OccupancyGrid, queue_size = 10)
self.coverage_number_pub = rospy.Publisher(self.coverage_number_topic, \
OccupancyGrid, queue_size = 10)
self.coverage_angles_pub = rospy.Publisher(self.coverage_angles_topic, \
OccupancyGrid, queue_size = 10)
def getCoverage(self):
return np.copy(self.coverage)
def getCoverageOgm(self):
return self.coverage_ogm
def initCoverage(self):
self.coverage = np.zeros((self.ogm_width, self.ogm_height))
return
def server_start(self):
rospy.init_node('coverage')
rospy.loginfo('Coverage node initialized.')
# Read ogm
rospy.Subscriber(self.ogm_topic, OccupancyGrid, self.read_ogm)
while self.ogm_compute:
pass
# Calculate brushfire field
self.brush = self.brushfire_cffi.obstacleBrushfireCffi(self.ogm)
near_obstacles = np.where(self.brush == 2)
iter = 0
while not rospy.is_shutdown():
self.updateCover(publish = False)
if not iter % 500:
self.coverage_pub.publish(self.coverage_ogm)
self.coverage_number_pub.publish(self.coverage_number_ogm)
self.coverage_angles_pub.publish(self.coverage_angles_ogm)
rospy.loginfo("Update coverage ogm!")
near_obstacles_cover = self.coverage[near_obstacles]
covered_obstacles = len(np.where(near_obstacles_cover >= 80)[0])
rospy.loginfo("Estimated coverage percentage {}".format(covered_obstacles/len(near_obstacles_cover)))
iter = 0
iter += 1
rospy.sleep(0.1)
return
# Read current pose of robot from odometry
def readRobotPose(self):
rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, self.odom_callback, queue_size = 1)
return
def updateCover(self, pose = None, publish = True, track_specs = True):
if pose == None:
self.readPose = True
self.readRobotPose()
while self.readPose:
pass
xx = int(self.robot_pose['x'])
yy = int(self.robot_pose['y'])
th = self.robot_pose['th']
th_deg = math.degrees(th)
else:
xx = pose[0]
yy = pose[1]
th_deg = pose[2]
# Check if robot moved
move = ((xx - self.previous_robot_pose['x'])**2 + \
(yy - self.previous_robot_pose['y'])**2 + \
(th_deg - self.previous_robot_pose['th'])**2)**(1/2)
# Update coverage only if robot moved
if move > 1.0:
for s in range(self.sensor_number):
cover_length = int(self.sensor_range[s] / self.resolution)
if self.sensor_shape[s] == 'rectangular':
indexes = Cffi.rectangularBrushfireCoverageCffi((xx,yy), self.ogm, cover_length, self.sensor_fov[s], th_deg, self.sensor_direction[s])
for x,y in indexes:
self.coverage[x, y] = 100 * self.sensor_reliability[s]
i = int(x + self.ogm_width * y)
self.coverage_ogm.data[i] = 100
if track_specs:
self.coverage_number_ogm.data[i] += 1
self.coverage_number[x, y] += 1
elif self.sensor_shape[s] == 'circular':
indexes = Cffi.circularRayCastCoverageCffi((xx,yy), self.ogm, cover_length, self.sensor_fov[s], th_deg, self.sensor_direction[s])
for x,y in indexes:
self.coverage[x, y] = 100 * self.sensor_reliability[s]
i = int(x + self.ogm_width * y)
self.coverage_ogm.data[i] = 100
if track_specs:
self.coverage_number[x, y] += 1
if self.coverage_number[x, y] < 100:
self.coverage_number_ogm.data[i] += 1
yaw_between_nodes = math.degrees(math.atan2(yy-y, xx-x))
angle = yaw_between_nodes + th_deg + self.sensor_direction[s]
# print('th', th_deg, 'dir', self.sensor_direction[s], 'yaw', yaw_between_nodes, 'angle', angle)
if angle > 180:
angle -= 360
if angle <= -180:
angle += 360
for a in range(len(self.bins)):
if angle >= self.bins[a][0] and angle < self.bins[a][1]:
self.coverage_angles[x][y][a] += 1
ii = int(x + self.ogm_width * y + self.ogm_height * self.ogm_width * a)
self.coverage_angles_ogm.data[ii] += 1
# print(self.bins[a], a, angle)
break
else:
rospy.loginfo("Error!Sensor's shape not found!")
return
if publish:
self.coverage_pub.publish(self.coverage_ogm)
if track_specs:
self.coverage_number_pub.publish(self.coverage_number_ogm)
max_val = np.max(self.coverage_number)
rospy.loginfo("Maximum number of coverage look ups of points is {}.".format(max_val))
self.coverage_angles_pub.publish(self.coverage_angles_ogm)
rospy.loginfo("Update coverage ogm!")
self.previous_robot_pose['x'] = xx
self.previous_robot_pose['y'] = yy
self.previous_robot_pose['th'] = th_deg
return
def checkAndUpdateCover(self, brushfire, pose = None, threshold = 1.):
if pose == None:
self.readPose = True
self.readRobotPose()
while self.readPose:
pass
xx = int(self.robot_pose['x'])
yy = int(self.robot_pose['y'])
th = self.robot_pose['th']
th_deg = math.degrees(th)
else:
xx = pose[0]
yy = pose[1]
th_deg = pose[2]
indexes = []
for s in range(self.sensor_number):
cover_length = int(self.sensor_range[s] / self.resolution)
if self.sensor_shape[s] == 'rectangular':
temp = Cffi.rectangularBrushfireCoverageCffi((xx,yy), self.ogm, cover_length, self.sensor_fov[s], th_deg, self.sensor_direction[s])
indexes.extend(temp)
elif self.sensor_shape[s] == 'circular':
temp = Cffi.circularRayCastCoverageCffi((xx,yy), self.ogm, cover_length, self.sensor_fov[s], th_deg, self.sensor_direction[s])
indexes.extend(temp)
else:
rospy.loginfo("Error!Sensor's shape not found!")
return
covered_area = self.coverage[zip(*indexes)]
brushfire_area = brushfire[zip(*indexes)]
near_obstacles_len = len(np.where(brushfire_area == 2)[0])
if near_obstacles_len == 0:
return False
old_obstacles_len = len(np.where((brushfire_area == 2) & (covered_area > 80))[0])
perc = old_obstacles_len / near_obstacles_len
if perc < threshold:
for x,y in indexes:
self.coverage[x, y] = 100 * self.sensor_reliability[0]
i = int(x + self.ogm_width * y)
self.coverage_ogm.data[i] = 100
updated = True
else:
updated = False
return updated
def checkNearbyObstacleCover(self, pose):
xx = pose[0]
yy = pose[1]
th_deg = pose[2]
return_obstacles = True
indexes = []
for s in range(self.sensor_number):
cover_length = int(self.sensor_range[s] / self.resolution)
if self.sensor_shape[s] == 'circular':
temp = Cffi.circularRayCastCoverageCffi((xx,yy), self.ogm, cover_length, self.sensor_fov[s], th_deg, self.sensor_direction[s], return_obstacles)
indexes.extend(temp)
else:
rospy.loginfo("Error!Sensor's shape not found!")
return
indexes = list(set(indexes))
return indexes
# Cb to read robot's pose
def odom_callback(self, msg):
self.current_pose = msg.pose.pose
# Calculate current x,y in map's frame (aka in pixels)
self.robot_pose['x'] = int((self.current_pose.position.x - self.origin['x'])/self.resolution)
self.robot_pose['y'] = int((self.current_pose.position.y - self.origin['y'])/self.resolution)
# Getting the Euler angles
q = (msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z,
msg.pose.pose.orientation.w)
angles = tf.transformations.euler_from_quaternion(q)
self.robot_pose['th'] = angles[2] # yaw
self.readPose = False
return
def read_ogm(self, data):
# OGM is a 2D array of size width x height
# The values are from 0 to 100
# 0 is an unoccupied pixel
# 100 is an occupied pixel
# 50 or -1 is the unknown
rospy.loginfo("Coverage node reading ogm.")
self.ogm_raw = np.array(data.data)
self.ogm_width = data.info.width
self.ogm_height = data.info.height
self.ogm_header = data.header
self.ogm = np.zeros((data.info.width, data.info.height), \
dtype = np.int)
# Reshape ogm to a 2D array
for x in range(0, data.info.width):
for y in range(0, data.info.height):
self.ogm[x][y] = data.data[x + data.info.width * y]
# Initilize coverage OGM with same size width x height
self.coverage = np.zeros((self.ogm_width, self.ogm_height))
self.coverage_ogm.info = data.info
self.coverage_ogm.data = np.zeros(self.ogm_width * self.ogm_height)
# Initilize coverage_number OGM with same size width x height
self.coverage_number = np.zeros((self.ogm_width, self.ogm_height))
self.coverage_number_ogm.info = data.info
self.coverage_number_ogm.data = np.zeros(self.ogm_width * self.ogm_height)
self.coverage_angles = np.zeros((self.ogm_width, self.ogm_height, self.number_of_bins))
self.coverage_angles_ogm.info.width = self.ogm_width
self.coverage_angles_ogm.info.height = self.ogm_height * self.number_of_bins
self.coverage_angles_ogm.data = np.zeros(self.ogm_width * self.ogm_height * self.number_of_bins)
rospy.loginfo("OGM read!")
self.ogm_compute = False
return
