From 956203300ba905cd8109d035c7a7bf44d8b45f49 Mon Sep 17 00:00:00 2001
From: liwang zhang <2638950452@qq.com>
Date: Fri, 17 Nov 2023 09:57:58 +0800
Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0map=E6=8E=A5=E5=8F=A3?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 robowaiter/scene/scene.py | 26 +++++++++++++++++++-------
 1 file changed, 19 insertions(+), 7 deletions(-)

diff --git a/robowaiter/scene/scene.py b/robowaiter/scene/scene.py
index 567ba9d..110e9e0 100644
--- a/robowaiter/scene/scene.py
+++ b/robowaiter/scene/scene.py
@@ -1,3 +1,4 @@
+import pickle
 import sys
 import time
 import grpc
@@ -110,21 +111,24 @@ class Scene:
         self.all_frontier_list = set()
         self.semantic_map = semantic_map
         self.auto_map = np.ones((800, 1550))
+        self.filename = "../proto/map_1.pkl"
+        with open(self.filename, 'rb') as file:
+            self.map_file = pickle.load(file)
 
 
     def reset(self):
-        # 基类reset，默认执行仿真器初始化操作
-        self.reset_sim()
+            # 基类reset，默认执行仿真器初始化操作
+            self.reset_sim()
 
-        # reset state
-        self.state = self.default_state
+            # reset state
+            self.state = self.default_state
 
 
 
-        print("场景初始化完成")
-        self._reset()
+            print("场景初始化完成")
+            self._reset()
 
-        self.running = True
+            self.running = True
 
     def run(self):
         # 基类run
@@ -700,3 +704,11 @@ class Scene:
         ginger_x, ginger_y, ginger_z = [int(scene.location.X), int(scene.location.Y),100]
         return math.sqrt((ginger_x - objx) ** 2 + (ginger_y - objy) ** 2 + (ginger_z - objz) ** 2)
 
+    # 根据map文件判断是否可达
+    def reachable(self, pos):
+        x, y = self.real2map(pos[0], pos[1])
+        if self.map_file[x, y] == 0:
+            return True
+        else:
+            return False
+