diff --git a/robowaiter/algos/navigator/navigate.py b/robowaiter/algos/navigator/navigate.py index f6b61de..6261e5a 100644 --- a/robowaiter/algos/navigator/navigate.py +++ b/robowaiter/algos/navigator/navigate.py @@ -112,6 +112,11 @@ class Navigator: print('plan pos:', next_pos, end=' ') yaw = self.get_yaw(pos, next_pos) self.scene.walk_to(next_pos[0], next_pos[1], math.degrees(yaw), velocity=self.v, dis_limit=10) + + # 拍照片 + if self.scene.take_picture: + self.scene.get_obstacle_point(self.scene.db, self.scene.status, map_ratio=self.scene.map_ratio) + self.planner.path = self.planner.path[next_step - 1:] # 去除已走过的路径 pos = np.array((self.scene.status.location.X, self.scene.status.location.Y)) print('reach pos:', pos) diff --git a/robowaiter/behavior_lib/act/GreetCustomer.py b/robowaiter/behavior_lib/act/GreetCustomer.py index fdf0608..80fdb14 100644 --- a/robowaiter/behavior_lib/act/GreetCustomer.py +++ b/robowaiter/behavior_lib/act/GreetCustomer.py @@ -22,8 +22,10 @@ class GreetCustomer(Act): def _update(self) -> ptree.common.Status: goal = Act.place_xy_yaw_dic['Bar'] - self.scene.walk_to(goal[0]-5,goal[1], 180, 180, 0) + # self.scene.walk_to(goal[0]-5,goal[1], 180, 180, 0) # self.scene.chat_bubble("欢迎光临！请问有什么可以帮您？") + self.scene.navigator.navigate(goal=(goal[0]-5,goal[1]), animation=False) + if self.scene.show_bubble: self.scene.chat_bubble("欢迎光临！") diff --git a/robowaiter/behavior_lib/act/ServeCustomer.py b/robowaiter/behavior_lib/act/ServeCustomer.py index b52f16c..1c31e4e 100644 --- a/robowaiter/behavior_lib/act/ServeCustomer.py +++ b/robowaiter/behavior_lib/act/ServeCustomer.py @@ -22,8 +22,8 @@ class ServeCustomer(Act): if self.scene.state['attention']['customer'] == {}: goal = Act.place_xy_yaw_dic['Bar'] - self.scene.walk_to(goal[0] - 5, goal[1], 180, 180, 0) - + # self.scene.walk_to(goal[0] - 5, goal[1], 180, 180, 0) + self.scene.navigator.navigate(goal=(goal[0] - 5, goal[1]), animation=False) customer = self.scene.state["attention"]["customer"] if customer not in self.scene.state["serve_state"]: diff --git a/robowaiter/llm_client/multi_rounds.py b/robowaiter/llm_client/multi_rounds.py index b0a539b..1c933a0 100644 --- a/robowaiter/llm_client/multi_rounds.py +++ b/robowaiter/llm_client/multi_rounds.py @@ -1,4 +1,5 @@ import json +import time import openai from colorama import init, Fore @@ -90,6 +91,7 @@ def get_response(sentence, history, allow_function_call = True): history.append({"role": "user", "content": sentence}) if sentence in fix_questions_dict: + time.sleep(2) return parse_fix_question(sentence) params = dict(model="RoboWaiter") diff --git a/robowaiter/scene/scene.py b/robowaiter/scene/scene.py index 0990460..4868270 100644 --- a/robowaiter/scene/scene.py +++ b/robowaiter/scene/scene.py @@ -17,7 +17,7 @@ import os from robowaiter.utils import get_root_path from sklearn.cluster import DBSCAN from matplotlib import pyplot as plt - +from robowaiter.algos.navigator.dstar_lite import euclidean_distance plt.rcParams['font.sans-serif'] = ['SimHei'] # 用来正常显示中文标签 plt.rcParams['axes.unicode_minus'] = False # 用来正常显示负号 @@ -214,12 +214,12 @@ class Scene: ''' 初始化各种各种算法 ''' - map_file = os.path.join(root_path, 'robowaiter/algos/navigator/map_5.pkl') - with open(map_file, 'rb') as file: - map = pickle.load(file) + # map_file = os.path.join(root_path, 'robowaiter/algos/navigator/map_5.pkl') + # with open(map_file, 'rb') as file: + # map = pickle.load(file) # 初始化探索、导航、操作 - self.navigator = Navigator(scene=self, area_range=[-350, 600, -400, 1450], map=map, scale_ratio=5) + self.navigator = Navigator(scene=self, area_range=[-350, 600, -400, 1450], map=copy.deepcopy(self.map_map_real), scale_ratio=5) # self.explorer # self.manipulator @@ -1299,6 +1299,17 @@ class Scene: if point[0] < -350 or point[0] > 600 or point[1] < -400 or point[1] > 1450: continue map[math.floor((point[0] + 350) / self.map_ratio), math.floor((point[1] + 400) / self.map_ratio)] = 1 + for obs in points: + obs = self.navigator.planner.real2map(obs) + (x, y) = obs + occupy_radius = self.navigator.planner.dyna_obs_radius # 避免robot被dyna_obs的占用区域包裹住 + # 圆形区域 + occupy_pos = [(i, j) for i in range(x - occupy_radius, x + occupy_radius + 1) + for j in range(y - occupy_radius, y + occupy_radius + 1) + if euclidean_distance((i, j), obs) < occupy_radius] + + for pos in occupy_pos: + map[pos] = 1 return map def draw_map(self,plt, map): @@ -1306,6 +1317,7 @@ class Scene: plt.imshow(map, cmap='binary', alpha=0.5, origin='lower', extent=(-400 / self.map_ratio, 1450 / self.map_ratio, -350 / self.map_ratio, 600 / self.map_ratio)) + plt.title('可达性地图') def get_id_object_world(self, id, scene): pixels = [] diff --git a/robowaiter/scene/tasks/VLN/VLN_greet_lead.py b/robowaiter/scene/tasks/VLN/VLN_greet_lead.py index 90f66d0..40a0fd7 100644 --- a/robowaiter/scene/tasks/VLN/VLN_greet_lead.py +++ b/robowaiter/scene/tasks/VLN/VLN_greet_lead.py @@ -6,15 +6,15 @@ import time from robowaiter.scene.scene import Scene -class SceneVLM(Scene): +class SceneVLN(Scene): def __init__(self, robot): super().__init__(robot) # 在这里加入场景中发生的事件， (事件发生的时间，事件函数) self.new_event_list = [ # (2, self.customer_say, (0, "请问哪里有空位啊？")), # (6, self.customer_say, (0, "我想坐高凳子。")), - (6, self.customer_say, (0, "你带我去吧。")), - (13, self.control_walker, (0, False,100, -250, 480,-90)), + (3, self.customer_say, (0, "你带我去吧。")), + (15, self.control_walker, (0, False,100, -250, 480,-90)), (-1, self.customer_say, (0, "谢谢你！这儿还不错！")), ] @@ -28,7 +28,9 @@ class SceneVLM(Scene): [19, 70, -200], #后门站着不动的 4 [21, 65, 1000, -90], #大胖男占了一号桌 5 [5, 230, 1200], #小女孩 6 - [26, -28, 0, 90] , #在设置一个在后门随机游走的 7 + [26, -28, -10, 90], + # [26, 60, 0, 90], + # [26, -28, 0, 90] , #在设置一个在后门随机游走的 7 # 设置为 26, 60, 0, 90] [31, 280, 1200, -45] # 8 ]) @@ -52,6 +54,6 @@ if __name__ == '__main__': robot = Robot() # create task - task = SceneVLM(robot) + task = SceneVLN(robot) task.reset() task.run()