def __init__(self) :
self._killall=False
#rospy.on_shutdown(self._on_node_shutdown)
self.route_nodes = NavRoute()
self.map_edges = MarkerArray()
#self.update_needed=False
rospy.loginfo("Creating Publishers ...")
self.policies_pub = rospy.Publisher('topological_edges_policies', MarkerArray)
rospy.loginfo("Done ...")
rospy.loginfo("Creating subscriber ...")
self.subs = rospy.Subscriber("mdp_plan_exec/current_policy_mode", NavRoute, self.policies_callback)
rospy.loginfo("Done ...")
#Waiting for Topological Map
self.map_received=False
rospy.Subscriber('topological_map', TopologicalMap, self.MapCallback)
rospy.loginfo("Waiting for Topological map ...")
while not self.map_received and not self._killall :
rospy.sleep(rospy.Duration.from_sec(0.05))
rospy.loginfo(" ...done")
rospy.loginfo("All Done ...")
def generate_current_nav_policy(self, policy_mdp):
policy_msg = NavRoute()
current_state_def = policy_mdp.current_state_def
current_dfa_state = policy_mdp.current_state_def["_da"]
queue = [policy_mdp.current_flat_state]
self.current_nav_policy_state_defs = {
policy_mdp.current_flat_state: current_state_def
}
while queue != []:
print(queue)
new_flat_state = queue.pop(0)
current_state_def = policy_mdp.flat_state_defs[new_flat_state]
if policy_mdp.flat_state_policy.has_key(new_flat_state):
action = policy_mdp.flat_state_policy[new_flat_state]
if action in self.mdp.nav_actions and current_state_def[
"_da"] == current_dfa_state:
source = self.mdp.get_waypoint_prop(
current_state_def["waypoint"])
policy_msg.source.append(source)
policy_msg.edge_id.append(action)
for suc_state in policy_mdp.flat_state_sucs[
new_flat_state]:
if not self.current_nav_policy_state_defs.has_key(
suc_state):
queue.append(suc_state)
self.current_nav_policy_state_defs[
suc_state] = policy_mdp.flat_state_defs[
suc_state]
return policy_msg
def search_route(self, origin, target):
if origin == "none" or target == "none" or origin == target:
return None
goal = get_node(self.top_map, target)
orig = get_node(self.top_map, origin)
to_expand = []
children = []
expanded = []
#print 'searching route from %s to %s' %(orig.name, goal.name)
#self.get_distance_to_node(goal, orig)
nte = NodeToExpand(orig.name, 'none', 0.0,
get_distance_to_node(goal, orig)) #Node to Expand
expanded.append(nte)
#to_expand.append(nte)
# exp_index=0
cen = orig #currently expanded node
children = get_conected_nodes(cen) #nodes current node is connected to
#print "children\n", children
not_goal = True
route_found = False
while not_goal:
if target in children:
not_goal = False
route_found = True
cdist = get_distance_to_node(cen, goal)
cnte = NodeToExpand(goal.name, nte.name,
nte.current_distance + cdist,
0.0) #Node to Expand
expanded.append(cnte)
#print "goal found"
else:
#print "Goal NOT found"
for i in children:
been_expanded = False
to_be_expanded = False
for j in expanded:
# search in expanded
if i == j.name:
been_expanded = True
old_expand_node = j
# found it. skip remaining
break
if not been_expanded:
# not in expanded, search in to_expand. can't be in both
for j in to_expand:
if i == j.name:
been_expanded = True
to_be_expanded = True
old_expand_node = j
# found it. skip remaining
break
if not been_expanded:
nnn = get_node(self.top_map, i)
tdist = get_distance_to_node(goal, nnn)
cdist = get_distance_to_node(cen, nnn)
cnte = NodeToExpand(nnn.name, nte.name,
nte.current_distance + cdist,
tdist) #Node to Expand
to_expand.append(cnte)
to_expand = sorted(to_expand,
key=lambda node: node.cost)
else:
nnn = get_node(self.top_map, i)
tdist = get_distance_to_node(goal, nnn)
cdist = get_distance_to_node(cen, nnn)
# update existing NTE with new data if a shorter route to it is found
if nte.current_distance + cdist < old_expand_node.current_distance:
old_expand_node.father = nte.name
old_expand_node.current_distance = nte.current_distance + cdist
old_expand_node.dist_to_target = tdist
old_expand_node.cost = old_expand_node.current_distance + old_expand_node.dist_to_target
if to_be_expanded:
# re-sort to_expand with new costs
to_expand = sorted(to_expand,
key=lambda node: node.cost)
if len(to_expand) > 0:
nte = to_expand.pop(0)
#print 'expanding node %s (%d)' %(nte.name,len(to_expand))
cen = get_node(self.top_map, nte.name)
expanded.append(nte)
children = get_conected_nodes(cen)
else:
not_goal = False
route_found = False
route = NavRoute()
# print "===== RESULT ====="
if route_found:
steps = []
val = len(expanded) - 1
steps.append(expanded[val])
next_node = expanded[val].father
#print next_node
while next_node != 'none':
for i in expanded:
if i.name == next_node:
steps.append(i)
next_node = i.father
break
steps.reverse()
val = len(steps)
for i in range(1, val):
edg = get_edges_between(self.top_map, steps[i].father,
steps[i].name)
route.source.append(steps[i].father)
route.edge_id.append(edg[0].edge_id)
#route.append(r)
#print route
return route
else:
return None
def search_route(self, origin, target):
"""
This function searches the route to reach the goal
"""
route = NavRoute()
if origin == "none" or target == "none" or origin == target:
return route
goal = self.get_node_from_tmap2(target)
orig = self.get_node_from_tmap2(origin)
to_expand=[]
children=[]
expanded=[]
nte = NodeToExpand(orig["node"]["name"], 'none', 0.0, get_distance_to_node_tmap2(goal, orig)) # Node to Expand
expanded.append(nte)
cen = orig # currently expanded node
children = get_conected_nodes_tmap2(cen) # nodes current node is connected to
not_goal=True
route_found=False
while not_goal :
if target in children:
not_goal=False
route_found=True
cdist = get_distance_to_node_tmap2(cen, goal)
cnte = NodeToExpand(goal["node"]["name"], nte.name, nte.current_distance+cdist, 0.0) # Node to Expand
expanded.append(cnte)
else :
for i in children:
been_expanded = False
to_be_expanded = False
for j in expanded: # search in expanded
if i == j.name:
been_expanded = True
old_expand_node = j # found it. skip remaining
break
if not been_expanded:
# not in expanded, search in to_expand. can't be in both
for j in to_expand:
if i == j.name:
been_expanded = True
to_be_expanded = True
old_expand_node = j
# found it. skip remaining
break
if not been_expanded:
nnn = self.get_node_from_tmap2(i)
tdist = get_distance_to_node_tmap2(goal, nnn)
cdist = get_distance_to_node_tmap2(cen, nnn)
cnte = NodeToExpand(nnn["node"]["name"], nte.name, nte.current_distance+cdist, tdist) # Node to Expand
to_expand.append(cnte)
to_expand = sorted(to_expand, key=lambda node: node.cost)
else:
nnn = self.get_node_from_tmap2(i)
tdist = get_distance_to_node_tmap2(goal, nnn)
cdist = get_distance_to_node_tmap2(cen, nnn)
# update existing NTE with new data if a shorter route to it is found
if nte.current_distance+cdist < old_expand_node.current_distance:
old_expand_node.father = nte.name
old_expand_node.current_distance = nte.current_distance+cdist
old_expand_node.dist_to_target = tdist
old_expand_node.cost = old_expand_node.current_distance + old_expand_node.dist_to_target
if to_be_expanded: # re-sort to_expand with new costs
to_expand = sorted(to_expand, key=lambda node: node.cost)
if len(to_expand)>0:
nte = to_expand.pop(0)
cen = self.get_node_from_tmap2(nte.name)
expanded.append(nte)
children = get_conected_nodes_tmap2(cen)
else:
not_goal=False
route_found=False
# print "===== RESULT ====="
if route_found:
steps=[]
val = len(expanded)-1
steps.append(expanded[val])
next_node = expanded[val].father
while next_node != 'none':
for i in expanded:
if i.name == next_node :
steps.append(i)
next_node = i.father
break
steps.reverse()
val = len(steps)
for i in range(1, val):
edg=self.get_edges_between_tmap2(steps[i].father, steps[i].name)
route.source.append(steps[i].father)
route.edge_id.append(edg[0]["edge_id"])
return route
def search_route(self, origin, target):
goal = get_node(self.top_map, target)
orig = get_node(self.top_map, origin)
to_expand = []
children = []
expanded = []
#print 'searching route from %s to %s' %(orig.name, goal.name)
#self.get_distance_to_node(goal, orig)
nte = NodeToExpand(orig.name, 'none', 0.0,
get_distance_to_node(goal, orig)) #Node to Expand
expanded.append(nte)
#to_expand.append(nte)
# exp_index=0
cen = orig #currently expanded node
children = get_conected_nodes(cen) #nodes current node is connected to
#print children
not_goal = True
route_found = False
while not_goal:
if target in children:
not_goal = False
route_found = True
cdist = get_distance_to_node(cen, goal)
cnte = NodeToExpand(goal.name, nte.name,
nte.current_distance + cdist,
0.0) #Node to Expand
expanded.append(cnte)
#print "goal found"
else:
#print "Goal NOT found"
for i in children:
been_expanded = False
for j in expanded:
if i == j.name:
been_expanded = True
for j in to_expand:
if i == j.name:
been_expanded = True
if not been_expanded:
nnn = get_node(self.top_map, i)
tdist = get_distance_to_node(goal, nnn)
cdist = get_distance_to_node(cen, nnn)
cnte = NodeToExpand(nnn.name, nte.name,
nte.current_distance + cdist,
tdist) #Node to Expand
to_expand.append(cnte)
to_expand = sorted(to_expand,
key=lambda node: node.cost)
if len(to_expand) > 0:
nte = to_expand.pop(0)
#print 'expanding node %s (%d)' %(nte.name,len(to_expand))
cen = get_node(self.top_map, nte.name)
expanded.append(nte)
children = get_conected_nodes(cen)
else:
not_goal = False
route_found = False
route = NavRoute()
# print "===== RESULT ====="
if route_found:
steps = []
val = len(expanded) - 1
steps.append(expanded[val])
next_node = expanded[val].father
#print next_node
while next_node != 'none':
for i in expanded:
if i.name == next_node:
steps.append(i)
next_node = i.father
break
steps.reverse()
val = len(steps)
for i in range(1, val):
edg = get_edges_between(self.top_map, steps[i].father,
steps[i].name)
route.source.append(steps[i].father)
route.edge_id.append(edg[0].edge_id)
#route.append(r)
return route
else:
return None
def __init__(self, map_name):
self.map_name = map_name
self.route_nodes = NavRoute()
self.updating = True
self.update_map(map_name)