From 4c4bbfde31e6597e281da22dcf314ed0a5c36843 Mon Sep 17 00:00:00 2001 From: ChenXL97 <908926798@qq.com> Date: Thu, 9 Nov 2023 17:06:34 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B5=8B=E8=AF=95=E5=AF=BC=E8=88=AA=E7=AE=97?= =?UTF-8?q?=E6=B3=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../algos/navigate/DstarLite/navigate.py | 6 +- robowaiter/behavior_lib/act/DealChat.py | 3 +- robowaiter/behavior_lib/act/MoveTo.py | 6 + robowaiter/scene/scene.py | 32 +++- run_robowaiter.py | 2 +- sub_task.ptml | 6 +- zoo/demo/导航寻路.py | 137 ++++++++++++++---- 7 files changed, 148 insertions(+), 44 deletions(-) diff --git a/robowaiter/algos/navigate/DstarLite/navigate.py b/robowaiter/algos/navigate/DstarLite/navigate.py index 3e66549..79cdd70 100644 --- a/robowaiter/algos/navigate/DstarLite/navigate.py +++ b/robowaiter/algos/navigate/DstarLite/navigate.py @@ -59,10 +59,12 @@ class Navigator: next_step = min(self.step_num, len(path)) (next_x, next_y) = path[next_step-1] print('plan pos:', (next_x, next_y), end=' ') - self.scene.walk_to(next_x, next_y, yaw, velocity=self.v) + 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) + # time.sleep(self.step_time) pos = np.array((self.scene.status.location.X, self.scene.status.location.Y)) print('reach pos:', pos) diff --git a/robowaiter/behavior_lib/act/DealChat.py b/robowaiter/behavior_lib/act/DealChat.py index d30b7fd..564e20b 100644 --- a/robowaiter/behavior_lib/act/DealChat.py +++ b/robowaiter/behavior_lib/act/DealChat.py @@ -39,7 +39,8 @@ class DealChat(Act): else: answer = ask_llm(chat) print(f"机器人回答：{answer}") - self.scene.chat_bubble(f"机器人回答：{answer}") + if self.scene.show_bubble: + self.scene.chat_bubble(f"机器人回答：{answer}") return ptree.common.Status.RUNNING diff --git a/robowaiter/behavior_lib/act/MoveTo.py b/robowaiter/behavior_lib/act/MoveTo.py index f107311..50cbe62 100644 --- a/robowaiter/behavior_lib/act/MoveTo.py +++ b/robowaiter/behavior_lib/act/MoveTo.py @@ -2,14 +2,20 @@ import py_trees as ptree from typing import Any from robowaiter.behavior_lib._base.Act import Act from robowaiter.algos.navigate.DstarLite.navigate import Navigator + class MoveTo(Act): def __init__(self, *args): super().__init__(*args) def _update(self) -> ptree.common.Status: + # self.scene.test_move() + navigator = Navigator(scene=self.scene, area_range=[-350, 600, -400, 1450], map=self.scene.state["map"]["2d"]) goal = self.scene.state['map']['obj_pos'][self.args[0]] navigator.navigate(goal, animation=False) + self.scene.state['condition_set'].add('At(Robot,Table)') + + return ptree.common.Status.RUNNING diff --git a/robowaiter/scene/scene.py b/robowaiter/scene/scene.py index ba9114f..5cc9f6a 100644 --- a/robowaiter/scene/scene.py +++ b/robowaiter/scene/scene.py @@ -34,6 +34,7 @@ def image_extract(camera_data): class Scene: robot = None event_list = [] + show_bubble = False default_state = { "map": { @@ -60,7 +61,7 @@ class Scene: def __init__(self,robot=None, sceneID=0): self.sceneID = sceneID - self.use_offset = True + self.use_offset = False self.start_time = time.time() self.time = 0 self.sub_task_seq = None @@ -132,7 +133,8 @@ class Scene: def create_chat_event(self,sentence): def customer_say(): print(f'顾客说：{sentence}') - self.chat_bubble(f'顾客说：{sentence}') + if self.show_bubble: + self.chat_bubble(f'顾客说：{sentence}') self.state['chat_list'].append(f'{sentence}') return customer_say @@ -161,13 +163,17 @@ class Scene: def walk_to(self, X, Y, Yaw, velocity=150, dis_limit=100): if self.use_offset: X, Y = X + loc_offset[0], Y + loc_offset[1] - stub.Do( - GrabSim_pb2.Action( - scene=self.sceneID, - action=GrabSim_pb2.Action.ActionType.WalkTo, - values=[X, Y, Yaw, velocity, dis_limit], - ) + + v = [X, Y, Yaw - 90, velocity, dis_limit] + print(v) + action = GrabSim_pb2.Action( + scene=self.sceneID, + action=GrabSim_pb2.Action.ActionType.WalkTo, + values=v ) + scene_info = stub.Do(action) + return scene_info + def reachable_check(self, X, Y, Yaw): if self.use_offset: @@ -374,4 +380,14 @@ class Scene: ) scene = stub.Do(action) + def test_move(self): + v_list = [[0, 880], [250, 1200], [-55, 750], [70, -200]] + scene = self.status + for walk_v in v_list: + walk_v = walk_v + [scene.rotation.Yaw - 90, 600, 100] + print("walk_v", walk_v) + action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v) + scene = stub.Do(action) + print(scene.info) + diff --git a/run_robowaiter.py b/run_robowaiter.py index 1c11b81..8592471 100644 --- a/run_robowaiter.py +++ b/run_robowaiter.py @@ -1,7 +1,7 @@ import os from robowaiter import Robot, task_map -TASK_NAME = 'VLM' +TASK_NAME = 'VLN' # create robot project_path = "./robowaiter" diff --git a/sub_task.ptml b/sub_task.ptml index 3473e6b..5829971 100644 --- a/sub_task.ptml +++ b/sub_task.ptml @@ -1,7 +1,7 @@ selector{ -cond At(Coffee,Bar) +cond At(Robot,Table) selector{ -cond At(Robot,CoffeeMachine) -act MakeCoffee +cond At(Robot,Bar) +act MoveTo(Table) } } diff --git a/zoo/demo/导航寻路.py b/zoo/demo/导航寻路.py index 379022f..21cf412 100644 --- a/zoo/demo/导航寻路.py +++ b/zoo/demo/导航寻路.py @@ -1,60 +1,139 @@ #!/usr/bin/env python3 # -*- encoding: utf-8 -*- +# enconding = utf8 import sys import time import grpc +sys.path.append('./') +sys.path.append('../') + import matplotlib.pyplot as plt import numpy as np from mpl_toolkits.axes_grid1 import make_axes_locatable -from proto import GrabSim_pb2 -from proto import GrabSim_pb2_grpc +from robowaiter.proto import GrabSim_pb2_grpc,GrabSim_pb2 -channel = grpc.insecure_channel('localhost:30001',options=[ - ('grpc.max_send_message_length', 1024*1024*1024), - ('grpc.max_receive_message_length', 1024*1024*1024) - ]) + +channel = grpc.insecure_channel('localhost:30001', options=[ + ('grpc.max_send_message_length', 1024 * 1024 * 1024), + ('grpc.max_receive_message_length', 1024 * 1024 * 1024) +]) sim_client = GrabSim_pb2_grpc.GrabSimStub(channel) -def map_test(map_id=0, scene_num=1): - initworld = sim_client.Init(GrabSim_pb2.NUL()) - print(sim_client.AcquireAvailableMaps(GrabSim_pb2.NUL())) - initworld = sim_client.SetWorld(GrabSim_pb2.BatchMap(count=scene_num, mapID=map_id)) +''' +初始化，卸载已经加载的关卡，清除所有机器人 +''' -def walk_test(scene_id=0, map_id=0): - """ - 移动底盘 - GrabSim_pb2.Action(sceneID=0, action=GrabSim_pb2.Action.ActionType.WalkTo, values=[x, y, yaw, velocity, dis]) - yaw: 机器人朝向; velocity:速度, -1代表瞬移; dis:最终达到的位置距离目标点最远距离, 如果超过此距离则目标位置不可达 - """ + +def Init(): + sim_client.Init(GrabSim_pb2.NUL()) + + +''' +获取当前可加载的地图信息(地图名字、地图尺寸) +''' + + +def AcquireAvailableMaps(): + AvailableMaps = sim_client.AcquireAvailableMaps(GrabSim_pb2.NUL()) + print(AvailableMaps) + + +''' +1、根据mapID加载指定地图 +2、如果scene_num>1,则根据地图尺寸偏移后加载多个相同地图 +3、这样就可以在一个关卡中训练多个地图 +''' + + +def SetWorld(map_id=0, scene_num=1): + print('------------------SetWorld----------------------') + world = sim_client.SetWorld(GrabSim_pb2.BatchMap(count=scene_num, mapID=map_id)) + + +''' +返回场景的状态信息 +1、返回机器人的位置和旋转 +2、返回各个关节的名字和旋转 +3、返回场景中标记的物品信息(名字、类型、位置、旋转) +4、返回场景中行人的信息(名字、位置、旋转、速度) +5、返回机器人手指和双臂的碰撞信息 +''' + + +def Observe(scene_id=0): + print('------------------show_env_info----------------------') scene = sim_client.Observe(GrabSim_pb2.SceneID(value=scene_id)) + print( + f"location:{[scene.location]}, rotation:{scene.rotation}\n", + f"joints number:{len(scene.joints)}, fingers number:{len(scene.fingers)}\n", + f"objects number: {len(scene.objects)}, walkers number: {len(scene.walkers)}\n" + f"timestep:{scene.timestep}, timestamp:{scene.timestamp}\n" + f"collision:{scene.collision}, info:{scene.info}") + +''' +重置场景 +1、重置桌子的宽度和高度 +2、清除生成的行人和物品 +3、重置关节角度、位置旋转 +4、清除碰撞信息 +5、重置场景中标记的物品 +''' + + +def Reset(scene_id=0): + print('------------------Reset----------------------') + scene = sim_client.Reset(GrabSim_pb2.ResetParams(scene=scene_id)) + print(scene) + + # 如果场景支持调整桌子 + # sim_client.Reset(GrabSim_pb2.ResetParams(scene = scene_id, adjust = True, height = 100.0, width = 100.0))" + + +""" +导航移动 +yaw:机器人朝向; +velocity:速度,>0代表移动,<0代表瞬移,=0代表只查询; +dis:最终达到的位置距离目标点最远距离,如果超过此距离则目标位置不可达 +""" + + +def navigation_move(scene_id=0, map_id=0): + print('------------------navigation_move----------------------') + scene = sim_client.Observe(GrabSim_pb2.SceneID(value=scene_id)) walk_value = [scene.location.X, scene.location.Y, scene.rotation.Yaw] - print('------------------walk_test----------------------') print("position:", walk_value) - if map_id == 3: # coffee - v_list = [[scene.location.X + 20, scene.location.Y - 500], [scene.location.X - 200, scene.location.Y - 300], - [scene.location.X - 200, scene.location.Y + 20], [0, 880], [250, 1200], [-55, 750], [70, -200]] + if map_id == 11: # coffee + v_list = [[0, 880], [250, 1200], [-55, 750], [70, -200]] else: - v_list = [[scene.location.X - 10, scene.location.Y - 20]] + v_list = [[0.0, 0.0]] for walk_v in v_list: walk_v = walk_v + [scene.rotation.Yaw - 90, 600, 100] print("walk_v", walk_v) action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v) scene = sim_client.Do(action) - print(scene.info) # print navigation info - time.sleep(2) + print(scene.info) + if __name__ == '__main__': - map_id = 3 # 地图编号: 3: 咖啡厅 - scene_num = 1 # 场景数量 - map_test(map_id, scene_num) # 场景加载测试 - time.sleep(5) + map_id = 11 # 地图编号 + scene_num = 1 # 场景数量 + + print('------------ 初始化加载场景 ------------') + Init() + AcquireAvailableMaps() + SetWorld(map_id, scene_num) + time.sleep(5.0) for i in range(scene_num): - print("------------------", i, "----------------------") - walk_test(i, map_id) # 导航寻路测试 \ No newline at end of file + print('------------ 场景操作 ------------') + Observe(i) + Reset(i) + + print('------------ 导航移动 ------------') + navigation_move(i, map_id) \ No newline at end of file