92 lines
3.4 KiB
Python
92 lines
3.4 KiB
Python
import math
|
||
import sys
|
||
|
||
import numpy as np
|
||
|
||
|
||
# from rrt import RRT
|
||
# from rrt_star import RRTStar
|
||
|
||
|
||
# def real2map(min_x, min_y, scale_ratio, x, y):
|
||
# '''
|
||
# 实际坐标->地图坐标 (向下取整)
|
||
# '''
|
||
# # x = round((x - self.min_x) / self.scale_ratio)
|
||
# # y = round((y - self.min_y) / self.scale_ratio)
|
||
# x = math.floor((x - min_x) / scale_ratio)
|
||
# y = math.floor((y - min_y) / scale_ratio)
|
||
# return x, y
|
||
|
||
|
||
def getDistance(pos1, pos2):
|
||
return math.sqrt((pos1[0] - pos2[0]) ** 2 + (pos1[1] - pos2[1]) ** 2)
|
||
|
||
|
||
def getNearestFrontier(cur_pos, frontiers):
|
||
dis_min = sys.maxsize
|
||
frontier_best = None
|
||
for frontier in frontiers:
|
||
dis = getDistance(frontier, cur_pos)
|
||
if dis <= dis_min:
|
||
dis_min = dis
|
||
frontier_best = frontier
|
||
return frontier_best
|
||
|
||
|
||
class Explore:
|
||
def __init__(self, _explore_range):
|
||
self.explore_range = _explore_range # 机器人感知范围（以机器人为中心，边长为2 * _explore_range的正方形）
|
||
self.visited = set()
|
||
self.all_frontier_list = set()
|
||
|
||
def explore(self, cur_pos):
|
||
for i in range(cur_pos[0] - self.explore_range, cur_pos[0] + self.explore_range + 1):
|
||
for j in range(cur_pos[1] - self.explore_range, cur_pos[1] + self.explore_range + 1):
|
||
# if self.isOutMap((i, j)):
|
||
# continue
|
||
# x, y = real2map(self.area_range[0], self.area_range[2], self.scale_ratio, i, j)
|
||
# if self.map[x, y] == 0:
|
||
if self.reachable((i, j)):
|
||
self.visited.add((i, j))
|
||
for i in range(cur_pos[0] - self.explore_range, cur_pos[0] + self.explore_range + 1):
|
||
for j in range(cur_pos[1] - self.explore_range, cur_pos[1] + self.explore_range + 1):
|
||
# if self.isOutMap((i, j)):
|
||
# continue
|
||
# x, y = real2map(self.area_range[0], self.area_range[2], self.scale_ratio, i, j)
|
||
if self.reachable((i, j)):
|
||
if self.isNewFrontier((i, j)):
|
||
self.all_frontier_list.add((i, j))
|
||
if len(self.all_frontier_list) == 0:
|
||
free_list = list(self.visited)
|
||
free_array = np.array(free_list)
|
||
print(f"探索完成！以下是场景中可以到达的点：{free_array}；其余点均是障碍物不可达")
|
||
return None
|
||
return getNearestFrontier(cur_pos, self.all_frontier_list)
|
||
|
||
def isNewFrontier(self, pos):
|
||
around_nodes = [(pos[0], pos[1] + 1), (pos[0], pos[1] - 1), (pos[0] - 1, pos[1]), (pos[0] + 1, pos[1])]
|
||
|
||
for node in around_nodes:
|
||
if node not in self.visited and self.reachable(node):
|
||
return True
|
||
if (pos[0], pos[1]) in self.all_frontier_list:
|
||
self.all_frontier_list.remove((pos[0], pos[1]))
|
||
return False
|
||
|
||
def reachable(self, pos):
|
||
# x, y = real2map(self.area_range[0], self.area_range[2], self.scale_ratio, pos[0], pos[1])
|
||
# if self.map[x, y] == 1:
|
||
# return True
|
||
# else:
|
||
# return False
|
||
|
||
# TODO: 调用reachable_check函数
|
||
pass
|
||
|
||
# def isOutMap(self, pos):
|
||
# if pos[0] <= self.area_range[0] or pos[0] >= self.area_range[1] or pos[1] <= self.area_range[2] or pos[1] >= \
|
||
# self.area_range[3]:
|
||
# return True
|
||
# return False
|