更新了导航算法

robowaiter/algos/navigate/DstarLite/__init__.py

@@ -1,4 +1,3 @@
-
 from . import navigate
 from . import dstar_lite
 

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)

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'):
@@ -117,7 +118,7 @@ class DStarLite:
                  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实际身位半径
+                 dyna_obs_radius=30,  # dyna_obs实际身位半径
                  ):
 
         # self.area_bounds = area
@@ -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 = [self.real2map(obs) for obs in dyna_obs]
-            dyna_obs[i] = self.real2map(dyna_obs[i])
 
-        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 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):
-            for j in range(y-self.dyna_obs_radius, y+self.dyna_obs_radius+1):
+        #         occupy_pos.append((i, j))
-                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)

robowaiter/algos/navigate/DstarLite/navigate.py

@@ -1,71 +1,80 @@
 #!/usr/bin/env python3
 # -*- encoding: utf-8 -*-
 import math
-import sys
+from dstar_lite import DStarLite, euclidean_distance
-import time
 
-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.step_length = step_length  # 步长(单次移动)
         self.step_num = self.step_length // self.scale_ratio  # 单次移动地图格数
-        self.v = 200                                          # 速度
+        self.v = velocity  # 速度
-        self.step_time = self.step_length/self.v + 0.1        # 单步移动时长
+        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)):
+    @staticmethod
-        # TODO: 使目标可达
+    def get_yaw(pos: (float, float), goal: (float, float)):
-        # 目标在障碍物上：从目标开始方形向外扩展，直到找到可行点
+        '''
-        # 目标在地图外面：起点和目标连线最靠近目标的可行点
+            得到移动方向
-        pass
+        '''
+        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))  # 机器人当前: 位置 和 朝向
+        pos = (self.scene.status.location.X, self.scene.status.location.Y)  # 机器人当前: 位置 和 朝向
-        yaw = self.scene.status.rotation.Yaw
         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)  # 画出搜索路径
+                    self.planner.draw_graph(step_num)  # 画出搜索路径
-                # time.sleep(self.step_time)
+                next_step = min(step_num, len(path))
-            pos = np.array((self.scene.status.location.X, self.scene.status.location.Y))
+                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:
 
 
 
-

robowaiter/algos/navigate/DstarLite/readme.md

@@ -1,4 +1,52 @@
-### dstar_lite.py ——Dstar lite算法文件 
+## 默认使用RRTStar+路径平滑
-### navigate.py ——导航类
+
-### test.py ——测试文件
+### apf.py: 势场法实现
-### map_5.pkl ——离散化地图文件
+
+### 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`
+####  构型空间膨胀？？
+`不需要`
+
+### 只考虑一定范围内的行人
+观测范围 / 反应半径
+
+`完成`
+#### 观测范围内有行人步长要减小
+`完成`

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.init_world(1, 11)
-    scene = Scene(sceneID=0)
+    scene = scene.Scene(sceneID=0)
 
     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.walk_to(100, 0, -90, dis_limit=10)
-    # scene.add_walker(50, 300, 0)
+    scene.clean_walker()
-    # scene.add_walker(-50, 500, 0)
+    scene.add_walker(50, 300, 0)
-    # scene.control_walker([scene.walker_control_generator(walkerID=0, autowalk=False, speed=20, X=-50, Y=600, Yaw=0)])
+    scene.add_walker(-50, 500, 0)
-    # scene.control_walker([scene.walker_control_generator(walkerID=1, autowalk=False, speed=20, X=100, Y=150, Yaw=0)])
+    scene.add_walker(0, 700, 0)
-    # goal = np.array((-100, 700))
+    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仿真器会闪退 ???

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)