更新了导航算法
This commit is contained in:
ChenXL97
parent
d5f8c289bf
commit
04b5c73355
|
|
@ -1,4 +1,3 @@
|
|||
|
||||
from . import navigate
|
||||
from . import dstar_lite
|
||||
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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:
|
|||
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -1,4 +1,52 @@
|
|||
### dstar_lite.py ——Dstar lite算法文件
|
||||
### navigate.py ——导航类
|
||||
### test.py ——测试文件
|
||||
### map_5.pkl ——离散化地图文件
|
||||
## 默认使用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`
|
||||
#### 构型空间膨胀？？
|
||||
`不需要`
|
||||
|
||||
### 只考虑一定范围内的行人
|
||||
观测范围 / 反应半径
|
||||
|
||||
`完成`
|
||||
#### 观测范围内有行人步长要减小
|
||||
`完成`
|
||||
|
|
|
|||
|
|
@ -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仿真器会闪退 ???
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
Loading…
Reference in New Issue