def navigate(self, target):
if self.closest_node is None or self.closest_node == 'none':
print('was not localised, so assume we are at the target')
o_node = get_node(self.map, target)
else:
o_node = get_node(self.map, self.closest_node)
print self.closest_node, target
g_node = get_node(self.map, target)
# Everything is Awesome!!!
# Target and Origin are Different and none of them is None
if (g_node is not None) and\
(o_node is not None) and\
(g_node.name != o_node.name):
rsearch = TopologicalRouteSearch(self.map)
route = rsearch.search_route(o_node.name, target)
print route
if route:
print "Navigating Case 1"
self.follow_route(route)
else:
print "There is no route to this node check your edges ..."
else:
# Target and Origin are the same
if (g_node.name == o_node.name):
print "Target and Origin Nodes are the same"
self.memProxy.raiseEvent("TopologicalNav/Status", "Success")
else:
print "Target or Origin Nodes were not found on Map"
self.memProxy.raiseEvent("TopologicalNav/Status", "Fail")
def draw_top_map(self):
topmap_image = self.imgFile.copy()
height, width, channels = topmap_image.shape
font = cv2.FONT_HERSHEY_SIMPLEX
origin = list()
origin.append(self.props['origin'][0])
origin.append(self.props['origin'][1] +
(height * self.props['resolution']))
origin.append(self.props['resolution'])
for i in self.top_map.nodes:
v1 = i.pose.position
for j in i.edges:
v2 = get_node(self.top_map, j.node).pose.position
self.draw_arrow(topmap_image,
v1,
v2, (255, 0, 0, 255),
origin,
thickness=2,
arrow_magnitude=5,
line_type=1)
xval = int((v1.x - origin[0]) / self.props['resolution'])
yval = int((origin[1] - v1.y) / self.props['resolution'])
if self.debug:
print xval, yval
cv2.circle(topmap_image, (int(xval), int(yval)), 10,
(0, 0, 255, 255), -1)
cv2.putText(topmap_image, i.name, (int(xval), int(yval - 5)), font,
0.3, (20, 20, 20), 1)
return topmap_image
def navigate(self, target):
if self.closest_node is None or self.closest_node == 'none':
logger.info('was not localised, so assume we are at the start')
o_node = get_node(self.map, target)
else:
o_node = get_node(self.map, self.closest_node)
logger.info(
'closest_nde: %s, target: %s' % (self.closest_node, target))
g_node = get_node(self.map, target)
# Everything is Awesome!!!
# Target and Origin are Different and none of them is None
try:
if (g_node is not None) and\
(o_node is not None) and\
(g_node.name != o_node.name):
rsearch = TopologicalRouteSearch(self.map)
route = rsearch.search_route(o_node.name, target)
logger.info('route: %s' % route)
if route:
logger.info('starting to follow route')
self.follow_route(route)
else:
logger.error(
'There is no route to this node check your edges ...')
else:
if (g_node is None) or (o_node is None):
logger.warning('g_node or o_node is None. Fail')
self.memProxy.raiseEvent("TopologicalNav/Status", "Fail")
elif(g_node.name == o_node.name):
logger.info(
'Target and Origin Nodes are the same. assume success'
)
self.memProxy.raiseEvent("TopologicalNav/Status",
"Success")
else:
logger.error(
"Target or Origin Nodes were not found on Map")
self.memProxy.raiseEvent("TopologicalNav/Status", "Fail")
except Exception as e:
logger.fatal(
'*** FATAL PROBLEM IN TOPOLOGICAL NAVIGATION: %s' % str(e))
self.memProxy.raiseEvent("TopologicalNav/Status", "Fail")
def get_route(self, target):
logger.info("get route %s->%s" % (self.closest_node, target))
g_node = get_node(self.map, target)
# Everything is Awesome!!!
# Target and Origin are Different and none of them is None
if (g_node is not None) and \
(o_node is not None) and \
(g_node.name != o_node.name):
rsearch = TopologicalRouteSearch(self.map)
route = rsearch.search_route(o_node.name, target)
logger.info("get route: found %s" % route)
return route
def get_route(self, target):
g_node = get_node(self.map, target)
print "get route", self.closest_node, target
# Everything is Awesome!!!
# Target and Origin are Different and none of them is None
if (g_node is not None) and \
(o_node is not None) and \
(g_node.name != o_node.name):
rsearch = TopologicalRouteSearch(self.map)
route = rsearch.search_route(o_node.name, target)
print route
return route
def _on_get_plan(self, data):
print('on_get_plan: %s' % data)
route = self.get_route(goal)
plan = []
for i in range(len(route.source)):
step = {}
cn = get_node(self.map, route.source[i])
step['from'] = route.source[i]
for j in cn.edges:
if j.edge_id == route.edge_id[i]:
step['edge_id'] = j.edge_id
step['action'] = j.action
step['dest'] = {}
step['dest']['node'] = j.node
nn = get_node(self.map, j.node)
step['dest']['x'] = nn.pose.position.x
step['dest']['y'] = nn.pose.position.y
plan.append(step)
if route:
self.memProxy.insertData("TopologicalNav/Route", plan.__repr__())
self.memProxy.raiseEvent("TopologicalNav/PlanReady", "True")
else:
self.memProxy.raiseEvent("TopologicalNav/PlanReady", "False")
self.get_plan = True
def follow_route(self, route):
"""
Follow Route
This function follows the chosen route to reach the goal
"""
nnodes = len(route.source)
self.navigation_activated = True
orig = route.source[0]
self.cancelled = False
logger.info(str(nnodes) + " Nodes on route to follow")
rindex = 0
nav_ok = True
route_len = len(route.edge_id)
o_node = get_node(self.map, orig)
a = get_edge_from_id(self.map,
route.source[0],
route.edge_id[0]).action
logger.info("first action " + a)
# If the robot is not on a node or the first action is not move base type
# navigate to closest node waypoint (only when first action is not move base)
self.navigation_activated = True
if self.current_node == 'none':
logger.info('Do planner to %s' % (self.closest_node))
self.current_target = orig
nav_ok = self.monitored_navigation(o_node, 'NAOqiPlanner/Goal')
while rindex < route_len and not self.cancelled:
# current action
cedg = get_edge_from_id(
self.map, route.source[rindex], route.edge_id[rindex]
)
a = cedg.action
# next action
if rindex < (route_len - 1):
a1 = get_edge_from_id(
self.map,
route.source[rindex + 1],
route.edge_id[rindex + 1]).action
else:
a1 = 'none'
self.current_action = a
self.next_action = a1
cnode = get_node(self.map, cedg.node)
if a == a1:
definite_action = "NAOqiPlanner/GoalXY"
else:
definite_action = a
logger.info("From " + route.source[rindex] +
" do " + a + " to " + cedg.node)
self.current_target = cedg.node
nav_ok = self.monitored_navigation(cnode, definite_action)
if nav_ok:
rindex = rindex + 1
elif self.fail_code == 0: # wrong node, go the right one again
pass
else:
logger.warning("qinav failed, but trying next node regardless")
rindex = rindex + 1
self.navigation_activated = False
if nav_ok:
self.memProxy.raiseEvent("TopologicalNav/Status", "Success")
else:
self.memProxy.raiseEvent("TopologicalNav/Status", "Fail")