def skeletonizationCffi(self, ogm, origin, resolution, ogml):
width = ogm.shape[0]
height = ogm.shape[1]
local = numpy.zeros(ogm.shape)
for i in range(0, width):
for j in range(0, height):
if ogm[i][j] < 49:
local[i][j] = 1
skeleton = Cffi.thinning(local, ogml)
skeleton = Cffi.prune(skeleton, ogml, 10)
viz = []
for i in range(0, width):
for j in range(0, height):
if skeleton[i][j] == 1:
viz.append([
i * resolution + origin['x'],
j * resolution + origin['y']
])
RvizHandler.printMarker(\
viz,\
1, # Type: Arrow
0, # Action: Add
"map", # Frame
"art_skeletonization_cffi", # Namespace
[0.5, 0, 0, 0.5], # Color RGBA
0.05 # Scale
)
return skeleton
def skeletonizationCffi(self, ogm, origin, resolution, ogml):
# width = ogm.shape[0]
# height = ogm.shape[1]
local = numpy.zeros(ogm.shape)
local[ogm < 49] = 1
# for i in range(0, width):
# for j in range(0, height):
# if ogm[i][j] < 49:
# local[i][j] = 1
skeleton = Cffi.thinning(local, ogml)
skeleton = Cffi.prune(skeleton, ogml, 10)
# viz = []
# for i in range(0, width):
# for j in range(0, height):
# if skeleton[i][j] == 1:
# viz.append([i * resolution + origin['x'],j * resolution + origin['y']])
viz = (numpy.array(numpy.where(skeleton == 1)).T * resolution +
[origin['x'], origin['y']]).tolist()
RvizHandler.printMarker(\
viz,\
1, # Type: Arrow
0, # Action: Add
"map", # Frame
"art_skeletonization_cffi", # Namespace
[0.5, 0, 0, 0.5], # Color RGBA
0.05 # Scale
)
return skeleton
def checkTarget(self, event):
# Check if we have a target or if the robot just wanders
if not self.inner_target_exists or not self.move_with_target or self.next_subtarget == len(
self.subtargets):
return
self.counter_to_next_sub -= 1
if self.counter_to_next_sub == 0:
Print.art_print('\n~~~~ Time reset ~~~~', Print.RED)
self.inner_target_exists = False
self.target_exists = False
self.force_random = True
return
# Get the robot pose in pixels
[rx, ry] = [
self.robot_perception.robot_pose['x_px'] -
self.robot_perception.origin['x'] /
self.robot_perception.resolution,
self.robot_perception.robot_pose['y_px'] -
self.robot_perception.origin['y'] /
self.robot_perception.resolution
]
######################### NOTE: QUESTION ##############################
# What if a later subtarget or the end has been reached before the next subtarget? Alter the code
# accordingly. Check if distance is less than 7 px (14 cm). Slice the list of subtargets and then reverse it.
for idx, st in enumerate(self.subtargets[self.next_subtarget::][::-1]):
# Right now, idx refers to the sliced & reversed array, fix it.
idx = len(self.subtargets) - 1 - idx
assert idx >= self.next_subtarget
dist = math.hypot(rx - st[0], ry - st[1])
if dist < 7:
self.next_subtarget = idx + 1
self.counter_to_next_sub = self.count_limit
if self.next_subtarget == len(self.subtargets):
self.target_exists = False
break
########################################################################
# Publish the current target
if self.next_subtarget == len(self.subtargets):
return
subtarget = [
self.subtargets[self.next_subtarget][0] *
self.robot_perception.resolution +
self.robot_perception.origin['x'],
self.subtargets[self.next_subtarget][1] *
self.robot_perception.resolution +
self.robot_perception.origin['y']
]
RvizHandler.printMarker(
[subtarget],
1, # Type: Arrow
0, # Action: Add
"map", # Frame
"art_next_subtarget", # Namespace
[0, 0, 0.8, 0.8], # Color RGBA
0.2 # Scale
)
def selectTarget(self, init_ogm, coverage, robot_pose, origin,
resolution, force_random=False):
target = [-1, -1]
######################### NOTE: QUESTION ##############################
# Implement a smart way to select the next target. You have the
# following tools: ogm_limits, Brushfire field, OGM skeleton,
# topological nodes.
# Find only the useful boundaries of OGM. Only there calculations
# have meaning
ogm_limits = OgmOperations.findUsefulBoundaries(init_ogm, origin, resolution)
# Blur the OGM to erase discontinuities due to laser rays
ogm = OgmOperations.blurUnoccupiedOgm(init_ogm, ogm_limits)
# Calculate Brushfire field
tinit = time.time()
brush = self.brush.obstaclesBrushfireCffi(ogm, ogm_limits)
Print.art_print("Brush time: " + str(time.time() - tinit), Print.ORANGE)
# Calculate skeletonization
tinit = time.time()
skeleton = self.topo.skeletonizationCffi(ogm,
origin, resolution, ogm_limits)
Print.art_print("Skeletonization time: " + str(time.time() - tinit), Print.ORANGE)
# Find topological graph
tinit = time.time()
nodes = self.topo.topologicalNodes(ogm, skeleton, coverage, origin,
resolution, brush, ogm_limits)
Print.art_print("Topo nodes time: " + str(time.time() - tinit), Print.ORANGE)
# Visualization of topological nodes
vis_nodes = []
for n in nodes:
vis_nodes.append([
n[0] * resolution + origin['x'],
n[1] * resolution + origin['y']
])
RvizHandler.printMarker(
vis_nodes,
1, # Type: Arrow
0, # Action: Add
"map", # Frame
"art_topological_nodes", # Namespace
[0.3, 0.4, 0.7, 0.5], # Color RGBA
0.1 # Scale
)
# Random point
if self.method == 'random' or force_random == True:
target = self.selectRandomTarget(ogm, coverage, brush, ogm_limits)
########################################################################
return target
def checkTarget(self, event):
# Check if we have a target or if the robot just wanders
if not self.inner_target_exists or not self.move_with_target or self.next_subtarget == len(
self.subtargets):
return
# Check if timer has expired
if self.timer_flag:
if self.timerThread.expired:
Print.art_print('\n~~~~ Time reset ~~~~', Print.RED)
self.inner_target_exists = False
self.target_exists = False
return
# Get the robot pose in pixels
[rx, ry] = [
self.robot_perception.robot_pose['x_px'] -
self.robot_perception.origin['x'] /
self.robot_perception.resolution,
self.robot_perception.robot_pose['y_px'] -
self.robot_perception.origin['y'] /
self.robot_perception.resolution
]
theta_robot = self.robot_perception.robot_pose['th']
# Clear achieved targets
self.subtargets = self.subtargets[self.next_subtarget:]
self.next_subtarget = 0
# If distance between the robot pose and the next subtarget is < 7 pixel consider that Target is Reached
dist = math.hypot(rx - self.subtargets[self.next_subtarget][0],
ry - self.subtargets[self.next_subtarget][1])
if dist < 7:
self.next_subtarget += 1
self.counter_to_next_sub = self.count_limit
# Check if the final subtarget has been approached
if self.next_subtarget == len(self.subtargets):
self.target_exists = False
# Publish the current target
if self.next_subtarget >= len(self.subtargets):
return
subtarget = [
self.subtargets[self.next_subtarget][0] *
self.robot_perception.resolution +
self.robot_perception.origin['x'],
self.subtargets[self.next_subtarget][1] *
self.robot_perception.resolution +
self.robot_perception.origin['y']
]
RvizHandler.printMarker([subtarget], 1, 0, "map", "art_next_subtarget",
[0, 0, 0.8, 0.8], 0.2)
def print_viz(skeleton, resolution, x, y):
i, j = numpy.where(skeleton == 1)
viz = zip(i * resolution + x, j * resolution + y)
RvizHandler.printMarker(
viz,
1, # Type: Arrow
0, # Action: Add
"map", # Frame
"art_skeletonization", # Namespace
[0.5, 0, 0, 0.5], # Color RGBA
0.05 # Scale
)
def skeletonization(self, ogm, origin, resolution, ogml):
width = ogm.shape[0]
height = ogm.shape[1]
useful_ogm = ogm[ogml['min_x']:ogml['max_x'],
ogml['min_y']:ogml['max_y']]
useful_width = useful_ogm.shape[0]
useful_height = useful_ogm.shape[1]
local = numpy.zeros(ogm.shape)
useful_local = numpy.zeros(useful_ogm.shape)
# for i in range(0, useful_width):
# for j in range(0, useful_height):
# if useful_ogm[i][j] < 49:
# useful_local[i][j] = 1
useful_local[numpy.where(useful_ogm < 49)] = 1 #todo allagi
skeleton = skeletonize(useful_local)
skeleton = self.pruning(skeleton, 10)
# padding
skeleton_final = numpy.zeros(ogm.shape)
skeleton_final[ogml['min_x']:ogml['max_x'],
ogml['min_y']:ogml['max_y']] = skeleton
viz = []
for i in range(0, width):
for j in range(0, height):
if skeleton_final[i][j] == 1:
viz.append([
i * resolution + origin['x'],
j * resolution + origin['y']
])
RvizHandler.printMarker(\
viz,\
1, # Type: Arrow
0, # Action: Add
"map", # Frame
"art_skeletonization", # Namespace
[0.5, 0, 0, 0.5], # Color RGBA
0.05 # Scale
)
#scipy.misc.imsave('/home/manos/Desktop/test.png', skeleton_final)
return skeleton_final
def skeletonizationCffi(self, ogm, origin, resolution, ogml):
width = ogm.shape[0]
height = ogm.shape[1]
# import time
local = numpy.zeros(ogm.shape)
# 10x faster
local[numpy.where(ogm < 49)] = 1
# local2 = numpy.zeros(ogm.shape)
# start = time.time()
# for i in range(0, width):
# for j in range(0, height):
# if ogm[i][j] < 49:
# local[i][j] = 1
# end = time.time()
# print ("loop took:", end-start)
# start = time.time()
# local[numpy.where(ogm<49)] = 1
# end = time.time()
# print ("numpy took:", end-start)
skeleton = Cffi.thinning(local, ogml)
skeleton = Cffi.prune(skeleton, ogml, 10)
viz = []
for i in range(0, width):
for j in range(0, height):
if skeleton[i][j] == 1:
viz.append([
i * resolution + origin['x'],
j * resolution + origin['y']
])
RvizHandler.printMarker(\
viz,\
1, # Type: Arrow
0, # Action: Add
"map", # Frame
"art_skeletonization_cffi", # Namespace
[0.5, 0, 0, 0.5], # Color RGBA
0.05 # Scale
)
return skeleton
def checkTarget(self, event):
# Check if we have a target or if the robot just wanders
if self.inner_target_exists == False or self.move_with_target == False or\
self.next_subtarget == len(self.subtargets):
return
# Check if timer has expired
if self.timerThread.expired == True:
Print.art_print('\n~~~~ Time reset ~~~~',Print.RED)
self.inner_target_exists = False
self.target_exists = False
return
# Get the robot pose in pixels
[rx, ry] = [\
self.robot_perception.robot_pose['x_px'] - \
self.robot_perception.origin['x'] / self.robot_perception.resolution,\
self.robot_perception.robot_pose['y_px'] - \
self.robot_perception.origin['y'] / self.robot_perception.resolution\
]
theta_robot = self.robot_perception.robot_pose['th']
# Clear achieved targets
self.subtargets = self.subtargets[self.next_subtarget:]
self.next_subtarget = 0
# Find the distance between the robot pose and all remaining subtargets
dx = [rx - st[0] for st in self.subtargets]
dy = [ry - st[1] for st in self.subtargets]
dist = [math.hypot(v[0], v[1]) for v in zip(dx, dy)]
# Try to optimize path only when there is more than one target available
if len(self.subtargets) > 1:
ogm = self.robot_perception.getMap()
theta_goals = np.arctan2(-np.array(dy),-np.array(dx))
dtheta = theta_goals - theta_robot
dtheta[dtheta > np.pi] -= 2*np.pi
dtheta[dtheta < -np.pi] += 2*np.pi
# Check if there are obstacles between robot and subtarget
obstacles_subtarget = []
for st,i in zip(self.subtargets, range(len(self.subtargets))):
# Find line in pixels between robot and goal
line_pxls = list(bresenham(int(round(st[0])), int(round(st[1])),\
int(round(rx)), int(round(ry))))
# Find if there are any obstacles in line
ogm_line = list(map(lambda pxl: ogm[pxl[0], pxl[1]], line_pxls))
N_occupied = len(list(filter(lambda x: x > 80, ogm_line)))
obstacles_subtarget.append(N_occupied)
# Check if one of next goals is aligned with the current robot theta
# In this case plan immediately for this goal in order to improve motion behaviour
for st, i in zip(self.subtargets, range(len(self.subtargets))):
if np.fabs(dtheta[i]) < np.pi/10 and obstacles_subtarget[i] == 0:
self.next_subtarget = i
# Keep resetting timer, while moving towards target (without any obstacles inbetween)
self.timerThread.reset()
# Check if any target is in distance
min_dist, min_idx = min(zip(dist, range(len(dist))))
if min_dist < 5:
self.next_subtarget = min_idx + 1
# Reset timer if a goal has been reached
self.timerThread.reset()
# Check if the final subtarget has been approached
if self.next_subtarget >= len(self.subtargets):
self.target_exists = False
# Publish the current target
if self.next_subtarget >= len(self.subtargets):
return
subtarget = [\
self.subtargets[self.next_subtarget][0]\
* self.robot_perception.resolution + \
self.robot_perception.origin['x'],
self.subtargets[self.next_subtarget][1]\
* self.robot_perception.resolution + \
self.robot_perception.origin['y']\
]
RvizHandler.printMarker(\
[subtarget],\
1, # Type: Arrow
0, # Action: Add
"map", # Frame
"art_next_subtarget", # Namespace
[0, 0, 0.8, 0.8], # Color RGBA
0.2 # Scale
)
def selectTarget(self):
# Cancel previous goal timer
self.timerThread.stop()
# IMPORTANT: The robot must be stopped if you call this function until
# it is over
# Check if we have a map
while self.robot_perception.have_map == False:
Print.art_print("Navigation: No map yet", Print.RED)
return
print "\nClearing all markers"
RvizHandler.printMarker(\
[[0, 0]],\
1, # Type: Arrow
3, # Action: delete all
"map", # Frame
"null", # Namespace
[0,0,0,0], # Color RGBA
0.1 # Scale
)
print '\n\n----------------------------------------------------------'
print "Navigation: Producing new target"
# We are good to continue the exploration
# Make this true in order not to call it again from the speeds assignment
self.target_exists = True
# Gets copies of the map and coverage
local_ogm = self.robot_perception.getMap()
local_ros_ogm = self.robot_perception.getRosMap()
local_coverage = self.robot_perception.getCoverage()
print "Got the map and Coverage"
self.path_planning.setMap(local_ros_ogm)
# Once the target has been found, find the path to it
# Get the global robot pose
g_robot_pose = self.robot_perception.getGlobalCoordinates(\
[self.robot_perception.robot_pose['x_px'],\
self.robot_perception.robot_pose['y_px']])
# Call the target selection function to select the next best goal
# Choose target function
self.path = []
force_random = False
# Potential Target
path_altered = False
previous_target = None
while len(self.path) == 0:
start = time.time()
target = self.target_selection.selectTarget(\
local_ogm,\
local_coverage,\
self.robot_perception.robot_pose,
self.robot_perception.origin,
self.robot_perception.resolution,
force_random)
# ipdb.set_trace()
# Check if there was any path alteration
if target[0] == True:
previous_target = target[2]
target = target[1]
path_altered = True
else:
target = target[1]
self.path = self.path_planning.createPath(\
g_robot_pose,\
target,
self.robot_perception.resolution)
print "Navigation: Path for target found with " + str(len(self.path)) +\
" points"
if len(self.path) == 0:
Print.art_print(\
"Path planning failed. Fallback to random target selection", \
Print.RED)
force_random = True
# Reverse the path to start from the robot
self.path = self.path[::-1]
# Break the path to subgoals every 2 pixels (1m = 20px)
step = 1
n_subgoals = (int) (len(self.path)/step)
self.subtargets = []
for i in range(0, n_subgoals):
self.subtargets.append(self.path[i * step])
self.subtargets.append(self.path[-1])
self.next_subtarget = 0
print "The path produced " + str(len(self.subtargets)) + " subgoals"
######################### NOTE: QUESTION ##############################
# The path is produced by an A* algorithm. This means that it is
# optimal in length but 1) not smooth and 2) length optimality
# may not be desired for coverage-based exploration
########################################################################
# Start timer thread
self.timerThread.start()
# Publish the path for visualization purposes
ros_path = Path()
ros_path.header.frame_id = "map"
for p in self.path:
ps = PoseStamped()
ps.header.frame_id = "map"
ps.pose.position.x = 0
ps.pose.position.y = 0
######################### NOTE: QUESTION ##############################
# Fill the ps.pose.position values to show the path in RViz
# You must understand what self.robot_perception.resolution
# and self.robot_perception.origin are.
# ipdb.set_trace()
ps.pose.position.x = p[0] * self.robot_perception.resolution \
+ self.robot_perception.origin['x']
ps.pose.position.y = p[1] * self.robot_perception.resolution \
+ self.robot_perception.origin['y']
########################################################################
ros_path.poses.append(ps)
self.path_publisher.publish(ros_path)
# Publish the subtargets for visualization purposes
subtargets_mark = []
for s in self.subtargets:
subt = [
s[0] * self.robot_perception.resolution + \
self.robot_perception.origin['x'],
s[1] * self.robot_perception.resolution + \
self.robot_perception.origin['y']
]
subtargets_mark.append(subt)
RvizHandler.printMarker(\
subtargets_mark,\
2, # Type: Sphere
0, # Action: Add
"map", # Frame
"art_subtargets", # Namespace
[0, 0.8, 0.0, 0.8], # Color RGBA
0.2 # Scale
)
# Print Old and Current Final Target
if path_altered == True:
subt = [
previous_target[0] * self.robot_perception.resolution + \
self.robot_perception.origin['x'],
previous_target[1] * self.robot_perception.resolution + \
self.robot_perception.origin['y']
]
# ipdb.set_trace()
RvizHandler.printMarker(\
[subt],\
3, # Type: Sphere
0, # Action: Add
"map", # Frame
"art_subtargets", # Namespace
[1.0, 0.0, 0.0, 0.8], # Color RGBA
0.2 # Scale
)
# ipdb.set_trace()
self.inner_target_exists = True
def selectTarget(self, init_ogm, coverage, robot_pose, origin, \
resolution, force_random = False):
target = [-1, -1]
######################### NOTE: QUESTION ##############################
# Implement a smart way to select the next target. You have the
# following tools: ogm_limits, Brushfire field, OGM skeleton,
# topological nodes.
# Find only the useful boundaries of OGM. Only there calculations
# have meaning
ogm_limits = OgmOperations.findUsefulBoundaries(
init_ogm, origin, resolution)
# Blur the OGM to erase discontinuities due to laser rays
ogm = OgmOperations.blurUnoccupiedOgm(init_ogm, ogm_limits)
# Calculate Brushfire field
tinit = time.time()
brush = self.brush.obstaclesBrushfireCffi(ogm, ogm_limits)
Print.art_print("Brush time: " + str(time.time() - tinit),
Print.ORANGE)
# Calculate skeletonization
tinit = time.time()
skeleton = self.topo.skeletonizationCffi(ogm, \
origin, resolution, ogm_limits)
Print.art_print("Skeletonization time: " + str(time.time() - tinit),
Print.ORANGE)
# Find topological graph
tinit = time.time()
nodes = self.topo.topologicalNodes(ogm, skeleton, coverage, origin, \
resolution, brush, ogm_limits)
Print.art_print("Topo nodes time: " + str(time.time() - tinit),
Print.ORANGE)
# Visualization of topological nodes
vis_nodes = []
for n in nodes:
vis_nodes.append([
n[0] * resolution + origin['x'],
n[1] * resolution + origin['y']
])
RvizHandler.printMarker(\
vis_nodes,\
1, # Type: Arrow
0, # Action: Add
"map", # Frame
"art_topological_nodes", # Namespace
[0.3, 0.4, 0.7, 0.5], # Color RGBA
0.1 # Scale
)
# Check at autonomous_expl.yaml if target_selector = 'random' or 'smart'
if self.method == 'random' or force_random == True:
target = self.selectRandomTarget(ogm, coverage, brush, ogm_limits)
elif self.method == 'smart' and force_random == False:
tinit = time.time()
# Get the robot pose in pixels
[rx, ry] = [\
robot_pose['x_px'] - \
origin['x'] / resolution,\
robot_pose['y_px'] - \
origin['y'] / resolution\
]
g_robot_pose = [rx, ry]
# If nodes > 25 the calculation time-cost is very big
# In order to avoid time-reset we perform sampling on
# the nodes list and take a half-sized sample
for i in range(0, len(nodes)):
nodes[i].append(i)
if (len(nodes) > 25):
nodes = random.sample(nodes, int(len(nodes) / 2))
# Initialize weigths
w_dist = [0] * len(nodes)
w_rot = [robot_pose['th']] * len(nodes)
w_cov = [0] * len(nodes)
w_topo = [0] * len(nodes)
# Calculate weights for every node in the topological graph
for i in range(0, len(nodes)):
# If path planning fails then give extreme values to weights
path = self.path_planning.createPath(g_robot_pose, nodes[i],
resolution)
if not path:
w_dist[i] = sys.maxsize
w_rot[i] = sys.maxsize
w_cov[i] = sys.maxsize
w_topo[i] = sys.maxsize
else:
for j in range(1, len(path)):
# Distance cost
w_dist[i] += math.hypot(path[j][0] - path[j - 1][0],
path[j][1] - path[j - 1][1])
# Rotational cost
w_rot[i] += abs(
math.atan2(path[j][0] - path[j - 1][0],
path[j][1] - path[j - 1][1]))
# Coverage cost
w_cov[i] += coverage[int(path[j][0])][int(
path[j][1])] / (len(path))
# Add the coverage cost of 0-th node of the path
w_cov[i] += coverage[int(path[0][0])][int(
path[0][1])] / (len(path))
# Topological cost
# This metric depicts wether the target node
# is placed in open space or near an obstacle
# We want the metric to be reliable so we also check node's neighbour cells
w_topo[i] += brush[nodes[i][0]][nodes[i][1]]
w_topo[i] += brush[nodes[i][0] - 1][nodes[i][1]]
w_topo[i] += brush[nodes[i][0] + 1][nodes[i][1]]
w_topo[i] += brush[nodes[i][0]][nodes[i][-1]]
w_topo[i] += brush[nodes[i][0]][nodes[i][+1]]
w_topo[i] += brush[nodes[i][0] - 1][nodes[i][1] - 1]
w_topo[i] += brush[nodes[i][0] + 1][nodes[i][1] + 1]
w_topo[i] = w_topo[i] / 7
# Normalize weights between 0-1
for i in range(0, len(nodes)):
w_dist[i] = 1 - (w_dist[i] - min(w_dist)) / (max(w_dist) -
min(w_dist))
w_rot[i] = 1 - (w_rot[i] - min(w_rot)) / (max(w_rot) -
min(w_rot))
w_cov[i] = 1 - (w_cov[i] - min(w_cov)) / (max(w_cov) -
min(w_cov))
w_topo[i] = 1 - (w_topo[i] - min(w_topo)) / (max(w_topo) -
min(w_topo))
# Set cost values
# We set each cost's priority based on experimental results
# from "Cost-Based Target Selection Techniques Towards Full Space
# Exploration and Coverage for USAR Applications
# in a Priori Unknown Environments" publication
C1 = w_topo
C2 = w_dist
C3 = [1] * len(nodes)
for i in range(0, len(nodes)):
C3[i] -= w_cov[i]
C4 = w_rot
# Priority Weight
C_PW = [0] * len(nodes)
# Smoothing Factor
C_SF = [0] * len(nodes)
# Target's Final Priority
C_FP = [0] * len(nodes)
for i in range(0, len(nodes)):
C_PW[i] = 2**3 * (1 - C1[i]) / .5 + 2**2 * (
1 - C2[i]) / .5 + 2**1 * (1 - C3[i]) / .5 + 2**0 * (
1 - C4[i]) / .5
C_SF[i] = (2**3 * (1 - C1[i]) + 2**2 * (1 - C2[i]) + 2**1 *
(1 - C3[i]) + 2**0 * (1 - C4[i])) / (2**4 - 1)
C_FP[i] = C_PW[i] * C_SF[i]
# Select the node with the smallest C_FP value
val, idx = min((val, idx) for (idx, val) in enumerate(C_FP))
Print.art_print(
"Select smart target time: " + str(time.time() - tinit),
Print.BLUE)
Print.art_print("Selected Target - Node: " + str(nodes[idx][2]),
Print.BLUE)
target = nodes[idx]
else:
Print.art_print(
"[ERROR] target_selector at autonomous_expl.yaml must be either 'random' or 'smart'",
Print.RED)
########################################################################
return target
def selectTarget(self, init_ogm, coverage, robot_pose, origin, \
resolution, force_random = False ):
target = [-1, -1]
######################### NOTE: QUESTION ##############################
# Implement a smart way to select the next target. You have the
# following tools: ogm_limits, Brushfire field, OGM skeleton,
# topological nodes.
# Find only the useful boundaries of OGM. Only there calculations
# have meaning
ogm_limits = OgmOperations.findUsefulBoundaries(
init_ogm, origin, resolution)
# Blur the OGM to erase discontinuities due to laser rays
ogm = OgmOperations.blurUnoccupiedOgm(init_ogm, ogm_limits)
# Calculate Brushfire field
tinit = time.time()
brush = self.brush.obstaclesBrushfireCffi(ogm, ogm_limits)
Print.art_print("Brush time: " + str(time.time() - tinit),
Print.ORANGE)
# Calculate skeletonization
tinit = time.time()
skeleton = self.topo.skeletonizationCffi(ogm, \
origin, resolution, ogm_limits)
Print.art_print("Skeletonization time: " + str(time.time() - tinit),
Print.ORANGE)
# Find topological graph
tinit = time.time()
nodes = self.topo.topologicalNodes(ogm, skeleton, coverage, origin, \
resolution, brush, ogm_limits)
for i in range(0, len(nodes)):
print " node " + str(nodes[i])
Print.art_print("Topo nodes time: " + str(time.time() - tinit),
Print.ORANGE)
# Visualization of topological nodes
vis_nodes = []
for n in nodes:
vis_nodes.append([
n[0] * resolution + origin['x'],
n[1] * resolution + origin['y']
])
RvizHandler.printMarker(\
vis_nodes,\
1, # Type: Arrow
0, # Action: Add
"map", # Frame
"art_topological_nodes", # Namespace
[0.3, 0.4, 0.7, 0.5], # Color RGBA
0.1 # Scale
)
if force_random == True:
target = self.selectRandomTarget(ogm, coverage, brush, ogm_limits)
force_random = False
print " force random: CANNOT CREATE PATH TO SELECTED POINT"
return target
else:
# get robot's position
#g_robot_pose = [robot_pose['x_px'] - int(origin['x'] / resolution),\
# robot_pose['y_px'] - int(origin['y'] / resolution)]
[rx , ry] = [robot_pose['x_px'] - int(origin['x'] / resolution),\
robot_pose['y_px'] - int(origin['y'] / resolution)]
# find all the x and y distances between robot and goals
dis_x = [rx - target[0] for target in nodes]
dis_y = [ry - target[1] for target in nodes]
# calculate the euclidean distance
dist = [math.hypot(dis[0], dis[1]) for dis in zip(dis_x, dis_y)]
# calculate the manhattan distance between the robot and all nodes
#dist = [scipy.spatial.distance.cityblock(nodes[i], g_robot_pose) for i in range(0,len(nodes)) ]
# target is the closest node
min_dist, min_idx = min(zip(dist, range(len(dist))))
goal = nodes[min_idx]
target = goal
print "TARGET " + str(target) + " TARGET IDX " + str(min_idx)
########################################################################
return target
def selectTarget(self, init_ogm, ros_ogm, coverage, robot_pose, origin, \
resolution, force_random = False):
target = [-1, -1]
######################### NOTE: QUESTION ##############################
# Implement a smart way to select the next target. You have the
# following tools: ogm_limits, Brushfire field, OGM skeleton,
# topological nodes.
# Find only the useful boundaries of OGM. Only there calculations
# have meaning
ogm_limits = OgmOperations.findUsefulBoundaries(
init_ogm, origin, resolution)
# Blur the OGM to erase discontinuities due to laser rays
ogm = OgmOperations.blurUnoccupiedOgm(init_ogm, ogm_limits)
# Calculate Brushfire field
tinit = time.time()
brush = self.brush.obstaclesBrushfireCffi(ogm, ogm_limits)
Print.art_print("Brush time: " + str(time.time() - tinit),
Print.ORANGE)
# Calculate skeletonization
tinit = time.time()
skeleton = self.topo.skeletonizationCffi(ogm, \
origin, resolution, ogm_limits)
Print.art_print("Skeletonization time: " + str(time.time() - tinit),
Print.ORANGE)
# Find topological graph
tinit = time.time()
nodes = self.topo.topologicalNodes(ogm, skeleton, coverage, origin, \
resolution, brush, ogm_limits)
Print.art_print("Topo nodes time: " + str(time.time() - tinit),
Print.ORANGE)
# Visualization of topological nodes
vis_nodes = []
for n in nodes:
vis_nodes.append([
n[0] * resolution + origin['x'],
n[1] * resolution + origin['y']
])
RvizHandler.printMarker(\
vis_nodes,\
1, # Type: Arrow
0, # Action: Add
"map", # Frame
"art_topological_nodes", # Namespace
[0.3, 0.4, 0.7, 0.5], # Color RGBA
0.1 # Scale
)
# Random point
if self.method == 'random' or force_random == True:
target = self.selectRandomTarget(ogm, coverage, brush, ogm_limits)
# Custom point
elif self.method == 'cost_based':
self.path_planning.setMap(ros_ogm)
g_robot_pose = [robot_pose['x_px'] - int(origin['x'] / resolution),\
robot_pose['y_px'] - int(origin['y'] / resolution)]
# Remove all covered nodes from the candidate list
nodes = np.array(nodes)
uncovered_idxs = np.array(
[coverage[n[0], n[1]] == 0 for n in nodes])
goals = nodes[uncovered_idxs]
# Initialize costs
w_dist = np.full(len(goals), np.inf)
w_turn = np.full(len(goals), np.inf)
w_topo = np.full(len(goals), np.inf)
w_cove = np.full(len(goals), np.inf)
for idx, node in zip(range(len(goals)), goals):
subgoals = np.array(
self.path_planning.createPath(g_robot_pose, node,
resolution))
# If path is empty then skip to the next iteration
if subgoals.size == 0:
continue
# subgoals should contain the robot pose, so we don't need to diff it explicitly
subgoal_vectors = np.diff(subgoals, axis=0)
# Distance cost
dists = [math.hypot(v[0], v[1]) for v in subgoal_vectors]
w_dist[idx] = np.sum(dists)
# Turning cost
w_turn[idx] = 0
theta = robot_pose['th']
for v in subgoal_vectors[:-1]:
st_x, st_y = v
theta2 = math.atan2(st_y - g_robot_pose[1],
st_x - g_robot_pose[0])
w_turn[idx] += abs(theta2 - theta)
theta = theta2
# Coverage cost
w_cove[idx] = sum(coverage[x][y] for x, y in subgoal_vectors)
# Topology cost
w_topo[idx] = brush[node[0], node[1]]
# Normalize weights
w_dist = (w_dist - min(w_dist)) / (max(w_dist) - min(w_dist))
w_turn = (w_turn - min(w_turn)) / (max(w_turn) - min(w_turn))
w_cove = (w_cove - min(w_cove)) / (max(w_cove) - min(w_cove))
w_topo = (w_topo - min(w_topo)) / (max(w_topo) - min(w_topo))
# Cost weights
c_topo = 4
c_dist = 3
c_cove = 2
c_turn = 1
# Calculate combination cost (final)
cost = c_dist * w_dist + c_turn * w_turn + c_cove * w_cove + c_topo * w_topo
min_dist, min_idx = min(zip(cost, range(len(cost))))
target = nodes[min_idx]
# Check if next target exists and if it exists, check if is close to previous
if target is None:
target = self.selectRandomTarget(ogm, coverage, brush,
ogm_limits)
else:
target_dist = math.hypot(target[0] - self.previous_target[0],
target[1] - self.previous_target[1])
if target_dist <= 5:
target = self.selectRandomTarget(ogm, coverage, brush,
ogm_limits)
########################################################################
self.previous_target = target
return target
def selectTarget(self, ogm, coverage, robot_pose, origin, resolution, force_random=False):
######################### NOTE: QUESTION ##############################
# Implement a smart way to select the next target. You have the following tools: ogm_limits, Brushfire field,
# OGM skeleton, topological nodes.
# Find only the useful boundaries of OGM. Only there calculations have meaning.
ogm_limits = OgmOperations.findUsefulBoundaries(ogm, origin, resolution)
# Blur the OGM to erase discontinuities due to laser rays
ogm = OgmOperations.blurUnoccupiedOgm(ogm, ogm_limits)
# Calculate Brushfire field
tinit = time.time()
brush = self.brush.obstaclesBrushfireCffi(ogm, ogm_limits)
Print.art_print("Brush time: " + str(time.time() - tinit), Print.ORANGE)
# Calculate skeletonization
tinit = time.time()
skeleton = topology.skeletonizationCffi(ogm, origin, resolution, ogm_limits)
Print.art_print("Skeletonization time: " + str(time.time() - tinit), Print.ORANGE)
# Find topological graph
tinit = time.time()
nodes = topology.topologicalNodes(ogm, skeleton, coverage, brush)
Print.art_print("Topo nodes time: " + str(time.time() - tinit), Print.ORANGE)
# Visualization of topological nodes
vis_nodes = [[n[0] * resolution + origin['x'], n[1] * resolution + origin['y']] for n in nodes]
RvizHandler.printMarker(
vis_nodes,
1, # Type: Arrow
0, # Action: Add
"map", # Frame
"art_topological_nodes", # Namespace
[0.3, 0.4, 0.7, 0.5], # Color RGBA
0.1 # Scale
)
try:
tinit = time.time()
except:
tinit = None
if self.method == 'random' or force_random:
target = self.selectRandomTarget(ogm, coverage, brush)
elif self.method_is_cost_based():
g_robot_pose = self.robot_perception.getGlobalCoordinates([robot_pose['x_px'], robot_pose['y_px']])
self.path_planning.setMap(self.robot_perception.getRosMap())
class MapContainer:
def __init__(self):
self.skeleton = skeleton
self.coverage = coverage
self.ogm = ogm
self.brush = brush
self.nodes = [node for node in nodes if TargetSelection.is_good(brush, ogm, coverage, node)]
self.robot_px = [robot_pose['x_px'] - origin['x'] / resolution,
robot_pose['y_px'] - origin['y'] / resolution]
self.theta = robot_pose['th']
@staticmethod
def create_path(path_target):
return self.path_planning.createPath(g_robot_pose, path_target, resolution)
target = self.select_by_cost(MapContainer())
else:
assert False, "Invalid target_selector method."
if tinit is not None:
Print.art_print("Target method {} time: {}".format(self.method, time.time() - tinit), Print.ORANGE)
return target
def checkTarget(self, event):
# Check if we have a target or if the robot just wanders
if self.inner_target_exists == False or self.move_with_target == False or\
self.next_subtarget == len(self.subtargets):
return
self.counter_to_next_sub -= 1
if self.counter_to_next_sub == 0:
Print.art_print('\n~~~~ Time reset ~~~~', Print.RED)
self.inner_target_exists = False
self.target_exists = False
return
# Get the robot pose in pixels
[rx, ry] = [\
self.robot_perception.robot_pose['x_px'] - \
self.robot_perception.origin['x'] / self.robot_perception.resolution,\
self.robot_perception.robot_pose['y_px'] - \
self.robot_perception.origin['y'] / self.robot_perception.resolution\
]
# Find the distance between the robot pose and the next subtarget
dist = math.hypot(\
rx - self.subtargets[self.next_subtarget][0], \
ry - self.subtargets[self.next_subtarget][1])
######################### NOTE: QUESTION ##############################
# What if a later subtarget or the end has been reached before the
# next subtarget? Alter the code accordingly.
# If the robot is in the starting point it immediately sets the next subtarget
if self.next_subtarget == 0:
self.next_subtarget += 1
self.counter_to_next_sub = self.count_limit
else:
# Check if there is a later subtarget, closer than the next one
# If one is found, make it the next subtarget and update the time
for i in range(self.next_subtarget + 1, len(self.subtargets) - 1):
# Find the distance between the robot pose and the later subtarget
dist_from_later = math.hypot(\
rx - self.subtargets[i][0], \
ry - self.subtargets[i][1])
if dist_from_later < 15:
self.next_subtarget = i
self.counter_to_next_sub = self.count_limit + 100
dist = dist_from_later
break
# If distance from subtarget is less than 5, go to the next one
if dist < 5:
self.next_subtarget += 1
self.counter_to_next_sub = self.count_limit
# Check if the final subtarget has been approached
if self.next_subtarget == len(self.subtargets):
self.target_exists = False
########################################################################
# Publish the current target
if self.next_subtarget == len(self.subtargets):
return
subtarget = [\
self.subtargets[self.next_subtarget][0]\
* self.robot_perception.resolution + \
self.robot_perception.origin['x'],
self.subtargets[self.next_subtarget][1]\
* self.robot_perception.resolution + \
self.robot_perception.origin['y']\
]
RvizHandler.printMarker(\
[subtarget],\
1, # Type: Arrow
0, # Action: Add
"map", # Frame
"art_next_subtarget", # Namespace
[0, 0, 0.8, 0.8], # Color RGBA
0.2 # Scale
)
def selectTarget(self, init_ogm, coverage, robot_pose, origin, \
resolution, force_random = False):
target = [-1, -1]
######################### NOTE: QUESTION ##############################
# Implement a smart way to select the next target. You have the
# following tools: ogm_limits, Brushfire field, OGM skeleton,
# topological nodes.
# Find only the useful boundaries of OGM. Only there calculations
# have meaning
ogm_limits = OgmOperations.findUsefulBoundaries(init_ogm, origin, resolution)
# Blur the OGM to erase discontinuities due to laser rays
ogm = OgmOperations.blurUnoccupiedOgm(init_ogm, ogm_limits)
# Calculate Brushfire field
tinit = time.time()
brush = self.brush.obstaclesBrushfireCffi(ogm, ogm_limits)
Print.art_print("Brush time: " + str(time.time() - tinit), Print.ORANGE)
# Calculate skeletonization
tinit = time.time()
skeleton = self.topo.skeletonizationCffi(ogm, \
origin, resolution, ogm_limits)
Print.art_print("Skeletonization time: " + str(time.time() - tinit), Print.ORANGE)
# Find topological graph
tinit = time.time()
nodes = self.topo.topologicalNodes(ogm, skeleton, coverage, origin, \
resolution, brush, ogm_limits)
Print.art_print("Topo nodes time: " + str(time.time() - tinit), Print.ORANGE)
# Visualization of topological nodes
vis_nodes = []
for n in nodes:
vis_nodes.append([
n[0] * resolution + origin['x'],
n[1] * resolution + origin['y']
])
RvizHandler.printMarker(\
vis_nodes,\
1, # Type: Arrow
0, # Action: Add
"map", # Frame
"art_topological_nodes", # Namespace
[0.3, 0.4, 0.7, 0.5], # Color RGBA
0.1 # Scale
)
# Random point
if self.method == 'random' or force_random == True:
target = self.selectRandomTarget(ogm, coverage, brush, ogm_limits)
########################################################################
# Challenge 6. Smart point selection demands autonomous_expl.yaml->target_selector: 'smart'
# Smart point selection
if self.method == 'smart' and force_random == False:
nextTarget = self.selectSmartTarget(coverage, brush, robot_pose, resolution, origin, nodes)
# Check if selectSmartTarget found a target
if nextTarget is not None:
# Check if the next target is the same as the previous
dist = math.hypot( nextTarget[0] - self.previous_target[0], nextTarget[1] - self.previous_target[1])
if dist > 5:
target = nextTarget
else:
target = self.selectRandomTarget(ogm, coverage, brush, ogm_limits)
else:
# No target found. Choose a random
target = self.selectRandomTarget(ogm, coverage, brush, ogm_limits)
self.previous_target = target
return target
def checkTarget(self, event):
"""
Check if target/sub-target reached.
:param event:
:return:
"""
if self.previous_next_subtarget is None or self.previous_next_subtarget < self.next_subtarget:
Print.art_print("next_subtarget = %d" % self.next_subtarget,
Print.ORANGE)
self.previous_next_subtarget = self.next_subtarget
# Check if we have a target or if the robot just wanders
if self.inner_target_exists == False \
or self.move_with_target == False \
or self.next_subtarget == len(self.subtargets):
return
self.counter_to_next_sub -= 1
if self.counter_to_next_sub == 0:
Print.art_print('\n~~~~ Time reset ~~~~', Print.RED)
self.inner_target_exists = False
self.target_exists = False
return
# Get the robot pose in: pixel units & global (map's) coordinate system
rp_l_px = [
self.robot_perception.robot_pose['x_px'],
self.robot_perception.robot_pose['y_px']
]
# rp_g_px = self.robot_perception.toGlobalCoordinates(rp_l_px)
[rx, ry] = [
self.robot_perception.robot_pose['x_px'] -
self.robot_perception.origin['x'] /
self.robot_perception.resolution,
self.robot_perception.robot_pose['y_px'] -
self.robot_perception.origin['y'] /
self.robot_perception.resolution
]
rp_g_px = [rx, ry]
# Find the distance between the robot pose and the next subtarget
v_g_px = map(operator.sub, rp_g_px,
self.subtargets[self.next_subtarget])
dist = math.hypot(v_g_px[0], v_g_px[1])
######################### NOTE: QUESTION ##############################
# What if a later subtarget or the end has been reached before the
# next subtarget? Alter the code accordingly.
# Check if distance is less than 7 px (14 cm)
# if dist < 5:
# self.next_subtarget += 1
# self.counter_to_next_sub = self.count_limit
# # Check if the final subtarget has been approached
# if self.next_subtarget == len(self.subtargets):
# self.target_exists = False
# Instead of checking only next sub-target check all remaining sub-targets (starting from end up to next
# sub-target) and decide if robot is closer to another sub-target at later point in path
for st_i in range(
len(self.subtargets) - 1, self.next_subtarget - 1, -1):
# @v_st_g_px: Difference of Global coordinates in Pixels between robot's position
# and i-th" sub-target's position
v_st_g_px = map(operator.sub, rp_g_px, self.subtargets[st_i])
if math.hypot(v_st_g_px[0], v_st_g_px[1]) < 5:
# reached @st_i, set next sub-target in path as robot's next sub-target
self.next_subtarget = st_i + 1
self.counter_to_next_sub = self.count_limit
if self.next_subtarget == len(self.subtargets):
self.target_exists = False
########################################################################
# Publish the current target
if self.next_subtarget == len(self.subtargets):
return
subtarget = [
self.subtargets[self.next_subtarget][0] *
self.robot_perception.resolution +
self.robot_perception.origin['x'],
self.subtargets[self.next_subtarget][1] *
self.robot_perception.resolution +
self.robot_perception.origin['y']
]
RvizHandler.printMarker(
[subtarget],
1, # Type: Arrow
0, # Action: Add
"map", # Frame
"art_next_subtarget", # Namespace
[0, 0, 0.8, 0.8], # Color RGBA
0.2 # Scale
)
def selectTarget(self):
# IMPORTANT: The robot must be stopped if you call this function until
# it is over
# Check if we have a map
while not self.robot_perception.have_map:
Print.art_print("Navigation: No map yet", Print.RED)
return
print "\nClearing all markers"
RvizHandler.printMarker(
[[0, 0]],
1, # Type: Arrow
3, # Action: delete all
"map", # Frame
"null", # Namespace
[0, 0, 0, 0], # Color RGBA
0.1 # Scale
)
print '\n\n----------------------------------------------------------'
print "Navigation: Producing new target"
# We are good to continue the exploration
# Make this true in order not to call it again from the speeds assignment
self.target_exists = True
# Gets copies of the map and coverage
local_ogm = self.robot_perception.getMap()
local_ros_ogm = self.robot_perception.getRosMap()
local_coverage = self.robot_perception.getCoverage()
print "Got the map and Coverage"
self.path_planning.setMap(local_ros_ogm)
# Once the target has been found, find the path to it
# Get the global robot pose
# Get robot pose in global (map's) coordinates
rp_l_px = [
self.robot_perception.robot_pose['x_px'],
self.robot_perception.robot_pose['y_px']
]
rp_g_px = self.robot_perception.toGlobalCoordinates(
rp_l_px, with_resolution=True)
g_robot_pose = rp_g_px
# Call the target selection function to select the next best goal
# Choose target function
self.path = []
force_random = False
while len(self.path) == 0:
start = time.time()
target = self.target_selection.selectTarget(
local_ogm, local_coverage, self.robot_perception.robot_pose,
self.robot_perception.origin, self.robot_perception.resolution,
force_random)
self.path = self.path_planning.createPath(
g_robot_pose, target, self.robot_perception.resolution)
print "Navigation: Path for target found with " + str(len(self.path)) + \
" points"
if len(self.path) == 0:
Print.art_print(
"Path planning failed. Fallback to random target selection",
Print.RED)
force_random = True
# Reverse the path to start from the robot
self.path = self.path[::-1]
# Break the path to sub-goals every 2 pixels (1m = 20px)
step = 1
n_subgoals = int(len(self.path) / step)
self.subtargets = []
for i in range(0, n_subgoals):
self.subtargets.append(self.path[i * step])
self.subtargets.append(self.path[-1])
self.next_subtarget = 0
print "The path produced " + str(len(self.subtargets)) + " subgoals"
######################### NOTE: QUESTION ##############################
# The path is produced by an A* algorithm. This means that it is
# optimal in length but 1) not smooth and 2) length optimality
# may not be desired for coverage-based exploration
########################################################################
self.counter_to_next_sub = self.count_limit
# Publish the path for visualization purposes
ros_path = Path()
ros_path.header.frame_id = "map"
for p in self.path:
ps = PoseStamped()
ps.header.frame_id = "map"
######################### NOTE: QUESTION ##############################
# Fill the ps.pose.position values to show the path in RViz
# You must understand what self.robot_perception.resolution
# and self.robot_perception.origin are.
# Convert p from pixel-units to meter
p_m = self.robot_perception.toMeterUnits(p, False)
# Convert p_m from global (map's) coordinates to relative (image's) coordinates (using resolution = 1)
p_m_r = self.robot_perception.toRelativeCoordinates(p_m, False)
# Fill in $ps object
ps.pose.position.x = p_m_r[0]
ps.pose.position.y = p_m_r[1]
########################################################################
ros_path.poses.append(ps)
self.path_publisher.publish(ros_path)
# Publish the subtargets for visualization purposes
subtargets_mark = []
for s in self.subtargets:
st_m = self.robot_perception.toMeterUnits(s, False)
st_m_r = self.robot_perception.toRelativeCoordinates(st_m, False)
subtargets_mark.append(st_m_r)
RvizHandler.printMarker(
subtargets_mark,
2, # Type: Sphere
0, # Action: Add
"map", # Frame
"art_subtargets", # Namespace
[0, 0.8, 0.0, 0.8], # Color RGBA
0.2 # Scale
)
self.inner_target_exists = True
def selectTarget(self, ogm, coverage, robot_pose, origin, \
resolution, force_random = False):
# Random point
if self.method == 'random' or force_random == True:
init_ogm = ogm
# Find only the useful boundaries of OGM. Only there calculations
# have meaning
ogm_limits = OgmOperations.findUsefulBoundaries(
init_ogm, origin, resolution)
# Blur the OGM to erase discontinuities due to laser rays
ogm = OgmOperations.blurUnoccupiedOgm(init_ogm, ogm_limits)
# Calculate Brushfire field
tinit = time.time()
brush = self.brush.obstaclesBrushfireCffi(ogm, ogm_limits)
Print.art_print("Brush time: " + str(time.time() - tinit),
Print.ORANGE)
# Calculate skeletonization
tinit = time.time()
skeleton = self.topo.skeletonizationCffi(ogm, \
origin, resolution, ogm_limits)
Print.art_print(
"Skeletonization time: " + str(time.time() - tinit),
Print.ORANGE)
# Find topological graph
tinit = time.time()
nodes = self.topo.topologicalNodes(ogm, skeleton, coverage, origin, \
resolution, brush, ogm_limits)
Print.art_print("Topo nodes time: " + str(time.time() - tinit),
Print.ORANGE)
# Visualization of topological nodes
vis_nodes = []
for n in nodes:
vis_nodes.append([
n[0] * resolution + origin['x'],
n[1] * resolution + origin['y']
])
RvizHandler.printMarker(\
vis_nodes,\
1, # Type: Arrow
0, # Action: Add
"map", # Frame
"art_topological_nodes", # Namespace
[0.3, 0.4, 0.7, 0.5], # Color RGBA
0.1 # Scale
)
target = self.selectRandomTarget(ogm, coverage, brush, ogm_limits)
return [False, target]
tinit = time.time()
g_robot_pose = [robot_pose['x_px'] - int(origin['x'] / resolution), \
robot_pose['y_px'] - int(origin['y'] / resolution)]
# Calculate coverage frontier with sobel filters
tinit = time.time()
cov_dx = scipy.ndimage.sobel(coverage, 0)
cov_dy = scipy.ndimage.sobel(coverage, 1)
cov_frontier = np.hypot(cov_dx, cov_dy)
cov_frontier *= 100 / np.max(cov_frontier)
cov_frontier = 100 * (cov_frontier > 80)
Print.art_print("Sobel filters time: " + str(time.time() - tinit),
Print.BLUE)
# Remove the edges that correspond to obstacles instead of frontiers (in a 5x5 radius)
kern = 5
tinit = time.time()
i_rng = np.matlib.repmat(
np.arange(-(kern / 2), kern / 2 + 1).reshape(kern, 1), 1, kern)
j_rng = np.matlib.repmat(np.arange(-(kern / 2), kern / 2 + 1), kern, 1)
for i in range((kern / 2), cov_frontier.shape[0] - (kern / 2)):
for j in range((kern / 2), cov_frontier.shape[1] - (kern / 2)):
if cov_frontier[i, j] == 100:
if np.any(ogm[i + i_rng, j + j_rng] > 99):
cov_frontier[i, j] = 0
Print.art_print("Frontier trimming time: " + str(time.time() - tinit),
Print.BLUE)
# Save coverage frontier as image (for debugging purposes)
# scipy.misc.imsave('test.png', np.rot90(cov_frontier))
# Frontier detection/grouping
tinit = time.time()
labeled_frontiers, num_frontiers = scipy.ndimage.label(
cov_frontier, np.ones((3, 3)))
Print.art_print("Frontier grouping time: " + str(time.time() - tinit),
Print.BLUE)
goals = np.full((num_frontiers, 2), -1)
w_dist = np.full(len(goals), -1)
w_turn = np.full(len(goals), -1)
w_size = np.full(len(goals), -1)
w_obst = np.full(len(goals), -1)
# Calculate the centroid and its cost, for each frontier
for i in range(1, num_frontiers + 1):
points = np.where(labeled_frontiers == i)
# Discard small groupings (we chose 20 as a treshold arbitrarily)
group_length = points[0].size
if group_length < 20:
labeled_frontiers[points] = 0
continue
sum_x = np.sum(points[0])
sum_y = np.sum(points[1])
centroid = np.array([sum_x / group_length,
sum_y / group_length]).reshape(2, 1)
# Find the point on the frontier nearest (2-norm) to the centroid, and use it as goal
nearest_idx = np.linalg.norm(np.array(points) - centroid,
axis=0).argmin()
print ogm[int(points[0][nearest_idx]), int(points[1][nearest_idx])]
goals[i - 1, :] = np.array(
[points[0][nearest_idx], points[1][nearest_idx]])
# Save centroids for later visualisation (for debugging purposes)
labeled_frontiers[int(goals[i - 1, 0]) + i_rng,
int(goals[i - 1, 1]) + j_rng] = i
# Calculate size of obstacles between robot and goal
line_pxls = list(bresenham(int(goals[i-1,0]), int(goals[i-1,1]),\
g_robot_pose[0], g_robot_pose[1]))
ogm_line = list(map(lambda pxl: ogm[pxl[0], pxl[1]], line_pxls))
N_occupied = len(list(filter(lambda x: x > 25, ogm_line)))
N_line = len(line_pxls)
w_obst[i - 1] = float(N_occupied) / N_line
# print('Occupied = '+str(N_occupied))
# print('Full Line = '+str(N_line))
# ipdb.set_trace()
# Manhattan distance
w_dist[i - 1] = scipy.spatial.distance.cityblock(
goals[i - 1, :], g_robot_pose)
# Missalignment
theta = np.arctan2(goals[i - 1, 1] - g_robot_pose[1],
goals[i - 1, 0] - g_robot_pose[0])
w_turn[i - 1] = (theta - robot_pose['th'])
if w_turn[i - 1] > np.pi:
w_turn[i - 1] -= 2 * np.pi
elif w_turn[i - 1] < -np.pi:
w_turn[i - 1] += 2 * np.pi
# We don't care about the missalignment direction so we abs() it
w_turn[i - 1] = np.abs(w_turn[i - 1])
# Frontier size
w_size[i - 1] = group_length
# Save frontier groupings as an image (for debugging purposes)
cmap = plt.cm.jet
norm = plt.Normalize(vmin=labeled_frontiers.min(),
vmax=labeled_frontiers.max())
image = cmap(norm(labeled_frontiers))
plt.imsave('frontiers.png', np.rot90(image))
# Remove invalid goals and weights
valids = w_dist != -1
goals = goals[valids]
w_dist = w_dist[valids]
w_turn = w_turn[valids]
w_size = w_size[valids]
w_obst = w_obst[valids]
# Normalize weights
w_dist = (w_dist - min(w_dist)) / (max(w_dist) - min(w_dist))
w_turn = (w_turn - min(w_turn)) / (max(w_turn) - min(w_turn))
w_size = (w_size - min(w_size)) / (max(w_size) - min(w_size))
# Cancel turn weight for frontiers that have obstacles
w_turn[np.where(w_obst != 0)] = 1
# Goal cost function
c_dist = 3
c_turn = 2
c_size = 1
c_obst = 4
costs = c_dist * w_dist + c_turn * w_turn + c_size * w_size + c_obst * w_obst
min_idx = costs.argmin()
Print.art_print("Target selection time: " + str(time.time() - tinit),
Print.ORANGE)
print costs
print goals
## Safety Distance from obstacles
# Goal Coordinates
grid_size = 20
[found_obstacle, closest_obstacle,
min_dist] = self.detectObstacles(grid_size, ogm, goals[min_idx])
if found_obstacle == False:
return [False, goals[min_idx]]
# Calculate new goal:
dist_from_obstacles = 10
normal_vector = goals[min_idx] - closest_obstacle
normal_vector = normal_vector / np.hypot(normal_vector[0],
normal_vector[1])
new_goal = closest_obstacle + dist_from_obstacles * normal_vector
# Check new goal for nearby obstacles
[found_obstacle, closest_obstacle, min_dist_new] = \
self.detectObstacles(grid_size, ogm, new_goal.round())
# Return
if min_dist < 7:
# return the target with max min_dist
if min_dist_new > min_dist:
return [True, new_goal.round(), goals[min_idx]]
else:
return [False, goals[min_idx]]
else:
return [False, goals[min_idx]]
def selectTarget(self):
# IMPORTANT: The robot must be stopped if you call this function until
# it is over
# Cancel previous goal timer
self.timerThread.stop()
# Check if we have a map
while not self.robot_perception.have_map:
Print.art_print("Navigation: No map yet", Print.RED)
return
print "Clearing all markers !\n\n"
RvizHandler.printMarker([[0, 0]], 1, 3, "map", "null", [0, 0, 0, 0],
0.1)
# Visualize Map Container
map_container_mark = []
for s in self.map_size:
map_container_mark.append([
s[0] * self.robot_perception.resolution +
self.robot_perception.origin['x'],
s[1] * self.robot_perception.resolution +
self.robot_perception.origin['y']
])
RvizHandler.printMarker(map_container_mark, 1, 0, "map",
"art_map_container", [1.0, 0.0, 0.0, 1.0], 0.3)
print '----------------------------------------------------------'
print 'Navigation: Producing new target'
# We are good to continue the exploration
# Make this true in order not to call it again from the speeds assignment
self.target_exists = True
########################################################################
# Get OGM Map and Coverage
start_ = time.time()
# Gets copies of the map and coverage
start = time.time()
local_ogm = self.robot_perception.getMap()
if self.debug:
print str('Robot Perception: Got the map in ') + str(
time.time() - start) + str(' seconds.')
start = time.time()
local_ros_ogm = self.robot_perception.getRosMap()
if self.debug:
print str('Robot Perception: Got the ros map in ') + str(
time.time() - start) + str(' seconds.')
start = time.time()
local_coverage = self.robot_perception.getCoverage()
if self.debug:
print str('Robot Perception: Got the coverage in ') + str(
time.time() - start) + str(' seconds.')
print str('Navigation: Got the map and Coverage in ') + str(
time.time() - start_) + str(' seconds.')
########################################################################
# Part 1 - OGM Enhancement
no_points = False
ogm_limits_before = []
if self.turtlebot_save_progress_images:
####################### Save OGM and Coverage ##########################
scipy.misc.imsave(
'/home/vvv/pictures_slam/ogm.png',
cv2.bitwise_not(
exposure.rescale_intensity(np.array(local_ogm,
dtype=np.uint8),
in_range=(0, 100),
out_range=(0, 255))))
scipy.misc.imsave('/home/vvv/pictures_slam/coverage.png',
local_coverage)
start_ = time.time()
print str(self.robot_perception.percentage_explored)
if self.limit_exploration_flag and self.robot_perception.percentage_explored < 1.0 and not self.first_run_flag:
#############################################################################
# Subpart 1 - Find the Useful boundaries of OGM
#############################################################################
start = time.time()
(points, ogm_limits_before) = self.ogm_limit_calculation(
local_ogm, self.robot_perception.resolution,
self.robot_perception.origin, 20, 20, 12, self.map_size)
if points is None or len(points) == 0:
print str('No Limits Points Calculated ')
no_points = True
else:
print str('Number of Limit Points Calculated is ') + str(len(points)) + str(' in ')\
+ str(time.time() - start) + str(' seconds.')
no_points = False
if self.debug:
print str('\n') + str(points) + str('\n')
#############################################################################
#########################################################################
# Subpart 2 - Send Drone to Limits and Match the to OGM Map
#########################################################################
startt = time.time()
if not no_points:
index = 0
p = points[index, :]
while len(points):
points = np.delete(
points, [index],
axis=0) # Delete Point thats Going to be Explored
# Send Drone to OGM Limits
start = time.time()
while not self.sent_drone_to_limit(p[0], p[1]):
pass
if self.debug:
Print.art_print(
str('Navigation: Get Image From Drone took ') +
str(time.time() - start) + str(' seconds.'),
Print.ORANGE)
# Image Matching
start = time.time()
if self.match_with_limit_pose:
if self.match_with_drone_pose:
[x_p, y_p] = [
int((self.drone_map_pose[0] -
self.robot_perception.origin['x']) /
self.robot_perception.resolution),
int((self.drone_map_pose[1] -
self.robot_perception.origin['y']) /
self.robot_perception.resolution)
]
local_ogm = ImageMatching.ogm_match_with_known_image_pose(
ogm=local_ogm,
new_data=self.drone_image,
coordinates=[x_p, y_p, self.drone_yaw],
map_boundries=[
self.map_size[0][0], self.map_size[3][0],
self.map_size[0][1], self.map_size[3][1]
],
debug=self.debug)
else:
local_ogm = ImageMatching.ogm_match_with_known_image_pose(
ogm=local_ogm,
new_data=self.drone_image,
coordinates=[
int(p[2]),
int(p[3]), self.drone_yaw
],
map_boundries=[
int(self.map_size[0][0]),
int(self.map_size[3][0]),
int(self.map_size[0][1]),
int(self.map_size[3][1])
],
debug=self.debug)
else:
local_ogm = ImageMatching.template_matching(
ogm=local_ogm,
drone_image=self.drone_image,
lim_x=int(p[2]),
lim_y=int(p[3]),
drone_yaw=self.drone_yaw,
window=200,
s_threshold=0.8,
debug=self.debug)
if self.debug:
Print.art_print(
str('Navigation: OGM Matching Function took ') +
str(time.time() - start) + str(' seconds.\n'),
Print.ORANGE)
# Calculate Point for Next Loop!
if len(points) > 0:
index = self.closest_limit_point(p[:2], points[:, :2])
p = points[index, :]
'''
print('\x1b[35;1m' + str('Navigation: Taking Image and Matching took ') +
str(time.time() - startt) + str(' seconds.') + '\x1b[0m')
'''
########################################################################
########################################################################
# Subpart 3 - -Copy Enhanced Data to ROS OGM
########################################################################
if not no_points:
start = time.time()
local_ros_ogm.data = cp_data_to_ros_ogm(
np.array(local_ros_ogm.data), local_ogm,
local_ros_ogm.info.width, local_ros_ogm.info.height)
if self.debug:
print('\x1b[35;1m' +
str('Navigation: Copying OGM Data took ') +
str(time.time() - start) + str(' seconds.') +
'\x1b[0m')
########################################################################
print('\x1b[38;1m' + str('Navigation: OGM Enhancement took ') +
str(time.time() - start_) + str(' seconds.') + '\x1b[0m')
if self.turtlebot_save_progress_images and self.limit_exploration_flag and not no_points:
########################## Save Enhance OGM ############################
scipy.misc.imsave(
'/home/vvv/pictures_slam/ogm_enhanced.png',
cv2.bitwise_not(
exposure.rescale_intensity(np.array(local_ogm,
dtype=np.uint8),
in_range=(0, 100),
out_range=(0, 255))))
########################################################################
# Part 2 - Set Map, Get Robot Position and Choose Target
start = time.time()
self.target_selection.path_planning.setMap(local_ros_ogm)
print str('Navigation: Set Map in ') + str(time.time() -
start) + str(' seconds.')
# Once the target has been found, find the path to it
# Get the global robot pose
start = time.time()
g_robot_pose = self.robot_perception.getGlobalCoordinates([
self.robot_perception.robot_pose['x_px'],
self.robot_perception.robot_pose['y_px']
])
print str('Navigation: Got Robot Pose in ') + str(
time.time() - start) + str(' seconds.')
# Call the target selection function to select the next best goal
self.path = []
force_random = False
while len(self.path) == 0:
# Get Target and Path to It!
start = time.time()
self.path = self.target_selection.selectTarget(
local_ogm, local_coverage, self.robot_perception.robot_pose,
self.robot_perception.origin, self.robot_perception.resolution,
g_robot_pose, ogm_limits_before, force_random)
#self.target_selection.path_planning.createPath(g_robot_pose, target, self.robot_perception.resolution)
if len(self.path) == 0:
Print.art_print(
'Navigation: Path planning failed. Fallback to random target selection',
Print.RED)
print str('\n')
force_random = True
else:
print str('Navigation: Target Selected and Path to Target found with ') + str(len(self.path)) \
+ str(' points in ') + str(time.time() - start) + str(' seconds.')
########################################################################
# Part 3 - Create Subgoals and Print Path to RViz
start = time.time()
# Reverse the path to start from the robot
self.path = self.path[::-1]
# Break the path to subgoals every 2 pixels (1m = 20px)
step = 1
n_subgoals = int(len(self.path) / step)
self.subtargets = []
for i in range(0, n_subgoals):
self.subtargets.append(self.path[i * step])
self.subtargets.append(self.path[-1])
self.next_subtarget = 0
if self.debug:
print str('Navigation: The path produced ') + str(len(self.subtargets)) + str(' subgoals in ') \
+ str(time.time() - start) + str('seconds.')
# Start timer thread
self.timerThread.start()
# Publish the path for visualization purposes
ros_path = Path()
ros_path.header.frame_id = self.map_frame
for p in self.path:
ps = PoseStamped()
ps.header.frame_id = self.map_frame
ps.pose.position.x = 0
ps.pose.position.y = 0
ps.pose.position.x = p[
0] * self.robot_perception.resolution + self.robot_perception.origin[
'x']
ps.pose.position.y = p[
1] * self.robot_perception.resolution + self.robot_perception.origin[
'y']
ros_path.poses.append(ps)
self.path_publisher.publish(ros_path)
# Publish the subtargets for visualization purposes
subtargets_mark = []
for s in self.subtargets:
subtargets_mark.append([
s[0] * self.robot_perception.resolution +
self.robot_perception.origin['x'],
s[1] * self.robot_perception.resolution +
self.robot_perception.origin['y']
])
RvizHandler.printMarker(subtargets_mark, 2, 0, 'map', 'art_subtargets',
[0, 0.8, 0.0, 0.8], 0.2)
# Update NUmber of Targets
self.robot_perception.num_of_explored_targets += 1
self.inner_target_exists = True
# Reset First Run Flag
if self.first_run_flag:
self.first_run_flag = False
def checkTarget(self, event):
# Check if we have a target or if the robot just wanders
if self.inner_target_exists == False or self.move_with_target == False or\
self.next_subtarget == len(self.subtargets):
return
self.counter_to_next_sub -= 1
if self.counter_to_next_sub == 0:
Print.art_print('\n~~~~ Time reset ~~~~',Print.RED)
self.inner_target_exists = False
self.target_exists = False
if self.Reset == False:
self.TimeOut = True
else:
self.Reset = False
return
# Get the robot pose in pixels
[rx, ry] = [\
self.robot_perception.robot_pose['x_px'] - \
self.robot_perception.origin['x'] / self.robot_perception.resolution,\
self.robot_perception.robot_pose['y_px'] - \
self.robot_perception.origin['y'] / self.robot_perception.resolution\
]
# remove visited sub targets
self.subtargets = self.subtargets[self.next_subtarget:]
# set the next sub target as the first one that should be visited
self.next_subtarget = 0
dis_x = [rx - target[0] for target in self.subtargets]
dis_y = [ry - target[1] for target in self.subtargets]
#for i in range(0,len(dis_x)):
# print " x dis " + str(dis_x[i])
# print " y dis " + str(dis_y[i])
# target distances
dist_t = [math.hypot(dis[0],dis[1]) for dis in zip(dis_x,dis_y) ]
# find the closest target
min_target = 0
min_dis = dist_t[0]
for i in range(0,len(dist_t)):
if min_dis > dist_t[i]:
min_dis = dist_t[i]
min_target = i
######################### NOTE: QUESTION ##############################
# What if a later subtarget or the end has been reached before the
# next subtarget? Alter the code accordingly.
# print " min target " + str(min_target) + " min dis " + str(min_dis)
# Check if distance is less than 7 px (14 cm)
if min_dis < 5:
self.next_subtarget = min_target + 1
self.counter_to_next_sub = self.count_limit
# print "REACHED"
# print " next target " + str(self.next_subtarget) + " len " +str(len(self.subtargets))
# Check if the final subtarget has been approached
if self.next_subtarget == len(self.subtargets):
self.target_exists = False
########################################################################
# Publish the current target
if self.next_subtarget == len(self.subtargets):
return
subtarget = [\
self.subtargets[self.next_subtarget][0]\
* self.robot_perception.resolution + \
self.robot_perception.origin['x'],
self.subtargets[self.next_subtarget][1]\
* self.robot_perception.resolution + \
self.robot_perception.origin['y']\
]
RvizHandler.printMarker(\
[subtarget],\
1, # Type: Arrow
0, # Action: Add
"map", # Frame
"art_next_subtarget", # Namespace
[0, 0, 0.8, 0.8], # Color RGBA
0.2 # Scale
)
def selectTarget(self):
# IMPORTANT: The robot must be stopped if you call this function until
# it is over
# Check if we have a map
while self.robot_perception.have_map == False:
Print.art_print("Navigation: No map yet", Print.RED)
return
print "\nClearing all markers"
RvizHandler.printMarker(\
[[0, 0]],\
1, # Type: Arrow
3, # Action: delete all
"map", # Frame
"null", # Namespace
[0,0,0,0], # Color RGBA
0.1 # Scale
)
print '\n\n----------------------------------------------------------'
print "Navigation: Producing new target"
# We are good to continue the exploration
# Make this true in order not to call it again from the speeds assignment
self.target_exists = True
# Gets copies of the map and coverage
local_ogm = self.robot_perception.getMap()
local_ros_ogm = self.robot_perception.getRosMap()
local_coverage = self.robot_perception.getCoverage()
print "Got the map and Coverage"
self.path_planning.setMap(local_ros_ogm)
# Once the target has been found, find the path to it
# Get the global robot pose
g_robot_pose = self.robot_perception.getGlobalCoordinates(\
[self.robot_perception.robot_pose['x_px'],\
self.robot_perception.robot_pose['y_px']])
# Call the target selection function to select the next best goal
# Choose target function
print "TimeOut Flag " +str(self.TimeOut)
self.path = []
force_random = False
while len(self.path) == 0:
start = time.time()
target = self.target_selection.selectTarget(\
local_ogm,\
local_coverage,\
self.robot_perception.robot_pose,
self.robot_perception.origin,
self.robot_perception.resolution,
force_random)
self.path = self.path_planning.createPath(\
g_robot_pose,\
target,
self.robot_perception.resolution)
print "Navigation: Path for target found with " + str(len(self.path)) +\
" points"
if len(self.path) == 0:
Print.art_print(\
"Path planning failed. Fallback to random target selection", \
Print.RED)
force_random = True
# Reverse the path to start from the robot
self.path = self.path[::-1]
tolerance = 0.000001
weight_data = 0.5
weight_smooth = 0.1
new = deepcopy(self.path)
dims = 2
change = tolerance
# Path smoothing using gradient descent
if len(self.path) > 3:
print "PATH SMOOTHING"
while change >= tolerance:
change = 0.0
for i in range(1, len(new) - 1):
for j in range(dims):
x_i = self.path[i][j]
y_i, y_prev, y_next = new[i][j], new[i - 1][j], new[i + 1][j]
y_i_saved = y_i
y_i += weight_data * (x_i - y_i) + weight_smooth * (y_next + y_prev - (2 * y_i))
new[i][j] = y_i
change += abs(y_i - y_i_saved)
self.path = new
# Break the path to subgoals every 2 pixels (1m = 20px)
step = 1
n_subgoals = (int) (len(self.path)/step)
self.subtargets = []
for i in range(0, n_subgoals):
self.subtargets.append(self.path[i * step])
self.subtargets.append(self.path[-1])
self.next_subtarget = 0
print "The path produced " + str(len(self.subtargets)) + " subgoals"
######################### NOTE: QUESTION ##############################
# The path is produced by an A* algorithm. This means that it is
# optimal in length but 1) not smooth and 2) length optimality
# may not be desired for coverage-based exploration
########################################################################
self.counter_to_next_sub = self.count_limit
# Publish the path for visualization purposes
ros_path = Path()
ros_path.header.frame_id = "map"
for p in self.path:
ps = PoseStamped()
ps.header.frame_id = "map"
######################### NOTE: QUESTION ##############################
# Fill the ps.pose.position values to show the path in RViz
# You must understand what self.robot_perception.resolution
# and self.robot_perception.origin are.
#p[0] * resolution => gives the path's point x-coordinate expressed in the map's coordinate system (meters)
# As the path's(p) first point-set is the pixels that are occupied by the robot, multyplying it by resolution (meter/pixel)
# gives the values of that point expressed in meters.
# Adding the robot's initial position we have a path starting from the robot's position expressed in the robot's frame.
ps.pose.position.x = p[0]*0.05 + self.robot_perception.origin['x']
ps.pose.position.y = p[1]*0.05 + self.robot_perception.origin['y']
# print " x val " + str(ps.pose.position.x)
# print " y val " + str(ps.pose.position.y )
# print " origin x " + str(self.robot_perception.origin['x'])
# print " origin y " + str(self.robot_perception.origin['y'])
# print " p0 " +str(p[0])
# print " p1 " +str(p[1])
########################################################################
ros_path.poses.append(ps)
self.path_publisher.publish(ros_path)
# Publish the subtargets for visualization purposes
subtargets_mark = []
for s in self.subtargets:
subt = [
s[0] * self.robot_perception.resolution + \
self.robot_perception.origin['x'],
s[1] * self.robot_perception.resolution + \
self.robot_perception.origin['y']
]
subtargets_mark.append(subt)
RvizHandler.printMarker(\
subtargets_mark,\
2, # Type: Sphere
0, # Action: Add
"map", # Frame
"art_subtargets", # Namespace
[0, 0.8, 0.0, 0.8], # Color RGBA
0.2 # Scale
)
self.inner_target_exists = True
def selectTarget(self):
# IMPORTANT: The robot must be stopped if you call this function until
# it is over
# Check if we have a map
while self.robot_perception.have_map == False:
Print.art_print("Navigation: No map yet", Print.RED)
return
print "\nClearing all markers"
RvizHandler.printMarker(\
[[0, 0]],\
1, # Type: Arrow
3, # Action: delete all
"map", # Frame
"null", # Namespace
[0,0,0,0], # Color RGBA
0.1 # Scale
)
print '\n\n----------------------------------------------------------'
print "Navigation: Producing new target"
# We are good to continue the exploration
# Make this true in order not to call it again from the speeds assignment
self.target_exists = True
# Gets copies of the map and coverage
local_ogm = self.robot_perception.getMap()
local_ros_ogm = self.robot_perception.getRosMap()
local_coverage = self.robot_perception.getCoverage()
print "Got the map and Coverage"
self.path_planning.setMap(local_ros_ogm)
# Once the target has been found, find the path to it
# Get the global robot pose
g_robot_pose = self.robot_perception.getGlobalCoordinates(\
[self.robot_perception.robot_pose['x_px'],\
self.robot_perception.robot_pose['y_px']])
# Call the target selection function to select the next best goal
# Choose target function
self.path = []
force_random = False
while len(self.path) == 0:
start = time.time()
target = self.target_selection.selectTarget(\
local_ogm,\
local_coverage,\
self.robot_perception.robot_pose,
self.robot_perception.origin,
self.robot_perception.resolution,
force_random)
self.path = self.path_planning.createPath(\
g_robot_pose,\
target,
self.robot_perception.resolution)
print "Navigation: Path for target found with " + str(len(self.path)) +\
" points"
if len(self.path) == 0:
Print.art_print(\
"Path planning failed. Fallback to random target selection", \
Print.RED)
force_random = True
# Reverse the path to start from the robot
self.path = self.path[::-1]
######################### NOTE: QUESTION ##############################
# The path is produced by an A* algorithm. This means that it is
# optimal in length but 1) not smooth and 2) length optimality
# may not be desired for coverage-based exploration
weight_data = 0.5 # How much weight to update the data (a)
weight_smooth = 0.1 # How much weight to smooth the coordinates (b)
min_change = 0.0001 # Minimum change per iteration to keep iterating
new_path = np.copy(np.array(self.path))
path_length = len(self.path[0])
change = min_change
while change >= min_change:
change = 0.0
for i in range(1, len(new_path) - 1):
for j in range(path_length):
# Initialize x, y
x_i = self.path[i][j]
y_i = new_path[i][j]
y_prev = new_path[i - 1][j]
y_next = new_path[i + 1][j]
y_i_saved = y_i
# Minimize the distance between coordinates of the original
# path (y) and the smoothed path (x). Also minimize the
# difference between the coordinates of the smoothed path
# at time step i, and neighboring time steps. In order to do
# all the minimizations, we use Gradient Ascent.
y_i += weight_data * (x_i - y_i) + weight_smooth * (
y_next + y_prev - (2 * y_i))
new_path[i][j] = y_i
change += abs(y_i - y_i_saved)
self.path = new_path
########################################################################
# Break the path to subgoals every 2 pixels (1m = 20px)
step = 1
n_subgoals = (int)(len(self.path) / step)
self.subtargets = []
for i in range(0, n_subgoals):
self.subtargets.append(self.path[i * step])
self.subtargets.append(self.path[-1])
self.next_subtarget = 0
print "The path produced " + str(len(self.subtargets)) + " subgoals"
self.counter_to_next_sub = self.count_limit
# Publish the path for visualization purposes
ros_path = Path()
ros_path.header.frame_id = "map"
for p in self.path:
ps = PoseStamped()
ps.header.frame_id = "map"
ps.pose.position.x = 0
ps.pose.position.y = 0
######################### NOTE: QUESTION ##############################
# Fill the ps.pose.position values to show the path in RViz
# You must understand what self.robot_perception.resolution
# and self.robot_perception.origin are.
# Multiply subgoal's coordinates with resolution and
# add robot.origin in order to translate between the (0,0) of the robot
# pose and (0,0) of the map
ps.pose.position.x = p[0] * self.robot_perception.resolution + \
self.robot_perception.origin['x']
ps.pose.position.y = p[1] * self.robot_perception.resolution + \
self.robot_perception.origin['y']
########################################################################
ros_path.poses.append(ps)
self.path_publisher.publish(ros_path)
# Publish the subtargets for visualization purposes
subtargets_mark = []
for s in self.subtargets:
subt = [
s[0] * self.robot_perception.resolution + \
self.robot_perception.origin['x'],
s[1] * self.robot_perception.resolution + \
self.robot_perception.origin['y']
]
subtargets_mark.append(subt)
RvizHandler.printMarker(\
subtargets_mark,\
2, # Type: Sphere
0, # Action: Add
"map", # Frame
"art_subtargets", # Namespace
[0, 0.8, 0.0, 0.8], # Color RGBA
0.2 # Scale
)
self.inner_target_exists = True
def selectTarget(self):
# IMPORTANT: The robot must be stopped if you call this function until
# it is over
# Check if we have a map
while self.robot_perception.have_map == False:
Print.art_print("Navigation: No map yet", Print.RED)
return
print "\nClearing all markers"
RvizHandler.printMarker(\
[[0, 0]],\
1, # Type: Arrow
3, # Action: delete all
"map", # Frame
"null", # Namespace
[0,0,0,0], # Color RGBA
0.1 # Scale
)
print '\n\n----------------------------------------------------------'
print "Navigation: Producing new target"
# We are good to continue the exploration
# Make this true in order not to call it again from the speeds assignment
self.target_exists = True
# Gets copies of the map and coverage
local_ogm = self.robot_perception.getMap()
local_ros_ogm = self.robot_perception.getRosMap()
local_coverage = self.robot_perception.getCoverage()
print "Got the map and Coverage"
self.path_planning.setMap(local_ros_ogm)
# Once the target has been found, find the path to it
# Get the global robot pose
g_robot_pose = self.robot_perception.getGlobalCoordinates(\
[self.robot_perception.robot_pose['x_px'],\
self.robot_perception.robot_pose['y_px']])
# Call the target selection function to select the next best goal
# Choose target function
self.path = []
force_random = False
while len(self.path) == 0:
start = time.time()
target = self.target_selection.selectTarget(\
local_ogm,\
local_coverage,\
self.robot_perception.robot_pose,
self.robot_perception.origin,
self.robot_perception.resolution,
force_random)
self.path = self.path_planning.createPath(\
g_robot_pose,\
target,
self.robot_perception.resolution)
print "Navigation: Path for target found with " + str(len(self.path)) +\
" points"
if len(self.path) == 0:
Print.art_print(\
"Path planning failed. Fallback to random target selection", \
Print.RED)
force_random = True
# Reverse the path to start from the robot
self.path = self.path[::-1]
#########################################
# Extra challenge #1
# A. Smooth path planner
if len(self.path) > 3:
x = np.array(self.path)
y = np.copy(x)
a = 0.5
b = 0.1
# FORMULA : y_i = y_i + a * (x_i - y_i) + b * (y_i+1 - 2 * y_i + y_i+1)
epsilon = np.sum(np.abs(a * (x[1:-1, :] - y[1:-1, :]) + b * (y[2:, :] - 2*y[1:-1, :] + y[:-2, :])))
while epsilon > 1e-3:
y[1:-1, :] += a * (x[1:-1, :] - y[1:-1, :]) + b * (y[2:, :] - 2*y[1:-1, :] + y[:-2, :])
epsilon = np.sum(np.abs(a * (x[1:-1, :] - y[1:-1, :]) + b * (y[2:, :] - 2*y[1:-1, :] + y[:-2, :])))
# Copy the smoother path
self.path = y.tolist()
# Break the path to subgoals every 2 pixels (1m = 20px)
step = 1
n_subgoals = (int) (len(self.path)/step)
self.subtargets = []
for i in range(0, n_subgoals):
self.subtargets.append(self.path[i * step])
self.subtargets.append(self.path[-1])
self.next_subtarget = 0
print "The path produced " + str(len(self.subtargets)) + " subgoals"
######################### NOTE: QUESTION ##############################
# The path is produced by an A* algorithm. This means that it is
# optimal in length but 1) not smooth and 2) length optimality
# may not be desired for coverage-based exploration
########################################################################
self.counter_to_next_sub = self.count_limit
# Publish the path for visualization purposes
ros_path = Path()
ros_path.header.frame_id = "map"
for p in self.path:
ps = PoseStamped()
ps.header.frame_id = "map"
ps.pose.position.x = 0
ps.pose.position.y = 0
######################### NOTE: QUESTION ##############################
# Fill the ps.pose.position values to show the path in RViz
# You must understand what self.robot_perception.resolution
# and self.robot_perception.origin are.
ps.pose.position.x = p[0] * self.robot_perception.resolution + self.robot_perception.origin['x']
ps.pose.position.y = p[1] * self.robot_perception.resolution + self.robot_perception.origin['y']
########################################################################
ros_path.poses.append(ps)
self.path_publisher.publish(ros_path)
# Publish the subtargets for visualization purposes
subtargets_mark = []
for s in self.subtargets:
subt = [
s[0] * self.robot_perception.resolution + \
self.robot_perception.origin['x'],
s[1] * self.robot_perception.resolution + \
self.robot_perception.origin['y']
]
subtargets_mark.append(subt)
RvizHandler.printMarker(\
subtargets_mark,\
2, # Type: Sphere
0, # Action: Add
"map", # Frame
"art_subtargets", # Namespace
[0, 0.8, 0.0, 0.8], # Color RGBA
0.2 # Scale
)
self.inner_target_exists = True
def targetSelection(self, initOgm, coverage, origin, resolution, robotPose,
force_random):
rospy.loginfo("-----------------------------------------")
rospy.loginfo(
"[Target Select Node] Robot_Pose[x, y, th] = [%f, %f, %f]",
robotPose['x'], robotPose['y'], robotPose['th'])
rospy.loginfo("[Target Select Node] OGM_Origin = [%i, %i]",
origin['x'], origin['y'])
rospy.loginfo("[Target Select Node] OGM_Size = [%u, %u]",
initOgm.shape[0], initOgm.shape[1])
# # willow stuff
ogm_limits = {}
# ogm_limits['min_x'] = 150 # used to be 200
# ogm_limits['max_x'] = 800 # used to be 800
# ogm_limits['min_y'] = 200
# ogm_limits['max_y'] = 800
# # corridor
# ogm_limits = {}
# ogm_limits['min_x'] = 100 # used to be 200
# ogm_limits['max_x'] = 500 # used to be 800
# ogm_limits['min_y'] = 100
# ogm_limits['max_y'] = 500
# Big Map
# ogm_limits = {}
ogm_limits['min_x'] = 200 # used to be 200
# ogm_limits['max_x'] = 800 # used to be 800
ogm_limits['max_x'] = 850
ogm_limits['min_y'] = 300
ogm_limits['max_y'] = 1080
# ogm_limits['max_y'] = 1100
# Big Map Modified
# ogm_limits = {}
# ogm_limits['min_x'] = 450 # used to be 200
# ogm_limits['max_x'] = 650 # used to be 800
# ogm_limits['min_y'] = 500
# ogm_limits['max_y'] = 700
# publisher
marker_pub = rospy.Publisher("/vis_nodes", MarkerArray, queue_size=1)
# Find only the useful boundaries of OGM. Only there calculations have meaning
# ogm_limits = OgmOperations.findUsefulBoundaries(initOgm, origin, resolution)
print ogm_limits
# Blur the OGM to erase discontinuities due to laser rays
#ogm = OgmOperations.blurUnoccupiedOgm(initOgm, ogm_limits)
ogm = initOgm
# find brushfire field
brush2 = self.brush.obstaclesBrushfireCffi(ogm, ogm_limits)
# Calculate skeletonization
skeleton = self.topo.skeletonizationCffi(ogm, origin, resolution,
ogm_limits)
# Find Topological Graph
tinit = time.time()
nodes = self.topo.topologicalNodes(ogm, skeleton, coverage, origin, \
resolution, brush2, ogm_limits)
# print took to calculate....
rospy.loginfo("Calculation time: %s", str(time.time() - tinit))
if len(nodes) == 0 and force_random:
brush = self.brush.coverageLimitsBrushfire(initOgm, coverage,
robotPose, origin,
resolution)
throw = set()
throw = self.filterGoal(brush, initOgm, resolution, origin)
brush.difference_update(throw)
goal = random.sample(brush, 1)
rospy.loginfo("nodes are zero and random node chosen!!!!")
th_rg = math.atan2(goal[0][1] - robotPose['y'], \
goal[0][0] - robotPose['x'])
self.target = [goal[0][0], goal[0][1], th_rg]
return self.target
if len(nodes) == 0:
brush = self.brush.coverageLimitsBrushfire(initOgm, coverage,
robotPose, origin,
resolution)
throw = set()
throw = self.filterGoal(brush, initOgm, resolution, origin)
brush.difference_update(throw)
distance_map = dict()
distance_map = self.calcDist(robotPose, brush)
self.target = min(distance_map, key=distance_map.get)
th_rg = math.atan2(self.target[1] - robotPose['y'], \
self.target[0] - robotPose['x'])
self.target = list(self.target)
self.target.append(th_rg)
return self.target
# pick Random node!!
if force_random:
ind = random.randrange(0, len(nodes))
rospy.loginfo('index is: %d', ind)
rospy.loginfo('Random raw node is: [%u, %u]', nodes[ind][0],
nodes[ind][1])
tempX = nodes[ind][0] * resolution + origin['x']
tempY = nodes[ind][1] * resolution + origin['y']
th_rg = math.atan2(tempY - robotPose['y'], \
tempX - robotPose['x'])
self.target = [tempX, tempY, th_rg]
rospy.loginfo("[Main Node] Random target found at [%f, %f]",
self.target[0], self.target[1])
rospy.loginfo("-----------------------------------------")
return self.target
if len(nodes) > 0:
rospy.loginfo("[Main Node] Nodes ready! Elapsed time = %fsec",
time.time() - tinit)
rospy.loginfo("[Main Node] # of nodes = %u", len(nodes))
# Remove previous targets from the list of nodes
rospy.loginfo("[Main Node] Prev. target = [%u, %u]", self.previousTarget[0], \
self.previousTarget[1])
if len(nodes) > 1:
nodes = [i for i in nodes if i != self.previousTarget]
vis_nodes = []
for n in nodes:
vis_nodes.append([
n[0] * resolution + origin['x'],
n[1] * resolution + origin['y']
])
RvizHandler.printMarker(\
vis_nodes,\
1, # Type: Arrow
0, # Action: Add
"map", # Frame
"art_topological_nodes", # Namespace
[0.3, 0.4, 0.7, 0.5], # Color RGBA
0.1 # Scale
)
self.publish_markers(marker_pub, vis_nodes)
# Check distance From Other goal
# for node in nodes:
# node_x = node[0] * resolution + origin['x']
# node_y = node[1] * resolution + origin['y']
# dist = math.hypot(node_x - other_goal['x'], node_y - other_goal['y'])
# if dist < 5 and len(nodes) > 2:
# nodes.remove(node)
# Calculate topological cost
# rayLength = 800 # in pixels
# obstThres = 49
# wTopo = []
# dRad = []
# for i in range(8):
# dRad.append(rayLength)
# for k in range(0, len(nodes)):
# # Determine whether the ray length passes the OGM limits
# if nodes[k][0] + rayLength > ogm.shape[0]:
# self.xLimitUp = ogm.shape[0] - 1
# else:
# self.xLimitUp = nodes[k][0] + rayLength
# if nodes[k][0] - rayLength < 0:
# self.xLimitDown = 0
# else:
# self.xLimitDown = nodes[k][0] - rayLength
# if nodes[k][1] + rayLength > ogm.shape[1]:
# self.yLimitUp = ogm.shape[1] - 1
# else:
# self.yLimitUp = nodes[k][1] + rayLength
# if nodes[k][1] - rayLength < 0:
# self.yLimitDown = 0
# else:
# self.yLimitDown = nodes[k][1] - rayLength
# #### Here We Do the Ray Casting ####
# # Find the distance between the node and obstacles
# for i in range(nodes[k][0], self.xLimitUp):
# if ogm[i][nodes[k][1]] > obstThres:
# dRad[0] = i - nodes[k][0]
# break
# for i in range(self.xLimitDown, nodes[k][0]):
# if ogm[i][nodes[k][1]] > obstThres:
# dRad[1] = nodes[k][0] - i
# break
# for i in range(nodes[k][1], self.yLimitUp):
# if ogm[nodes[k][0]][i] > obstThres:
# dRad[2] = i - nodes[k][1]
# break
# for i in range(self.yLimitDown, nodes[k][1]):
# if ogm[nodes[k][0]][i] > obstThres:
# dRad[3] = nodes[k][1] - i
# break
# for i in range(nodes[k][0], self.xLimitUp):
# for j in range(nodes[k][1], self.yLimitUp):
# if ogm[i][j] > obstThres:
# crosscut = \
# math.sqrt((i - nodes[k][0])**2 + (j - nodes[k][1])**2)
# dRad[4] = crosscut
# break
# else:
# break
# if ogm[i][j] > obstThres:
# break
# for i in range(self.xLimitDown, nodes[k][0]):
# for j in range(self.yLimitDown, nodes[k][1]):
# if ogm[i][j] > obstThres:
# crosscut = \
# math.sqrt((nodes[k][0] - i)**2 + (nodes[k][1] - j)**2)
# dRad[5] = crosscut
# break
# else:
# break
# if ogm[i][j] > obstThres:
# break
# for i in range(nodes[k][0], self.xLimitUp):
# for j in range(self.yLimitDown, nodes[k][1]):
# if ogm[i][j] > obstThres:
# crosscut = \
# math.sqrt((i - nodes[k][0])**2 + (nodes[k][1] - j)**2)
# dRad[6] = crosscut
# break
# else:
# break
# if ogm[i][j] > obstThres:
# break
# for i in range(self.xLimitDown, nodes[k][0]):
# for j in range(nodes[k][1], self.yLimitUp):
# if ogm[i][j] > obstThres:
# crosscut = \
# math.sqrt((nodes[k][0] - i)**2 + (j - nodes[k][1])**2)
# dRad[7] = crosscut
# break
# else:
# break
# if ogm[i][j] > obstThres:
# break
#
# wTopo.append(sum(dRad) / 8)
# for i in range(len(nodes)):
# rospy.logwarn("Topo Cost is: %f ",wTopo[i])
# Calculate distance cost
wDist = []
nodesX = []
nodesY = []
for i in range(len(nodes)):
nodesX.append(nodes[i][0])
nodesY.append(nodes[i][1])
for i in range(len(nodes)):
#dist = math.sqrt((nodes[i][0] * resolution + origin['x'] - robotPose['x'])**2 + \
# (nodes[i][1] * resolution + origin['y'] - robotPose['y'])**2)
dist = math.sqrt((nodes[i][0] + origin['x_px'] - robotPose['x_px'])**2 + \
(nodes[i][1] + origin['y_px'] - robotPose['y_px'])**2)
# Manhattan Dist
# dist = abs(nodes[i][0] + origin['x_px'] - robotPose['x_px']) + \
# abs(nodes[i][1] + origin['y_px'] - robotPose['y_px'])
# numpy.var is covariance
# tempX = ((robotPose['x'] - nodesX[i] * resolution + origin['x'])**2) / (2 * numpy.var(nodesX))
# tempY = ((robotPose['y'] - nodesY[i] * resolution + origin['y'])**2) / (2 * numpy.var(nodesY))
# tempX = ((robotPose['x_px'] - nodesX[i] + origin['x_px'])**2) / (2 * numpy.var(nodesX))
# tempY = ((robotPose['y_px'] - nodesY[i] + origin['y_px'])**2) / (2 * numpy.var(nodesY))
# try:
# temp = 1 - math.exp(tempX + tempY) + 0.001 # \epsilon << 1
# except OverflowError:
# print 'OverflowError!!!'
# temp = 10**30
# gaussCoeff = 1 / temp
gaussCoeff = 1
wDist.append(dist * gaussCoeff)
#for i in range(len(nodes)):
# rospy.logwarn("Distance Cost is: %f ",wDist[i])
#return self.target
# Calculate coverage cost
# dSamp = 1 / resolution
# wCove = []
# for k in range(len(nodes)):
# athr = 0
# for i in range(-1, 1):
# indexX = int(nodes[k][0] + i * dSamp)
# if indexX >= 0:
# for j in range(-1, 1):
# indexY = int(nodes[k][1] + j * dSamp)
# if indexY >= 0:
# athr += coverage[indexX][indexY]
# wCove.append(athr)
# for i in range(len(nodes)):
# rospy.logwarn("Cove Cost is: %f ",wCove[i])
# Calculate rotational cost
# wRot = []
# for i in range(len(nodes)):
# dTh = math.atan2(nodes[i][1] - robotPose['y_px'], \
# nodes[i][0] - robotPose['x_px']) - robotPose['th']
# wRot.append(dTh)
# for i in range(len(nodes)):
# rospy.logwarn("Rot Cost is: %f ",wRot[i])
# Normalize costs
# wTopoNorm = []
wDistNorm = []
# wCoveNorm = []
# wRotNorm = []
for i in range(len(nodes)):
# if max(wTopo) - min(wTopo) == 0:
# normTopo = 0
# else:
# normTopo = 1 - (wTopo[i] - min(wTopo)) / (max(wTopo) - min(wTopo))
if max(wDist) - min(wDist) == 0:
normDist = 0
else:
normDist = 1 - (wDist[i] - min(wDist)) / (max(wDist) -
min(wDist))
# if max(wCove) - min(wCove) == 0:
# normCove = 0
# else:
# normCove = 1 - (wCove[i] - min(wCove)) / (max(wCove) - min(wCove))
# if max(wRot) - min(wRot) == 0:
# normRot = 0
# else:
# normRot = 1 - (wRot[i] - min(wRot)) / (max(wRot) - min(wRot))
# wTopoNorm.append(normTopo)
wDistNorm.append(normDist)
# wCoveNorm.append(normCove)
# wRotNorm.append(normRot)
# rospy.logwarn("Printing TopoNorm....\n")
# print wTopoNorm
# rospy.logwarn("Printing DistNorm....\n")
# print wDistNorm
# rospy.logwarn("Printing CoveNorm....\n")
# print wCoveNorm
# rospy.logwarn("Printing RotNorm....\n")
# print wRotNorm
# Calculate Priority Weight
priorWeight = []
for i in range(len(nodes)):
#pre = round((wDistNorm[i] / 0.5), 0)
pre = wDistNorm[i] / 0.5
# pre = 1 * round((wTopoNorm[i] / 0.5), 0) + \
# 8 * round((wDistNorm[i] / 0.5), 0) + \
# 4 * round((wCoveNorm[i] / 0.5), 0) + \
# 2 * round((wRotNorm[i] / 0.5), 0)
#pre = 1 * round((wDistNorm[i] / 0.5), 0) + \
# 2 * round((wTopoNorm[i] / 0.5), 0)
# + round((wRotNorm[i] / 0.5), 0)
priorWeight.append(pre)
# Calculate smoothing factor
smoothFactor = []
for i in range(len(nodes)):
coeff = 1 - wDistNorm[i]
# coeff = (1 * (1 - wTopoNorm[i]) + 8 * (1 - wDistNorm[i]) + \
# 4 * (1 - wCoveNorm[i]) + 2 * (1 - wRotNorm[i])) / (2**4 - 1)
#coeff = ((1 - wDistNorm[i]) + 2 * (1 - wTopoNorm[i])) / (2**2 - 1)
smoothFactor.append(coeff)
# Calculate costs
self.costs = []
for i in range(len(nodes)):
self.costs.append(smoothFactor[i] * priorWeight[i])
# print 'len nodes is:'
# print len(nodes)
goals_and_costs = zip(nodes, self.costs)
#print goals_and_costs
goals_and_costs.sort(key=lambda t: t[1], reverse=False)
#sorted(goals_and_costs, key=operator.itemgetter(1))
#print goals_and_costs
# ind = random.randrange(0,min(6, len(nodes)))
rospy.loginfo("[Main Node] Raw node = [%u, %u]",
goals_and_costs[0][0][0], goals_and_costs[0][0][1])
tempX = goals_and_costs[0][0][0] * resolution + origin['x']
tempY = goals_and_costs[0][0][1] * resolution + origin['y']
th_rg = math.atan2(tempY - robotPose['y'], \
tempX - robotPose['x'])
self.target = [tempX, tempY, th_rg]
rospy.loginfo("[Main Node] Eligible node found at [%f, %f]",
self.target[0], self.target[1])
rospy.loginfo("[Main Node] Node Index: %u", i)
rospy.loginfo("[Main Node] Node Cost = %f", goals_and_costs[0][1])
rospy.loginfo("-----------------------------------------")
self.previousTarget = [
goals_and_costs[0][0][0], goals_and_costs[0][0][1]
]
return self.target
def skeletonizationCffi(self, ogm, origin, resolution, ogml):
width = ogm.shape[0]
height = ogm.shape[1]
local = numpy.zeros(ogm.shape)
start = time.time()
########################################################################
####################### Original Code ##################################
# for i in range(0, width):
# for j in range(0, height):
# if ogm[i][j] < 49:
# local[i][j] = 1
############################ Added code #################################
step = 50
first = [0, 0]
last = [0, 0]
# Find the area in which there is propability of coverage
for i in range(0, width, step):
for j in range(0, height, step):
if ogm[i][j] < 49:
if first == [0, 0]:
first = [i, j]
last = [i, j]
# Search only in the area found
for i in range(first[0] - step, last[0] + step):
for j in range(first[1] - step, last[1] + step):
if ogm[i][j] < 49:
local[i][j] = 1
#########################################################################
end = time.time()
print end - start
skeleton = Cffi.thinning(local, ogml)
skeleton = Cffi.prune(skeleton, ogml, 10)
viz = []
for i in range(0, width):
for j in range(0, height):
if skeleton[i][j] == 1:
viz.append([
i * resolution + origin['x'],
j * resolution + origin['y']
])
RvizHandler.printMarker(\
viz,\
1, # Type: Arrow
0, # Action: Add
"map", # Frame
"art_skeletonization_cffi", # Namespace
[0.5, 0, 0, 0.5], # Color RGBA
0.05 # Scale
)
return skeleton
def selectTarget(self, init_ogm, coverage, robot_pose, origin, \
resolution, force_random = False):
target = [-1, -1]
######################### NOTE: QUESTION ##############################
# Implement a smart way to select the next target. You have the
# following tools: ogm_limits, Brushfire field, OGM skeleton,
# topological nodes.
# Find only the useful boundaries of OGM. Only there calculations
# have meaning
ogm_limits = OgmOperations.findUsefulBoundaries(
init_ogm, origin, resolution)
#print(ogm_limits)
# Blur the OGM to erase discontinuities due to laser rays
ogm = OgmOperations.blurUnoccupiedOgm(init_ogm, ogm_limits)
# Calculate Brushfire field
tinit = time.time()
brush = self.brush.obstaclesBrushfireCffi(ogm, ogm_limits)
Print.art_print("Brush time: " + str(time.time() - tinit),
Print.ORANGE)
# Calculate skeletonization
tinit = time.time()
skeleton = self.topo.skeletonizationCffi(ogm, \
origin, resolution, ogm_limits)
Print.art_print("Skeletonization time: " + str(time.time() - tinit),
Print.ORANGE)
# Find topological graph
tinit = time.time()
nodes = self.topo.topologicalNodes(ogm, skeleton, coverage, origin, \
resolution, brush, ogm_limits)
Print.art_print("Topo nodes time: " + str(time.time() - tinit),
Print.ORANGE)
# Visualization of topological nodes
vis_nodes = []
for n in nodes:
vis_nodes.append([
n[0] * resolution + origin['x'],
n[1] * resolution + origin['y']
])
RvizHandler.printMarker(\
vis_nodes,\
1, # Type: Arrow
0, # Action: Add
"map", # Frame
"art_topological_nodes", # Namespace
[0.3, 0.4, 0.7, 0.5], # Color RGBA
0.1 # Scale
)
# Random point
#if statement that makes the target selection random in case the remaining nodes are two
if len(nodes) <= 2:
target = self.selectRandomTarget(ogm, coverage, brush, ogm_limits)
return target
pose_global_x = int(robot_pose['x_px'] - origin['x'] / resolution)
pose_global_y = int(robot_pose['y_px'] - origin['y'] / resolution)
#the folowing variables receive the values read by the sonar sensor
sonar_left = self.sonar.sonar_left_range
sonar_right = self.sonar.sonar_right_range
sonar_front = self.sonar.sonar_front_range
#a total sum is calculated for the front,right and left sonar sensors
sonar_sum = sonar_left + sonar_right + sonar_front
numrows = ogm.shape[1]
numcols = ogm.shape[0]
#if statement used in case the robot is in a tight spot and determines the target in the direction there is maximum space
if sonar_sum < 1.2:
target = self.sonar_avoidance(pose_global_x, pose_global_y, ogm,
numcols, numrows)
return target
#in case of a time out or a failure in path planning
if self.method == 'random' or force_random == True:
target = [
self.node2_index_x, self.node2_index_y
] #sets the current target as the previously calculated second best-scored target
if self.timeout_happened == 1:
target = self.sonar_avoidance(pose_global_x, pose_global_y,
ogm, numcols, numrows)
self.timeout_happened = 0
return target
self.timeout_happened = 1
return target
########################################################################
sum_min = 10000000
dist_min = 10000000
node_index_x = 0 #x value of the node with the lowest total cost
node_index_y = 0 #y value of the node with the lowest total cost
topo_cost = [] #list of topological costs for each node
dist_cost = [] #list of distance costs for each node
cov_cost = [] #list of coverage costs for each node
#using the brushfire array in order to calculate topology cost for each node
for n in nodes:
sum_temp = 0
index = n[1] + 1
while brush[n[0], index] != 0:
sum_temp += 1
index += 1
if index == numrows - 1: #numrows
break
index = n[1] - 1
while brush[n[0], index] != 0:
sum_temp += 1
index -= 1
if index == 0:
break
index = n[0] + 1
while brush[index, n[1]] != 0:
sum_temp += 1
index += 1
if index == numcols - 1: #numcols
break
index = n[0] - 1
while brush[index, n[1]] != 0:
sum_temp += 1
index -= 1
if index == 0:
break
topo_cost.append(sum_temp / 4)
#using the coverage array in order to calculate coverage cost for each node
numrows = len(coverage)
numcols = len(coverage[0])
for n in nodes:
total_sum = 0
sum_temp = 0
index = n[1] + 1
if index >= numrows or n[0] >= numcols:
total_sum = 5000
cov_cost.append(total_sum)
continue
while coverage[n[0], index] != 100:
sum_temp += 1
index += 1
if index >= numcols - 1: #numrows-1:
break
total_sum += sum_temp * coverage[n[0], index] / 100
index = n[1] - 1
sum_temp = 0
while coverage[n[0], index] != 100:
sum_temp += 1
index -= 1
if index == 0:
break
total_sum += sum_temp * coverage[n[0], index] / 100
index = n[0] + 1
sum_temp = 0
if index >= numcols or n[1] >= numrows:
total_sum = 5000
cov_cost.append(total_sum)
continue
while coverage[index, n[1]] != 100:
sum_temp += 1
index += 1
if index >= numrows - 1: #numcols-1
break
total_sum += sum_temp * coverage[index, n[1]] / 100
index = n[0] - 1
sum_temp = 0
while coverage[index, n[1]] != 100:
sum_temp += 1
index -= 1
if index == 0:
break
total_sum += sum_temp * coverage[index, n[1]] / 100
if total_sum == 0:
total_sum = 5000
cov_cost.append(total_sum)
pose_global_x = int(robot_pose['x_px'] - origin['x'] / resolution)
pose_global_y = int(robot_pose['y_px'] - origin['y'] / resolution)
#eucledean distance between the robot pose and each node
dist = math.sqrt(
math.pow(pose_global_x - n[0], 2) +
math.pow(pose_global_y - n[1], 2))
dist_cost.append(dist)
maxi = 0
for i in range(0, len(cov_cost)):
if cov_cost[i] != 5000 and cov_cost[i] > maxi:
maxi = cov_cost[i]
for i in range(0, len(cov_cost)):
if cov_cost[i] == 5000:
cov_cost[i] = maxi * 1.2
#lists to store the normalized costs for each node
topo_cost_norm = []
dist_cost_norm = []
cov_cost_norm = []
final_cost = []
min_final_cost = 1000000
for i in range(0, len(dist_cost)):
topo_cost_norm.append((np.float(topo_cost[i] - min(topo_cost)) /
np.float(max(topo_cost) - min(topo_cost))))
dist_cost_norm.append((dist_cost[i] - min(dist_cost)) /
(max(dist_cost) - min(dist_cost)))
if np.float(max(cov_cost) - min(cov_cost)) != 0:
cov_cost_norm.append((np.float(cov_cost[i] - min(cov_cost)) /
np.float(max(cov_cost) - min(cov_cost))))
if max(cov_cost) != min(cov_cost):
final_cost.append(
4 * topo_cost_norm[i] + 2 * dist_cost_norm[i] +
4 * cov_cost_norm[i]
) #optimal factor values in order to determine the best node to approach
else:
final_cost.append(
6 * topo_cost_norm[i] + 4 * dist_cost_norm[i]
) # exception if statement for the case coverage array cannot be used yet
if (final_cost[i] < min_final_cost):
min_final_cost = final_cost[i]
self.node2_index_x = node_index_x #storing the second best node to approach in case of path planning failure or time out
self.node2_index_y = node_index_y #
node_index_x = nodes[i][0]
node_index_y = nodes[i][1]
target = [node_index_x, node_index_y]
#in case current target is the same with the previous , sonar avoidance function is used to modify the robot pose
if target == self.previous_target:
target = self.sonar_avoidance(pose_global_x, pose_global_y, ogm,
numcols, numrows)
self.previous_target = target
return target
def checkTarget(self, event):
# Check if we have a target or if the robot just wanders
if self.inner_target_exists == False or self.move_with_target == False or\
self.next_subtarget == len(self.subtargets):
return
self.counter_to_next_sub -= 1
if self.counter_to_next_sub == 0:
Print.art_print('\n~~~~ Time reset ~~~~', Print.RED)
self.inner_target_exists = False
self.target_exists = False
return
# Get the robot pose in pixels
[rx, ry] = [\
self.robot_perception.robot_pose['x_px'] - \
self.robot_perception.origin['x'] / self.robot_perception.resolution,\
self.robot_perception.robot_pose['y_px'] - \
self.robot_perception.origin['y'] / self.robot_perception.resolution\
]
for i in range(self.next_subtarget, len(self.subtargets)):
# Find the distance between the robot pose and the next subtarget
dist = math.hypot(\
rx - self.subtargets[i][0], \
ry - self.subtargets[i][1])
######################### NOTE: QUESTION ##############################
# What if a later subtarget or the end has been reached before the
# next subtarget? Alter the code accordingly.
# Check if distance is less than 7 px (14 cm)
if dist < 7:
self.next_subtarget = 1 + i
self.counter_to_next_sub = self.count_limit
# Check if the final subtarget has been approached
if self.next_subtarget == len(self.subtargets):
self.target_exists = False
########################################################################
# Publish the current target
if self.next_subtarget == len(self.subtargets):
return
subtarget = [\
self.subtargets[self.next_subtarget][0]\
* self.robot_perception.resolution + \
self.robot_perception.origin['x'],
self.subtargets[self.next_subtarget][1]\
* self.robot_perception.resolution + \
self.robot_perception.origin['y']\
]
RvizHandler.printMarker(\
[subtarget],\
1, # Type: Arrow
0, # Action: Add
"map", # Frame
"art_next_subtarget", # Namespace
[0, 0, 0.8, 0.8], # Color RGBA
0.2 # Scale
)
def checkTarget(self, event):
# Check if we have a target or if the robot just wanders
if self.inner_target_exists == False or self.move_with_target == False or \
self.next_subtarget == len(self.subtargets):
return
self.counter_to_next_sub -= 1
if self.counter_to_next_sub == 0:
Print.art_print('\n~~~~ Time reset ~~~~', Print.RED)
self.inner_target_exists = False
self.target_exists = False
return
# Get the robot pose in pixels
[rx, ry] = [
self.robot_perception.robot_pose['x_px'] -
self.robot_perception.origin['x'] /
self.robot_perception.resolution,
self.robot_perception.robot_pose['y_px'] -
self.robot_perception.origin['y'] /
self.robot_perception.resolution
]
# # Find the distance between the robot pose and the next subtarget
# dist = math.hypot(
# rx - self.subtargets[self.next_subtarget][0],
# ry - self.subtargets[self.next_subtarget][1])
######################### NOTE: QUESTION ##############################
# What if a later subtarget or the end has been reached before the
# next subtarget? Alter the code accordingly.
# Check if distance is less than 7 px (14 cm)
# if dist < 5:
# self.next_subtarget += 1
# self.counter_to_next_sub = self.count_limit
# # Check if the final subtarget has been approached
# if self.next_subtarget == len(self.subtargets):
# self.target_exists = False
# find the distance between robot and ALL subtargets
dists = (math.hypot(rx - self.subtargets[subtarget][0],
ry - self.subtargets[subtarget][1])
for subtarget, _ in enumerate(self.subtargets))
# check the distance of each subtarget from the robot
# go towards the nearest and change index of next_subtarget
# accordingly
for idx, dist in enumerate(dists):
if dist < 5:
self.next_subtarget = idx + 1
self.counter_to_next_sub = self.count_limit
# Check if the final subtarget has been approached
if self.next_subtarget == len(self.subtargets):
self.target_exists = False
########################################################################
# Publish the current target
if self.next_subtarget == len(self.subtargets):
return
subtarget = [
self.subtargets[self.next_subtarget][0] *
self.robot_perception.resolution +
self.robot_perception.origin['x'],
self.subtargets[self.next_subtarget][1] *
self.robot_perception.resolution +
self.robot_perception.origin['y']
]
RvizHandler.printMarker(
[subtarget],
1, # Type: Arrow
0, # Action: Add
"map", # Frame
"art_next_subtarget", # Namespace
[0, 0, 0.8, 0.8], # Color RGBA
0.2 # Scale
)
def checkTarget(self, event):
# Check if we have a target or if the robot just wanders
if self.inner_target_exists == False or self.move_with_target == False or\
self.next_subtarget == len(self.subtargets):
return
self.counter_to_next_sub -= 1
if self.counter_to_next_sub == 0:
Print.art_print('\n~~~~ Time reset ~~~~',Print.RED)
self.inner_target_exists = False
self.target_exists = False
return
# Get the robot pose in pixels
[rx, ry] = [\
self.robot_perception.robot_pose['x_px'] - \
self.robot_perception.origin['x'] / self.robot_perception.resolution,\
self.robot_perception.robot_pose['y_px'] - \
self.robot_perception.origin['y'] / self.robot_perception.resolution\
]
######################### NOTE: QUESTION ##############################
# What if a later subtarget or the end has been reached before the
# next subtarget? Alter the code accordingly.
# Challenge 5
# Instead of checking the distance of the next target,
# check the distance of the remaining targets, therefore you may reach to a next subject
# bypassing current.
for i in range(self.next_subtarget, len(self.subtargets)):
# Find the distance between the robot pose and the next subtarget
dist = math.hypot( rx - self.subtargets[i][0], ry - self.subtargets[i][1])
# check distance with the i_th target
if i != (len(self.subtargets)-1):
if dist < 10: #if distance found to be small from a terget set as next the target i + 1
self.next_subtarget = i + 1
self.counter_to_next_sub = self.count_limit
else:
if dist < 5: #if distance found to be small from a terget set as next the target i + 1
self.next_subtarget = i + 1
self.counter_to_next_sub = self.count_limit
# Check if the final subtarget has been approached
if self.next_subtarget == len(self.subtargets):
self.target_exists = False
########################################################################
# Publish the current target
if self.next_subtarget == len(self.subtargets):
return
subtarget = [\
self.subtargets[self.next_subtarget][0]\
* self.robot_perception.resolution + \
self.robot_perception.origin['x'],
self.subtargets[self.next_subtarget][1]\
* self.robot_perception.resolution + \
self.robot_perception.origin['y']\
]
RvizHandler.printMarker(\
[subtarget],\
1, # Type: Arrow
0, # Action: Add
"map", # Frame
"art_next_subtarget", # Namespace
[0, 0, 0.8, 0.8], # Color RGBA
0.2 # Scale
)