def markNodesPrunable(self):
""" Prunes unneeded nodes."""
for node in self.nodes:
node.isPrunable = True
# 1) Create a path for every query node. Each path will contain only that evidence node.
startPaths = []
for node in self.nodes:
if node.type == NodeType["Query"]:
p = Path(node, Direction["Start"])
self.markActivePath(p)
startPaths.append(p)
#print "startPaths: {}".format(startPaths)
# 2) Add the neighbors of each query node to the paths.
paths = []
for path in startPaths:
for node in path[-1][0].parents:
p = path.clone()
p.append(node, Direction["Parent"])
if node.type == NodeType["Evidence"]:
self.markActivePath(p)
paths.append(p)
for node in path[-1][0].children:
p = path.clone()
p.append(node, Direction["Child"])
if node.type == NodeType["Evidence"]:
self.markActivePath(p)
paths.append(p)
# 3) Expand the paths as far as we can.
while len(paths) > 0:
#print "paths: {}".format(paths)
prevPaths = paths
paths = []
for curPath in prevPaths:
for node in curPath[-1][0].parents:
if not self.isBlocked(
curPath[-1],
(node, Direction["Parent"])) and not node in curPath:
p = curPath.clone()
p.append(node, Direction["Parent"])
paths.append(p)
if node.type == NodeType["Evidence"]:
self.markActivePath(p)
for node in curPath[-1][0].children:
if not self.isBlocked(
curPath[-1],
(node, Direction["Child"])) and not node in curPath:
p = curPath.clone()
p.append(node, Direction["Child"])
paths.append(p)
if node.type == NodeType["Evidence"]:
self.markActivePath(p)
def stop_wanted(location):
if location == 'tmp':
loc = tmpdir
elif location == 'workdir':
loc = workdir
else:
loc = Path(location)
return loc.append('STOP').exists
def get_paths(self,name):
paths = rospy.get_param(name)
for path_item in paths:
path = None
path = Path(path_item.get("name"),[])
wp_list = path_item.get("poses")
for wp in wp_list:
is_door = wp.get("type") == 0
name = wp.get("name")
if is_door:
move_forward = wp.get("forward")
door = next((d for d in self.connections if d.id == name ), None)
waypoint1 = door.get_first_waypoint(move_forward)
waypoint2 = door.get_last_waypoint(move_forward)
path.append(waypoint1)
path.append(waypoint2)
else:
wayp = next((w for w in self.waypoints if w.id == name ), None)
path.append(wayp)
self.paths.append(copy.deepcopy(path))
