class TargetSelection:
# Constructor
def __init__(self, selection_method):
self.goals_position = []
self.goals_value = []
self.omega = 0.0
self.radius = 0
self.method = selection_method
self.previous_target = []
self.brush = Brushfires()
self.topo = Topology()
self.path_planning = PathPlanning()
self.previous_target.append(50)
self.previous_target.append(50)
self.node2_index_x = 0
self.node2_index_y = 0
self.sonar = SonarDataAggregator()
self.timeout_happened = 0
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 selectRandomTarget(self, ogm, coverage, brushogm, ogm_limits):
# The next target in pixels
tinit = time.time()
next_target = [0, 0]
found = False
while not found:
x_rand = random.randint(0, ogm.shape[0] - 1)
y_rand = random.randint(0, ogm.shape[1] - 1)
if ogm[x_rand][y_rand] < 50 and coverage[x_rand][y_rand] < 50 and \
brushogm[x_rand][y_rand] > 5:
next_target = [x_rand, y_rand]
found = True
Print.art_print("Select random target time: " + str(time.time() - tinit), \
Print.ORANGE)
return next_target
#sonar_avoidance is a function that calculates the direction the robot should head in order to escape a tight spot
def sonar_avoidance(self, pose_global_x, pose_global_y, ogm, numcols,
numrows):
tally = [
0, 0, 0, 0
] #a sum is calculated for north,east,south and west direction
index = pose_global_y + 1
while ogm[pose_global_x, index] != 100:
tally[0] = tally[0] + 1
index += 1
if index == numrows - 1: #numrows
break
index = pose_global_y - 1
while ogm[pose_global_x, index] != 100:
tally[1] = tally[1] + 1
index -= 1
if index == 0:
break
index = pose_global_x + 1
while ogm[index, pose_global_y] != 100:
tally[2] = tally[2] + 1
index += 1
if index == numcols - 1: #numcols
break
index = pose_global_x - 1
while ogm[index, pose_global_y] != 100:
tally[3] = tally[3] + 1
index -= 1
if index == 0:
break
index = tally.index(max(tally))
if index == 0:
target = [pose_global_x, pose_global_y + (int)(tally[0] / 2)]
return target
elif index == 1:
target = [pose_global_x, pose_global_y - (int)(tally[1] / 2)]
return target
elif index == 2:
target = [pose_global_x + (int)(tally[2] / 2), pose_global_y]
return target
elif index == 3:
target = [pose_global_x - (int)(tally[3] / 2), pose_global_y]
return target
class TargetSelection:
# Constructor
def __init__(self, selection_method):
self.goals_position = []
self.goals_value = []
self.omega = 0.0
self.radius = 0
self.method = selection_method
self.brush = Brushfires()
self.topo = Topology()
self.path_planning = PathPlanning()
# Initialize previous target
self.previous_target = [-1, -1]
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 selectRandomTarget(self, ogm, coverage, brushogm, ogm_limits):
# The next target in pixels
tinit = time.time()
next_target = [0, 0]
found = False
while not found:
x_rand = random.randint(0, ogm.shape[0] - 1)
y_rand = random.randint(0, ogm.shape[1] - 1)
if ogm[x_rand][y_rand] < 50 and coverage[x_rand][y_rand] < 50 and \
brushogm[x_rand][y_rand] > 5:
next_target = [x_rand, y_rand]
found = True
Print.art_print("Select random target time: " + str(time.time() - tinit), \
Print.ORANGE)
return next_target
class TargetSelect:
def __init__(self):
self.xLimitUp = 0
self.xLimitDown = 0
self.yLimitUp = 0
self.yLimitDown = 0
self.brush = Brushfires()
self.topo = Topology()
self.target = [-1, -1]
self.previousTarget = [-1, -1]
self.costs = []
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 rotateRobot(self):
velocityMsg = Twist()
angularSpeed = 0.3
relativeAngle = 2 * math.pi
currentAngle = 0
rospy.loginfo("Roatating robot...")
velocityMsg.linear.x = 0
velocityMsg.linear.y = 0
velocityMsg.linear.z = 0
velocityMsg.angular.x = 0
velocityMsg.angular.y = 0
velocityMsg.angular.z = angularSpeed
t0 = rospy.Time.now().to_sec()
rospy.logwarn(rospy.get_caller_id() + ": Rotate Robot! Please wait...")
while currentAngle < relativeAngle:
self.velocityPub.publish(velocityMsg)
t1 = rospy.Time.now().to_sec()
currentAngle = angularSpeed * (t1 - t0)
velocityMsg.angular.z = 0
self.velocityPub.publish(velocityMsg)
rospy.logwarn(rospy.get_caller_id() + ": Robot Rotation OVER!")
return
def publish_markers(self, marker_pub, vis_nodes):
markers = MarkerArray()
c = 0
for n in vis_nodes:
c += 1
msg = Marker()
msg.header.frame_id = "map"
msg.ns = "lines"
msg.action = msg.ADD
msg.type = msg.CUBE
msg.id = c
#msg.scale.x = 1.0
#msg.scale.y = 1.0
#msg.scale.z = 1.0
# I guess I have to take into consideration resolution too
msg.pose.position.x = n[0]
msg.pose.position.y = n[1]
msg.pose.position.z = 0
msg.pose.orientation.x = 0.0
msg.pose.orientation.y = 0.0
msg.pose.orientation.z = 0.05
msg.pose.orientation.w = 0.05
msg.scale.x = 0.2
msg.scale.y = 0.2
msg.scale.z = 0.2
msg.color.a = 1.0 # Don't forget to set the alpha!
msg.color.r = 0.0
msg.color.g = 1.0
msg.color.b = 0.0
# print 'I publish msg now!!!'
markers.markers.append(msg)
marker_pub.publish(markers)
#
return
def selectRandomTarget(self, ogm, brush, origin, ogmLimits, resolution):
rospy.logwarn("[Main Node] Random Target Selection!")
target = [-1, -1]
found = False
while not found:
x_rand = random.randint(0, int(ogm.shape[0] - 1))
y_rand = random.randint(0, int(ogm.shape[1] - 1))
if ogm[x_rand][y_rand] <= 49 and brush[x_rand][
y_rand] != -1: # and \#coverage[x_rand][y_rand] != 100:
tempX = x_rand * resolution + int(origin['x'])
tempY = y_rand * resolution + int(origin['y'])
target = [tempX, tempY]
found = True
rospy.loginfo("[Main Node] Random node selected at [%f, %f]",
target[0], target[1])
rospy.loginfo("-----------------------------------------")
return self.target
def calcDist(self, robotPose, brush):
distance_map = dict()
for goal in brush:
dist = math.hypot(goal[0] - robotPose['x'],
goal[1] - robotPose['y'])
distance_map[goal] = dist
return distance_map
def filterGoal(self, brush2, ogm, resolution, origin):
throw = set()
for goal in brush2:
goal = list(goal)
for i in range(-9, 10):
if int(goal[0] / resolution -
origin['x'] / resolution) + i >= len(ogm):
break
if ogm[int(goal[0]/resolution - origin['x']/resolution) + i]\
[int(goal[1]/resolution - origin['y']/resolution)] > 49 \
or ogm[int(goal[0]/resolution - origin['x']/resolution) + i]\
[int(goal[1]/resolution - origin['y']/resolution)] == -1:
goal = tuple(goal)
throw.add(goal)
break
for goal in brush2:
goal = list(goal)
for j in range(-9, 10):
if int(goal[1] / resolution -
origin['y'] / resolution) + j >= len(ogm[0]):
break
if ogm[int(goal[0]/resolution - origin['x']/resolution)]\
[int(goal[1]/resolution - origin['y']/resolution) + j] > 49 \
or ogm[int(goal[0]/resolution - origin['x']/resolution) + i]\
[int(goal[1]/resolution - origin['y']/resolution)] == -1:
goal = tuple(goal)
throw.add(goal)
break
for goal in brush2:
goal = list(goal)
for i in range(-9, 10):
if int(goal[0]/resolution - origin['x']/resolution) + i >= len(ogm) or \
int(goal[1]/resolution - origin['y']/resolution) + i >= len(ogm[0]):
break
if ogm[int(goal[0]/resolution - origin['x']/resolution) + i]\
[int(goal[1]/resolution - origin['y']/resolution) + i] > 49 \
or ogm[int(goal[0]/resolution - origin['x']/resolution) + i]\
[int(goal[1]/resolution - origin['y']/resolution) + i] == -1:
goal = tuple(goal)
throw.add(goal)
break
for goal in brush2:
goal = list(goal)
for i in range(-9, 10):
if int(goal[0]/resolution - origin['x']/resolution) + i >= len(ogm) or \
int(goal[1]/resolution - origin['y']/resolution) + i >= len(ogm[0]):
break
if ogm[int(goal[0]/resolution - origin['x']/resolution) + i]\
[int(goal[1]/resolution - origin['y']/resolution) - i] > 49 \
or ogm[int(goal[0]/resolution - origin['x']/resolution) + i]\
[int(goal[1]/resolution - origin['y']/resolution) - i] == -1:
goal = tuple(goal)
throw.add(goal)
break
return throw
class TargetSelection(object):
# Constructor
def __init__(self, selection_method):
self.goals_position = []
self.goals_value = []
self.omega = 0.0
self.radius = 0
self.method = selection_method
if self.method_is_cost_based():
from robot_perception import RobotPerception
self.robot_perception = RobotPerception()
self.cost_based_properties = rospy.get_param("cost_based_properties")
numpy.set_printoptions(precision=3, threshold=numpy.nan, suppress=True)
self.brush = Brushfires()
self.path_planning = PathPlanning()
def method_is_cost_based(self):
return self.method in ['cost_based', 'cost_based_fallback']
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
@staticmethod
def selectRandomTarget(ogm, coverage, brushogm):
# The next target in pixels
while True:
x_rand = random.randint(0, ogm.shape[0] - 1)
y_rand = random.randint(0, ogm.shape[1] - 1)
if ogm[x_rand][y_rand] < 50 and coverage[x_rand][y_rand] < 50 and brushogm[x_rand][y_rand] > 5:
return x_rand, y_rand
@staticmethod
def is_good(brush, ogm, coverage, target=None):
"""
:rtype: numpy.ndarray
"""
if target is not None:
x, y = target
return ogm[x][y] < 50 and coverage[x][y] < 50 and brush[x][y] > 5
else:
return numpy.logical_and(numpy.logical_and(ogm < 50, coverage < 50), brush > 5)
def select_by_cost(self, map_info):
nodes, paths, topo_costs = zip(*self.choose_best_nodes(map_info))
if nodes[0] is None: # choose_best_node's yield when no path is found will make nodes = (None,)
return -1, -1
elif len(nodes) == 1:
return nodes[0]
best_path_idx = self.weight_costs(
topo_costs,
[self.distance_cost(path, map_info.robot_px) for path in paths], # Distance
[self.coverage_cost(path, map_info.coverage) for path in paths], # Coverage
[self.rotation_cost(path, map_info.robot_px, map_info.theta) for path in paths] # Rotational
).argmax()
assert paths[best_path_idx]
target = nodes[best_path_idx]
Print.art_print("Best: {}".format(best_path_idx), Print.BLUE)
return target
def choose_best_nodes(self, map_info):
# Since path planning takes a lot of time for many nodes we should reduce the possible result to the nodes
# with the best distance and topological costs.
if self.method == 'cost_based_fallback':
yield self.closer_node(map_info), None, None
return
nodes = list(self.cluster_nodes(map_info.nodes, map_info.robot_px, self.cost_based_properties['node_clusters']))
topo_costs = [self._topological_cost(node, map_info.ogm) for node in nodes]
best_nodes_idx = self.weight_costs(
topo_costs,
[euclidean(node, map_info.robot_px) for node in nodes]
).argsort()[::-1] # We need descending order since we now want to maximize.
count = 0
for idx in best_nodes_idx:
node = nodes[idx]
path = map_info.create_path(node)
if path:
count += 1
yield node, path, topo_costs[idx]
if count == self.cost_based_properties['max_paths']:
break
if count == 0:
Print.art_print("Failed to create any path. Falling back to closer unoccupied.", Print.RED)
self.method = 'cost_based_fallback'
yield self.closer_node(map_info), None, None
@staticmethod
def cluster_nodes(nodes_original, robot_px, n_clusters):
Print.art_print("Trying to cluster:" + str(nodes_original), Print.BLUE)
whitened = whiten(nodes_original)
_, cluster_idx = kmeans2(whitened, n_clusters)
Print.art_print("Ended with clusters:" + str(cluster_idx), Print.BLUE)
keyfun = lambda x: cluster_idx[x[0]]
nodes = sorted(enumerate(nodes_original), key=keyfun)
for _, group in itertools.groupby(nodes, key=keyfun):
# For each cluster pick the farthest one.
max_idx = max(group, key=lambda x: euclidean(robot_px, x[1]))[0]
yield nodes_original[max_idx]
def weight_costs(self, *cost_vectors, **kwargs):
costs = self.normalize_costs(numpy.array(tuple(vector for vector in cost_vectors)))
if 'weights' in kwargs:
weights = kwargs['weights']
else:
weights = 2 ** numpy.arange(costs.shape[0] - 1, -1, -1)
return numpy.average(costs, axis=0, weights=weights)
@staticmethod
def closer_node(map_info):
robot_px = numpy.array(map_info.robot_px)
nodes = numpy.array(numpy.where(TargetSelection.is_good(
numpy.array(map_info.brush),
numpy.array(map_info.ogm),
numpy.array(map_info.coverage),
target=None
))).transpose()
closer_idx = numpy.linalg.norm(nodes - robot_px, axis=1).argmin()
return nodes[closer_idx]
def _topological_cost(self, node, ogm):
threshold = self.cost_based_properties['topo_threshold']
return self.topological_cost(node, ogm, threshold)
@staticmethod
def topological_cost(node, ogm, threshold):
result = 0
for dir_x, dir_y in [(0, 1), (1, 1), (1, 0), (1, -1), (0, -1), (-1, -1), (-1, 0), (-1, 1)]:
cost = 0
idx = 0
x, y = node
while cost < threshold:
idx += 1
x_c = x + idx * dir_x
y_c = y + idx * dir_y
cost = (x - x_c) ** 2 + (y - y_c) ** 2
if ogm[x_c][y_c] > 50:
break
elif ogm[x_c][y_c] in [50, -1]:
Print.art_print("{},{} is unknown!".format(x_c, y_c), Print.RED)
cost = threshold
break
result += min(threshold, cost)
return result
@staticmethod
def distance_cost(path, robot_px):
weighted_distances = (TargetSelection.distance(node1, node2) * TargetSelection.distance_coeff(node1, robot_px)
for node1, node2 in zip(path[:-1], path[1:]))
return sum(weighted_distances)
@staticmethod
def distance_coeff(node, robot_px, s=30, epsilon=0.0001):
x_n, y_n = node
x_g, y_g = robot_px
coeff = 1 - math.exp(-((x_n - x_g) ** 2 / (2 * s ** 2) + (y_n - y_g) ** 2 / (2 * s ** 2))) + epsilon
return 1 / coeff
@staticmethod
def distance(node_a, node_b):
x_1, y_1 = node_a
x_2, y_2 = node_b
return math.sqrt((x_1 - x_2) ** 2 + (y_1 - y_2) ** 2)
@staticmethod
def rotation_cost(path, robot_px, theta):
rotation = 0
rx, ry = robot_px
theta_old = theta
for node in path:
st_x, st_y = node
theta_new = math.atan2(st_y - ry, st_x - rx)
rotation += abs(theta_new - theta_old)
theta_old = theta_new
return rotation
@staticmethod
def coverage_cost(path, coverage):
coverage_sum = sum(coverage[x][y] for x, y in path)
return coverage_sum
@staticmethod
def normalize_costs(costs):
"""
:rtype: numpy.ndarray
"""
Print.art_print("Costs before normalization:\n" + str(costs), Print.BLUE)
assert (costs >= 0).all()
return 1 - (costs.transpose() / numpy.abs(costs).max(axis=1)).transpose()
class TargetSelection:
# Constructor
def __init__(self, selection_method):
self.goals_position = []
self.goals_value = []
self.omega = 0.0
self.radius = 0
self.method = selection_method
self.brush = Brushfires()
self.topo = Topology()
self.path_planning = PathPlanning()
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)
# Furhest node-point
if self.method == 'furthest':
target = self.selectFurthestTarget(ogm, coverage, brush,
ogm_limits, nodes)
return target
def selectFurthestTarget(self, ogm, coverage, brushogm, ogm_limits, nodes):
# The next target in pixels
tinit = time.time()
next_target = [0, 0]
for i in range(len(nodes) - 1, 0, -1):
x = nodes[i][0]
y = nodes[i][1]
if ogm[x][y] < 50 and coverage[x][y] < 50 and \
brushogm[x][y] > 10:
next_target = [x, y]
Print.art_print("Select furthest target time: " + str(time.time() - tinit), \
Print.ORANGE)
return next_target
return self.selectRandomTarget(ogm, coverage, brushogm, ogm_limits)
########################################################################
def selectRandomTarget(self, ogm, coverage, brushogm, ogm_limits):
# The next target in pixels
tinit = time.time()
next_target = [0, 0]
found = False
while not found:
x_rand = random.randint(0, ogm.shape[0] - 1)
y_rand = random.randint(0, ogm.shape[1] - 1)
if ogm[x_rand][y_rand] < 50 and coverage[x_rand][y_rand] < 50 and \
brushogm[x_rand][y_rand] > 5:
next_target = [x_rand, y_rand]
found = True
Print.art_print("Select random target time: " + str(time.time() - tinit), \
Print.ORANGE)
return next_target
class TargetSelection:
# Constructor
def __init__(self, selection_method):
self.goals_position = []
self.goals_value = []
self.omega = 0.0
self.radius = 0
self.method = selection_method
self.brush = Brushfires()
self.topo = Topology()
self.path_planning = PathPlanning()
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 selectRandomTarget(self, ogm, coverage, brushogm, ogm_limits):
# The next target in pixels
tinit = time.time()
next_target = [0, 0]
found = False
while not found:
x_rand = random.randint(0, ogm.shape[0] - 1)
y_rand = random.randint(0, ogm.shape[1] - 1)
if ogm[x_rand][y_rand] < 50 and coverage[x_rand][y_rand] < 50 and \
brushogm[x_rand][y_rand] > 5:
next_target = [x_rand, y_rand]
found = True
Print.art_print("Select random target time: " + str(time.time() - tinit), \
Print.ORANGE)
return next_target
class TargetSelection:
# Constructor
def __init__(self, selection_method):
self.goals_position = []
self.goals_value = []
self.omega = 0.0
self.radius = 0
self.method = selection_method
self.brush = Brushfires()
self.topo = Topology()
self.path_planning = PathPlanning()
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 selectRandomTarget(self, ogm, coverage, brushogm, ogm_limits):
# The next target in pixels
tinit = time.time()
next_target = [0, 0]
found = False
while not found:
x_rand = random.randint(0, ogm.shape[0] - 1)
y_rand = random.randint(0, ogm.shape[1] - 1)
if ogm[x_rand][y_rand] < 50 and coverage[x_rand][y_rand] < 50 and \
brushogm[x_rand][y_rand] > 5:
next_target = [x_rand, y_rand]
found = True
Print.art_print("Select random target time: " + str(time.time() - tinit), \
Print.ORANGE)
return next_target
def lineOccupation(self, ogm_line):
occupied = 0
potentially_occupied = 0
found_obstacle = False
for grid in ogm_line:
if grid > 90:
occupied_cells += 1 + potentially_occupied
elif grid < 55 and grid > 45 and found_obstacle:
potentially_occupied += 1
elif grid < 20:
potentially_occupied = 0
found_obstacle = False
return occupied
def detectObstacles(self, grid_size, ogm, point):
# Goal Coordinates
x_goal = point[0]
y_goal = point[1]
obstacles_found = False
min_dist = 0
closest_obstacle = None
# Obstacles in a grid_size x grid_size neighborhood
ogm_segment = ogm[x_goal-grid_size: x_goal+grid_size,\
y_goal-grid_size: y_goal+grid_size]
obs_idxs = np.where(ogm_segment > 80)
# If there are no obstacles terminate normally
if obs_idxs[0].size == 0:
obstacles_found = False
return [False, closest_obstacle, min_dist]
# Find the closest obstacle
obs_idxs = np.array([obs_idxs[0], obs_idxs[1]]).transpose()
distances = np.hypot(obs_idxs[0:, 0] - grid_size,
obs_idxs[0:, 1] - grid_size)
closest_idx = distances.argmin()
min_dist = distances.min()
closest_obstacle = obs_idxs[closest_idx] - np.array(
[grid_size, grid_size]) + point
return [True, closest_obstacle, min_dist]
class TargetSelection:
# Constructor
def __init__(self, selection_method):
self.goals_position = []
self.goals_value = []
self.omega = 0.0
self.radius = 0
self.method = selection_method
self.brush = Brushfires()
self.topo = Topology()
self.path_planning = PathPlanning()
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)
# print(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)
# 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)
#closest point
elif self.method == 'closest_node':
target = self.selectClosestTarget(robot_pose, vis_nodes, nodes)
elif self.method == 'cost_based':
target = self.selectCostBasedTarget(robot_pose, vis_nodes, brush)
return target
def selectCostBasedTarget(self, robot_pose, vis_nodes, brush):
dist_cost = []
topol_cost = []
turn_cost = []
dist_cost_norm = []
topol_cost_norm = []
turn_cost_norm = []
cost = 0
index_final = 0
[rx, ry] = [robot_pose['x'], robot_pose['y']]
for n in vis_nodes:
#distance cost
dist_cost.append(pow(pow(n[1] - ry, 2) + pow(n[0] - rx, 2),
0.5)) #distance from node
#topological cost
for i in range(-1, 1):
for j in range(-1, 1):
if i == 0 and j == 0:
continue
cost += brush[int(n[0]) + i][int(n[1]) + j]
topol_cost.append((1 / 8) * cost)
#rotational cost
angle = round(
math.degrees(
math.atan2(n[1] - ry + 0.00001, n[0] - rx + 0.00001)))
turn_cost.append(angle % 360)
# normalisation
for cost in dist_cost:
normalised_cost = 1 - ((cost - min(dist_cost)) /
(max(dist_cost) / min(dist_cost)))
dist_cost_norm.append(normalised_cost)
for cost in topol_cost:
normalised_cost = 1 - ((cost - min(topol_cost)) /
(max(topol_cost) - min(topol_cost)))
topol_cost_norm.append(normalised_cost)
for cost in turn_cost:
normalised_cost = 1 - ((cost - min(turn_cost)) /
(max(turn_cost) - min(turn_cost)))
turn_cost_norm.append(normalised_cost)
final_cost = []
for i in range(len(topol_cost_norm)):
cost = 4 * dist_cost_norm[i] + 2 * turn_cost_norm[
i] + topol_cost_norm[i]
final_cost.append(cost)
#find target with the
index_final = final_cost.index(max(final_cost))
target = vis_nodes[index_final]
return target
def selectClosestTarget(self, robot_pose, vis_nodes, nodes):
[rx, ry] = [robot_pose['x'], robot_pose['y']]
dist = []
for n in vis_nodes:
dist.append(pow(pow(n[1] - ry, 2) + pow(n[0] - rx, 2),
0.5)) #distance from node
index_min = dist.index(min(dist))
next_target = [nodes[index_min][0], nodes[index_min][1]]
return next_target
########################################################################
def selectRandomTarget(self, ogm, coverage, brushogm, ogm_limits):
# The next target in pixels
tinit = time.time()
next_target = [0, 0]
found = False
while not found:
x_rand = random.randint(0, ogm.shape[0] - 1)
y_rand = random.randint(0, ogm.shape[1] - 1)
if ogm[x_rand][y_rand] < 50 and coverage[x_rand][y_rand] < 50 and \
brushogm[x_rand][y_rand] > 5:
next_target = [x_rand, y_rand]
found = True
Print.art_print("Select random target time: " + str(time.time() - tinit), \
Print.ORANGE)
return next_target
class TargetSelection:
# Constructor
def __init__(self, selection_method):
self.goals_position = []
self.goals_value = []
self.omega = 0.0
self.radius = 0
self.method = selection_method
self.previous_target = [-1, -1]
self.brush = Brushfires()
self.topo = Topology()
self.path_planning = PathPlanning()
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
#############################################################################
# Challenge 6. select Smart Target Function
# this function follows the methodology presented
# on lecture 9.
def selectSmartTarget(self, coverage, brush, robot_pose, resolution, origin, nodes):
tinit = time.time()
# Get the robot pose in pixels
[rx, ry] = [int(round(robot_pose['x_px'] - origin['x'] / resolution)), int(round(robot_pose['y_px'] - origin['y'] / resolution))]
# Initialize weights matrix
weights = []
# Do procedure described in presentation 9
# for each node.
for i, node in enumerate(nodes):
# Calculate the path
path = np.flipud(self.path_planning.createPath([rx, ry], node, resolution))
# Check if it found a path
if path.shape[0] > 2:
# Vectors of the path
vectors = path[1:, :] - path[:-1, :]
# Calculate paths weighted distance
vectorsMean = vectors.mean(axis=0)
vectorsVar = vectors.var(axis=0)
dists = np.sqrt(np.einsum('ij,ij->i', vectors, vectors))
weightCoeff = 1 / (1 - np.exp(-np.sum((vectors - vectorsMean)**2 / (2 * vectorsVar), axis=1)) + 1e-4)
weightDists = np.sum(weightCoeff + dists)
# Topological weight
weightTopo = brush[node[0], node[1]]
# Cosine of the angles
c = np.sum(vectors[1:, :] * vectors[:-1, :], axis=1) / np.linalg.norm(vectors[1:, :], axis=1) / np.linalg.norm(vectors[:-1, :], axis=1)
# Sum of all angles
weightTurn = np.sum(abs(np.arccos(np.clip(c, -1, 1))))
# Calculate the coverage weight
pathIndex = np.rint(path).astype(int)
weightCove = 1 - np.sum(coverage[pathIndex[:, 0], pathIndex[:, 1]]) / (path.shape[0] * 255)
weights.append([i, weightDists, weightTopo, weightTurn, weightCove])
if len(weights) > 0:
weight = np.array(weights)
# Normalize the weights at [0,1]
weight[:,1:] = 1 - ((weight[:,1:] - np.min(weight[:,1:], axis=0)) / (np.max(weight[:,1:], axis=0) - np.min(weight[:,1:], axis=0)))
# Calculatete the final weights
finalWeights = 8 * weight[:, 2] + 4 * weight[:, 1] + 2 * weight[:, 4] + weight[:, 3]
# Find the best path
index = int(weight[max(xrange(len(finalWeights)), key=finalWeights.__getitem__)][0])
target = nodes[index]
Print.art_print("Smart target selection time: " + str(time.time() - tinit), Print.ORANGE)
return target
else:
Print.art_print("Smart target selection failed!!! Time: " + str(time.time() - tinit), Print.ORANGE)
return None
############################################################################
def selectRandomTarget(self, ogm, coverage, brushogm, ogm_limits):
# The next target in pixels
tinit = time.time()
next_target = [0, 0]
found = False
while not found:
x_rand = random.randint(0, ogm.shape[0] - 1)
y_rand = random.randint(0, ogm.shape[1] - 1)
if ogm[x_rand][y_rand] < 50 and coverage[x_rand][y_rand] < 50 and \
brushogm[x_rand][y_rand] > 5:
next_target = [x_rand, y_rand]
found = True
Print.art_print("Select random target time: " + str(time.time() - tinit), \
Print.ORANGE)
return next_target
class TargetSelection:
# Constructor
def __init__(self, selection_method):
self.goals_position = []
self.goals_value = []
self.omega = 0.0
self.radius = 0
self.method = selection_method
self.brush = Brushfires()
self.topo = Topology()
self.path_planning = PathPlanning()
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 selectRandomTarget(self, ogm, coverage, brushogm, ogm_limits):
# The next target in pixels
tinit = time.time()
next_target = [0, 0]
found = False
while not found:
x_rand = random.randint(0, ogm.shape[0] - 1)
y_rand = random.randint(0, ogm.shape[1] - 1)
if ogm[x_rand][y_rand] < 50 and coverage[x_rand][y_rand] < 50 and \
brushogm[x_rand][y_rand] > 5:
next_target = [x_rand, y_rand]
print " RANDOM TARGET " + str(next_target)
found = True
Print.art_print("Select random target time: " + str(time.time() - tinit), \
Print.ORANGE)
return next_target
class TargetSelect:
def __init__(self):
self.xLimitUp = 0
self.xLimitDown = 0
self.yLimitUp = 0
self.yLimitDown = 0
self.brush = Brushfires()
self.topo = Topology()
self.target = [-1, -1]
self.previousTarget = [-1, -1]
self.costs = []
def targetSelection(self, initOgm, coverage, origin, resolution, robotPose, flag, other_goal, 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 params
# ogm_limits = {}
# ogm_limits['min_x'] = 350 # used to be 200
# ogm_limits['max_x'] = 800 # used to be 800
# ogm_limits['min_y'] = 200
# ogm_limits['max_y'] = 800
# # 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'] = 710
# publisher
marker_pub = rospy.Publisher("/robot1/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
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 < 1 and len(nodes) > 2:
nodes.remove(node)
# 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
# Calculate distance cost
wDist = []
nodesX = []
nodesY = []
for i in range(0, len(nodes)):
nodesX.append(nodes[i][0])
nodesY.append(nodes[i][1])
for i in range(0, len(nodes)):
dist = math.sqrt((nodes[i][0] * resolution + origin['x_px'] - robotPose['x_px'])**2 + \
(nodes[i][1] * resolution + origin['y_px'] - robotPose['y_px'])**2)
# for i in range(len(nodes)):
# rospy.logwarn("Distance Cost is: %f ",wDist[i])
gaussCoeff = 1
wDist.append(dist * gaussCoeff)
#return self.target
# Normalize costs
# wTopoNorm = []
wDistNorm = []
# wCoveNorm = []
# wRotNorm = []
for i in range(0, len(nodes)):
# if max(wTopo) - min(wTopo) == 0:
# normTopo = 0
# else:
# normTopo = 1 - (wTopo[i] - min(wTopo)) / (max(wTopo) - min(wTopo))
# if wDist[i] == max(wDist):
# nodes.remove(nodes[i])
# continue
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)
# Calculate Priority Weight
priorWeight = []
for i in range(0, len(nodes)):
pre = wDistNorm[i] / 0.5
#pre = 1
priorWeight.append(pre)
# Calculate smoothing factor
smoothFactor = []
for i in range(0, len(nodes)):
coeff = 1 - wDistNorm[i]
smoothFactor.append(coeff)
# Calculate costs
self.costs = []
for i in range(0, 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
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 rotateRobot(self):
velocityMsg = Twist()
angularSpeed = 0.3
relativeAngle = 2*math.pi
currentAngle = 0
rospy.loginfo("Roatating robot...")
velocityMsg.linear.x = 0
velocityMsg.linear.y = 0
velocityMsg.linear.z = 0
velocityMsg.angular.x = 0
velocityMsg.angular.y = 0
velocityMsg.angular.z = angularSpeed
t0 = rospy.Time.now().to_sec()
rospy.logwarn(rospy.get_caller_id() + ": Rotate Robot! Please wait...")
while currentAngle < relativeAngle:
self.velocityPub.publish(velocityMsg)
t1 = rospy.Time.now().to_sec()
currentAngle = angularSpeed * (t1 - t0)
velocityMsg.angular.z = 0
self.velocityPub.publish(velocityMsg)
rospy.logwarn(rospy.get_caller_id() + ": Robot Rotation OVER!")
return
def publish_markers(self, marker_pub, vis_nodes):
markers = MarkerArray()
c = 0
for n in vis_nodes:
c += 1
msg = Marker()
msg.header.frame_id = "map"
msg.ns = "lines"
msg.action = msg.ADD
msg.type = msg.CUBE
msg.id = c
#msg.scale.x = 1.0
#msg.scale.y = 1.0
#msg.scale.z = 1.0
# I guess I have to take into consideration resolution too
msg.pose.position.x = n[0]
msg.pose.position.y = n[1]
msg.pose.position.z = 0
msg.pose.orientation.x = 0.0
msg.pose.orientation.y = 0.0
msg.pose.orientation.z = 0.05
msg.pose.orientation.w = 0.05
msg.scale.x = 0.2
msg.scale.y = 0.2
msg.scale.z = 0.2
msg.color.a = 1.0 # Don't forget to set the alpha!
msg.color.r = 0.0
msg.color.g = 1.0
msg.color.b = 0.0
# print 'I publish msg now!!!'
markers.markers.append(msg)
marker_pub.publish(markers)
#
return
def calcDist(self, robotPose, brush):
distance_map = dict()
for goal in brush:
dist = math.hypot(goal[0] - robotPose['x'], goal[1] - robotPose['y'])
distance_map[goal] = dist
return distance_map
def filterGoal(self, brush2, ogm, resolution, origin):
throw = set()
for goal in brush2:
goal = list(goal)
for i in range(-9,10):
if int(goal[0]/resolution - origin['x']/resolution) + i >= len(ogm):
break
if ogm[int(goal[0]/resolution - origin['x']/resolution) + i]\
[int(goal[1]/resolution - origin['y']/resolution)] > 49 \
or ogm[int(goal[0]/resolution - origin['x']/resolution) + i]\
[int(goal[1]/resolution - origin['y']/resolution)] == -1:
goal = tuple(goal)
throw.add(goal)
break
for goal in brush2:
goal = list(goal)
for j in range(-9,10):
if int(goal[1]/resolution - origin['y']/resolution) + j >= len(ogm[0]):
break
if ogm[int(goal[0]/resolution - origin['x']/resolution)]\
[int(goal[1]/resolution - origin['y']/resolution) + j] > 49 \
or ogm[int(goal[0]/resolution - origin['x']/resolution) + i]\
[int(goal[1]/resolution - origin['y']/resolution)] == -1:
goal = tuple(goal)
throw.add(goal)
break
for goal in brush2:
goal = list(goal)
for i in range(-9,10):
if int(goal[0]/resolution - origin['x']/resolution) + i >= len(ogm) or \
int(goal[1]/resolution - origin['y']/resolution) + i >= len(ogm[0]):
break
if ogm[int(goal[0]/resolution - origin['x']/resolution) + i]\
[int(goal[1]/resolution - origin['y']/resolution) + i] > 49 \
or ogm[int(goal[0]/resolution - origin['x']/resolution) + i]\
[int(goal[1]/resolution - origin['y']/resolution) + i] == -1:
goal = tuple(goal)
throw.add(goal)
break
for goal in brush2:
goal = list(goal)
for i in range(-9, 10):
if int(goal[0]/resolution - origin['x']/resolution) + i >= len(ogm) or \
int(goal[1]/resolution - origin['y']/resolution) + i >= len(ogm[0]):
break
if ogm[int(goal[0]/resolution - origin['x']/resolution) + i]\
[int(goal[1]/resolution - origin['y']/resolution) - i] > 49 \
or ogm[int(goal[0]/resolution - origin['x']/resolution) + i]\
[int(goal[1]/resolution - origin['y']/resolution) - i] == -1:
goal = tuple(goal)
throw.add(goal)
break
return throw
class TargetSelection:
# Constructor
def __init__(self, selection_method):
self.initial_time = time()
self.method = selection_method
self.initialize_gains = False
self.brush = Brushfires()
self.topology = Topology()
self.path_planning = PathPlanning()
self.droneConnector = DroneCommunication()
# Parameters from YAML File
self.debug = True #rospy.get_param('debug')
self.map_discovery_purpose = rospy.get_param('map_discovery_purpose')
self.color_evaluation_flag = rospy.get_param('color_rating')
self.drone_color_evaluation_topic = rospy.get_param(
'drone_pub_color_rating')
self.evaluate_potential_targets_srv_name = rospy.get_param(
'rate_potential_targets_srv')
# Explore Gains
self.g_color = 0.0
self.g_brush = 0.0
self.g_corner = 0.0
self.g_distance = 0.0
self.set_gain()
if self.color_evaluation_flag:
# Color Evaluation Service
self.color_evaluation_service = rospy.ServiceProxy(
self.evaluate_potential_targets_srv_name, EvaluateTargets)
# Subscribe to Color Evaluation Topic to Get Results from Color Evaluation
self.drone_color_evaluation_sub = rospy.Subscriber(
self.drone_color_evaluation_topic, ColorEvaluationArray,
self.color_evaluation_cb)
# Parameters
self.targets_color_evaluated = False # Set True Once Color Evaluation of Targets Completed
self.color_evaluation = [] # Holds the Color Evaluation of Targets
self.corner_evaluation = [
] # Holds the Number of Corners Near Each Target
# Target Selection Function
def selectTarget(self,
init_ogm,
coverage,
robot_pose,
origin,
resolution,
g_robot_pose,
previous_limits=[],
force_random=False):
# Initialize Target
target = [-1, -1]
if self.running_time() > 15:
print('\x1b[37;1m' + str('15 Minutes Constraint Passed!!!') +
'\x1b[0m')
# Find only the useful boundaries of OGM
start = time()
ogm_limits = OgmOperations.findUsefulBoundaries(init_ogm,
origin,
resolution,
print_result=True,
step=20)
if self.debug:
print('\x1b[34;1m' + str('Target Selection: OGM Boundaries ') +
str(ogm_limits) + str(' in ') + str(time() - start) +
str(' seconds.') + '\x1b[0m')
# Blur the OGM to erase discontinuities due to laser rays
start = time()
ogm = OgmOperations.blurUnoccupiedOgm(init_ogm, ogm_limits)
if self.debug:
print('\x1b[34;1m' + str('Target Selection: OGM Blurred in ') +
str(time() - start) + str(' seconds.') + '\x1b[0m')
# Calculate Brushfire field
start = time()
brush = self.brush.obstaclesBrushfireCffi(ogm, ogm_limits)
if self.debug:
print('\x1b[34;1m' + str('Target Selection: Brush in ') +
str(time() - start) + str(' seconds.') + '\x1b[0m')
# Calculate Robot Position
[rx, ry] = [
robot_pose['x_px'] - origin['x'] / resolution,
robot_pose['y_px'] - origin['y'] / resolution
]
# Calculate Skeletonization
start = time()
skeleton = self.topology.skeletonizationCffi(ogm, origin, resolution,
ogm_limits)
if self.debug:
print('\x1b[34;1m' + str('Target Selection: Skeletonization in ') +
str(time() - start) + str(' seconds.') + '\x1b[0m')
# Find Topological Graph
start = time()
potential_targets = self.topology.topologicalNodes(
ogm,
skeleton,
coverage,
brush,
final_num_of_nodes=25,
erase_distance=100,
step=15)
if self.debug:
print('\x1b[34;1m' +
str('Target Selection: Topological Graph in ') +
str(time() - start) + str(' seconds.') + '\x1b[0m')
print('\x1b[34;1m' +
str("The Potential Targets to be Checked are ") +
str(len(potential_targets)) + '\x1b[0m')
if len(potential_targets) == 0:
print('\x1b[32;1m' +
str('\n------------------------------------------') +
str("Finished Space Exploration!!! ") +
str('------------------------------------------\n') +
'\x1b[0m')
sleep(10000)
# Visualization of Topological Graph
vis__potential_targets = []
for n in potential_targets:
vis__potential_targets.append([
n[0] * resolution + origin['x'],
n[1] * resolution + origin['y']
])
RvizHandler.printMarker(vis__potential_targets, 1, 0, "map",
"art_topological_nodes", [0.3, 0.4, 0.7, 0.5],
0.1)
# Check if we have given values to Gains
if not self.initialize_gains:
self.set_gain()
# Random Point Selection if Needed
if self.method == 'random' or force_random:
# Get Distance from Potential Targets
distance = np.zeros((len(potential_targets), 1), dtype=np.float32)
for idx, target in enumerate(potential_targets):
distance[idx] = hypot(rx - target[0], ry - target[1])
distance *= 255.0 / distance.max()
path = self.selectRandomTarget(ogm, coverage, brush, ogm_limits,
potential_targets, distance,
resolution, g_robot_pose)
if path is not None:
return path
else:
return []
# Sent Potential Targets for Color Evaluation (if Flag is Enable)
if self.color_evaluation_flag:
start_color = time()
while not self.sent_potential_targets_for_color_evaluation(
potential_targets, resolution, origin):
pass
# Initialize Arrays for Target Selection
id = np.array(range(0, len(potential_targets))).reshape(-1, 1)
brushfire = np.zeros((len(potential_targets), 1), dtype=np.float32)
distance = np.zeros((len(potential_targets), 1), dtype=np.float32)
color = np.zeros((len(potential_targets), 1), dtype=np.float32)
corners = np.zeros((len(potential_targets), 1), dtype=np.float32)
score = np.zeros((len(potential_targets), 1), dtype=np.float32)
# Calculate Distance and Brush Evaluation
start = time()
for idx, target in enumerate(potential_targets):
distance[idx] = hypot(rx - target[0], ry - target[1])
brushfire[idx] = brush[target[0], target[1]]
if self.debug:
print('\x1b[35;1m' +
str('Distance and Brush Evaluation Calculated in ') +
str(time() - start) + str(' seconds.') + '\x1b[0m')
# Wait for Color Evaluation to be Completed
if self.color_evaluation_flag:
while not self.targets_color_evaluated:
pass
color = np.array(self.color_evaluation).reshape(-1, 1)
corners = np.array(self.corner_evaluation,
dtype=np.float64).reshape(-1, 1)
# Reset Flag for Next Run
self.targets_color_evaluated = False
if self.debug:
print('\x1b[35;1m' + str('Color Evaluation Calculated in ') +
str(time() - start_color) + str(' seconds.') + '\x1b[0m')
# Normalize Evaluation Arrays to [0, 255]
distance *= 255.0 / distance.max()
brushfire *= 255.0 / brushfire.max()
if self.color_evaluation_flag:
# color max is 640*320 = 204800
color *= 255.0 / color.max()
color = 255.0 - color
corners *= 255.0 / corners.max()
# Calculate Score to Choose Best Target
# Final Array = [ Id. | [X] | [Y] | Color | Brush | Dist. | Num. of Corners | Score ]
# 0 1 2 3 4 5 6 7
# Max is: 255 + 255 - 0 - 0 = +510
# Min is: 0 + 0 - 255 - 255 = -510
evaluation = np.concatenate((id, potential_targets, color, brushfire,
distance, corners, score),
axis=1)
for e in evaluation:
# Choose Gains According to Type of Exploration (Default is Exploration)
if self.map_discovery_purpose == 'coverage':
e[7] = self.g_color * e[3] + self.g_brush * e[
4] - self.g_distance * e[5] - self.g_corner * e[6]
elif self.map_discovery_purpose == 'combined':
# Gains Change over Time
self.set_gain()
e[7] = self.g_color * e[3] + self.g_brush * e[
4] - self.g_distance * e[5] - self.g_corner * e[6]
else:
e[7] = self.g_color * e[3] + self.g_brush * e[
4] - self.g_distance * e[5] - self.g_corner * e[6]
# Normalize Score to [0, 255] and Sort According to Best Score (Increasingly)
evaluation[:, 7] = self.rescale(
evaluation[:,
7], (-self.g_distance * 255.0 - self.g_corner * 255.0),
(self.g_color * 255.0 + self.g_brush * 255.0), 0.0, 255.0)
evaluation = evaluation[evaluation[:, 7].argsort()]
# Best Target is in the Bottom of Evaluation Table Now
target = [
evaluation[[len(potential_targets) - 1], [1]],
evaluation[[len(potential_targets) - 1], [2]]
]
# Print The Score of the Target Selected
if len(previous_limits) != 0:
if not previous_limits['min_x'] < target[0] < previous_limits['max_x'] and not\
previous_limits['min_y'] < target[1] < previous_limits['max_y']:
print('\x1b[38;1m' +
str('Selected Target was inside Explored Area.') +
'\x1b[0m')
print('\x1b[35;1m' + str('Selected Target was ') +
str(int(evaluation.item(
(len(potential_targets) - 1), 0))) + str(' with score of ') +
str(evaluation.item(
(len(potential_targets) - 1), 7)) + str('.') + '\x1b[0m')
return self.path_planning.createPath(g_robot_pose, target, resolution)
# Send SIGNAL to Drone to Color Evaluate Potential Targets Function
def sent_potential_targets_for_color_evaluation(self, targets, resolution,
origin):
# Calculate Position of Targets in Map
targets = np.asarray(targets, dtype=np.float64)
for target in targets:
target[0] = target[0] * resolution + origin['x']
target[1] = target[1] * resolution + origin['y']
# Create Color Evaluation Request Message
color_evaluation_request_msg = EvaluateTargetsRequest()
color_evaluation_request_msg.pos_x = np.array(targets[:, 0])
color_evaluation_request_msg.pos_y = np.array(targets[:, 1])
# Call Service
rospy.wait_for_service(self.evaluate_potential_targets_srv_name)
try:
response = self.color_evaluation_service(
color_evaluation_request_msg)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
return response
class TargetSelection:
# Constructor
def __init__(self, selection_method):
self.goals_position = []
self.goals_value = []
self.omega = 0.0
self.radius = 0
self.method = selection_method
self.brush = Brushfires()
self.topo = Topology()
self.path_planning = PathPlanning()
def selectTarget(self, init_ogm, coverage, robot_pose, origin, \
resolution, force_random, tries):
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.
# CHALLENGE 6
# 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
)
# Robot Position
x = robot_pose['x_px'] + abs(origin['x'] / resolution)
y = robot_pose['y_px'] + abs(origin['y'] / resolution)
# Random point
if self.method == 'random' and force_random == True:
target = self.selectRandomTarget(ogm, coverage, brush, ogm_limits)
# CHALLENGE 6
# ADDED
elif (tries == 0):
target = self.MAXTargetSel(ogm, coverage, brush, ogm_limits, nodes,
x, y)
elif (tries == 1):
target = self.MINTargetSel(ogm, coverage, brush, ogm_limits, nodes,
x, y)
else:
target = self.selectRandomTarget(ogm, coverage, brush, ogm_limits)
# target = self.myTargetSelection(ogm, coverage, brush, ogm_limits, nodes, x, y)
########################################################################
return target
def selectRandomTarget(self, ogm, coverage, brushogm, ogm_limits):
# The next target in pixels
tinit = time.time()
next_target = [0, 0]
found = False
while not found:
x_rand = random.randint(0, ogm.shape[0] - 1)
y_rand = random.randint(0, ogm.shape[1] - 1)
if ogm[x_rand][y_rand] < 50 and coverage[x_rand][y_rand] < 50 and \
brushogm[x_rand][y_rand] > 5:
next_target = [x_rand, y_rand]
found = True
Print.art_print("Select random target time: " + str(time.time() - tinit), \
Print.ORANGE)
return next_target
def MAXTargetSel(self, ogm, coverage, brushogm, ogm_limits, nodes, x, y):
tinit = time.time()
next_target = [0, 0]
a = []
b = []
d = []
print(x)
print(y)
for n in nodes:
#The max distance topo node from robot
temp = (math.pow(x - n[0], 2) + math.pow(y - n[1], 2))
a.append(temp)
b.append(n[0])
d.append(n[1])
c = a.index(max(a))
next_target = [b[c], d[c]]
Print.art_print("Select MAX target time: " + str(time.time() - tinit), \
Print.ORANGE)
print(next_target)
return next_target
def MINTargetSel(self, ogm, coverage, brushogm, ogm_limits, nodes, x, y):
tinit = time.time()
next_target = [0, 0]
a = []
b = []
d = []
print(x)
print(y)
for n in nodes:
#The min distance topo node from robot
temp = -(math.pow(x - n[0], 2) + math.pow(y - n[1], 2))
a.append(temp)
b.append(n[0])
d.append(n[1])
c = a.index(max(a))
next_target = [b[c], d[c]]
Print.art_print("Select MIN target time: " + str(time.time() - tinit), \
Print.ORANGE)
print(next_target)
return next_target
class TargetSelection:
# Constructor
def __init__(self, selection_method):
self.goals_position = []
self.goals_value = []
self.omega = 0.0
self.radius = 0
self.method = selection_method
self.brush = Brushfires()
self.topo = Topology()
self.path_planning = PathPlanning()
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
)
# SMART TARGET SELECTION USING:
# 1. Brush-fire field
# 2. OGM Skeleton
# 3. Topological Nodes
# 4. Coverage field
# 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:
# Use brushfire to increase temp_score
temp_score = (brush[n[0]][n[1]] -
BRUSH_MIN) / (BRUSH_MAX - BRUSH_MIN) * BRUSH_WEIGHT
# 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[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)
########################################################################
return target
def selectRandomTarget(self, ogm, coverage, brushogm, ogm_limits):
# The next target in pixels
tinit = time.time()
next_target = [0, 0]
found = False
while not found:
x_rand = random.randint(0, ogm.shape[0] - 1)
y_rand = random.randint(0, ogm.shape[1] - 1)
if ogm[x_rand][y_rand] < 50 and coverage[x_rand][y_rand] < 50 and \
brushogm[x_rand][y_rand] > 5:
next_target = [x_rand, y_rand]
found = True
Print.art_print("Select random target time: " + str(time.time() - tinit), \
Print.ORANGE)
return next_target
class TargetSelection:
# Constructor
def __init__(self, selection_method):
self.goals_position = []
self.goals_value = []
self.path = []
self.prev_target = [0, 0]
self.omega = 0.0
self.radius = 0
self.method = selection_method
self.brush = Brushfires()
self.topo = Topology()
self.path_planning = PathPlanning()
self.robot_perception = RobotPerception() # can i use that?
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)
print len(nodes)
# 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)
# CTN
if self.method == 'CTN':
target = self.selectNearestTopologyNode(robot_pose=robot_pose,
resolution=resolution,
nodes=nodes)
if target is None:
target = self.selectRandomTarget(ogm, coverage, brush,
ogm_limits)
# target = self.selectNearestTopologyNode(robot_pose=robot_pose, resolution=resolution, nodes=nodes, ogm=ogm,
# coverage=coverage, brushogm=brush)
########################################################################
return target
def selectRandomTarget(self, ogm, coverage, brushogm, ogm_limits):
# The next target in pixels
tinit = time.time()
next_target = [0, 0]
found = False
while not found:
x_rand = random.randint(0, ogm.shape[0] - 1)
y_rand = random.randint(0, ogm.shape[1] - 1)
if ogm[x_rand][y_rand] < 50 and coverage[x_rand][y_rand] < 50 and \
brushogm[x_rand][y_rand] > 5:
next_target = [x_rand, y_rand]
found = True
Print.art_print(
"Select random target time: " + str(time.time() - tinit),
Print.ORANGE)
return next_target
# way too slow, maybe i didn't quite understood weigthed path find?
def selectNearestTopologyNode(self, robot_pose, resolution, nodes):
# The next target in pixels
tinit = time.time()
next_target = [0, 0]
w_dist = 10000
x_g, y_g = 500, 500
sigma = 100
g_robot_pose = self.robot_perception.getGlobalCoordinates(
[robot_pose['x_px'], robot_pose['y_px']]) # can I use that?
for node in nodes:
# print resolution
self.path = self.path_planning.createPath(
g_robot_pose, node, resolution) # can I use that?
if len(self.path) == 0:
break
x_n, y_n = node[0], node[1]
exp = ((x_n - x_g)**2 + (y_n - y_g)**2) / (2 * (sigma**2))
scale_factor = 1 / (1 - math.exp(-exp) + 0.01)
coords = zip(
*self.path) # dist is [[x1 x2 x3..],[y1 y2 y3 ..]] #transpose
coord_shift = np.empty_like(coords)
coord_shift[0] = coords[0][-1:] + coords[0][:-1] # (xpi+1)
coord_shift[1] = coords[1][-1:] + coords[1][:-1] # (ypi+1)
coords = np.asarray(coords)
dist = [
(a - b)**2 for a, b in zip(coords[:, 1:], coord_shift[:, 1:])
] # (xpi - xpi+1)^2 , (ypi-ypi+1)^2
dist = sum(np.sqrt(dist[0] + dist[1]))
w = dist * (1 / scale_factor)
if w < w_dist and len(self.path) != 0:
if self.prev_target == node:
break
w_dist = w
next_target = node
self.prev_target = next_target # dont select the same target it we failed already
Print.art_print(
"Select nearest node target time: " + str(time.time() - tinit),
Print.ORANGE)
return next_target