def _add_hall_loc(self, node_info):
[xmin, xmax] = node_info[5:7]
[ymin, ymax] = node_info[7:9]
# Currently can't handle nodes that are not parallel to an axis
# Currently nodes are listed as a line with no width
# This adds width of 0.4m so it can detect when it's in that space
# Vertical
if xmin == xmax:
xmin = xmin - self.node_width / 2
xmax = xmax + self.node_width / 2
# Horizontal
elif ymin == ymax:
ymin = ymin - self.node_width / 2
ymax = ymax + self.node_width / 2
else:
print("something has gone wrong. One of these pairs should be ==")
print(node_info[0], xmin, xmax, ymin, ymax)
self.G.add_node(node_info[0],
node_loc=[cust_interval(xmin, xmax),
cust_interval(ymin, ymax)],
hum_cond=node_info[-1]) # Adds the human presence condition
def _add_door_loc_sans_a(self, node_info):
# Handle rooms first
# Rooms will get one node in the door and one spanning the hall
# Handles rooms then extra doors
name = node_info[0]
if 'd' not in node_info[0]:
name = node_info[0] + '_d00'
self.doors.append(name)
[xmin, xmax] = node_info[5:7]
[ymin, ymax] = node_info[7:9]
# Currently can't handle doors that are not parallel to an axis
# Currently a door is listed as a line with no width
# This adds width of 0.4m so it can detect when it's in that space
# Vertical
if xmin == xmax:
# This is where things get very hand-coded (ugh)
# TODO: Make this less world specific
# Spans the hallway but doesn't overlap with the door node
if xmin < 15:
xmax = xmin + self.hall_width
else:
xmin = xmax - self.hall_width
middle_y = (ymin + ymax) / 2
ymin = middle_y - self.hall_node_width / 2
ymax = middle_y + self.hall_node_width / 2
# Horizontal
elif ymin == ymax:
# This is where things get very hand-coded (ugh)
# TODO: Make this less world specific
# Spans the hallway but doesn't overlap with the door node
if ymin < 10:
ymax = ymin + self.hall_width
else:
ymin = ymax - self.hall_width
middle_x = (xmin + xmax) / 2
xmin = middle_x - self.hall_node_width / 2
xmax = middle_x + self.hall_node_width / 2
else:
print("something has gone wrong. One of these pairs should be ==")
print(xmin, xmax, ymin, ymax)
# Hallway node outside doorway
self.G.add_node(name,
node_loc=[cust_interval(xmin, xmax),
cust_interval(ymin, ymax)],
hum_cond=node_info[-1]) # Adds the human presence condition
def _add_room_loc(self, node_info):
# This will give the system x,y bounds for each room
# This is used to choose where to autonomously drive next
if node_info[1] is not 'x':
# Get the midpoint in the space
mid_x = (node_info[1] + node_info[2]) / 2
mid_y = (node_info[3] + node_info[4]) / 2
self.G.add_node(node_info[0],
node_loc=[cust_interval(mid_x - 0.1, mid_x + 0.1),
cust_interval(mid_y - 0.1, mid_y + 0.1)],
rm_loc=[[node_info[1], node_info[2]], [node_info[3], node_info[4]]],
hum_cond=node_info[-1]) # Adds the human presence condition
def _add_edge_weights(self):
# Set up to add weights that are a custom interval of [0, 0]
# nx.set_edge_attributes(self.G, , 'weight_no')
for (n1, n2) in self.G.edges():
# print(n1, n2)
self.G[n1][n2]['weight_no'] = cust_interval(0) # No humans present
self.G[n1][n2]['weight_hum'] = cust_interval(0) # Humans present
self.G[n1][n2]['trav_data_no'] = []
self.G[n1][n2]['trav_data_hum'] = []
hum_cond = self.G.nodes[n1]['hum_cond']
if hum_cond != self.G.nodes[n2]['hum_cond']:
# Always use the exterior hall conditions over interior hall conditions
if n1[0] == 'h':
hum_cond = self.G.nodes[n2]['hum_cond']
elif n2[0] == 'h':
hum_cond = self.G.nodes[n1]['hum_cond']
elif hum_cond > self.G.nodes[n2]['hum_cond']:
hum_cond = self.G.nodes[n2]['hum_cond']
self.G[n1][n2]['hum_cond'] = hum_cond
def _add_excl_loc(self, node_info):
self.G.add_node(node_info[0],
node_loc=[cust_interval(node_info[5], node_info[6]),
cust_interval(node_info[7], node_info[8])],
rm_loc=[[node_info[5], node_info[6]], [node_info[7], node_info[8]]],
hum_cond=node_info[-1]) # Adds the human presence condition