def callback_obst(centre_point):
# rospy.logwarn(len(diagram.walls))
# rospy.logwarn((centre_point.pose.position.x, centre_point.pose.position.y,
# centre_point.pose.position.z))
global obst_UGV1
obst_UGV1 = Obstacle(init.gridalize((centre_point.pose.position.x, centre_point.pose.position.y, centre_point.pose.position.z), scale), 'obst_UGV')
obst_UGV1.setSize(init.gridalize((1, 1, 2), scale))
callback_obst_flg = True
def callback_obst_person1(centre_point):
# rospy.logwarn(len(diagram.walls))
# rospy.logwarn((centre_point.pose.position.x, centre_point.pose.position.y,
# centre_point.pose.position.z))
global obstDict_rough
global obstDict_fine
global pathBlocked
global roughTrajectory
obstacle_rough = Obstacle(
init.gridalize(
(centre_point.pose.position.x, centre_point.pose.position.y,
centre_point.pose.position.z), scale_rough),
init.gridalize(1 * 1.2, scale_rough), init.gridalize(3, scale_rough),
mapBound_grid_rough[2], 'obst_person')
obstacle_fine = Obstacle(
init.gridalize(
(centre_point.pose.position.x, centre_point.pose.position.y,
centre_point.pose.position.z), scale_fine),
init.gridalize(1, scale_fine), init.gridalize(3, scale_fine),
mapBound_grid_fine[2], 'obst_person')
obstDict_rough['obst_person1'] = obstacle_rough
obstDict_fine['obst_person1'] = obstacle_fine
pathBlocked = checkifPathBlocked(obstDict_rough, roughTrajectory)
callback_obst_person1_flg = True
def callback_current(data): # data contains the current point
# rospy.loginfo((data.pose.position.x, data.pose.position.y, data.pose.position.z))
global current_position
global currentPoint
current_position = (data.pose.position.x, data.pose.position.y,
data.pose.position.z)
temptuple = tuple(init.gridalize(current_position, scale))
tempPoint = Point()
tempPoint.x = temptuple[0]
tempPoint.y = temptuple[1]
tempPoint.z = temptuple[2]
del currentPoint.points[:]
currentPoint.points.append(tempPoint)
currentPoint.pose.position.x = temptuple[0]
currentPoint.pose.position.y = temptuple[1]
currentPoint.pose.position.z = temptuple[2]
pointPub.publish(currentPoint)
# print 'currentPoint pos. 3', currentPoint.points[0].x, currentPoint.points[0].y, currentPoint.points[0].z
callback_current_flg = True
# try:
# Performance measurement
len_of_path = 0
execution_time = 0
vertex_expension = 0
# Initialization
scale = 4
rospy.init_node('astar_node', anonymous=True) # rosnode name
(pathPub, pointsPub, boundPub,
obstPub) = init.initPublishers() # initialize publishers
rospy.sleep(0.3) # it takes time to initialize publishers
rate = rospy.Rate(1) # loop runs at x Hertz
(length_of_map, width_of_map, height_of_map) = init.gridalize((5, 5, 3), scale)
current_point = tuple(init.gridalize((0, 0, 0), scale))
target_point = tuple(init.gridalize((5, 5, 3), scale))
diagram = GridWithWeights(length_of_map, width_of_map, height_of_map)
# diagram.walls = []
# initialize obstacles with position of their centre point
obst_UAV1 = Obstacle(init.gridalize((0.5, 2.5, 1), scale),
init.gridalize(0.25, scale), init.gridalize(2, scale),
'obst_UAV')
obst_UAV2 = Obstacle(init.gridalize((1.5, 0.5, 1.5), scale),
init.gridalize(0.25, scale), init.gridalize(2, scale),
'obst_UAV')
obst_UAV3 = Obstacle(init.gridalize((3.5, 4.5, 2), scale),
init.gridalize(0.25, scale), init.gridalize(2, scale),
def generateRefinedTrajectory(roughTrajectory):
print '-------------------------------------'
print 'Generating the refined path...'
global mapBound_grid_fine
global diagram_fine
global current_position
global scale_rough
global scale_fine
global obstDict_fine
global heap_percolation
global scaleRatio
global experiment_time
# initialize environment
startPoint = tuple(init.gridalize(current_position, scale_fine))
print
print 'startPoint: ', startPoint
# (roughTrajectory[i-2][0]*scaleRatio, roughTrajectory[i-2][1]*scaleRatio, roughTrajectory[i-2][2]*scaleRatio)
distsq_min = 10000000
closestPoint_id = 0
for i in range(len(roughTrajectory)):
distsq = (roughTrajectory[i][0] * scaleRatio - startPoint[0])**2 + (
roughTrajectory[i][1] * scaleRatio - startPoint[1])**2 + (
roughTrajectory[i][2] * scaleRatio - startPoint[2])**2
if distsq < distsq_min:
distsq_min = distsq
closestPoint_id = i
print 'closestPoint_id: ', closestPoint_id
print 'closestPoint: ', (roughTrajectory[closestPoint_id][0] * scaleRatio,
roughTrajectory[closestPoint_id][1] * scaleRatio,
roughTrajectory[closestPoint_id][2] * scaleRatio)
try:
endPoint = (roughTrajectory[closestPoint_id + 2][0] * scaleRatio,
roughTrajectory[closestPoint_id + 2][1] * scaleRatio,
roughTrajectory[closestPoint_id + 2][2] * scaleRatio)
except IndexError:
endPoint = (roughTrajectory[-1][0] * scaleRatio,
roughTrajectory[-1][1] * scaleRatio,
roughTrajectory[-1][2] * scaleRatio)
heap_percolation = 0
print 'endPoint: ', endPoint
boundMarker = visualization.setBoundary(mapBound_grid_fine)
obstMarkerArray = visualization.setObstacle3(obstDict_fine.values())
# Publish the boundary and obstacle
boundPub.publish(boundMarker)
obstPub.publish(obstMarkerArray)
for key in obstDict_fine:
if obstDict_fine[key].conflict(startPoint):
# set the startPoint to the nearest point outside the obstacle
try:
dist = math.sqrt(
(startPoint[0] - obstDict_fine[key].centre_point[0])**2 +
(startPoint[1] - obstDict_fine[key].centre_point[1])**2)
startPoint = tuple(
(int((startPoint[0] - obstDict_fine[key].centre_point[0]) *
(obstDict_fine[key].radius / dist) +
obstDict_fine[key].centre_point[0] + 1),
int((startPoint[1] - obstDict_fine[key].centre_point[1]) *
(obstDict_fine[key].radius / dist) +
obstDict_fine[key].centre_point[1] + 1),
startPoint[2]))
rospy.logfatal('changd startPoint!')
print startPoint
break
except ZeroDivisionError:
print 'startPoint x/y is ', startPoint[0], startPoint[1]
print 'obst centre point is ', obstDict_fine[key].centre_point[
0], obstDict_fine[key].centre_point[1]
print 'obst radius is ', obstDict_fine[key].radius
print 'dist is ', dist
rospy.signal_shutdown()
# if obstDict_fine[key].conflict(endPoint):
while obstDict_fine[key].conflict(endPoint):
closestPoint_id += 1
try:
endPoint = (roughTrajectory[closestPoint_id + 2][0] *
scaleRatio,
roughTrajectory[closestPoint_id + 2][1] *
scaleRatio,
roughTrajectory[closestPoint_id + 2][2] *
scaleRatio)
except IndexError:
endPoint = (roughTrajectory[-1][0] * scaleRatio,
roughTrajectory[-1][1] * scaleRatio,
roughTrajectory[-1][2] * scaleRatio)
# rospy.logfatal('Destination conflicts with obstacle!')
# print 'Centre of obstacle: ', obstDict_fine[key].centre_point
# print 'Radius of obstacle: ', obstDict_fine[key].radius
# rospy.signal_shutdown()
# Plan the path
print
print 'Planning path...'
start_time = timeit.default_timer()
came_from, cost_so_far = a_star_search(diagram_fine, obstDict_fine,
startPoint, endPoint, 1.5)
finalTrajectory = reconstruct_path(came_from,
start=startPoint,
goal=endPoint)
execution_time = timeit.default_timer() - start_time
f = open(experiment_time + '_UAV3_execT', 'a')
f.write('refinedPath\t' + str(execution_time) + '\n')
f.close()
# These four values are all visualization markers!
(sourcePoint, goalPoint, neighbourPoint,
finalPath) = visualization.setPathMarkers(finalTrajectory, came_from)
finalPath.text = '3'
# Publish the path
pointsPub.publish(sourcePoint)
pointsPub.publish(goalPoint)
pointsPub.publish(neighbourPoint)
pathPub.publish(finalPath)
refinedPub.publish(finalPath)
print 'Path sent.'
# Performance measurement
len_of_path = cost_so_far[endPoint]
vertex_expension = len(neighbourPoint.points)
# f = open('cost_so_far', 'w')
# f.write(str(cost_so_far))
# f.close()
print
print 'Length of path: ', float(len_of_path / scale_fine)
print 'Path planning execution time: ', execution_time
print 'Vetices expanded: ', vertex_expension
print 'Heap percolated: ', heap_percolation
f = open(experiment_time + '_UAV3_pathL', 'a')
f.write('refinedPath\t' + str(len_of_path / scale_fine) + '\n')
f.close()
f = open(experiment_time + '_UAV3_heapPerc', 'a')
f.write('refinedPath\t' + str(heap_percolation) + '\n')
f.close()
return finalTrajectory
def generateRoughTrajectory():
print '====================================='
print 'Generating the rough path...'
global mapBound_grid_rough
global diagram_rough
global current_position
global target_position
global scale_rough
global obstDict_rough
global heap_percolation
global experiment_time
# initialize environment
startPoint = tuple(init.gridalize(current_position, scale_rough))
endPoint = tuple(init.gridalize(target_position, scale_rough))
heap_percolation = 0
print 'startPoint: ', startPoint
print 'endPoint: ', endPoint
for key in obstDict_rough:
if obstDict_rough[key].conflict(startPoint):
# set the startPoint to the nearest point outside the obstacle
dist = math.sqrt(
(startPoint[0] - obstDict_rough[key].centre_point[0])**2 +
(startPoint[1] - obstDict_rough[key].centre_point[1])**2)
startPoint = tuple(
(int((startPoint[0] - obstDict_rough[key].centre_point[0]) *
(obstDict_rough[key].radius / dist) +
obstDict_rough[key].centre_point[0] + 1),
int((startPoint[1] - obstDict_rough[key].centre_point[1]) *
(obstDict_rough[key].radius / dist) +
obstDict_rough[key].centre_point[1] + 1), startPoint[2]))
print 'changd startPoint: ', startPoint
break
if obstDict_rough[key].conflict(endPoint):
rospy.logfatal('Destination conflicts with obstacle!')
print 'Centre of obstacle: ', obstDict_rough[key].centre_point
print 'Radius of obstacle: ', obstDict_rough[key].radius
rospy.signal_shutdown()
# Plan the path
start_time = timeit.default_timer()
came_from, cost_so_far = a_star_search(diagram_rough, obstDict_rough,
startPoint, endPoint, 1.1)
roughTrajectory = reconstruct_path(came_from,
start=startPoint,
goal=endPoint)
execution_time = timeit.default_timer() - start_time
f = open(experiment_time + '_UAV3_execT', 'a')
f.write('roughPath\t' + str(execution_time) + '\n')
f.close()
f = open(experiment_time + '_UAV3_rufPath', 'a')
f.write(time.strftime('%H:%M:%S') + '\n' + str(roughTrajectory) + '\n\n\n')
f.close()
len_of_path = cost_so_far[endPoint]
vertex_expension = len(came_from)
print
print 'Length of path: ', float(len_of_path / scale_rough)
print 'Path planning execution time: ', execution_time
print 'Vetices expanded: ', vertex_expension
print 'Heap percolated: ', heap_percolation
f = open(experiment_time + '_UAV3_pathL', 'a')
f.write('roughPath\t' + str(len_of_path / scale_rough) + '\n')
f.close()
f = open(experiment_time + '_UAV3_heapPerc', 'a')
f.write('roughPath\t' + str(heap_percolation) + '\n')
f.close()
return roughTrajectory
pathBlocked = True
callback_obst_UAV1_flg = True
callback_obst_UAV2_flg = True
callback_obst_UGV1_flg = False
callback_obst_person1_flg = False
callback_current_flg = True
callback_target_flg = True
''' Performance measurement '''
len_of_path = 0
execution_time = 0
vertex_expension = 0
heap_percolation = 0
experiment_time = time.strftime('%m-%d %H:%M')
''' Initialize grid map '''
# rough grid map
mapBound_grid_rough = init.gridalize(mapBound_metre, scale_rough)
diagram_rough = GridWithWeights(mapBound_grid_rough)
# fine grid map
mapBound_grid_fine = init.gridalize(mapBound_metre, scale_fine)
diagram_fine = GridWithWeights(mapBound_grid_fine)
''' Initialize obstacle dictionary '''
obstDict_rough = {}
obstDict_fine = {}
roughTrajectory = []
''' Callback functions '''
# receive obstacle position
while callback_obst_UAV1_flg:
obstSub = rospy.Subscriber('/UAV_2/pose', PoseStamped, callback_obst_UAV1)
callback_obst_UAV1_flg = False
while callback_obst_UAV2_flg:
obstSub = rospy.Subscriber('/UAV_1/pose', PoseStamped, callback_obst_UAV2)
callback_obst_UAV1_flg = True
callback_obst_UGV1_flg = True
callback_obst_person1_flg = True
callback_current_flg = True
# callback_target_flg = True
''' Performance measurement '''
len_of_path = 0
execution_time = 0
vertex_expension = 0
experiment_time = time.strftime('%m-%d %H:%M')
''' Intialize markers '''
currentPoint = init.initPointMarkers()
currentPoint.color.r = 1.0
currentPoint.color.g = 1.0
currentPoint.id = 3
temptuple = tuple(init.gridalize(current_position, scale))
tempPoint = Point()
tempPoint.x = temptuple[0]
tempPoint.y = temptuple[1]
tempPoint.z = temptuple[2]
currentPoint.points.append(tempPoint)
currentPoint.pose.position.x = temptuple[0]
currentPoint.pose.position.y = temptuple[1]
currentPoint.pose.position.z = temptuple[2]
pathPoint = init.initPathMarkers()
knownObstPoint = init.initObstMarkers()
knownObstPoint.color.r = 0.8
knownObstPoint.color.g = 0.4
knownObstPoint.id = 31
# try:
# Performance measurement
len_of_path = 0
execution_time = 0
vertex_expension = 0
# Initialization
scale = 10
rospy.init_node('astar_node', anonymous=True) # rosnode name
(pathPub, obstPub, pointsPub) = init.initPublishers() # initialize publishers
rospy.sleep(0.3) # it takes time to initialize publishers
rate = rospy.Rate(1) # loop runs at x Hertz
(length_of_map, width_of_map, height_of_map) = init.gridalize((4, 4, 2.5), scale)
current_point = tuple(init.gridalize((0.5, 0.5, 1), scale))
target_point = tuple(init.gridalize((3, 3, 1.5), scale))
diagram = GridWithWeights(length_of_map, width_of_map, height_of_map)
# diagram.walls = []
# initialize obstacles with position of their centre point
# obst_UAV1 = Obstacle(init.gridalize((0.5, 2.5, 1), scale), 'obst_UAV')
# obst_UAV2 = Obstacle(init.gridalize((1.5, 0.5, 1.5), scale), 'obst_UAV')
# obst_UAV3 = Obstacle(init.gridalize((3.5, 4.5, 2), scale), 'obst_UAV')
obst_UGV1 = Obstacle(init.gridalize((1.5, 4.5, 2), scale), 'obst_UGV')
# obst_UGV2 = Obstacle(init.gridalize((4.5, 2.5, 2), scale), 'obst_UGV')
# obst_person1 = Obstacle(init.gridalize((3, 2.5, 0), scale), 'obst_person')
# obst_UAV1 = Obstacle(init.gridalize((random.uniform(0.25,1.75), random.uniform(2.25,3.75), random.uniform(1,3)), scale), 'obst_UAV')
# obst_UAV2 = Obstacle(init.gridalize((random.uniform(0.25,1.75), random.uniform(0.25,1.75), random.uniform(1,3)), scale), 'obst_UAV')
