Skip to content

Commit

Permalink
Challenge 3 - 7
Browse files Browse the repository at this point in the history
  • Loading branch information
gonidelis committed Jul 20, 2020
1 parent 073c0e5 commit 8e65f6e
Show file tree
Hide file tree
Showing 3 changed files with 89 additions and 36 deletions.
20 changes: 20 additions & 0 deletions art_autonomous_exploration/src/navigation.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -215,6 +215,25 @@ def selectTarget(self):
# 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)
Expand All @@ -231,6 +250,7 @@ def selectTarget(self):
# may not be desired for coverage-based exploration
########################################################################


self.counter_to_next_sub = self.count_limit

# Publish the path for visualization purposes
Expand Down
68 changes: 46 additions & 22 deletions art_autonomous_exploration/src/target_selection.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,30 +44,26 @@ def selectTarget(self, init_ogm, coverage, robot_pose, origin, \
# Find only the useful boundaries of OGM. Only there calculations
# have meaning
ogm_limits = OgmOperations.findUsefulBoundaries(init_ogm, origin, resolution)
#print "ogm_limits", ogm_limits

# Blur the OGM to erase discontinuities due to laser rays
ogm = OgmOperations.blurUnoccupiedOgm(init_ogm, ogm_limits)
#print "ogm", ogm

# Calculate Brushfire field
tinit = time.time()
brush = self.brush.obstaclesBrushfireCffi(ogm, ogm_limits)
Print.art_print("Brush time: " + str(time.time() - tinit), Print.ORANGE)
#print "brush", brush

# 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)
#print "skeleton", skeleton

# 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)
#print "nodes", nodes

# Visualization of topological nodes
vis_nodes = []
Expand All @@ -88,38 +84,66 @@ def selectTarget(self, init_ogm, coverage, robot_pose, origin, \

# SMART TARGET SELECTION USING:
# 1. Brush-fire field
# 2. OMG Skeleton
# 2. OGM Skeleton
# 3. Topological Nodes
# 4. Coverage field
max_brush = -999
min_dist = 999
# 5. OGM Limits

# Next subtarget is selected based on a
# weighted-calculated score for each node. The score
# is calculated using normalized values of the brush
# field and the number of branches. The weight values
# are defined experimentaly through the tuning method.
temp_score = 0
max_score = 0
best_node = nodes[0]

# the max-min boundaries are set arbitarily
BRUSH_MAX = 17
BRUSH_MIN = 1
BRUSH_WEIGHT = 2.5
BRANCH_MAX = 10
BRANCH_MIN = 0
BRANCH_WEIGHT = 2.5
DISTANCE_MIN = 0
DISTANCE_MAX = 40
DISTANCE_WEIGHT = 0.5

for n in nodes:

# Find the node with max brush value ->
# which tends to have max distance from the obstacles
if brush[n[0]][n[1]] > max_brush:
max_brush = brush[n[0]][n[1]]
brush_x = n[0]
brush_y = n[1]
# Use brushfire to increase temp_score
temp_score = (brush[n[0]][n[1]] - BRUSH_MIN) / (BRUSH_MAX - BRUSH_MIN) * BRUSH_WEIGHT

# Use OMG Skeleton to find potential
# Use OGM Skeleton to find potential
# branches following the target
branches = 0
for i in range(-1,2):
for j in range(-1,2):
if (i != 0 or j != 0):
branches += skeleton[brush_x+i][brush_y+j]
# If branches >= 2 --> path continues beyond the target
if branches >= 2:
final_x = brush_x
final_y = brush_y
branches += skeleton[n[0]+i][n[1]+j]

# Use OGM-Skeleton to increase temp_score (select a goal with more future options)
temp_score += (branches - BRANCH_MIN) / (BRANCH_MAX - BRUSH_MIN) * BRANCH_WEIGHT

# Use OGM-Limits to decrease temp_score
# the goal closest to OGM limits is best exploration option
distance = math.sqrt((ogm_limits['max_x'] - n[0]) ** 2 + (ogm_limits['max_y'] - n[1]) ** 2)
temp_score -= (distance - DISTANCE_MIN) / (DISTANCE_MAX - DISTANCE_MIN) * DISTANCE_WEIGHT

# If temp_score is higher than current max
# score, then max score is updated and current node
# becomes next goal - target
if temp_score > max_score:
max_score = temp_score
best_node = n

final_x = best_node[0]
final_y = best_node[1]
target = [final_x, final_y]

# Random point
#if self.method == 'random' or force_random == True:
# target = self.selectRandomTarget(ogm, coverage, brush, ogm_limits)
# if self.method == 'random' or force_random == True:
# target = self.selectRandomTarget(ogm, coverage, brush, ogm_limits)
########################################################################

return target
Expand Down
37 changes: 23 additions & 14 deletions art_autonomous_exploration/src/utilities.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,12 +39,16 @@ def brushfireFromObstacles(ogm, brush, ogml):
for i in range(len(y)):
yi[i] = ffi.cast("int *", y[i].ctypes.data)

br_c = lib.brushfireFromObstacles(xi, yi, len(x), len(x[0]),
br_c = lib.brushfireFromObstacles(xi, yi, len(x), len(x[0]),
ogml['min_x'], ogml['max_x'], ogml['min_y'], ogml['max_y'])
# TODO: Must be faster!
for i in range(ogm.shape[0]):
for j in range(ogm.shape[1]):
brush[i][j] = yi[i][j]
# # TODO: Must be faster!
# for i in range(ogm.shape[0]):
# for j in range(ogm.shape[1]):
# brush[i][j] = yi[i][j]
#
# Extra challenge #2
# Optimize targer selection calculations
brush[:ogm.shape[0], :ogm.shape[1]] = np.array(y)

return brush

Expand All @@ -69,6 +73,12 @@ def thinning(skeleton, ogml):
for i in range(skeleton.shape[0]):
for j in range(skeleton.shape[1]):
skeleton[i][j] = yi[i][j]

# Extra Challenge #2
# Optimize target selection calculations
# itime = time.time()
# skeleton = np.array(y)

Print.art_print("Skeletonization final copy: " + str(time.time() - itime), Print.BLUE)
return skeleton

Expand All @@ -87,7 +97,7 @@ def prune(skeleton, ogml, iterations):

br_c = lib.prune(xi, yi, len(x), len(x[0]),
ogml['min_x'], ogml['max_x'], ogml['min_y'], ogml['max_y'], iterations)

# TODO: Must be faster!
for i in range(skeleton.shape[0]):
for j in range(skeleton.shape[1]):
Expand Down Expand Up @@ -118,7 +128,7 @@ def findUsefulBoundaries(ogm, origin, resolution):
min_y = origin['y'] / resolution
max_x = origin['x'] / resolution
max_y = origin['y'] / resolution

x = ogm.shape[0]
y = ogm.shape[1]

Expand Down Expand Up @@ -175,15 +185,15 @@ def findUsefulBoundaries(ogm, origin, resolution):
min_y * resolution + origin['y']
],
[
max_x * resolution + origin['x'],
max_x * resolution + origin['x'],
min_y * resolution + origin['y']
],
[
max_x * resolution + origin['x'],
max_x * resolution + origin['x'],
max_y * resolution + origin['y']
],
[
min_x * resolution + origin['x'],
min_x * resolution + origin['x'],
max_y * resolution + origin['y']
]
],\
Expand All @@ -197,9 +207,9 @@ def findUsefulBoundaries(ogm, origin, resolution):


return {
'min_x': min_x,
'max_x': max_x,
'min_y': min_y,
'min_x': min_x,
'max_x': max_x,
'min_y': min_y,
'max_y': max_y
}

Expand Down Expand Up @@ -272,4 +282,3 @@ def printMarker(poses, m_type, action, frame, ns, color, scale):


RvizHandler.markers_publisher.publish(markers)

0 comments on commit 8e65f6e

Please sign in to comment.