From 04b5c733558314f8e13ed1524004040fd62f69f0 Mon Sep 17 00:00:00 2001 From: ChenXL97 <908926798@qq.com> Date: Mon, 13 Nov 2023 21:52:15 +0800 Subject: [PATCH] =?UTF-8?q?=E6=9B=B4=E6=96=B0=E4=BA=86=E5=AF=BC=E8=88=AA?= =?UTF-8?q?=E7=AE=97=E6=B3=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../algos/navigate/DstarLite/__init__.py | 1 - .../navigate/DstarLite/discretize_map.py | 59 +++++++++++++ .../algos/navigate/DstarLite/dstar_lite.py | 41 +++++----- .../algos/navigate/DstarLite/navigate.py | 82 ++++++++++--------- robowaiter/algos/navigate/DstarLite/readme.md | 56 ++++++++++++- robowaiter/algos/navigate/DstarLite/test.py | 29 ++++--- robowaiter/behavior_lib/act/Make.py | 1 + 7 files changed, 195 insertions(+), 74 deletions(-) create mode 100644 robowaiter/algos/navigate/DstarLite/discretize_map.py diff --git a/robowaiter/algos/navigate/DstarLite/__init__.py b/robowaiter/algos/navigate/DstarLite/__init__.py index 1ceb32a..2802065 100644 --- a/robowaiter/algos/navigate/DstarLite/__init__.py +++ b/robowaiter/algos/navigate/DstarLite/__init__.py @@ -1,4 +1,3 @@ - from . import navigate from . import dstar_lite diff --git a/robowaiter/algos/navigate/DstarLite/discretize_map.py b/robowaiter/algos/navigate/DstarLite/discretize_map.py new file mode 100644 index 0000000..f16dd92 --- /dev/null +++ b/robowaiter/algos/navigate/DstarLite/discretize_map.py @@ -0,0 +1,59 @@ +# !/usr/bin/env python3 +# -*- encoding: utf-8 -*- + + +import matplotlib.pyplot as plt +import numpy as np +import pickle +import os + + + + + +def draw_grid_map(grid_map): + # 生成新的地图图像 + plt.imshow(grid_map, cmap='binary', alpha=0.5, origin='lower') # 黑白网格 + + # 绘制坐标轴 + plt.xlabel('y', loc='right') + plt.ylabel('x', loc='top') + + # 显示网格线 + plt.grid(color='black', linestyle='-', linewidth=0.5) + + # 显示图像 + plt.show() + #plt.pause(0.01) + + + + +if __name__ == '__main__': + # control.init_world(scene_num=1, mapID=3) + # scene = control.Scene(sceneID=0) + # + # X = int(950/5) # 采点数量 + # Y = int(1850/5) + # map = np.zeros((X, Y)) + # + # for x in range(X): + # for y in range(Y): + # if not scene.reachable_check(x*5-350, y*5-400, Yaw=0): + # map[x, y] = 1 + # print(x, y) + # + # + # file_name = 'map_5.pkl' + # if not os.path.exists(file_name): + # open(file_name, 'w').close() + # with open(file_name, 'wb') as file: + # pickle.dump(map, file) + # print('保存成功') + + + file_name = 'map_5.pkl' + if os.path.exists(file_name): + with open(file_name, 'rb') as file: + map = pickle.load(file) + draw_grid_map(map) \ No newline at end of file diff --git a/robowaiter/algos/navigate/DstarLite/dstar_lite.py b/robowaiter/algos/navigate/DstarLite/dstar_lite.py index 6a83108..e685a58 100644 --- a/robowaiter/algos/navigate/DstarLite/dstar_lite.py +++ b/robowaiter/algos/navigate/DstarLite/dstar_lite.py @@ -23,7 +23,8 @@ def manhattan_distance(start, end): # 曼哈顿距离 def euclidean_distance(start, end): # 欧式距离 # return np.linalg.norm(start-end) - return math.sqrt((start[0] - end[0]) ** 2 + (start[1] - end[1]) ** 2) + # return math.sqrt((start[0] - end[0]) ** 2 + (start[1] - end[1]) ** 2) + return math.hypot(start[0] - end[0], start[1] - end[1]) def heuristic(start, end, name='euclidean'): @@ -115,9 +116,9 @@ class PriorityQueue: class DStarLite: def __init__(self, map: np.array([int, int]), # [X, Y] - area_range, # [x_min, x_max, y_min, y_max] 实际坐标范围 - scale_ratio=5, # 地图缩放率 - dyna_obs_radius=20 # dyna_obs实际身位半径 + area_range, # [x_min, x_max, y_min, y_max] 实际坐标范围 + scale_ratio=5, # 地图缩放率 + dyna_obs_radius=30, # dyna_obs实际身位半径 ): # self.area_bounds = area @@ -128,7 +129,7 @@ class DStarLite: (self.x_min, self.x_max, self.y_min, self.y_max) = area_range self.scale_ratio = scale_ratio self.dyna_obs_list = [] # 动态障碍物位置列表( 当前地图 ) [(x, y)] - self.dyna_obs_radius = math.ceil(dyna_obs_radius/scale_ratio) # dyna_obs缩放后身位半径 + self.dyna_obs_radius = math.ceil(dyna_obs_radius / scale_ratio) # dyna_obs缩放后身位半径 # free:0, obs:1, dyna_obs:2 self.idx_to_object = { @@ -140,7 +141,7 @@ class DStarLite: self.object_to_cost = { "free": 0, "obstacle": float('inf'), - "dynamic obstacle": 50 + "dynamic obstacle": 100 } self.compute_cost_map() @@ -259,7 +260,7 @@ class DStarLite: self.rhs[s] = min([self.c(s, s_) + self.g[s_] for s_ in succ]) self.update_vertex(s) - def _planning(self, s_start, s_goal, dyna_obs, step_num=None, debug=False): + def _planning(self, s_start, s_goal, dyna_obs, debug=False): ''' 规划路径(实际实现) Args: @@ -268,7 +269,7 @@ class DStarLite: ''' # 确保目标合法 if not self.in_bounds_without_obstacle(s_goal): - return None + return [] # 第一次规划需要初始化rhs并将goal加入队列，计算最短路径 if self.s_goal is None: self.s_start = tuple(s_start) @@ -281,7 +282,6 @@ class DStarLite: # 后续规划只更新起点，直接使用原路径(去掉已走过部分) else: self.s_start = tuple(s_start) - self.path = self.path[step_num:] # 根据obs更新map, cost_map, edge_cost changed_pos = self.update_map(dyna_obs=dyna_obs) if changed_pos: @@ -299,17 +299,16 @@ class DStarLite: # pass return self.path - def planning(self, s_start, s_goal, dyna_obs, step_num=None, debug=False): + def planning(self, s_start, s_goal, dyna_obs, debug=False): ''' 路径规划(供外部调用，处理实际坐标和地图坐标的转换) ''' # 实际坐标 -> 地图坐标 s_start = self.real2map(s_start) s_goal = self.real2map(s_goal) - for i in range(len(dyna_obs)): - dyna_obs[i] = self.real2map(dyna_obs[i]) + dyna_obs = [self.real2map(obs) for obs in dyna_obs] - self._planning(s_start, s_goal, dyna_obs, step_num, debug) + self._planning(s_start, s_goal, dyna_obs, debug) # 地图坐标->实际坐标 path = [self.map2real(node) for node in self.path] @@ -319,7 +318,7 @@ class DStarLite: ''' 得到路径 Args: - step_num: 路径步数 (None表示返回完整路径) + step_num: 路径步数 return: path: [(x, y), ...] ''' @@ -327,7 +326,6 @@ class DStarLite: return [] path = [] cur = self.s_start - # if step_num is None: # 得到完整路径 while cur != self.s_goal: succ = self.get_neighbors(cur) cur = succ[np.argmin([self.c(cur, s_) + self.g[s_] for s_ in succ])] @@ -411,10 +409,13 @@ class DStarLite: 根据dyna_obs中心位置，计算其占用的所有网格位置 ''' (x, y) = obs_pos - occupy_pos = [] - for i in range(x-self.dyna_obs_radius, x+self.dyna_obs_radius+1): - for j in range(y-self.dyna_obs_radius, y+self.dyna_obs_radius+1): - occupy_pos.append((i, j)) + # for i in range(x - self.dyna_obs_radius, x + self.dyna_obs_radius + 1): + # for j in range(y - self.dyna_obs_radius, y + self.dyna_obs_radius + 1): + # occupy_pos.append((i, j)) + occupy_pos = [(i, j) for i in range(x - self.dyna_obs_radius, x + self.dyna_obs_radius + 1) + for j in range(y - self.dyna_obs_radius, y + self.dyna_obs_radius + 1) + if euclidean_distance((i, j), obs_pos) < self.dyna_obs_radius] + occupy_pos = filter(self.in_bounds_without_obstacle, occupy_pos) # 确保位置在地图范围内 且 不是静态障碍物 return list(occupy_pos) @@ -507,4 +508,4 @@ class DStarLite: plt.xlabel('y', loc='right') plt.ylabel('x', loc='top') plt.grid(color='black', linestyle='-', linewidth=0.5) - plt.pause(0.3) + plt.pause(0.2) diff --git a/robowaiter/algos/navigate/DstarLite/navigate.py b/robowaiter/algos/navigate/DstarLite/navigate.py index 79cdd70..89597ce 100644 --- a/robowaiter/algos/navigate/DstarLite/navigate.py +++ b/robowaiter/algos/navigate/DstarLite/navigate.py @@ -1,71 +1,80 @@ #!/usr/bin/env python3 # -*- encoding: utf-8 -*- import math -import sys -import time +from dstar_lite import DStarLite, euclidean_distance -import matplotlib.pyplot as plt -import numpy as np -from mpl_toolkits.axes_grid1 import make_axes_locatable - - -from robowaiter.algos.navigate.DstarLite.dstar_lite import DStarLite class Navigator: ''' 导航类 ''' - def __init__(self, scene, area_range, map, scale_ratio=5): + def __init__(self, scene, area_range, map, scale_ratio=5, step_length=150, velocity=150, react_radius=250): self.scene = scene - self.area_range = area_range # 地图实际坐标范围 xmin, xmax, ymin, ymax - self.map = map # 缩放并离散化的地图 array(X,Y) - self.scale_ratio = scale_ratio # 地图缩放率 - self.step_length = 50 # 步长(单次移动) + self.area_range = area_range # 地图实际坐标范围 xmin, xmax, ymin, ymax + self.map = map # 缩放并离散化的地图 array(X,Y) + self.scale_ratio = scale_ratio # 地图缩放率 + self.step_length = step_length # 步长(单次移动) self.step_num = self.step_length // self.scale_ratio # 单次移动地图格数 - self.v = 200 # 速度 - self.step_time = self.step_length/self.v + 0.1 # 单步移动时长 - + self.v = velocity # 速度 + self.react_radius = react_radius # robot反应半径 self.planner = DStarLite(area_range=area_range, map=map, scale_ratio=scale_ratio) - @staticmethod - def is_reached(pos: np.array((float, float)), goal: np.array((float, float)), dis_limit=25): + def is_reached(pos: (float, float), goal: (float, float), dis_limit=50): ''' 判断是否到达目标 ''' - dis = np.linalg.norm(pos - goal) + dis = math.hypot(pos[0]-goal[0], pos[1]-goal[1]) + # dis = np.linalg.norm(pos - goal) return dis < dis_limit - def reset_goal(self, goal:(float, float)): - # TODO: 使目标可达 - # 目标在障碍物上：从目标开始方形向外扩展，直到找到可行点 - # 目标在地图外面：起点和目标连线最靠近目标的可行点 - pass + @staticmethod + def get_yaw(pos: (float, float), goal: (float, float)): + ''' + 得到移动方向 + ''' + return math.degrees(math.atan2(goal[0] - pos[0], -(goal[1] - pos[1]))) + + def legalize_goal(self, goal: (float, float)): + ''' + TODO: 处理非法目标 + 目标在障碍物上：从目标开始方形向外扩展，直到找到可行点 + 目标在地图外面：起点和目标连线最靠近目标的可行点 + ''' + return goal def navigate(self, goal: (float, float), animation=True): ''' 单次导航，直到到达目标 ''' + if not self.scene.reachable_check(goal[0], goal[1], 0): + goal = self.legalize_goal(goal) - pos = np.array((self.scene.status.location.X, self.scene.status.location.Y)) # 机器人当前: 位置 和 朝向 - yaw = self.scene.status.rotation.Yaw + pos = (self.scene.status.location.X, self.scene.status.location.Y) # 机器人当前: 位置 和 朝向 print('------------------navigation_start----------------------') while not self.is_reached(pos, goal): dyna_obs = [(walker.pose.X, walker.pose.Y) for walker in self.scene.status.walkers] # 动态障碍物(顾客)位置列表 - path = self.planner.planning(pos, goal, dyna_obs, step_num=self.step_num) + dyna_obs = [obs for obs in dyna_obs if euclidean_distance(obs, pos) < self.react_radius] # 过滤观测范围外的dyna_obs + # 周围有dyna_obs则步长减半 + if dyna_obs: + step_num = self.step_num // 2 + else: + step_num = self.step_num + path = self.planner.planning(pos, goal, dyna_obs) if path: - next_step = min(self.step_num, len(path)) - (next_x, next_y) = path[next_step-1] - print('plan pos:', (next_x, next_y), end=' ') - scene_info = self.scene.walk_to(next_x, next_y, yaw, velocity=self.v) - yaw = scene_info.rotation.Yaw - if animation: - self.planner.draw_graph(self.step_num) # 画出搜索路径 - # time.sleep(self.step_time) - pos = np.array((self.scene.status.location.X, self.scene.status.location.Y)) + self.planner.draw_graph(step_num) # 画出搜索路径 + next_step = min(step_num, len(path)) + next_pos = path[next_step - 1] + print('plan pos:', next_pos, end=' ') + yaw = self.get_yaw(pos, next_pos) + self.scene.walk_to(next_pos[0], next_pos[1], yaw, velocity=self.v, dis_limit=10) + # pos = (self.scene.status.location.X, self.scene.status.location.Y) + # if self.is_reached(pos, next_pos): + self.planner.path = self.planner.path[next_step - 1:] # 去除已走过的路径 + pos = (self.scene.status.location.X, self.scene.status.location.Y) print('reach pos:', pos) self.planner.reset() # 完成一轮导航，重置变量 @@ -76,4 +85,3 @@ class Navigator: - diff --git a/robowaiter/algos/navigate/DstarLite/readme.md b/robowaiter/algos/navigate/DstarLite/readme.md index 3a77674..585e602 100644 --- a/robowaiter/algos/navigate/DstarLite/readme.md +++ b/robowaiter/algos/navigate/DstarLite/readme.md @@ -1,4 +1,52 @@ -### dstar_lite.py ——Dstar lite算法文件 -### navigate.py ——导航类 -### test.py ——测试文件 -### map_5.pkl ——离散化地图文件 \ No newline at end of file +## 默认使用RRTStar+路径平滑 + +### apf.py: 势场法实现 + +### discretize_map.py: 地图离散化并压缩 + +### map_5.pkl: 地图文件(5倍压缩) + +### navigate.py: 导航类 + +### pathsmoothing.py: 路径平滑 + +### rrt.py: RRT实现 + +### rrt_star.py: RRTStar 实现 + +### test.py: 测试文件 + + + + +## TODO + +### 目标不合法 +#### 初始目标不合法 +目标在障碍物上：从目标开始方形向外扩展，直到找到可行点 +目标在地图外面：起点和目标连线最靠近目标的可行点 +对不合法的目标做单独处理，生成新的目标 + +#### 在移动过程中目标被占据 +给出目标但不会行移动，程序会继续运行，重新计算规划路径，给出新目标 + + +### 规划中断情况 + + +### 计算转向角 +`完成` + + +### 有些本来可达的位置却无法走到（系统bug?） +例如从初始位置(247, 500) 移动到(115, -10)，无法到达 +`(已解决)将dis_limit设为小值5而非0` +#### 构型空间膨胀？？ +`不需要` + +### 只考虑一定范围内的行人 +观测范围 / 反应半径 + +`完成` +#### 观测范围内有行人步长要减小 +`完成` diff --git a/robowaiter/algos/navigate/DstarLite/test.py b/robowaiter/algos/navigate/DstarLite/test.py index 2f0e4ed..525ad10 100644 --- a/robowaiter/algos/navigate/DstarLite/test.py +++ b/robowaiter/algos/navigate/DstarLite/test.py @@ -2,11 +2,12 @@ import os import pickle import time import random +import math import matplotlib.pyplot as plt import numpy as np -from robowaiter.scene.scene import Scene,init_world # TODO: 文件名改成Scene.py +from robowaiter.scene import scene from navigate import Navigator @@ -19,14 +20,13 @@ if __name__ == '__main__': with open(file_name, 'rb') as file: map = pickle.load(file) - init_world(1, 11) - scene = Scene(sceneID=0) + scene.init_world(1, 11) + scene = scene.Scene(sceneID=0) - navigator = Navigator(scene=scene, area_range=[-350, 600, -400, 1450], map=map) + navigator = Navigator(scene=scene, area_range=[-350, 600, -400, 1450], map=map) '''场景1: 无行人环境 robot到达指定目标''' - goal = np.array((-100, 700)) - + # goal = np.array((-100, 700)) '''场景2: 静止行人环境 robot到达指定目标''' # scene.clean_walker() @@ -35,12 +35,17 @@ if __name__ == '__main__': # goal = np.array((-100, 700)) '''场景3: 移动行人环境 robot到达指定目标''' - # scene.clean_walker() - # scene.add_walker(50, 300, 0) - # scene.add_walker(-50, 500, 0) - # scene.control_walker([scene.walker_control_generator(walkerID=0, autowalk=False, speed=20, X=-50, Y=600, Yaw=0)]) - # scene.control_walker([scene.walker_control_generator(walkerID=1, autowalk=False, speed=20, X=100, Y=150, Yaw=0)]) - # goal = np.array((-100, 700)) + scene.walk_to(100, 0, -90, dis_limit=10) + scene.clean_walker() + scene.add_walker(50, 300, 0) + scene.add_walker(-50, 500, 0) + scene.add_walker(0, 700, 0) + scene.control_walker([scene.walker_control_generator(walkerID=0, autowalk=False, speed=50, X=-50, Y=600, Yaw=0)]) + scene.control_walker([scene.walker_control_generator(walkerID=1, autowalk=False, speed=50, X=100, Y=150, Yaw=0)]) + scene.control_walker([scene.walker_control_generator(walkerID=2, autowalk=False, speed=50, X=0, Y=0, Yaw=0)]) + + goal = (-100, 700) + # goal = (-300) '''场景4: 行人自由移动 robot到达指定目标''' # # TODO: autowalk=True仿真器会闪退 ??? diff --git a/robowaiter/behavior_lib/act/Make.py b/robowaiter/behavior_lib/act/Make.py index 8388cc2..3c8fac3 100644 --- a/robowaiter/behavior_lib/act/Make.py +++ b/robowaiter/behavior_lib/act/Make.py @@ -23,6 +23,7 @@ class Make(Act): "add": {f'On(Coffee,Table)'}, } return info + def _update(self) -> ptree.common.Status: op_type = 1 self.scene.move_task_area(op_type)