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')