Merge branch 'main' of https://github.com/HPCL-EI/RoboWaiter
This commit is contained in:
Caiyishuai
commit
50d7110f5f
143
README.md
143
README.md
|
|
@ -14,162 +14,39 @@ pip install -e .
|
|||
|
||||
### 快速入门
|
||||
1. 安装UE及Harix插件，打开默认项目并运行
|
||||
2. 运行 run_robowaiter.py 文件即可实现机器人控制端与仿真器的交互
|
||||
2. 运行 tasks 文件夹下的任意场景即可实现机器人控制端与仿真器的交互
|
||||
|
||||
|
||||
# 运行流程介绍
|
||||
run_robowaiter.py 入口文件如下：
|
||||
```python
|
||||
import os
|
||||
from robowaiter import Robot, task_map
|
||||
# 代码框架介绍
|
||||
|
||||
TASK_NAME = 'GQA'
|
||||
|
||||
# create robot
|
||||
project_path = "./robowaiter"
|
||||
ptml_path = os.path.join(project_path, 'robot/Default.ptml')
|
||||
behavior_lib_path = os.path.join(project_path, 'behavior_lib')
|
||||
|
||||
robot = Robot(ptml_path,behavior_lib_path)
|
||||
|
||||
# create task
|
||||
task = task_map[TASK_NAME](robot)
|
||||
task.reset()
|
||||
task.run()
|
||||
```
|
||||
|
||||
## Robot
|
||||
Robot是机器人类，包括从ptml加载行为树的方法，以及执行行为树的方法等
|
||||
|
||||
|
||||
## task_map
|
||||
task_map是任务字典，通过任务缩写来返回相应的场景类。
|
||||
## tasks
|
||||
tasks文件夹中存放的场景定义及运行代码。
|
||||
|
||||
| 缩写 | 任务 |
|
||||
|----|---------|
|
||||
|---------------------|-------------|
|
||||
| AEM | 主动探索和记忆 |
|
||||
| GQA | 具身多轮对话 |
|
||||
| VLN | 视觉语言导航 |
|
||||
| VLM | 视觉语言操作 |
|
||||
| OT | 复杂开放任务 |
|
||||
| AT | 自主任务 |
|
||||
| CafeDailyOperations | 整体展示：咖啡厅的一天 |
|
||||
| Interact | 命令行自由交互 |
|
||||
|
||||
|
||||
## Scene
|
||||
Scene是场景基类，task_map返回的任务场景都继承于Scene。
|
||||
该类实现了一些通用的场景操作接口。
|
||||
|
||||
### 场景中物品类别
|
||||
|
||||
| ID | Item |
|
||||
|-----|----------------------|
|
||||
| 0 | Mug |
|
||||
| 1 | Banana |
|
||||
| 2 | Toothpaste |
|
||||
| 3 | Bread |
|
||||
| 4 | Softdrink |
|
||||
| 5 | Yogurt |
|
||||
| 6 | ADMilk |
|
||||
| 7 | VacuumCup |
|
||||
| 8 | Bernachon |
|
||||
| 9 | BottledDrink |
|
||||
| 10 | PencilVase |
|
||||
| 11 | Teacup |
|
||||
| 12 | Caddy |
|
||||
| 13 | Dictionary |
|
||||
| 14 | Cake |
|
||||
| 15 | Date |
|
||||
| 16 | Stapler |
|
||||
| 17 | LunchBox |
|
||||
| 18 | Bracelet |
|
||||
| 19 | MilkDrink |
|
||||
| 20 | CocountWater |
|
||||
| 21 | Walnut |
|
||||
| 22 | HamSausage |
|
||||
| 23 | GlueStick |
|
||||
| 24 | AdhensiveTape |
|
||||
| 25 | Calculator |
|
||||
| 26 | Chess |
|
||||
| 27 | Orange |
|
||||
| 28 | Glass |
|
||||
| 29 | Washbowl |
|
||||
| 30 | Durian |
|
||||
| 31 | Gum |
|
||||
| 32 | Towl |
|
||||
| 33 | OrangeJuice |
|
||||
| 34 | Cardcase |
|
||||
| 35 | RubikCube |
|
||||
| 36 | StickyNotes |
|
||||
| 37 | NFCJuice |
|
||||
| 38 | SpringWater |
|
||||
| 39 | Apple |
|
||||
| 40 | Coffee |
|
||||
| 41 | Gauze |
|
||||
| 42 | Mangosteen |
|
||||
| 43 | SesameSeedCake |
|
||||
| 44 | Glove |
|
||||
| 45 | Mouse |
|
||||
| 46 | Kettle |
|
||||
| 47 | Atomize |
|
||||
| 48 | Chips |
|
||||
| 49 | SpongeGourd |
|
||||
| 50 | Garlic |
|
||||
| 51 | Potato |
|
||||
| 52 | Tray |
|
||||
| 53 | Hemomanometer |
|
||||
| 54 | TennisBall |
|
||||
| 55 | ToyDog |
|
||||
| 56 | ToyBear |
|
||||
| 57 | TeaTray |
|
||||
| 58 | Sock |
|
||||
| 59 | Scarf |
|
||||
| 60 | ToiletPaper |
|
||||
| 61 | Milk |
|
||||
| 62 | Soap |
|
||||
| 63 | Novel |
|
||||
| 64 | Watermelon |
|
||||
| 65 | Tomato |
|
||||
| 66 | CleansingFoam |
|
||||
| 67 | CocountMilk |
|
||||
| 68 | SugarlessGum |
|
||||
| 69 | MedicalAdhensiveTape |
|
||||
| 70 | SourMilkDrink |
|
||||
| 71 | PaperCup |
|
||||
| 72 | Tissue |
|
||||
| 73 | YogurtDrink |
|
||||
| 74 | Newspaper |
|
||||
| 75 | Box |
|
||||
| 76 | PaperCupStarbucks |
|
||||
| 77 | CoffeeMachine |
|
||||
| 78 | GingerLHand |
|
||||
| 79 | GingerRHand |
|
||||
| 80 | Straw |
|
||||
| 81 | Cake |
|
||||
| 82 | Tray |
|
||||
| 83 | Bread |
|
||||
| 84 | Glass |
|
||||
| 85 | Door |
|
||||
| 86 | Mug |
|
||||
| 87 | Machine |
|
||||
| 88 | Packaged Coffee |
|
||||
| 89 | Cube Sugar |
|
||||
| 90 | Apple |
|
||||
| 91 | Spoon |
|
||||
| 92 | Drinks |
|
||||
| 93 | Drink |
|
||||
| 94 | Take-Away Cup |
|
||||
| 95 | Saucer |
|
||||
| 96 | Trash Bin |
|
||||
| 97 | Knife |
|
||||
| 251 | Ginger |
|
||||
| 252 | Floor |
|
||||
| 253 | Roof |
|
||||
| 254 | Wall |
|
||||
注意：78及以后无法使用add_object方法生成
|
||||
|
||||
# 调用大模型接口
|
||||
运行llm_client.py文件调用大模型进行多轮对话。
|
||||
```shell
|
||||
python llm_client.py
|
||||
cd robowaiter/llm_client
|
||||
python multi_rounds.py
|
||||
```
|
||||
输入字符即可等待回答，输入end表示对话结束。
|
||||
输入字符即可等待回答
|
||||
|
|
|
|||
|
|
@ -103,7 +103,7 @@ class Bahavior(ptree.behaviour.Behaviour):
|
|||
|
||||
|
||||
|
||||
# let behavior node interact with the scene
|
||||
# let behavior node Interact with the scene
|
||||
def set_scene(self, scene=None):
|
||||
if scene:
|
||||
self.scene = scene
|
||||
|
|
|
|||
|
|
@ -64,6 +64,7 @@ class DealChat(Act):
|
|||
|
||||
self.scene.robot.expand_sub_task_tree(goal_set)
|
||||
|
||||
|
||||
def get_object_info(self,**args):
|
||||
try:
|
||||
obj = args['obj']
|
||||
|
|
|
|||
|
|
@ -19,4 +19,5 @@ class DelSubTree(Act):
|
|||
|
||||
sub_task_tree = self.parent
|
||||
self.scene.sub_task_seq.children.remove(sub_task_tree)
|
||||
self.scene.draw_current_bt()
|
||||
return Status.RUNNING
|
||||
|
|
@ -9,7 +9,7 @@ ok, 有需要您再找我。
|
|||
做一杯咖啡
|
||||
好的，我马上做咖啡
|
||||
create_sub_task
|
||||
{"goal":"On(Coffee,CoffeeTable)"}00
|
||||
{"goal":"On(Coffee,CoffeeTable)"}
|
||||
|
||||
不用了。
|
||||
好的，您有需要再跟我说
|
||||
|
|
|
|||
|
|
@ -96,6 +96,8 @@ class Robot(object):
|
|||
# print_tree_from_root(self.bt.root)
|
||||
print("行为树扩展完成！")
|
||||
|
||||
self.scene.draw_current_bt()
|
||||
|
||||
# 获取所有action的pre,add,del列表
|
||||
def collect_action_nodes(self):
|
||||
action_list = []
|
||||
|
|
|
|||
Binary file not shown.
|
After Width: | Height: | Size: 794 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 116 KiB |
|
|
@ -18,6 +18,9 @@ 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
|
||||
from PIL import Image
|
||||
from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas
|
||||
|
||||
plt.rcParams['font.sans-serif'] = ['SimHei'] # 用来正常显示中文标签
|
||||
plt.rcParams['axes.unicode_minus'] = False # 用来正常显示负号
|
||||
|
||||
|
|
@ -56,7 +59,7 @@ def show_image(camera_data):
|
|||
# matplotlib中的plt方法 对矩阵d 进行图形绘制，如果 深度相机拍摄的带深度的图片（图片名字中有depth信息），则转换成黑白图即灰度图
|
||||
plt.imshow(d, cmap="gray" if "depth" in im.name.lower() else None)
|
||||
# 图像展示在屏幕上
|
||||
plt.show()
|
||||
# plt.show()
|
||||
|
||||
return d
|
||||
|
||||
|
|
@ -68,6 +71,9 @@ class Scene:
|
|||
signal_event_list = []
|
||||
# show_bubble = True
|
||||
event_signal = "None"
|
||||
# step_interval = 1
|
||||
# camera_interval = 1.5
|
||||
output_path = os.path.join(os.path.dirname(__file__), "outputs")
|
||||
|
||||
default_state = {
|
||||
"map": {
|
||||
|
|
@ -113,6 +119,7 @@ class Scene:
|
|||
self.start_time = time.time()
|
||||
self.time = 0
|
||||
self.sub_task_seq = None
|
||||
os.makedirs(self.output_path,exist_ok=True)
|
||||
|
||||
self.show_bubble = True
|
||||
# 图像分割
|
||||
|
|
@ -129,14 +136,13 @@ class Scene:
|
|||
with open(file_name, 'rb') as file:
|
||||
self.map_map_real = pickle.load(file)
|
||||
|
||||
# init robot
|
||||
if robot:
|
||||
robot.set_scene(self)
|
||||
robot.load_BT()
|
||||
self.robot = robot
|
||||
|
||||
self.init_robot(robot)
|
||||
|
||||
self.robot_changed = False
|
||||
self.last_event_time = 0
|
||||
self.last_camera_time = -99999
|
||||
self.last_step_time = -99999
|
||||
|
||||
# 1-7 正常执行, 8-10 控灯操作移动到6, 11-12窗帘操作不需要移动,
|
||||
self.op_dialog = ["", "制作咖啡", "倒水", "夹点心", "拖地", "擦桌子", "开筒灯", "搬椅子", # 1-7
|
||||
|
|
@ -212,6 +218,14 @@ class Scene:
|
|||
|
||||
self.init_algos() # 初始化各种算法类
|
||||
|
||||
def init_robot(self, robot):
|
||||
# init robot
|
||||
self.robot = robot
|
||||
|
||||
if robot:
|
||||
robot.set_scene(self)
|
||||
return robot.load_BT()
|
||||
|
||||
def init_algos(self):
|
||||
'''
|
||||
初始化各种各种算法
|
||||
|
|
@ -250,11 +264,13 @@ class Scene:
|
|||
# 基类step，默认执行行为树tick操作
|
||||
self.time = time.time() - self.start_time
|
||||
|
||||
# if self.time - self.last_step_time > self.step_interval:
|
||||
self.deal_event()
|
||||
self.deal_new_event()
|
||||
self.deal_signal_event()
|
||||
self._step()
|
||||
self.robot_changed = self.robot.step()
|
||||
self.last_step_time = self.time
|
||||
|
||||
def deal_new_event(self):
|
||||
if len(self.new_event_list) > 0:
|
||||
|
|
@ -306,9 +322,7 @@ class Scene:
|
|||
|
||||
return set_sub_task
|
||||
|
||||
def new_set_goal(self, goal):
|
||||
g = eval("{'" + goal + "'}")
|
||||
self.state['chat_list'].append(("Goal", g))
|
||||
|
||||
def new_set_goal(self,goal):
|
||||
self.state['chat_list'].append(("Goal",goal))
|
||||
|
||||
|
|
@ -1162,6 +1176,9 @@ class Scene:
|
|||
# print("物体世界偏移的坐标: ", world_offest_coordinates)
|
||||
return world_coordinates
|
||||
|
||||
def ui_func(self,args):
|
||||
pass
|
||||
|
||||
def get_obstacle_point(self, db, scene, map_ratio, update_info_count=0):
|
||||
|
||||
# if abs(self.last_take_pic_tim - self.time)<
|
||||
|
|
@ -1176,10 +1193,8 @@ class Scene:
|
|||
|
||||
not_key_objs_id = {255, 254, 253, 107, 81}
|
||||
|
||||
img_data_segment = get_camera([GrabSim_pb2.CameraName.Head_Segment])
|
||||
img_data_depth = get_camera([GrabSim_pb2.CameraName.Head_Depth])
|
||||
img_data_color = get_camera([GrabSim_pb2.CameraName.Head_Color])
|
||||
|
||||
img_data_segment,img_data_depth,img_data_color = self.get_cameras()
|
||||
im_segment = img_data_segment.images[0]
|
||||
im_depth = img_data_depth.images[0]
|
||||
im_color = img_data_color.images[0]
|
||||
|
|
@ -1191,6 +1206,7 @@ class Scene:
|
|||
d_color = np.frombuffer(im_color.data, dtype=im_color.dtype).reshape(
|
||||
(im_color.height, im_color.width, im_color.channels))
|
||||
|
||||
|
||||
items = img_data_segment.info.split(";")
|
||||
objs_id = {}
|
||||
for item in items:
|
||||
|
|
@ -1267,6 +1283,10 @@ class Scene:
|
|||
plt.axis('off')
|
||||
plt.title("目标检测")
|
||||
|
||||
# self.ui_func(("draw_img","img_label_obj",d_color))
|
||||
|
||||
|
||||
|
||||
for key, value in object_pixels.items():
|
||||
if key == 0:
|
||||
continue
|
||||
|
|
@ -1336,7 +1356,25 @@ class Scene:
|
|||
plt.text(0, 0.3, f'更新语义信息：{update_info_count}', fontsize=10)
|
||||
# plt.text(0, 0.1, f'已存语义信息：{self.infoCount}', fontsize=10)
|
||||
|
||||
plt.show()
|
||||
output_path = os.path.join(self.output_path,"vision.png")
|
||||
plt.savefig(output_path)
|
||||
|
||||
# canvas = FigureCanvas(plt.gcf())
|
||||
# fig = plt.gcf() # 获取当前figure
|
||||
# image = fig.canvas.print_to_buffer()
|
||||
# width, height = fig.get_size_inches() # 获取图像的宽度和高度（单位：英寸）
|
||||
# dpi = fig.dpi # 获取图像的DPI
|
||||
#
|
||||
# width_px = int(width * dpi) # 转换为像素
|
||||
# height_px = int(height * dpi) # 转换为像素
|
||||
#
|
||||
# image_pil = Image.frombuffer('RGB', (width, height), image, 'raw')
|
||||
#
|
||||
# # 转换为numpy数组
|
||||
# image_np = np.asarray(image_pil)
|
||||
self.ui_func(("draw_from_file","img_label_canvas",output_path))
|
||||
plt.close()
|
||||
# plt.show()
|
||||
# return cur_obstacle_world_points
|
||||
|
||||
def updateMap(self, points):
|
||||
|
|
@ -1369,10 +1407,12 @@ class Scene:
|
|||
def get_id_object_world(self, id, scene):
|
||||
pixels = []
|
||||
world_points = []
|
||||
img_data_segment = get_camera([GrabSim_pb2.CameraName.Head_Segment])
|
||||
|
||||
img_data_segment,img_data_depth,_ = self.get_cameras()
|
||||
# img_data_segment = get_camera([GrabSim_pb2.CameraName.Head_Segment])
|
||||
im_segment = img_data_segment.images[0]
|
||||
|
||||
img_data_depth = get_camera([GrabSim_pb2.CameraName.Head_Depth])
|
||||
# img_data_depth = get_camera([GrabSim_pb2.CameraName.Head_Depth])
|
||||
im_depth = img_data_depth.images[0]
|
||||
|
||||
d_segment = np.frombuffer(im_segment.data, dtype=im_segment.dtype).reshape(
|
||||
|
|
@ -1390,3 +1430,14 @@ class Scene:
|
|||
for pixel in pixels:
|
||||
world_points.append(self.transform_co(img_data_depth, pixel[0], pixel[1], d_depth[pixel[0]][pixel[1]][0]))
|
||||
return world_points
|
||||
|
||||
def get_cameras(self):
|
||||
# if self.time - self.last_camera_time > self.camera_interval:
|
||||
self.img_data_segment = get_camera([GrabSim_pb2.CameraName.Head_Segment])
|
||||
time.sleep(0.2)
|
||||
self.img_data_depth = get_camera([GrabSim_pb2.CameraName.Head_Depth])
|
||||
time.sleep(0.2)
|
||||
self.img_data_color = get_camera([GrabSim_pb2.CameraName.Head_Color])
|
||||
self.last_camera_time = self.time
|
||||
|
||||
return self.img_data_segment,self.img_data_depth,self.img_data_color
|
||||
|
|
@ -1,106 +0,0 @@
|
|||
"""
|
||||
视觉语言导航
|
||||
识别顾客（NPC）靠近、打招呼、对话、领位导航到适合人数的空闲餐桌
|
||||
开始条件：监测到顾客靠近
|
||||
结束条件：完成领位，语音：“请问您想喝点什么？”，并等待下一步指令
|
||||
"""
|
||||
import os
|
||||
import pickle
|
||||
import time
|
||||
import random
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
|
||||
from robowaiter.scene.scene import Scene,init_world # TODO: 文件名改成Scene.py
|
||||
|
||||
from robowaiter.scene.scene import Scene
|
||||
from robowaiter.utils import get_root_path
|
||||
from robowaiter.algos.navigator.navigate import Navigator
|
||||
from robowaiter.algos.navigator import test
|
||||
|
||||
class SceneVLN(Scene):
|
||||
def __init__(self, robot):
|
||||
super().__init__(robot)
|
||||
# 在这里加入场景中发生的事件， (事件发生的时间，事件函数)
|
||||
self.event_list = [
|
||||
(5, self.create_chat_event("测试VLN：前往2号桌")),
|
||||
]
|
||||
|
||||
def _reset(self):
|
||||
root_path = get_root_path()
|
||||
file_name = os.path.join(root_path,'robowaiter/algos/navigator/map_5.pkl')
|
||||
with open(file_name, 'rb') as file:
|
||||
map = pickle.load(file)
|
||||
|
||||
self.state['map']['2d'] = map
|
||||
self.state['map']['obj_pos']['Table'] = np.array((-100, 700))
|
||||
|
||||
def _run(self):
|
||||
file_name = '../../algos/navigator/map_5.pkl'
|
||||
if os.path.exists(file_name):
|
||||
with open(file_name, 'rb') as file:
|
||||
map = pickle.load(file)
|
||||
|
||||
# self.init_world(1, 11)
|
||||
# scene = self.status
|
||||
|
||||
navigator = Navigator(scene=self, area_range=[-350, 600, -400, 1450], map=map)
|
||||
|
||||
'''场景1: 无行人环境 robot到达指定目标'''
|
||||
# goal = np.array((-100, 700))
|
||||
|
||||
'''场景2: 静止行人环境 robot到达指定目标'''
|
||||
# scene.clean_walker()
|
||||
# scene.add_walker(50, 300, 0)
|
||||
# scene.add_walker(-50, 500, 0)
|
||||
# goal = np.array((-100, 700))
|
||||
|
||||
'''场景3: 移动行人环境 robot到达指定目标'''
|
||||
self.walk_to(100, 0, -90, dis_limit=10)
|
||||
self.clean_walker()
|
||||
self.add_walkers([[50, 300], [-50, 500], [0, 700]])
|
||||
# scene.add_walker(50, 300, 0)
|
||||
# scene.add_walker(-50, 500, 0)
|
||||
# scene.add_walker(0, 700, 0)
|
||||
self.control_walker(
|
||||
[self.walker_control_generator(walkerID=0, autowalk=False, speed=50, X=-50, Y=600, Yaw=0)])
|
||||
self.control_walker(
|
||||
[self.walker_control_generator(walkerID=1, autowalk=False, speed=50, X=100, Y=150, Yaw=0)])
|
||||
self.control_walker([self.walker_control_generator(walkerID=2, autowalk=False, speed=50, X=0, Y=0, Yaw=0)])
|
||||
|
||||
# goal = (-100, 700)
|
||||
# goal = (-300)
|
||||
# goal = (340.0, 900.0)
|
||||
|
||||
# goal = (240.0, 1000.0)
|
||||
# goal = (340.0, 900.0)
|
||||
goal = (240.0, 1160.0)
|
||||
|
||||
'''场景4: 行人自由移动 robot到达指定目标'''
|
||||
# # TODO: autowalk=True仿真器会闪退 ???
|
||||
# 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=True, speed=20, X=0, Y=0, Yaw=0)])
|
||||
# scene.control_walker([scene.walker_control_generator(walkerID=1, autowalk=True, speed=20, X=0, Y=0, Yaw=0)])
|
||||
# time.sleep(5)
|
||||
# goal = np.array((-100, 700))
|
||||
|
||||
navigator.navigate(goal, animation=False)
|
||||
|
||||
self.clean_walker()
|
||||
print(self.status.collision) # TODO: 不显示碰撞信息 ???
|
||||
pass
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
import os
|
||||
from robowaiter.robot.robot import Robot
|
||||
|
||||
robot = Robot()
|
||||
|
||||
# create task
|
||||
task = SceneVLN(robot)
|
||||
task.reset()
|
||||
task.run()
|
||||
|
|
@ -279,7 +279,8 @@ def render_dot_tree(root: behaviour.Behaviour,
|
|||
name: str = None,
|
||||
target_directory: str = os.getcwd(),
|
||||
with_blackboard_variables: bool = False,
|
||||
with_qualified_names: bool = False):
|
||||
with_qualified_names: bool = False,
|
||||
png_only = True):
|
||||
"""
|
||||
Render the dot tree to .dot, .svg, .png. files in the current
|
||||
working directory. These will be named with the root behaviour name.
|
||||
|
|
@ -322,10 +323,16 @@ def render_dot_tree(root: behaviour.Behaviour,
|
|||
filename_wo_extension_to_convert = root.ins_name if name is None else name
|
||||
filename_wo_extension = utilities.get_valid_filename(filename_wo_extension_to_convert)
|
||||
filenames = {}
|
||||
for extension, writer in {"dot": graph.write, "png": graph.write_png, "svg": graph.write_svg}.items():
|
||||
|
||||
if png_only:
|
||||
write_dict = {"png": graph.write_png}
|
||||
else:
|
||||
write_dict = {"dot": graph.write, "png": graph.write_png, "svg": graph.write_svg}
|
||||
|
||||
for extension, writer in write_dict.items():
|
||||
filename = filename_wo_extension + '.' + extension
|
||||
pathname = os.path.join(target_directory, filename)
|
||||
print("Writing {}".format(pathname))
|
||||
writer(pathname)
|
||||
filenames[extension] = pathname
|
||||
return filenames
|
||||
return filenames["png"]
|
||||
|
|
|
|||
|
|
@ -1,17 +1,10 @@
|
|||
selector
|
||||
sequence
|
||||
cond CustomerChatting()
|
||||
act DealChat()
|
||||
sequence
|
||||
cond HasSubTask()
|
||||
sequence
|
||||
act SubTaskPlaceHolder()
|
||||
sequence
|
||||
cond FocusingCustomer()
|
||||
act ServeCustomer()
|
||||
sequence
|
||||
cond NewCustomerComing()
|
||||
selector
|
||||
cond At(Robot,Bar)
|
||||
act MoveTo(Bar)
|
||||
act GreetCustomer()
|
||||
sequence{
|
||||
cond Holding(Nothing)
|
||||
sequence{
|
||||
cond Is(Curtain,Off)
|
||||
act Turn(Curtain,On)
|
||||
}
|
||||
sequence{
|
||||
cond Is(AC,Off)
|
||||
act Turn(AC,On)
|
||||
}}
|
||||
|
|
@ -2,27 +2,28 @@ selector
|
|||
{
|
||||
sequence
|
||||
{
|
||||
cond CustomerChatting()
|
||||
cond Chatting()
|
||||
act DealChat()
|
||||
|
||||
} sequence
|
||||
{
|
||||
cond HasSubTask()
|
||||
sequence
|
||||
{
|
||||
act SubTaskPlaceHolder()
|
||||
|
||||
} sequence
|
||||
} } sequence
|
||||
{
|
||||
cond FocusingCustomer()
|
||||
act ServeCustomer()
|
||||
|
||||
} sequence
|
||||
{
|
||||
cond NewCustomerComing()
|
||||
cond NewCustomer()
|
||||
selector
|
||||
{
|
||||
cond At(Robot,Bar)
|
||||
act MoveTo(Bar)
|
||||
|
||||
} act GreetCustomer()
|
||||
} sequence
|
||||
{
|
||||
cond AnomalyDetected()
|
||||
act ResolveAnomaly()
|
||||
}}
|
||||
|
|
@ -3,37 +3,19 @@ ordering=out;
|
|||
graph [fontname="times-roman"];
|
||||
node [fontname="times-roman"];
|
||||
edge [fontname="times-roman"];
|
||||
"a3ac082f-ad46-4cc9-86b4-3e76c3c564d3" [fillcolor=cyan, fontcolor=black, fontsize=20, height=0.01, label=Selector, shape=diamond, style=filled, width=0.01];
|
||||
"717a1290-56bc-41e1-9b4b-e6df93d40e64" [fillcolor=orange, fontcolor=black, fontsize=20, height=0.01, label=Sequence, shape=octagon, style=filled, width=0.01];
|
||||
"a3ac082f-ad46-4cc9-86b4-3e76c3c564d3" -> "717a1290-56bc-41e1-9b4b-e6df93d40e64";
|
||||
"b3236b13-d5f2-4643-ae7c-6f27bd4ab6f2" [fillcolor=yellow, fontcolor=black, fontsize=20, label="CustomerChatting()", shape=ellipse, style=filled];
|
||||
"717a1290-56bc-41e1-9b4b-e6df93d40e64" -> "b3236b13-d5f2-4643-ae7c-6f27bd4ab6f2";
|
||||
"28015225-b7a4-4f38-b26f-4dbe5eea4154" [fillcolor=lawngreen, fontcolor=black, fontsize=20, label="DealChat()", shape=box, style=filled];
|
||||
"717a1290-56bc-41e1-9b4b-e6df93d40e64" -> "28015225-b7a4-4f38-b26f-4dbe5eea4154";
|
||||
"ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e" [fillcolor=orange, fontcolor=black, fontsize=20, height=0.01, label=Sequence, shape=octagon, style=filled, width=0.01];
|
||||
"a3ac082f-ad46-4cc9-86b4-3e76c3c564d3" -> "ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e";
|
||||
"2f77f976-499c-4601-9d8c-86dd80d66dfa" [fillcolor=yellow, fontcolor=black, fontsize=20, label="HasSubTask()", shape=ellipse, style=filled];
|
||||
"ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e" -> "2f77f976-499c-4601-9d8c-86dd80d66dfa";
|
||||
"2428792c-3896-443d-8744-f5e286644fad" [fillcolor=orange, fontcolor=black, fontsize=20, height=0.01, label=Sequence, shape=octagon, style=filled, width=0.01];
|
||||
"ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e" -> "2428792c-3896-443d-8744-f5e286644fad";
|
||||
"4bb7a9c3-521b-408f-b403-1fd25d54b192" [fillcolor=lawngreen, fontcolor=black, fontsize=20, label="SubTaskPlaceHolder()", shape=box, style=filled];
|
||||
"2428792c-3896-443d-8744-f5e286644fad" -> "4bb7a9c3-521b-408f-b403-1fd25d54b192";
|
||||
"64b2362a-d99e-4e99-8772-ad1419e53a2e" [fillcolor=orange, fontcolor=black, fontsize=20, height=0.01, label=Sequence, shape=octagon, style=filled, width=0.01];
|
||||
"ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e" -> "64b2362a-d99e-4e99-8772-ad1419e53a2e";
|
||||
"fd7124ba-c9d1-4fde-8063-e8337d335121" [fillcolor=yellow, fontcolor=black, fontsize=20, label="FocusingCustomer()", shape=ellipse, style=filled];
|
||||
"64b2362a-d99e-4e99-8772-ad1419e53a2e" -> "fd7124ba-c9d1-4fde-8063-e8337d335121";
|
||||
"169ebec9-3645-4fbb-a533-5186a8e5967b" [fillcolor=lawngreen, fontcolor=black, fontsize=20, label="ServeCustomer()", shape=box, style=filled];
|
||||
"64b2362a-d99e-4e99-8772-ad1419e53a2e" -> "169ebec9-3645-4fbb-a533-5186a8e5967b";
|
||||
"bea12066-ecbd-49b0-8934-efcb2c38b5f5" [fillcolor=orange, fontcolor=black, fontsize=20, height=0.01, label=Sequence, shape=octagon, style=filled, width=0.01];
|
||||
"ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e" -> "bea12066-ecbd-49b0-8934-efcb2c38b5f5";
|
||||
"a847a0cc-c5af-4757-aa8c-8baef72788dc" [fillcolor=yellow, fontcolor=black, fontsize=20, label="NewCustomerComing()", shape=ellipse, style=filled];
|
||||
"bea12066-ecbd-49b0-8934-efcb2c38b5f5" -> "a847a0cc-c5af-4757-aa8c-8baef72788dc";
|
||||
"99354f05-1716-46b2-9151-d88eac0a5b27" [fillcolor=cyan, fontcolor=black, fontsize=20, height=0.01, label=Selector, shape=diamond, style=filled, width=0.01];
|
||||
"bea12066-ecbd-49b0-8934-efcb2c38b5f5" -> "99354f05-1716-46b2-9151-d88eac0a5b27";
|
||||
"19de10a3-7554-43a3-b892-34cb2e32ab9a" [fillcolor=yellow, fontcolor=black, fontsize=20, label="At(Robot,Bar)", shape=ellipse, style=filled];
|
||||
"99354f05-1716-46b2-9151-d88eac0a5b27" -> "19de10a3-7554-43a3-b892-34cb2e32ab9a";
|
||||
"4286d652-c4ef-4522-a7f1-b5c865dcc4c9" [fillcolor=lawngreen, fontcolor=black, fontsize=20, label="MoveTo(Bar)", shape=box, style=filled];
|
||||
"99354f05-1716-46b2-9151-d88eac0a5b27" -> "4286d652-c4ef-4522-a7f1-b5c865dcc4c9";
|
||||
"a3766a34-5152-4f82-8fba-8bf1f6b3830b" [fillcolor=lawngreen, fontcolor=black, fontsize=20, label="GreetCustomer()", shape=box, style=filled];
|
||||
"bea12066-ecbd-49b0-8934-efcb2c38b5f5" -> "a3766a34-5152-4f82-8fba-8bf1f6b3830b";
|
||||
"c9a8671e-b0b8-4587-a4de-a094fd678334" [fillcolor=orange, fontcolor=black, fontsize=20, height=0.01, label=Selector, shape=octagon, style=filled, width=0.01];
|
||||
"bc597ff6-b3cb-4573-9dea-944fdb38d045" [fillcolor=yellow, fontcolor=black, fontsize=20, label="Holding(Nothing)", shape=ellipse, style=filled];
|
||||
"c9a8671e-b0b8-4587-a4de-a094fd678334" -> "bc597ff6-b3cb-4573-9dea-944fdb38d045";
|
||||
"a8e26121-f5cc-4ba5-b98e-86bd84de06d6" [fillcolor=orange, fontcolor=black, fontsize=20, height=0.01, label=Sequence, shape=octagon, style=filled, width=0.01];
|
||||
"c9a8671e-b0b8-4587-a4de-a094fd678334" -> "a8e26121-f5cc-4ba5-b98e-86bd84de06d6";
|
||||
"676d8bda-2c30-44a5-bca2-ade68761403a" [fillcolor=yellow, fontcolor=black, fontsize=20, label="Is(Curtain,Off)", shape=ellipse, style=filled];
|
||||
"a8e26121-f5cc-4ba5-b98e-86bd84de06d6" -> "676d8bda-2c30-44a5-bca2-ade68761403a";
|
||||
"c3d096cf-f848-46a3-9497-5398614798e8" [fillcolor=lawngreen, fontcolor=black, fontsize=20, label="Turn(Curtain,On)", shape=box, style=filled];
|
||||
"a8e26121-f5cc-4ba5-b98e-86bd84de06d6" -> "c3d096cf-f848-46a3-9497-5398614798e8";
|
||||
"400b4104-ff96-4d64-89b2-49dc6e4e3cbd" [fillcolor=orange, fontcolor=black, fontsize=20, height=0.01, label=Sequence, shape=octagon, style=filled, width=0.01];
|
||||
"c9a8671e-b0b8-4587-a4de-a094fd678334" -> "400b4104-ff96-4d64-89b2-49dc6e4e3cbd";
|
||||
"3fc48afc-1cef-45f5-826d-2571d7979a2f" [fillcolor=yellow, fontcolor=black, fontsize=20, label="Is(AC,Off)", shape=ellipse, style=filled];
|
||||
"400b4104-ff96-4d64-89b2-49dc6e4e3cbd" -> "3fc48afc-1cef-45f5-826d-2571d7979a2f";
|
||||
"47af4df0-8811-45a5-95e2-f0fc9458ddfd" [fillcolor=lawngreen, fontcolor=black, fontsize=20, label="Turn(AC,On)", shape=box, style=filled];
|
||||
"400b4104-ff96-4d64-89b2-49dc6e4e3cbd" -> "47af4df0-8811-45a5-95e2-f0fc9458ddfd";
|
||||
}
|
||||
|
|
|
|||
Binary file not shown.
|
Before Width: | Height: | Size: 98 KiB After Width: | Height: | Size: 34 KiB |
|
|
@ -4,208 +4,100 @@
|
|||
<!-- Generated by graphviz version 9.0.0 (20230911.1827)
|
||||
-->
|
||||
<!-- Title: pastafarianism Pages: 1 -->
|
||||
<svg width="1699pt" height="417pt"
|
||||
viewBox="0.00 0.00 1699.49 416.87" xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink">
|
||||
<g id="graph0" class="graph" transform="scale(1 1) rotate(0) translate(4 412.87)">
|
||||
<svg width="716pt" height="220pt"
|
||||
viewBox="0.00 0.00 715.70 219.87" xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink">
|
||||
<g id="graph0" class="graph" transform="scale(1 1) rotate(0) translate(4 215.87)">
|
||||
<title>pastafarianism</title>
|
||||
<polygon fill="white" stroke="none" points="-4,4 -4,-412.87 1695.49,-412.87 1695.49,4 -4,4"/>
|
||||
<!-- a3ac082f-ad46-4cc9-86b4-3e76c3c564d3 -->
|
||||
<polygon fill="white" stroke="none" points="-4,4 -4,-215.87 711.7,-215.87 711.7,4 -4,4"/>
|
||||
<!-- c9a8671e-b0b8-4587-a4de-a094fd678334 -->
|
||||
<g id="node1" class="node">
|
||||
<title>a3ac082f-ad46-4cc9-86b4-3e76c3c564d3</title>
|
||||
<polygon fill="cyan" stroke="black" points="495.87,-408.87 412.37,-377.62 495.87,-346.37 579.37,-377.62 495.87,-408.87"/>
|
||||
<text text-anchor="middle" x="495.87" y="-370.24" font-family="Times New Roman,serif" font-size="20.00">Selector</text>
|
||||
<title>c9a8671e-b0b8-4587-a4de-a094fd678334</title>
|
||||
<polygon fill="orange" stroke="black" points="383.11,-178.04 383.11,-197.85 345.67,-211.87 292.73,-211.87 255.29,-197.85 255.29,-178.04 292.73,-164.03 345.67,-164.03 383.11,-178.04"/>
|
||||
<text text-anchor="middle" x="319.2" y="-180.57" font-family="Times New Roman,serif" font-size="20.00">Selector</text>
|
||||
</g>
|
||||
<!-- 717a1290-56bc-41e1-9b4b-e6df93d40e64 -->
|
||||
<!-- bc597ff6-b3cb-4573-9dea-944fdb38d045 -->
|
||||
<g id="node2" class="node">
|
||||
<title>717a1290-56bc-41e1-9b4b-e6df93d40e64</title>
|
||||
<polygon fill="orange" stroke="black" points="395.38,-276.54 395.38,-296.35 352.9,-310.37 292.83,-310.37 250.35,-296.35 250.35,-276.54 292.83,-262.53 352.9,-262.53 395.38,-276.54"/>
|
||||
<text text-anchor="middle" x="322.87" y="-279.07" font-family="Times New Roman,serif" font-size="20.00">Sequence</text>
|
||||
<title>bc597ff6-b3cb-4573-9dea-944fdb38d045</title>
|
||||
<ellipse fill="yellow" stroke="black" cx="114.2" cy="-104.11" rx="114.2" ry="22.1"/>
|
||||
<text text-anchor="middle" x="114.2" y="-96.74" font-family="Times New Roman,serif" font-size="20.00">Holding(Nothing)</text>
|
||||
</g>
|
||||
<!-- a3ac082f-ad46-4cc9-86b4-3e76c3c564d3->717a1290-56bc-41e1-9b4b-e6df93d40e64 -->
|
||||
<!-- c9a8671e-b0b8-4587-a4de-a094fd678334->bc597ff6-b3cb-4573-9dea-944fdb38d045 -->
|
||||
<g id="edge1" class="edge">
|
||||
<title>a3ac082f-ad46-4cc9-86b4-3e76c3c564d3->717a1290-56bc-41e1-9b4b-e6df93d40e64</title>
|
||||
<path fill="none" stroke="black" d="M461.27,-358.78C435.8,-345.66 400.81,-327.62 372.38,-312.97"/>
|
||||
<polygon fill="black" stroke="black" points="374.07,-309.9 363.58,-308.43 370.87,-316.13 374.07,-309.9"/>
|
||||
<title>c9a8671e-b0b8-4587-a4de-a094fd678334->bc597ff6-b3cb-4573-9dea-944fdb38d045</title>
|
||||
<path fill="none" stroke="black" d="M276.22,-169.79C246.35,-157.87 206.15,-141.82 173.26,-128.69"/>
|
||||
<polygon fill="black" stroke="black" points="174.86,-125.56 164.27,-125.1 172.26,-132.06 174.86,-125.56"/>
|
||||
</g>
|
||||
<!-- ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e -->
|
||||
<g id="node5" class="node">
|
||||
<title>ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e</title>
|
||||
<polygon fill="orange" stroke="black" points="742.38,-276.54 742.38,-296.35 699.9,-310.37 639.83,-310.37 597.35,-296.35 597.35,-276.54 639.83,-262.53 699.9,-262.53 742.38,-276.54"/>
|
||||
<text text-anchor="middle" x="669.87" y="-279.07" font-family="Times New Roman,serif" font-size="20.00">Sequence</text>
|
||||
</g>
|
||||
<!-- a3ac082f-ad46-4cc9-86b4-3e76c3c564d3->ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e -->
|
||||
<g id="edge4" class="edge">
|
||||
<title>a3ac082f-ad46-4cc9-86b4-3e76c3c564d3->ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e</title>
|
||||
<path fill="none" stroke="black" d="M530.24,-359C556.02,-345.79 591.68,-327.51 620.51,-312.74"/>
|
||||
<polygon fill="black" stroke="black" points="621.74,-316.04 629.04,-308.37 618.55,-309.81 621.74,-316.04"/>
|
||||
</g>
|
||||
<!-- b3236b13-d5f2-4643-ae7c-6f27bd4ab6f2 -->
|
||||
<!-- a8e26121-f5cc-4ba5-b98e-86bd84de06d6 -->
|
||||
<g id="node3" class="node">
|
||||
<title>b3236b13-d5f2-4643-ae7c-6f27bd4ab6f2</title>
|
||||
<ellipse fill="yellow" stroke="black" cx="125.87" cy="-202.61" rx="125.87" ry="22.1"/>
|
||||
<text text-anchor="middle" x="125.87" y="-195.24" font-family="Times New Roman,serif" font-size="20.00">CustomerChatting()</text>
|
||||
<title>a8e26121-f5cc-4ba5-b98e-86bd84de06d6</title>
|
||||
<polygon fill="orange" stroke="black" points="391.72,-94.2 391.72,-114.02 349.24,-128.03 289.16,-128.03 246.68,-114.02 246.68,-94.2 289.16,-80.19 349.24,-80.19 391.72,-94.2"/>
|
||||
<text text-anchor="middle" x="319.2" y="-96.74" font-family="Times New Roman,serif" font-size="20.00">Sequence</text>
|
||||
</g>
|
||||
<!-- 717a1290-56bc-41e1-9b4b-e6df93d40e64->b3236b13-d5f2-4643-ae7c-6f27bd4ab6f2 -->
|
||||
<!-- c9a8671e-b0b8-4587-a4de-a094fd678334->a8e26121-f5cc-4ba5-b98e-86bd84de06d6 -->
|
||||
<g id="edge2" class="edge">
|
||||
<title>717a1290-56bc-41e1-9b4b-e6df93d40e64->b3236b13-d5f2-4643-ae7c-6f27bd4ab6f2</title>
|
||||
<path fill="none" stroke="black" d="M278.16,-266.88C250.24,-255.28 213.99,-240.22 183.86,-227.7"/>
|
||||
<polygon fill="black" stroke="black" points="185.46,-224.58 174.88,-223.97 182.77,-231.04 185.46,-224.58"/>
|
||||
<title>c9a8671e-b0b8-4587-a4de-a094fd678334->a8e26121-f5cc-4ba5-b98e-86bd84de06d6</title>
|
||||
<path fill="none" stroke="black" d="M319.2,-163.72C319.2,-156.29 319.2,-147.89 319.2,-139.83"/>
|
||||
<polygon fill="black" stroke="black" points="322.7,-139.92 319.2,-129.92 315.7,-139.92 322.7,-139.92"/>
|
||||
</g>
|
||||
<!-- 28015225-b7a4-4f38-b26f-4dbe5eea4154 -->
|
||||
<g id="node4" class="node">
|
||||
<title>28015225-b7a4-4f38-b26f-4dbe5eea4154</title>
|
||||
<polygon fill="lawngreen" stroke="black" points="376.24,-220.61 269.49,-220.61 269.49,-184.61 376.24,-184.61 376.24,-220.61"/>
|
||||
<text text-anchor="middle" x="322.87" y="-195.24" font-family="Times New Roman,serif" font-size="20.00">DealChat()</text>
|
||||
</g>
|
||||
<!-- 717a1290-56bc-41e1-9b4b-e6df93d40e64->28015225-b7a4-4f38-b26f-4dbe5eea4154 -->
|
||||
<g id="edge3" class="edge">
|
||||
<title>717a1290-56bc-41e1-9b4b-e6df93d40e64->28015225-b7a4-4f38-b26f-4dbe5eea4154</title>
|
||||
<path fill="none" stroke="black" d="M322.87,-262.22C322.87,-252.88 322.87,-242.01 322.87,-232.2"/>
|
||||
<polygon fill="black" stroke="black" points="326.37,-232.43 322.87,-222.43 319.37,-232.43 326.37,-232.43"/>
|
||||
</g>
|
||||
<!-- 2f77f976-499c-4601-9d8c-86dd80d66dfa -->
|
||||
<!-- 400b4104-ff96-4d64-89b2-49dc6e4e3cbd -->
|
||||
<g id="node6" class="node">
|
||||
<title>2f77f976-499c-4601-9d8c-86dd80d66dfa</title>
|
||||
<ellipse fill="yellow" stroke="black" cx="486.87" cy="-202.61" rx="92.45" ry="22.1"/>
|
||||
<text text-anchor="middle" x="486.87" y="-195.24" font-family="Times New Roman,serif" font-size="20.00">HasSubTask()</text>
|
||||
<title>400b4104-ff96-4d64-89b2-49dc6e4e3cbd</title>
|
||||
<polygon fill="orange" stroke="black" points="562.72,-94.2 562.72,-114.02 520.24,-128.03 460.16,-128.03 417.68,-114.02 417.68,-94.2 460.16,-80.19 520.24,-80.19 562.72,-94.2"/>
|
||||
<text text-anchor="middle" x="490.2" y="-96.74" font-family="Times New Roman,serif" font-size="20.00">Sequence</text>
|
||||
</g>
|
||||
<!-- ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e->2f77f976-499c-4601-9d8c-86dd80d66dfa -->
|
||||
<!-- c9a8671e-b0b8-4587-a4de-a094fd678334->400b4104-ff96-4d64-89b2-49dc6e4e3cbd -->
|
||||
<g id="edge5" class="edge">
|
||||
<title>ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e->2f77f976-499c-4601-9d8c-86dd80d66dfa</title>
|
||||
<path fill="none" stroke="black" d="M626.96,-266.26C600.92,-254.61 567.48,-239.66 539.8,-227.28"/>
|
||||
<polygon fill="black" stroke="black" points="541.44,-224.18 530.88,-223.3 538.58,-230.57 541.44,-224.18"/>
|
||||
<title>c9a8671e-b0b8-4587-a4de-a094fd678334->400b4104-ff96-4d64-89b2-49dc6e4e3cbd</title>
|
||||
<path fill="none" stroke="black" d="M358,-168.38C382.01,-156.89 413.11,-142.01 439.11,-129.56"/>
|
||||
<polygon fill="black" stroke="black" points="440.29,-132.88 447.8,-125.4 437.27,-126.56 440.29,-132.88"/>
|
||||
</g>
|
||||
<!-- 2428792c-3896-443d-8744-f5e286644fad -->
|
||||
<!-- 676d8bda-2c30-44a5-bca2-ade68761403a -->
|
||||
<g id="node4" class="node">
|
||||
<title>676d8bda-2c30-44a5-bca2-ade68761403a</title>
|
||||
<ellipse fill="yellow" stroke="black" cx="124.2" cy="-22.1" rx="96.17" ry="22.1"/>
|
||||
<text text-anchor="middle" x="124.2" y="-14.72" font-family="Times New Roman,serif" font-size="20.00">Is(Curtain,Off)</text>
|
||||
</g>
|
||||
<!-- a8e26121-f5cc-4ba5-b98e-86bd84de06d6->676d8bda-2c30-44a5-bca2-ade68761403a -->
|
||||
<g id="edge3" class="edge">
|
||||
<title>a8e26121-f5cc-4ba5-b98e-86bd84de06d6->676d8bda-2c30-44a5-bca2-ade68761403a</title>
|
||||
<path fill="none" stroke="black" d="M274.46,-84.75C246.5,-73.28 210.25,-58.41 180.36,-46.14"/>
|
||||
<polygon fill="black" stroke="black" points="182.06,-43.06 171.48,-42.5 179.4,-49.53 182.06,-43.06"/>
|
||||
</g>
|
||||
<!-- c3d096cf-f848-46a3-9497-5398614798e8 -->
|
||||
<g id="node5" class="node">
|
||||
<title>c3d096cf-f848-46a3-9497-5398614798e8</title>
|
||||
<polygon fill="lawngreen" stroke="black" points="399.57,-40.1 238.82,-40.1 238.82,-4.1 399.57,-4.1 399.57,-40.1"/>
|
||||
<text text-anchor="middle" x="319.2" y="-14.72" font-family="Times New Roman,serif" font-size="20.00">Turn(Curtain,On)</text>
|
||||
</g>
|
||||
<!-- a8e26121-f5cc-4ba5-b98e-86bd84de06d6->c3d096cf-f848-46a3-9497-5398614798e8 -->
|
||||
<g id="edge4" class="edge">
|
||||
<title>a8e26121-f5cc-4ba5-b98e-86bd84de06d6->c3d096cf-f848-46a3-9497-5398614798e8</title>
|
||||
<path fill="none" stroke="black" d="M319.2,-79.97C319.2,-71.24 319.2,-61.2 319.2,-52.01"/>
|
||||
<polygon fill="black" stroke="black" points="322.7,-52.11 319.2,-42.11 315.7,-52.11 322.7,-52.11"/>
|
||||
</g>
|
||||
<!-- 3fc48afc-1cef-45f5-826d-2571d7979a2f -->
|
||||
<g id="node7" class="node">
|
||||
<title>2428792c-3896-443d-8744-f5e286644fad</title>
|
||||
<polygon fill="orange" stroke="black" points="742.38,-192.7 742.38,-212.52 699.9,-226.53 639.83,-226.53 597.35,-212.52 597.35,-192.7 639.83,-178.69 699.9,-178.69 742.38,-192.7"/>
|
||||
<text text-anchor="middle" x="669.87" y="-195.24" font-family="Times New Roman,serif" font-size="20.00">Sequence</text>
|
||||
<title>3fc48afc-1cef-45f5-826d-2571d7979a2f</title>
|
||||
<ellipse fill="yellow" stroke="black" cx="490.2" cy="-22.1" rx="72.3" ry="22.1"/>
|
||||
<text text-anchor="middle" x="490.2" y="-14.72" font-family="Times New Roman,serif" font-size="20.00">Is(AC,Off)</text>
|
||||
</g>
|
||||
<!-- ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e->2428792c-3896-443d-8744-f5e286644fad -->
|
||||
<!-- 400b4104-ff96-4d64-89b2-49dc6e4e3cbd->3fc48afc-1cef-45f5-826d-2571d7979a2f -->
|
||||
<g id="edge6" class="edge">
|
||||
<title>ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e->2428792c-3896-443d-8744-f5e286644fad</title>
|
||||
<path fill="none" stroke="black" d="M669.87,-262.22C669.87,-254.79 669.87,-246.39 669.87,-238.33"/>
|
||||
<polygon fill="black" stroke="black" points="673.37,-238.42 669.87,-228.42 666.37,-238.42 673.37,-238.42"/>
|
||||
<title>400b4104-ff96-4d64-89b2-49dc6e4e3cbd->3fc48afc-1cef-45f5-826d-2571d7979a2f</title>
|
||||
<path fill="none" stroke="black" d="M490.2,-79.97C490.2,-72.46 490.2,-63.99 490.2,-55.92"/>
|
||||
<polygon fill="black" stroke="black" points="493.7,-56.06 490.2,-46.06 486.7,-56.06 493.7,-56.06"/>
|
||||
</g>
|
||||
<!-- 64b2362a-d99e-4e99-8772-ad1419e53a2e -->
|
||||
<g id="node9" class="node">
|
||||
<title>64b2362a-d99e-4e99-8772-ad1419e53a2e</title>
|
||||
<polygon fill="orange" stroke="black" points="905.38,-192.7 905.38,-212.52 862.9,-226.53 802.83,-226.53 760.35,-212.52 760.35,-192.7 802.83,-178.69 862.9,-178.69 905.38,-192.7"/>
|
||||
<text text-anchor="middle" x="832.87" y="-195.24" font-family="Times New Roman,serif" font-size="20.00">Sequence</text>
|
||||
</g>
|
||||
<!-- ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e->64b2362a-d99e-4e99-8772-ad1419e53a2e -->
|
||||
<g id="edge8" class="edge">
|
||||
<title>ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e->64b2362a-d99e-4e99-8772-ad1419e53a2e</title>
|
||||
<path fill="none" stroke="black" d="M709.74,-265.43C731.74,-254.38 759.28,-240.56 782.73,-228.78"/>
|
||||
<polygon fill="black" stroke="black" points="784.12,-232 791.48,-224.39 780.98,-225.75 784.12,-232"/>
|
||||
</g>
|
||||
<!-- bea12066-ecbd-49b0-8934-efcb2c38b5f5 -->
|
||||
<g id="node12" class="node">
|
||||
<title>bea12066-ecbd-49b0-8934-efcb2c38b5f5</title>
|
||||
<polygon fill="orange" stroke="black" points="1384.38,-192.7 1384.38,-212.52 1341.9,-226.53 1281.83,-226.53 1239.35,-212.52 1239.35,-192.7 1281.83,-178.69 1341.9,-178.69 1384.38,-192.7"/>
|
||||
<text text-anchor="middle" x="1311.87" y="-195.24" font-family="Times New Roman,serif" font-size="20.00">Sequence</text>
|
||||
</g>
|
||||
<!-- ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e->bea12066-ecbd-49b0-8934-efcb2c38b5f5 -->
|
||||
<g id="edge11" class="edge">
|
||||
<title>ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e->bea12066-ecbd-49b0-8934-efcb2c38b5f5</title>
|
||||
<path fill="none" stroke="black" d="M742.57,-276.18C862.55,-260.89 1101.31,-230.45 1228.15,-214.28"/>
|
||||
<polygon fill="black" stroke="black" points="1228.34,-217.79 1237.82,-213.05 1227.46,-210.84 1228.34,-217.79"/>
|
||||
</g>
|
||||
<!-- 4bb7a9c3-521b-408f-b403-1fd25d54b192 -->
|
||||
<!-- 47af4df0-8811-45a5-95e2-f0fc9458ddfd -->
|
||||
<g id="node8" class="node">
|
||||
<title>4bb7a9c3-521b-408f-b403-1fd25d54b192</title>
|
||||
<polygon fill="lawngreen" stroke="black" points="574.49,-129.44 373.24,-129.44 373.24,-93.44 574.49,-93.44 574.49,-129.44"/>
|
||||
<text text-anchor="middle" x="473.87" y="-104.07" font-family="Times New Roman,serif" font-size="20.00">SubTaskPlaceHolder()</text>
|
||||
<title>47af4df0-8811-45a5-95e2-f0fc9458ddfd</title>
|
||||
<polygon fill="lawngreen" stroke="black" points="707.7,-40.1 580.7,-40.1 580.7,-4.1 707.7,-4.1 707.7,-40.1"/>
|
||||
<text text-anchor="middle" x="644.2" y="-14.72" font-family="Times New Roman,serif" font-size="20.00">Turn(AC,On)</text>
|
||||
</g>
|
||||
<!-- 2428792c-3896-443d-8744-f5e286644fad->4bb7a9c3-521b-408f-b403-1fd25d54b192 -->
|
||||
<!-- 400b4104-ff96-4d64-89b2-49dc6e4e3cbd->47af4df0-8811-45a5-95e2-f0fc9458ddfd -->
|
||||
<g id="edge7" class="edge">
|
||||
<title>2428792c-3896-443d-8744-f5e286644fad->4bb7a9c3-521b-408f-b403-1fd25d54b192</title>
|
||||
<path fill="none" stroke="black" d="M627.82,-182.48C596.86,-168.4 554.7,-149.22 522.33,-134.49"/>
|
||||
<polygon fill="black" stroke="black" points="524.12,-131.46 513.57,-130.51 521.22,-137.83 524.12,-131.46"/>
|
||||
</g>
|
||||
<!-- fd7124ba-c9d1-4fde-8063-e8337d335121 -->
|
||||
<g id="node10" class="node">
|
||||
<title>fd7124ba-c9d1-4fde-8063-e8337d335121</title>
|
||||
<ellipse fill="yellow" stroke="black" cx="720.87" cy="-111.44" rx="128.52" ry="22.1"/>
|
||||
<text text-anchor="middle" x="720.87" y="-104.07" font-family="Times New Roman,serif" font-size="20.00">FocusingCustomer()</text>
|
||||
</g>
|
||||
<!-- 64b2362a-d99e-4e99-8772-ad1419e53a2e->fd7124ba-c9d1-4fde-8063-e8337d335121 -->
|
||||
<g id="edge9" class="edge">
|
||||
<title>64b2362a-d99e-4e99-8772-ad1419e53a2e->fd7124ba-c9d1-4fde-8063-e8337d335121</title>
|
||||
<path fill="none" stroke="black" d="M803.73,-178.42C789.1,-166.77 771.27,-152.57 755.9,-140.33"/>
|
||||
<polygon fill="black" stroke="black" points="758.36,-137.82 748.36,-134.33 754,-143.3 758.36,-137.82"/>
|
||||
</g>
|
||||
<!-- 169ebec9-3645-4fbb-a533-5186a8e5967b -->
|
||||
<g id="node11" class="node">
|
||||
<title>169ebec9-3645-4fbb-a533-5186a8e5967b</title>
|
||||
<polygon fill="lawngreen" stroke="black" points="1020.49,-129.44 867.24,-129.44 867.24,-93.44 1020.49,-93.44 1020.49,-129.44"/>
|
||||
<text text-anchor="middle" x="943.87" y="-104.07" font-family="Times New Roman,serif" font-size="20.00">ServeCustomer()</text>
|
||||
</g>
|
||||
<!-- 64b2362a-d99e-4e99-8772-ad1419e53a2e->169ebec9-3645-4fbb-a533-5186a8e5967b -->
|
||||
<g id="edge10" class="edge">
|
||||
<title>64b2362a-d99e-4e99-8772-ad1419e53a2e->169ebec9-3645-4fbb-a533-5186a8e5967b</title>
|
||||
<path fill="none" stroke="black" d="M861.74,-178.42C877.67,-165.62 897.45,-149.73 913.6,-136.76"/>
|
||||
<polygon fill="black" stroke="black" points="915.36,-139.83 920.97,-130.84 910.98,-134.37 915.36,-139.83"/>
|
||||
</g>
|
||||
<!-- a847a0cc-c5af-4757-aa8c-8baef72788dc -->
|
||||
<g id="node13" class="node">
|
||||
<title>a847a0cc-c5af-4757-aa8c-8baef72788dc</title>
|
||||
<ellipse fill="yellow" stroke="black" cx="1186.87" cy="-111.44" rx="148.14" ry="22.1"/>
|
||||
<text text-anchor="middle" x="1186.87" y="-104.07" font-family="Times New Roman,serif" font-size="20.00">NewCustomerComing()</text>
|
||||
</g>
|
||||
<!-- bea12066-ecbd-49b0-8934-efcb2c38b5f5->a847a0cc-c5af-4757-aa8c-8baef72788dc -->
|
||||
<g id="edge12" class="edge">
|
||||
<title>bea12066-ecbd-49b0-8934-efcb2c38b5f5->a847a0cc-c5af-4757-aa8c-8baef72788dc</title>
|
||||
<path fill="none" stroke="black" d="M1280,-178.88C1263.4,-167.04 1242.97,-152.46 1225.48,-139.99"/>
|
||||
<polygon fill="black" stroke="black" points="1227.51,-137.14 1217.34,-134.18 1223.44,-142.84 1227.51,-137.14"/>
|
||||
</g>
|
||||
<!-- 99354f05-1716-46b2-9151-d88eac0a5b27 -->
|
||||
<g id="node14" class="node">
|
||||
<title>99354f05-1716-46b2-9151-d88eac0a5b27</title>
|
||||
<polygon fill="cyan" stroke="black" points="1436.87,-142.69 1353.37,-111.44 1436.87,-80.19 1520.37,-111.44 1436.87,-142.69"/>
|
||||
<text text-anchor="middle" x="1436.87" y="-104.07" font-family="Times New Roman,serif" font-size="20.00">Selector</text>
|
||||
</g>
|
||||
<!-- bea12066-ecbd-49b0-8934-efcb2c38b5f5->99354f05-1716-46b2-9151-d88eac0a5b27 -->
|
||||
<g id="edge13" class="edge">
|
||||
<title>bea12066-ecbd-49b0-8934-efcb2c38b5f5->99354f05-1716-46b2-9151-d88eac0a5b27</title>
|
||||
<path fill="none" stroke="black" d="M1343.73,-178.88C1360.85,-166.67 1382.05,-151.54 1399.9,-138.81"/>
|
||||
<polygon fill="black" stroke="black" points="1401.55,-141.94 1407.65,-133.28 1397.48,-136.24 1401.55,-141.94"/>
|
||||
</g>
|
||||
<!-- a3766a34-5152-4f82-8fba-8bf1f6b3830b -->
|
||||
<g id="node17" class="node">
|
||||
<title>a3766a34-5152-4f82-8fba-8bf1f6b3830b</title>
|
||||
<polygon fill="lawngreen" stroke="black" points="1691.49,-129.44 1538.24,-129.44 1538.24,-93.44 1691.49,-93.44 1691.49,-129.44"/>
|
||||
<text text-anchor="middle" x="1614.87" y="-104.07" font-family="Times New Roman,serif" font-size="20.00">GreetCustomer()</text>
|
||||
</g>
|
||||
<!-- bea12066-ecbd-49b0-8934-efcb2c38b5f5->a3766a34-5152-4f82-8fba-8bf1f6b3830b -->
|
||||
<g id="edge16" class="edge">
|
||||
<title>bea12066-ecbd-49b0-8934-efcb2c38b5f5->a3766a34-5152-4f82-8fba-8bf1f6b3830b</title>
|
||||
<path fill="none" stroke="black" d="M1369.59,-187.32C1413.74,-176.07 1475.98,-159.54 1529.87,-142.69 1538.78,-139.91 1548.16,-136.78 1557.3,-133.61"/>
|
||||
<polygon fill="black" stroke="black" points="1558.17,-137.02 1566.45,-130.4 1555.85,-130.41 1558.17,-137.02"/>
|
||||
</g>
|
||||
<!-- 19de10a3-7554-43a3-b892-34cb2e32ab9a -->
|
||||
<g id="node15" class="node">
|
||||
<title>19de10a3-7554-43a3-b892-34cb2e32ab9a</title>
|
||||
<ellipse fill="yellow" stroke="black" cx="1347.87" cy="-22.1" rx="95.64" ry="22.1"/>
|
||||
<text text-anchor="middle" x="1347.87" y="-14.72" font-family="Times New Roman,serif" font-size="20.00">At(Robot,Bar)</text>
|
||||
</g>
|
||||
<!-- 99354f05-1716-46b2-9151-d88eac0a5b27->19de10a3-7554-43a3-b892-34cb2e32ab9a -->
|
||||
<g id="edge14" class="edge">
|
||||
<title>99354f05-1716-46b2-9151-d88eac0a5b27->19de10a3-7554-43a3-b892-34cb2e32ab9a</title>
|
||||
<path fill="none" stroke="black" d="M1414.41,-88.4C1403.11,-77.32 1389.28,-63.74 1377.16,-51.84"/>
|
||||
<polygon fill="black" stroke="black" points="1379.76,-49.5 1370.17,-44.99 1374.86,-54.49 1379.76,-49.5"/>
|
||||
</g>
|
||||
<!-- 4286d652-c4ef-4522-a7f1-b5c865dcc4c9 -->
|
||||
<g id="node16" class="node">
|
||||
<title>4286d652-c4ef-4522-a7f1-b5c865dcc4c9</title>
|
||||
<polygon fill="lawngreen" stroke="black" points="1589.74,-40.1 1461.99,-40.1 1461.99,-4.1 1589.74,-4.1 1589.74,-40.1"/>
|
||||
<text text-anchor="middle" x="1525.87" y="-14.72" font-family="Times New Roman,serif" font-size="20.00">MoveTo(Bar)</text>
|
||||
</g>
|
||||
<!-- 99354f05-1716-46b2-9151-d88eac0a5b27->4286d652-c4ef-4522-a7f1-b5c865dcc4c9 -->
|
||||
<g id="edge15" class="edge">
|
||||
<title>99354f05-1716-46b2-9151-d88eac0a5b27->4286d652-c4ef-4522-a7f1-b5c865dcc4c9</title>
|
||||
<path fill="none" stroke="black" d="M1459.32,-88.4C1471.74,-76.21 1487.23,-61.02 1500.13,-48.35"/>
|
||||
<polygon fill="black" stroke="black" points="1502.42,-51.01 1507.1,-41.51 1497.51,-46.02 1502.42,-51.01"/>
|
||||
<title>400b4104-ff96-4d64-89b2-49dc6e4e3cbd->47af4df0-8811-45a5-95e2-f0fc9458ddfd</title>
|
||||
<path fill="none" stroke="black" d="M529.46,-82.71C551.25,-71.39 578.4,-57.28 600.82,-45.64"/>
|
||||
<polygon fill="black" stroke="black" points="602.15,-48.89 609.41,-41.17 598.92,-42.68 602.15,-48.89"/>
|
||||
</g>
|
||||
</g>
|
||||
</svg>
|
||||
|
|
|
|||
|
Before Width: | Height: | Size: 15 KiB After Width: | Height: | Size: 7.1 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 92 KiB |
|
|
@ -0,0 +1,171 @@
|
|||
from PyQt5.QtWidgets import QApplication, QMainWindow, QLabel
|
||||
from PyQt5.QtGui import QPixmap
|
||||
from PyQt5.QtCore import Qt
|
||||
from PyQt5.QtCore import QTimer, QCoreApplication
|
||||
import sys
|
||||
import os
|
||||
|
||||
from robowaiter.utils.ui.window import Ui_MainWindow
|
||||
from robowaiter.utils.basic import get_root_path
|
||||
from PyQt5.QtCore import QThread
|
||||
import queue
|
||||
import numpy as np
|
||||
from PyQt5.QtGui import QImage, QPixmap
|
||||
from PyQt5.QtCore import Qt
|
||||
|
||||
# 创建线程安全的队列
|
||||
scene_queue = queue.Queue()
|
||||
ui_queue = queue.Queue()
|
||||
|
||||
root_path = get_root_path()
|
||||
|
||||
class TaskThread(QThread):
|
||||
def __init__(self, task, scene_cls,robot_cls, *args, **kwargs):
|
||||
super(TaskThread, self).__init__(*args, **kwargs)
|
||||
self.task = task
|
||||
self.scene_cls = scene_cls
|
||||
self.robot_cls = robot_cls
|
||||
|
||||
def run(self):
|
||||
self.task(self.scene_cls,self.robot_cls)
|
||||
|
||||
def run_scene(scene_cls,robot_cls):
|
||||
scene = scene_cls(robot_cls(),scene_queue,ui_queue)
|
||||
|
||||
scene.reset()
|
||||
scene.run()
|
||||
#
|
||||
# try:
|
||||
# # 机器人系统的主循环
|
||||
#
|
||||
# except Exception as e:
|
||||
# # 打印异常信息到命令行
|
||||
# print("Robot system error:", str(e))
|
||||
|
||||
|
||||
|
||||
class UI(QMainWindow, Ui_MainWindow):
|
||||
scene = None
|
||||
|
||||
def __init__(self,scene_cls,robot_cls):
|
||||
app = QApplication(sys.argv)
|
||||
MainWindow = QMainWindow()
|
||||
super(UI, self).__init__(MainWindow)
|
||||
self.scene_cls = scene_cls
|
||||
self.robot_cls = robot_cls
|
||||
|
||||
self.setupUi(MainWindow) # 初始化UI
|
||||
|
||||
# 初始化
|
||||
self.btn_say.clicked.connect(self.btn_say_on_click)
|
||||
|
||||
# 设置一个定时器，每隔100ms检查一次队列
|
||||
timer = QTimer()
|
||||
timer.setInterval(100) # 设置检查队列的间隔时间
|
||||
timer.timeout.connect(self.handle_queue_messages) # 当定时器超时时，处理队列中的消息
|
||||
timer.start() # 启动定时器
|
||||
|
||||
MainWindow.show()
|
||||
|
||||
thread = TaskThread(run_scene,scene_cls,robot_cls)
|
||||
thread.start()
|
||||
|
||||
sys.exit(app.exec_())
|
||||
|
||||
def btn_say_on_click(self):
|
||||
question = self.edit_say.text()
|
||||
if question[-1] == ")":
|
||||
print(f"设置目标:{question}")
|
||||
self.scene_func(("new_set_goal",question))
|
||||
# self.scene.new_set_goal(question)
|
||||
else:
|
||||
self.scene_func(("customer_say","System",question))
|
||||
# self.scene.customer_say("System", question)
|
||||
|
||||
def scene_func(self,args):
|
||||
scene_queue.put(args)
|
||||
|
||||
def handle_queue_messages(self):
|
||||
while not ui_queue.empty():
|
||||
message = ui_queue.get()
|
||||
function_name = message[0]
|
||||
function = getattr(self, function_name, None)
|
||||
|
||||
args = []
|
||||
if len(message)>1:
|
||||
args = message[1:]
|
||||
|
||||
result = function(*args)
|
||||
|
||||
def setupUi(self, MainWindow):
|
||||
Ui_MainWindow.setupUi(self,MainWindow)
|
||||
|
||||
# dir_path = os.path.dirname(__file__)
|
||||
|
||||
# 加载并显示图片
|
||||
# img_path = os.path.join(root_path, "robowaiter/utils/draw_bt/test.png")
|
||||
# self.draw_from_file("img_label_bt",img_path)
|
||||
# self.label.setAlignment(Qt.AlignCenter) # 图片居中显示
|
||||
|
||||
def draw_from_file(self,control_name,file_name):
|
||||
control = getattr(self,control_name,None)
|
||||
# 加载并显示图片
|
||||
pixmap = QPixmap(file_name) # 替换为你的图片路径
|
||||
# self.label.setPixmap(pixmap)
|
||||
control.setPixmap(self.scale_pixmap_to_label(pixmap, control))
|
||||
|
||||
def draw_img(self,control_name,img):
|
||||
control = getattr(self,control_name,None)
|
||||
# 加载并显示图片
|
||||
print(img)
|
||||
img = self.ndarray_to_qimage(img)
|
||||
print(img)
|
||||
pixmap = QPixmap.fromImage(img) # 替换为你的图片路径
|
||||
# self.label.setPixmap(pixmap)
|
||||
control.setPixmap(self.scale_pixmap_to_label(pixmap, control))
|
||||
|
||||
def draw_canvas(self,control_name,canvas):
|
||||
control = getattr(self,control_name,None)
|
||||
# 加载并显示图片
|
||||
|
||||
pixmap = QApplication.grabWidget(canvas)
|
||||
|
||||
# self.label.setPixmap(pixmap)
|
||||
control.setPixmap(self.scale_pixmap_to_label(pixmap, control))
|
||||
|
||||
|
||||
def ndarray_to_qimage(self,array: np.ndarray) -> QImage:
|
||||
# 假设你的ndarray是uint8类型的，且是RGB格式（三个通道）
|
||||
height, width, channels = array.shape
|
||||
if channels ==3:
|
||||
bytes_per_line = channels * width
|
||||
image = QImage(array.data, width, height, bytes_per_line, QImage.Format_RGB888)
|
||||
else:
|
||||
# 对于灰度图像
|
||||
image = QImage(array.data, width, height, QImage.Format_Grayscale8)
|
||||
|
||||
return image
|
||||
|
||||
def scale_pixmap_to_label(self, pixmap, label):
|
||||
# 获取QLabel的大小
|
||||
label_width = label.width()
|
||||
label_height = label.height()
|
||||
|
||||
# 保持图片的长宽比，同时确保图片的一个维度适应QLabel的大小
|
||||
scaled_pixmap = pixmap.scaledToWidth(label_width, Qt.SmoothTransformation)
|
||||
if scaled_pixmap.height() > label_height:
|
||||
scaled_pixmap = pixmap.scaledToHeight(label_height, Qt.SmoothTransformation)
|
||||
|
||||
return scaled_pixmap
|
||||
|
||||
def set_scene(self, scene):
|
||||
self.scene = scene
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
# app = QApplication(sys.argv)
|
||||
# MainWindow = QMainWindow()
|
||||
ui = UI()
|
||||
# ui.setupUi(MainWindow)
|
||||
# MainWindow.show()
|
||||
# sys.exit(app.exec_())
|
||||
|
|
@ -0,0 +1,84 @@
|
|||
"""
|
||||
交互式场景，输入
|
||||
"""
|
||||
import tkinter as tk
|
||||
from robowaiter.utils.ui.pyqt5 import UI
|
||||
import os
|
||||
|
||||
# todo: 接收点单信息，大模型生成任务规划
|
||||
|
||||
from robowaiter.scene.scene import Scene
|
||||
from robowaiter.utils.ui.ui import RobotInterface
|
||||
from robowaiter.utils.bt.draw import render_dot_tree
|
||||
class SceneUI(Scene):
|
||||
scene_queue = None
|
||||
ui_queue = None
|
||||
# camera_interval = 4
|
||||
|
||||
def __init__(self, robot,scene_queue,ui_queue):
|
||||
self.scene_queue = scene_queue
|
||||
self.ui_queue = ui_queue
|
||||
|
||||
super().__init__(robot)
|
||||
# 在这里加入场景中发生的事件
|
||||
|
||||
# while True:
|
||||
# if not self.scene_queue.empty():
|
||||
# param = self.scene_queue.get()
|
||||
# # 处理参数...
|
||||
|
||||
# self.ui_queue.put(('say',"test"))
|
||||
|
||||
def init_robot(self, robot):
|
||||
# init robot
|
||||
self.robot = robot
|
||||
|
||||
if robot:
|
||||
robot.set_scene(self)
|
||||
robot.load_BT()
|
||||
self.draw_current_bt()
|
||||
|
||||
def draw_current_bt(self):
|
||||
render_dot_tree(self.robot.bt.root,target_directory=self.output_path,name="current_bt")
|
||||
self.ui_queue.put(('draw_from_file',"img_label_bt", f"{self.output_path}/current_bt.png"))
|
||||
|
||||
def ui_func(self,args):
|
||||
self.ui_queue.put(args)
|
||||
|
||||
def _reset(self):
|
||||
pass
|
||||
|
||||
def _step(self):
|
||||
# print("已运行")
|
||||
self.handle_queue_messages()
|
||||
# if len(self.sub_task_seq.children) == 0:
|
||||
# question = input("请输入指令：")
|
||||
# if question[-1] == ")":
|
||||
# print(f"设置目标:{question}")
|
||||
# self.new_set_goal(question)
|
||||
# else:
|
||||
# self.customer_say("System",question)
|
||||
|
||||
|
||||
def handle_queue_messages(self):
|
||||
while not self.scene_queue.empty():
|
||||
message = self.scene_queue.get()
|
||||
function_name = message[0]
|
||||
function = getattr(self, function_name, None)
|
||||
|
||||
args = []
|
||||
if len(message)>1:
|
||||
args = message[1:]
|
||||
|
||||
result = function(*args)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
from robowaiter.robot.robot import Robot
|
||||
|
||||
robot = Robot()
|
||||
ui = UI(SceneUI, Robot)
|
||||
|
||||
# create task
|
||||
# task = SceneUI(robot,ui)
|
||||
|
||||
|
|
@ -0,0 +1,110 @@
|
|||
import tkinter as tk
|
||||
from robowaiter.utils.basic import get_root_path
|
||||
from tkinter import PhotoImage
|
||||
import os
|
||||
root_path = get_root_path()
|
||||
|
||||
class Robot:
|
||||
def __init__(self):
|
||||
self.is_running = False
|
||||
|
||||
def start(self):
|
||||
if not self.is_running:
|
||||
self.is_running = True
|
||||
# 在这里添加启动机器人的逻辑代码
|
||||
print("机器人正在启动...")
|
||||
|
||||
def stop(self):
|
||||
if self.is_running:
|
||||
self.is_running = False
|
||||
# 在这里添加停止机器人的逻辑代码
|
||||
print("机器人正在停止...")
|
||||
|
||||
def get_status(self):
|
||||
if self.is_running:
|
||||
return "运行中"
|
||||
else:
|
||||
return "已停止"
|
||||
|
||||
|
||||
class RobotInterface:
|
||||
def __init__(self, master):
|
||||
self.master = master
|
||||
# self.robot = Robot() # 假设这里有一个名为Robot的类，表示机器人系统
|
||||
|
||||
# 创建启动机器人按钮
|
||||
self.start_button = tk.Button(master, text="启动机器人", command=self.start_robot)
|
||||
self.start_button.pack()
|
||||
|
||||
# 创建停止机器人按钮
|
||||
self.stop_button = tk.Button(master, text="停止机器人", command=self.stop_robot)
|
||||
self.stop_button.pack()
|
||||
|
||||
# 创建显示机器人状态的标签
|
||||
self.status_label = tk.Label(master, text="机器人状态：")
|
||||
self.status_label.pack()
|
||||
|
||||
# 创建用于显示图片的三个标签
|
||||
self.image_label1 = tk.Label(master)
|
||||
self.image_label1.pack()
|
||||
self.image_label2 = tk.Label(master)
|
||||
self.image_label2.pack()
|
||||
self.image_label3 = tk.Label(master)
|
||||
self.image_label3.pack()
|
||||
|
||||
# dir_path = os.path.dirname(__file__)
|
||||
# # 加载图片
|
||||
# self.image1 = PhotoImage(file=os.path.join(dir_path,"1.png")) # 替换为实际图片路径
|
||||
# self.image2 = PhotoImage(file=os.path.join(dir_path,"2.png")) # 替换为实际图片路径
|
||||
# self.image3 = PhotoImage(file=os.path.join(dir_path,"3.png")) # 替换为实际图片路径
|
||||
#
|
||||
# # 显示第一张图片
|
||||
# self.show_image(1)
|
||||
|
||||
def display_binary_image(self,data):
|
||||
# 创建一个Tkinter窗口
|
||||
|
||||
# 创建一个像素画布
|
||||
canvas = tk.Canvas(self.master, width=len(data[0]), height=len(data), bg='white')
|
||||
canvas.pack()
|
||||
|
||||
# 在画布上绘制像素点
|
||||
for i in range(len(data)):
|
||||
for j in range(len(data[0])):
|
||||
if data[i][j] == 1:
|
||||
canvas.create_rectangle(j, i, j + 1, i + 1, fill='black')
|
||||
|
||||
def show_image(self, image_number):
|
||||
if image_number == 1:
|
||||
self.image_label1.config(image=self.image1)
|
||||
elif image_number == 2:
|
||||
self.image_label2.config(image=self.image2)
|
||||
elif image_number == 3:
|
||||
self.image_label3.config(image=self.image3)
|
||||
else:
|
||||
print("无效的图片编号")
|
||||
|
||||
def start_robot(self):
|
||||
self.robot.start() # 假设Robot类有一个start方法来启动机器人
|
||||
self.update_status("机器人已启动。")
|
||||
|
||||
def stop_robot(self):
|
||||
self.robot.stop() # 假设Robot类有一个stop方法来停止机器人
|
||||
self.update_status("机器人已停止。")
|
||||
|
||||
def update_status(self, status):
|
||||
self.status_label.config(text="机器人状态： " + status)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
|
||||
root = tk.Tk()
|
||||
interface = RobotInterface(root)
|
||||
binary_image = [
|
||||
[0, 1, 0],
|
||||
[1, 1, 1],
|
||||
[0, 1, 0]
|
||||
]
|
||||
interface.display_binary_image(binary_image)
|
||||
|
||||
root.mainloop()
|
||||
|
|
@ -0,0 +1 @@
|
|||
pyuic5 -x window.ui -o window.py
|
||||
|
|
@ -0,0 +1,67 @@
|
|||
# -*- coding: utf-8 -*-
|
||||
|
||||
# Form implementation generated from reading ui file 'window.ui'
|
||||
#
|
||||
# Created by: PyQt5 UI code generator 5.15.7
|
||||
#
|
||||
# WARNING: Any manual changes made to this file will be lost when pyuic5 is
|
||||
# run again. Do not edit this file unless you know what you are doing.
|
||||
|
||||
|
||||
from PyQt5 import QtCore, QtGui, QtWidgets
|
||||
|
||||
|
||||
class Ui_MainWindow(object):
|
||||
def setupUi(self, MainWindow):
|
||||
MainWindow.setObjectName("MainWindow")
|
||||
MainWindow.resize(960, 1080)
|
||||
MainWindow.setAutoFillBackground(True)
|
||||
self.centralwidget = QtWidgets.QWidget(MainWindow)
|
||||
self.centralwidget.setObjectName("centralwidget")
|
||||
self.edit_say = QtWidgets.QLineEdit(self.centralwidget)
|
||||
self.edit_say.setGeometry(QtCore.QRect(450, 30, 221, 31))
|
||||
self.edit_say.setObjectName("edit_say")
|
||||
self.btn_say = QtWidgets.QPushButton(self.centralwidget)
|
||||
self.btn_say.setGeometry(QtCore.QRect(680, 40, 75, 23))
|
||||
self.btn_say.setObjectName("btn_say")
|
||||
self.img_label_canvas = QtWidgets.QLabel(self.centralwidget)
|
||||
self.img_label_canvas.setGeometry(QtCore.QRect(0, 160, 811, 441))
|
||||
self.img_label_canvas.setStyleSheet("border: 2px solid black;")
|
||||
self.img_label_canvas.setText("")
|
||||
self.img_label_canvas.setObjectName("img_label_canvas")
|
||||
self.img_label_bt = QtWidgets.QLabel(self.centralwidget)
|
||||
self.img_label_bt.setGeometry(QtCore.QRect(0, 670, 811, 311))
|
||||
self.img_label_bt.setStyleSheet("border: 2px solid black;")
|
||||
self.img_label_bt.setText("")
|
||||
self.img_label_bt.setObjectName("img_label_bt")
|
||||
self.label_5 = QtWidgets.QLabel(self.centralwidget)
|
||||
self.label_5.setGeometry(QtCore.QRect(400, 630, 111, 16))
|
||||
self.label_5.setObjectName("label_5")
|
||||
MainWindow.setCentralWidget(self.centralwidget)
|
||||
self.menubar = QtWidgets.QMenuBar(MainWindow)
|
||||
self.menubar.setGeometry(QtCore.QRect(0, 0, 960, 23))
|
||||
self.menubar.setObjectName("menubar")
|
||||
MainWindow.setMenuBar(self.menubar)
|
||||
self.statusbar = QtWidgets.QStatusBar(MainWindow)
|
||||
self.statusbar.setObjectName("statusbar")
|
||||
MainWindow.setStatusBar(self.statusbar)
|
||||
|
||||
self.retranslateUi(MainWindow)
|
||||
QtCore.QMetaObject.connectSlotsByName(MainWindow)
|
||||
|
||||
def retranslateUi(self, MainWindow):
|
||||
_translate = QtCore.QCoreApplication.translate
|
||||
MainWindow.setWindowTitle(_translate("MainWindow", "MainWindow"))
|
||||
self.edit_say.setText(_translate("MainWindow", "做一杯咖啡"))
|
||||
self.btn_say.setText(_translate("MainWindow", "说话"))
|
||||
self.label_5.setText(_translate("MainWindow", "语义地图"))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
import sys
|
||||
app = QtWidgets.QApplication(sys.argv)
|
||||
MainWindow = QtWidgets.QMainWindow()
|
||||
ui = Ui_MainWindow()
|
||||
ui.setupUi(MainWindow)
|
||||
MainWindow.show()
|
||||
sys.exit(app.exec_())
|
||||
|
|
@ -0,0 +1,106 @@
|
|||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<ui version="4.0">
|
||||
<class>MainWindow</class>
|
||||
<widget class="QMainWindow" name="MainWindow">
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<width>960</width>
|
||||
<height>1080</height>
|
||||
</rect>
|
||||
</property>
|
||||
<property name="windowTitle">
|
||||
<string>MainWindow</string>
|
||||
</property>
|
||||
<property name="autoFillBackground">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<widget class="QWidget" name="centralwidget">
|
||||
<widget class="QLineEdit" name="edit_say">
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>450</x>
|
||||
<y>30</y>
|
||||
<width>221</width>
|
||||
<height>31</height>
|
||||
</rect>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>做一杯咖啡</string>
|
||||
</property>
|
||||
</widget>
|
||||
<widget class="QPushButton" name="btn_say">
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>680</x>
|
||||
<y>40</y>
|
||||
<width>75</width>
|
||||
<height>23</height>
|
||||
</rect>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>说话</string>
|
||||
</property>
|
||||
</widget>
|
||||
<widget class="QLabel" name="img_label_canvas">
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>0</x>
|
||||
<y>160</y>
|
||||
<width>811</width>
|
||||
<height>441</height>
|
||||
</rect>
|
||||
</property>
|
||||
<property name="styleSheet">
|
||||
<string notr="true">border: 2px solid black;</string>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
</widget>
|
||||
<widget class="QLabel" name="img_label_bt">
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>0</x>
|
||||
<y>670</y>
|
||||
<width>811</width>
|
||||
<height>311</height>
|
||||
</rect>
|
||||
</property>
|
||||
<property name="styleSheet">
|
||||
<string notr="true">border: 2px solid black;</string>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
</widget>
|
||||
<widget class="QLabel" name="label_5">
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>400</x>
|
||||
<y>630</y>
|
||||
<width>111</width>
|
||||
<height>16</height>
|
||||
</rect>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>语义地图</string>
|
||||
</property>
|
||||
</widget>
|
||||
</widget>
|
||||
<widget class="QMenuBar" name="menubar">
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<width>960</width>
|
||||
<height>23</height>
|
||||
</rect>
|
||||
</property>
|
||||
</widget>
|
||||
<widget class="QStatusBar" name="statusbar"/>
|
||||
</widget>
|
||||
<resources/>
|
||||
<connections/>
|
||||
</ui>
|
||||
|
|
@ -1,16 +0,0 @@
|
|||
import os
|
||||
from robowaiter import Robot, task_map
|
||||
|
||||
TASK_NAME = 'VLM'
|
||||
|
||||
# create robot
|
||||
project_path = "./robowaiter"
|
||||
ptml_path = os.path.join(project_path, 'robot/Default.ptml')
|
||||
behavior_lib_path = os.path.join(project_path, 'behavior_lib')
|
||||
|
||||
robot = Robot(ptml_path, behavior_lib_path)
|
||||
|
||||
# create task
|
||||
task = task_map[TASK_NAME](robot)
|
||||
task.reset()
|
||||
task.run()
|
||||
|
|
@ -1,31 +0,0 @@
|
|||
selector{
|
||||
|
||||
sequence{
|
||||
cond On(Coffee,CoffeeTable)
|
||||
cond On(Coffee,CoffeeTable)
|
||||
}
|
||||
|
||||
sequence{
|
||||
cond Holding(Nothing)
|
||||
act Make(Coffee)
|
||||
}
|
||||
sequence{
|
||||
cond At(Robot,CoffeeTable)
|
||||
cond Holding(Coffee)
|
||||
act PutDown(Coffee,CoffeeTable)
|
||||
}
|
||||
sequence{
|
||||
cond At(Robot,Bar)
|
||||
cond Holding(Coffee)
|
||||
act PutDown(Coffee,Bar)
|
||||
}
|
||||
sequence{
|
||||
cond At(Robot,WaterTable)
|
||||
cond Holding(Coffee)
|
||||
act PutDown(Coffee,WaterTable)
|
||||
}
|
||||
sequence{
|
||||
cond Holding(Coffee)
|
||||
act MoveTo(CoffeeTable)
|
||||
}
|
||||
}
|
||||
|
|
@ -40,7 +40,7 @@ class SceneAEM(Scene):
|
|||
map_ratio = self.map_ratio
|
||||
db = DBSCAN(eps=map_ratio, min_samples=int(map_ratio / 2))
|
||||
|
||||
file_name = '../../proto/map_1.pkl'
|
||||
file_name = '../../robowaiter/proto/map_1.pkl'
|
||||
if os.path.exists(file_name):
|
||||
with open(file_name, 'rb') as file:
|
||||
map = pickle.load(file)
|
||||
|
|
@ -117,7 +117,7 @@ class SceneAEM(Scene):
|
|||
{"id": f"{i}", "name": f"{cur_objs[i].name}", "location": f"{cur_objs[i].location}",
|
||||
"height": f"{cur_objs[i].location.Z}"})
|
||||
|
||||
with open('../../proto/objs.json', 'w') as file:
|
||||
with open('../../robowaiter/proto/objs.json', 'w') as file:
|
||||
json.dump(obj_json_data, file)
|
||||
|
||||
# for i in range(-350, 600):
|
||||
|
|
@ -1,10 +1,5 @@
|
|||
"""
|
||||
人提出请求，机器人完成任务
|
||||
1. 做咖啡（固定动画）：接收到做咖啡指令、走到咖啡机、拿杯子、操作咖啡机、取杯子、送到客人桌子上
|
||||
2. 倒水
|
||||
3. 夹点心
|
||||
|
||||
具体描述：设计一套点单规则（如菜单包含咖啡、水、点心等），按照规则拟造随机的订单。在收到订单后，通过大模型让机器人输出合理的备餐计划，并尝试在模拟环境中按照这个规划实现任务。
|
||||
交互式场景，输入
|
||||
|
||||
"""
|
||||
|
||||
|
|
@ -27,9 +22,9 @@ class SubScene(Scene):
|
|||
question = input("请输入指令：")
|
||||
if question[-1] == ")":
|
||||
print(f"设置目标:{question}")
|
||||
self.set_goal(question)()
|
||||
self.new_set_goal(question)
|
||||
else:
|
||||
self.state['chat_list'].append(question)
|
||||
self.customer_say("System",question)
|
||||
|
||||
|
||||
|
||||
|
|
@ -3,8 +3,6 @@
|
|||
|
||||
测试导航算法的动态避障(Obstacle Detection and Avoidance, ODA)能力
|
||||
|
||||
|
||||
|
||||
"""
|
||||
import os
|
||||
import pickle
|
||||
|
|
@ -1,27 +0,0 @@
|
|||
import py_trees as ptree
|
||||
from typing import Any
|
||||
|
||||
# _base Behavior
|
||||
class Bahavior(ptree.behaviour.Behaviour):
|
||||
scene = None
|
||||
def __init__(self, name: str, scene):
|
||||
super().__init__(name)
|
||||
self.scene = scene
|
||||
|
||||
def setup(self, **kwargs: Any) -> None:
|
||||
return super().setup(**kwargs)
|
||||
|
||||
def initialise(self) -> None:
|
||||
return super().initialise()
|
||||
|
||||
def _update(self) -> ptree.common.Status:
|
||||
print("this is just a _base behavior node.")
|
||||
|
||||
|
||||
def update(self) -> ptree.common.Status:
|
||||
re = self._update()
|
||||
print(f"{self.__class__.__name__}: {re.value}")
|
||||
return re
|
||||
|
||||
def terminate(self, new_status: ptree.common.Status) -> None:
|
||||
return super().terminate(new_status)
|
||||
|
|
@ -1,23 +0,0 @@
|
|||
import py_trees as ptree
|
||||
from typing import Any
|
||||
from robowaiter.behavior_lib._base.Behavior import Bahavior
|
||||
|
||||
class Chatting(Bahavior):
|
||||
def __init__(self, name: str, scene):
|
||||
super().__init__(name, scene)
|
||||
|
||||
def setup(self, **kwargs: Any) -> None:
|
||||
return super().setup(**kwargs)
|
||||
|
||||
def initialise(self) -> None:
|
||||
return super().initialise()
|
||||
|
||||
def _update(self) -> ptree.common.Status:
|
||||
# if self.scene.status?
|
||||
if self.scene.state['chat_list'] == []:
|
||||
return ptree.common.Status.FAILURE
|
||||
else:
|
||||
return ptree.common.Status.SUCCESS
|
||||
|
||||
def terminate(self, new_status: ptree.common.Status) -> None:
|
||||
return super().terminate(new_status)
|
||||
|
|
@ -1,20 +0,0 @@
|
|||
import py_trees as ptree
|
||||
from typing import Any
|
||||
from robowaiter.behavior_lib._base.Behavior import Bahavior
|
||||
|
||||
class CoffeeCupFound(Bahavior):
|
||||
def __init__(self, name: str, scene):
|
||||
super().__init__(name, scene)
|
||||
|
||||
def setup(self, **kwargs: Any) -> None:
|
||||
return super().setup(**kwargs)
|
||||
|
||||
def initialise(self) -> None:
|
||||
return super().initialise()
|
||||
|
||||
def update(self) -> ptree.common.Status:
|
||||
print("Start checking IsChatting...")
|
||||
return ptree.common.Status.SUCCESS
|
||||
|
||||
def terminate(self, new_status: ptree.common.Status) -> None:
|
||||
return super().terminate(new_status)
|
||||
|
|
@ -1,22 +0,0 @@
|
|||
import py_trees as ptree
|
||||
from typing import Any
|
||||
from robowaiter.behavior_lib._base.Behavior import Bahavior
|
||||
|
||||
class CoffeeCupGrasped(Bahavior):
|
||||
def __init__(self, name: str, scene):
|
||||
super().__init__(name, scene)
|
||||
|
||||
|
||||
def setup(self, **kwargs: Any) -> None:
|
||||
return super().setup(**kwargs)
|
||||
|
||||
def initialise(self) -> None:
|
||||
return super().initialise()
|
||||
|
||||
def update(self) -> ptree.common.Status:
|
||||
print('Start checking IsChatting...')
|
||||
return ptree.common.Status.SUCCESS
|
||||
|
||||
def terminate(self, new_status: ptree.common.Status) -> None:
|
||||
return super().terminate(new_status)
|
||||
|
||||
|
|
@ -1,21 +0,0 @@
|
|||
import py_trees as ptree
|
||||
from typing import Any
|
||||
|
||||
class CoffeeCupPlaced(ptree.behaviour.Behaviour):
|
||||
|
||||
def __init__(self, name: str, scene):
|
||||
super().__init__(name)
|
||||
|
||||
def setup(self, **kwargs: Any) -> None:
|
||||
return super().setup(**kwargs)
|
||||
|
||||
def initialise(self) -> None:
|
||||
return super().initialise()
|
||||
|
||||
def update(self) -> ptree.common.Status:
|
||||
print('Start checking IsChatting...')
|
||||
return ptree.common.Status.SUCCESS
|
||||
|
||||
def terminate(self, new_status: ptree.common.Status) -> None:
|
||||
return super().terminate(new_status)
|
||||
|
||||
|
|
@ -1,26 +0,0 @@
|
|||
import py_trees as ptree
|
||||
from typing import Any
|
||||
from robowaiter.behavior_lib._base.Behavior import Bahavior
|
||||
from robowaiter.llm_client.ask_llm import ask_llm
|
||||
|
||||
class DealChat(Bahavior):
|
||||
def __init__(self, name: str, scene):
|
||||
super().__init__(name, scene)
|
||||
|
||||
def setup(self, **kwargs: Any) -> None:
|
||||
return super().setup(**kwargs)
|
||||
|
||||
def initialise(self) -> None:
|
||||
return super().initialise()
|
||||
|
||||
def _update(self) -> ptree.common.Status:
|
||||
# if self.scene.status?
|
||||
chat = self.scene.state['chat_list'].pop()
|
||||
answer = ask_llm(chat)
|
||||
print(f"机器人回答：{answer}")
|
||||
self.scene.chat_bubble(f"机器人回答：{answer}")
|
||||
|
||||
return ptree.common.Status.RUNNING
|
||||
|
||||
def terminate(self, new_status: ptree.common.Status) -> None:
|
||||
return super().terminate(new_status)
|
||||
|
|
@ -1,21 +0,0 @@
|
|||
import py_trees as ptree
|
||||
from typing import Any
|
||||
|
||||
class DestinationAReached(ptree.behaviour.Behaviour):
|
||||
|
||||
def __init__(self, name: str, scene):
|
||||
super().__init__(name)
|
||||
|
||||
def setup(self, **kwargs: Any) -> None:
|
||||
return super().setup(**kwargs)
|
||||
|
||||
def initialise(self) -> None:
|
||||
return super().initialise()
|
||||
|
||||
def update(self) -> ptree.common.Status:
|
||||
print('Start checking IsChatting...')
|
||||
return ptree.common.Status.SUCCESS
|
||||
|
||||
def terminate(self, new_status: ptree.common.Status) -> None:
|
||||
return super().terminate(new_status)
|
||||
|
||||
|
|
@ -1,21 +0,0 @@
|
|||
import py_trees as ptree
|
||||
from typing import Any
|
||||
|
||||
class FindCoffeeCup(ptree.behaviour.Behaviour):
|
||||
|
||||
def __init__(self, name: str, scene):
|
||||
super().__init__(name)
|
||||
|
||||
def setup(self, **kwargs: Any) -> None:
|
||||
return super().setup(**kwargs)
|
||||
|
||||
def initialise(self) -> None:
|
||||
return super().initialise()
|
||||
|
||||
def update(self) -> ptree.common.Status:
|
||||
print('Start checking IsChatting...')
|
||||
return ptree.common.Status.SUCCESS
|
||||
|
||||
def terminate(self, new_status: ptree.common.Status) -> None:
|
||||
return super().terminate(new_status)
|
||||
|
||||
|
|
@ -1,21 +0,0 @@
|
|||
import py_trees as ptree
|
||||
from typing import Any
|
||||
|
||||
class Grasp(ptree.behaviour.Behaviour):
|
||||
|
||||
def __init__(self, name: str, scene):
|
||||
super().__init__(name)
|
||||
|
||||
def setup(self, **kwargs: Any) -> None:
|
||||
return super().setup(**kwargs)
|
||||
|
||||
def initialise(self) -> None:
|
||||
return super().initialise()
|
||||
|
||||
def _update(self) -> ptree.common.Status:
|
||||
print('Start checking IsChatting...')
|
||||
return ptree.common.Status.SUCCESS
|
||||
|
||||
def terminate(self, new_status: ptree.common.Status) -> None:
|
||||
return super().terminate(new_status)
|
||||
|
||||
|
|
@ -1,21 +0,0 @@
|
|||
import py_trees as ptree
|
||||
from typing import Any
|
||||
|
||||
class GraspCoffeeCup(ptree.behaviour.Behaviour):
|
||||
|
||||
def __init__(self, name: str, scene):
|
||||
super().__init__(name)
|
||||
|
||||
def setup(self, **kwargs: Any) -> None:
|
||||
return super().setup(**kwargs)
|
||||
|
||||
def initialise(self) -> None:
|
||||
return super().initialise()
|
||||
|
||||
def update(self) -> ptree.common.Status:
|
||||
print('Start checking IsChatting...')
|
||||
return ptree.common.Status.SUCCESS
|
||||
|
||||
def terminate(self, new_status: ptree.common.Status) -> None:
|
||||
return super().terminate(new_status)
|
||||
|
||||
|
|
@ -1,21 +0,0 @@
|
|||
import py_trees as ptree
|
||||
from typing import Any
|
||||
|
||||
class Istask(ptree.behaviour.Behaviour):
|
||||
|
||||
def __init__(self, name: str, scene):
|
||||
super().__init__(name)
|
||||
|
||||
def setup(self, **kwargs: Any) -> None:
|
||||
return super().setup(**kwargs)
|
||||
|
||||
def initialise(self) -> None:
|
||||
return super().initialise()
|
||||
|
||||
def update(self) -> ptree.common.Status:
|
||||
print('Start checking IsChatting...')
|
||||
return ptree.common.Status.SUCCESS
|
||||
|
||||
def terminate(self, new_status: ptree.common.Status) -> None:
|
||||
return super().terminate(new_status)
|
||||
|
||||
|
|
@ -1,21 +0,0 @@
|
|||
import py_trees as ptree
|
||||
from typing import Any
|
||||
|
||||
class MoveTo(ptree.behaviour.Behaviour):
|
||||
|
||||
def __init__(self, name: str, scene, a, b, c, d):
|
||||
super().__init__(name)
|
||||
|
||||
def setup(self, **kwargs: Any) -> None:
|
||||
return super().setup(**kwargs)
|
||||
|
||||
def initialise(self) -> None:
|
||||
return super().initialise()
|
||||
|
||||
def _update(self) -> ptree.common.Status:
|
||||
print('Start checking IsChatting...')
|
||||
return ptree.common.Status.SUCCESS
|
||||
|
||||
def terminate(self, new_status: ptree.common.Status) -> None:
|
||||
return super().terminate(new_status)
|
||||
|
||||
|
|
@ -1,21 +0,0 @@
|
|||
import py_trees as ptree
|
||||
from typing import Any
|
||||
|
||||
class PlaceCoffeeCup(ptree.behaviour.Behaviour):
|
||||
|
||||
def __init__(self, name: str, scene):
|
||||
super().__init__(name)
|
||||
|
||||
def setup(self, **kwargs: Any) -> None:
|
||||
return super().setup(**kwargs)
|
||||
|
||||
def initialise(self) -> None:
|
||||
return super().initialise()
|
||||
|
||||
def update(self) -> ptree.common.Status:
|
||||
print('Start checking IsChatting...')
|
||||
return ptree.common.Status.SUCCESS
|
||||
|
||||
def terminate(self, new_status: ptree.common.Status) -> None:
|
||||
return super().terminate(new_status)
|
||||
|
||||
|
|
@ -1,21 +0,0 @@
|
|||
import py_trees as ptree
|
||||
from typing import Any
|
||||
|
||||
class ReachDestinationA(ptree.behaviour.Behaviour):
|
||||
|
||||
def __init__(self, name: str, scene):
|
||||
super().__init__(name)
|
||||
|
||||
def setup(self, **kwargs: Any) -> None:
|
||||
return super().setup(**kwargs)
|
||||
|
||||
def initialise(self) -> None:
|
||||
return super().initialise()
|
||||
|
||||
def update(self) -> ptree.common.Status:
|
||||
print('Start checking IsChatting...')
|
||||
return ptree.common.Status.SUCCESS
|
||||
|
||||
def terminate(self, new_status: ptree.common.Status) -> None:
|
||||
return super().terminate(new_status)
|
||||
|
||||
|
|
@ -1,21 +0,0 @@
|
|||
import py_trees as ptree
|
||||
from typing import Any
|
||||
|
||||
class SeqTest(ptree.behaviour.Behaviour):
|
||||
|
||||
def __init__(self, name: str, scene):
|
||||
super().__init__(name)
|
||||
|
||||
def setup(self, **kwargs: Any) -> None:
|
||||
return super().setup(**kwargs)
|
||||
|
||||
def initialise(self) -> None:
|
||||
return super().initialise()
|
||||
|
||||
def update(self) -> ptree.common.Status:
|
||||
print('Start checking IsChatting...')
|
||||
return ptree.common.Status.SUCCESS
|
||||
|
||||
def terminate(self, new_status: ptree.common.Status) -> None:
|
||||
return super().terminate(new_status)
|
||||
|
||||
|
|
@ -1,21 +0,0 @@
|
|||
import py_trees as ptree
|
||||
from typing import Any
|
||||
|
||||
class TestTask(ptree.behaviour.Behaviour):
|
||||
|
||||
def __init__(self, name: str, scene):
|
||||
super().__init__(name)
|
||||
|
||||
def setup(self, **kwargs: Any) -> None:
|
||||
return super().setup(**kwargs)
|
||||
|
||||
def initialise(self) -> None:
|
||||
return super().initialise()
|
||||
|
||||
def update(self) -> ptree.common.Status:
|
||||
print('Start checking IsChatting...')
|
||||
return ptree.common.Status.SUCCESS
|
||||
|
||||
def terminate(self, new_status: ptree.common.Status) -> None:
|
||||
return super().terminate(new_status)
|
||||
|
||||
|
|
@ -1,124 +0,0 @@
|
|||
"""
|
||||
顶层行为树中的动作与条件节点
|
||||
"""
|
||||
|
||||
from typing import *
|
||||
import py_trees
|
||||
from py_trees import common
|
||||
from py_trees.common import Status
|
||||
|
||||
|
||||
##############################################################
|
||||
# 条件节点
|
||||
##############################################################
|
||||
|
||||
class IsChatting(py_trees.behaviour.Behaviour):
|
||||
def __init__(self, name: str = ""):
|
||||
super().__init__(name)
|
||||
|
||||
def setup(self, **kwargs: Any) -> None:
|
||||
return super().setup(**kwargs)
|
||||
|
||||
def initialise(self) -> None:
|
||||
return super().initialise()
|
||||
|
||||
def update(self) -> Status:
|
||||
print('Start checking IsChatting...')
|
||||
return common.Status.SUCCESS
|
||||
|
||||
def terminate(self, new_status: Status) -> None:
|
||||
return super().terminate(new_status)
|
||||
|
||||
|
||||
class IsTakingAction(py_trees.behaviour.Behaviour):
|
||||
def __init__(self, name: str = ""):
|
||||
super().__init__(name)
|
||||
|
||||
def setup(self, **kwargs: Any) -> None:
|
||||
return super().setup(**kwargs)
|
||||
|
||||
def initialise(self) -> None:
|
||||
return super().initialise()
|
||||
|
||||
def update(self) -> Status:
|
||||
print('Start checking IsTakingAction...')
|
||||
return common.Status.SUCCESS
|
||||
|
||||
def terminate(self, new_status: Status) -> None:
|
||||
return super().terminate(new_status)
|
||||
|
||||
|
||||
class IsSomethingMore(py_trees.behaviour.Behaviour):
|
||||
def __init__(self, name: str = ""):
|
||||
super().__init__(name)
|
||||
|
||||
def setup(self, **kwargs: Any) -> None:
|
||||
return super().setup(**kwargs)
|
||||
|
||||
def initialise(self) -> None:
|
||||
return super().initialise()
|
||||
|
||||
def update(self) -> Status:
|
||||
print('Start checking IsSomethingMore...')
|
||||
return common.Status.SUCCESS
|
||||
|
||||
def terminate(self, new_status: Status) -> None:
|
||||
return super().terminate(new_status)
|
||||
|
||||
|
||||
##############################################################
|
||||
# 动作节点
|
||||
##############################################################
|
||||
|
||||
class Chatting(py_trees.behaviour.Behaviour):
|
||||
def __init__(self, name: str = ""):
|
||||
super().__init__(name)
|
||||
|
||||
def setup(self, **kwargs: Any) -> None:
|
||||
return super().setup(**kwargs)
|
||||
|
||||
def initialise(self) -> None:
|
||||
return super().initialise()
|
||||
|
||||
def update(self) -> Status:
|
||||
print('Start executing Chatting...')
|
||||
return common.Status.SUCCESS
|
||||
|
||||
def terminate(self, new_status: Status) -> None:
|
||||
return super().terminate(new_status)
|
||||
|
||||
|
||||
class TakingAction(py_trees.behaviour.Behaviour):
|
||||
def __init__(self, name: str = ""):
|
||||
super().__init__(name)
|
||||
|
||||
def setup(self, **kwargs: Any) -> None:
|
||||
return super().setup(**kwargs)
|
||||
|
||||
def initialise(self) -> None:
|
||||
return super().initialise()
|
||||
|
||||
def update(self) -> Status:
|
||||
print('Start executing TakingAction...')
|
||||
return common.Status.SUCCESS
|
||||
|
||||
def terminate(self, new_status: Status) -> None:
|
||||
return super().terminate(new_status)
|
||||
|
||||
|
||||
class TakingMoreAction(py_trees.behaviour.Behaviour):
|
||||
def __init__(self, name: str = ""):
|
||||
super().__init__(name)
|
||||
|
||||
def setup(self, **kwargs: Any) -> None:
|
||||
return super().setup(**kwargs)
|
||||
|
||||
def initialise(self) -> None:
|
||||
return super().initialise()
|
||||
|
||||
def update(self) -> Status:
|
||||
print('Start executing TakingMoreAction...')
|
||||
return common.Status.SUCCESS
|
||||
|
||||
def terminate(self, new_status: Status) -> None:
|
||||
return super().terminate(new_status)
|
||||
|
|
@ -1,66 +0,0 @@
|
|||
import py_trees
|
||||
from behavior_library import *
|
||||
|
||||
|
||||
def LoadMainTree() -> py_trees.trees.BehaviourTree:
|
||||
"""
|
||||
此方法用于加载固定的顶层行为树（不包括实际执行）
|
||||
|
||||
Args: None
|
||||
"""
|
||||
|
||||
seq_subtree_0 = py_trees.composites.Sequence(
|
||||
name='seq_subtree_0',
|
||||
memory=False,
|
||||
children=[IsChatting(), Chatting()]
|
||||
)
|
||||
|
||||
seq_subtree_1 = py_trees.composites.Sequence(
|
||||
name='seq_subtree_1',
|
||||
memory=False,
|
||||
children=[IsTakingAction(), TakingAction()]
|
||||
)
|
||||
|
||||
seq_subtree_2 = py_trees.composites.Sequence(
|
||||
name='seq_subtree_2',
|
||||
memory=False,
|
||||
children=[IsSomethingMore(), TakingMoreAction()]
|
||||
)
|
||||
|
||||
root = py_trees.composites.Selector(
|
||||
name='selector_root',
|
||||
memory=False,
|
||||
children=[seq_subtree_0, seq_subtree_1, seq_subtree_2]
|
||||
)
|
||||
|
||||
return py_trees.trees.BehaviourTree(root)
|
||||
|
||||
|
||||
def LoadSubTree(path: str) -> py_trees.behaviour.Behaviour:
|
||||
"""
|
||||
此方法用于从ptml文件中加载行为树（不包括实际执行）
|
||||
|
||||
Args:
|
||||
-- path: ptml文件的路径
|
||||
"""
|
||||
# TODO
|
||||
pass
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
btree = LoadMainTree()
|
||||
|
||||
|
||||
def print_tree(tree):
|
||||
print(py_trees.display.unicode_tree(root=tree.root, show_status=True))
|
||||
|
||||
|
||||
try:
|
||||
btree.tick_tock(
|
||||
period_ms=500,
|
||||
number_of_iterations=py_trees.trees.CONTINUOUS_TICK_TOCK,
|
||||
pre_tick_handler=None,
|
||||
post_tick_handler=print_tree
|
||||
)
|
||||
except KeyboardInterrupt:
|
||||
btree.interrupt()
|
||||
|
|
@ -1,59 +0,0 @@
|
|||
#!/usr/bin/env python3
|
||||
# -*- encoding: utf-8 -*-
|
||||
import sys
|
||||
import time
|
||||
import grpc
|
||||
|
||||
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
|
||||
|
||||
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 joint_test(scene_id=0):
|
||||
print('------------------joint_test----------------------')
|
||||
action_list = [[0, 0, 0, 0, 0, 30, 0, 0, 0, 0, 0, 0, 0, 0, 36.0, -39.37, 37.2, -92.4, 4.13, -0.62, 0.4],
|
||||
[0, 0, 0, 0, 0, 30, 0, 0, 0, 0, 0, 0, 0, 0, 36.0, -39.62, 34.75, -94.80, 3.22, -0.26, 0.85],
|
||||
[0, 0, 0, 0, 0, 30, 0, 0, 0, 0, 0, 0, 0, 0, 32.63, -32.80, 15.15, -110.70, 6.86, 2.36, 0.40],
|
||||
[0, 0, 0, 0, 0, 30, 0, 0, 0, 0, 0, 0, 0, 0, 28.18, -27.92, 6.75, -115.02, 9.46, 4.28, 1.35],
|
||||
[0, 0, 0, 0, 0, 30, 0, 0, 0, 0, 0, 0, 0, 0, 4.09, -13.15, -11.97, -107.35, 13.08, 8.58, 3.33]]
|
||||
|
||||
for value in action_list:
|
||||
action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.RotateJoints, values=value)
|
||||
scene = sim_client.Do(action)
|
||||
|
||||
for i in range(8, 21): # arm
|
||||
print(
|
||||
f"{scene.joints[i].name}:{scene.joints[i].angle} location:{scene.joints[i].location.X},{scene.joints[i].location.Y},{scene.joints[i].location.Z}"
|
||||
)
|
||||
print('')
|
||||
for i in range(5, 10): # Right hand
|
||||
print(
|
||||
f"{scene.fingers[i].name} angle:{scene.fingers[i].angle} location:{scene.fingers[i].location[0].X},{scene.fingers[i].location[0].Y},{scene.fingers[i].location[0].Z}"
|
||||
)
|
||||
print('----------------------------------------')
|
||||
time.sleep(0.03)
|
||||
time.sleep(1)
|
||||
|
||||
if __name__ == '__main__':
|
||||
map_id = 3 # 地图编号: 3: 咖啡厅
|
||||
scene_num = 1 # 场景数量
|
||||
map_test(map_id, scene_num) # 场景加载测试
|
||||
time.sleep(5)
|
||||
|
||||
for i in range(scene_num):
|
||||
print("------------------", i, "----------------------")
|
||||
joint_test(i) # 关节控制测试
|
||||
|
|
@ -1,94 +0,0 @@
|
|||
#!/usr/bin/env python3
|
||||
# -*- encoding: utf-8 -*-
|
||||
# enconding = utf8
|
||||
import sys
|
||||
import time
|
||||
import grpc
|
||||
|
||||
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
|
||||
|
||||
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 control_robot_action(scene_id=0, type=0, action=0, message="你好"):
|
||||
scene = sim_client.ControlRobot(GrabSim_pb2.ControlInfo(scene=scene_id, type=type, action=action, content=message))
|
||||
if (scene.info == "action success"):
|
||||
return True
|
||||
else:
|
||||
return False
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
map_id = 3 # 地图编号: 0：空房间 1：室内 2:咖啡厅1.0 3: 咖啡厅2.0 4:餐厅 5:养老院 6：会议室
|
||||
scene_num = 1 # 场景数量
|
||||
map_test(map_id, scene_num) # 场景加载测试
|
||||
time.sleep(5)
|
||||
|
||||
# 制作咖啡
|
||||
control_robot_action(0, 0, 1, "开始制作咖啡")
|
||||
result = control_robot_action(0, 1, 1)
|
||||
if (result):
|
||||
control_robot_action(0, 1, 2)
|
||||
control_robot_action(0, 1, 3)
|
||||
control_robot_action(0, 1, 4)
|
||||
else:
|
||||
control_robot_action(0, 0, 1, "制作咖啡失败")
|
||||
|
||||
# 倒水
|
||||
control_robot_action(0, 0, 1, "开始倒水")
|
||||
result = control_robot_action(0, 2, 1)
|
||||
if (result):
|
||||
control_robot_action(0, 2, 2)
|
||||
control_robot_action(0, 2, 3)
|
||||
control_robot_action(0, 2, 4)
|
||||
control_robot_action(0, 2, 5)
|
||||
else:
|
||||
control_robot_action(0, 0, 1, "倒水失败")
|
||||
|
||||
# 夹点心
|
||||
control_robot_action(0, 0, 1, "开始夹点心")
|
||||
result = control_robot_action(0, 3, 1)
|
||||
if (result):
|
||||
control_robot_action(0, 3, 2)
|
||||
control_robot_action(0, 3, 3)
|
||||
control_robot_action(0, 3, 4)
|
||||
control_robot_action(0, 3, 5)
|
||||
control_robot_action(0, 3, 6)
|
||||
control_robot_action(0, 3, 7)
|
||||
else:
|
||||
control_robot_action(0, 0, 1, "夹点心失败")
|
||||
|
||||
# 拖地
|
||||
control_robot_action(0, 0, 1, "开始拖地")
|
||||
result = control_robot_action(0, 4, 1)
|
||||
if (result):
|
||||
control_robot_action(0, 4, 2)
|
||||
control_robot_action(0, 4, 3)
|
||||
control_robot_action(0, 4, 4)
|
||||
else:
|
||||
control_robot_action(0, 0, 1, "拖地失败")
|
||||
|
||||
# 擦桌子
|
||||
control_robot_action(0, 0, 1, "开始擦桌子")
|
||||
result = control_robot_action(0, 5, 1)
|
||||
if (result):
|
||||
control_robot_action(0, 5, 2)
|
||||
control_robot_action(0, 5, 3)
|
||||
else:
|
||||
control_robot_action(0, 0, 1, "擦桌子失败")
|
||||
|
|
@ -1,47 +0,0 @@
|
|||
#!/usr/bin/env python3
|
||||
# -*- encoding: utf-8 -*-
|
||||
import sys
|
||||
import time
|
||||
import grpc
|
||||
|
||||
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
|
||||
|
||||
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 reset(scene_id=0):
|
||||
scene = sim_client.Reset(GrabSim_pb2.ResetParams(scene=scene_id))
|
||||
|
||||
def show_env_info(scene_id=0):
|
||||
scene = sim_client.Observe(GrabSim_pb2.SceneID(value=scene_id))
|
||||
print('------------------show_env_info----------------------')
|
||||
print(
|
||||
f"location:{[scene.location.X, scene.location.Y]}, rotation:{scene.rotation.Yaw}\n",
|
||||
f"joints number:{len(scene.joints)}, fingers number:{len(scene.fingers)}\n", f"objects number: {len(scene.objects)}\n"
|
||||
f"rotation:{scene.rotation}, timestep:{scene.timestep}\n"
|
||||
f"timestamp:{scene.timestamp}, collision:{scene.collision}, info:{scene.info}")
|
||||
|
||||
if __name__ == '__main__':
|
||||
map_id = 3 # 地图编号: 3: 咖啡厅
|
||||
scene_num = 1 # 场景数量
|
||||
map_test(map_id, scene_num) # 场景加载测试
|
||||
time.sleep(5)
|
||||
|
||||
for i in range(scene_num):
|
||||
print("------------------", i, "----------------------")
|
||||
reset(i) # 场景重置测试
|
||||
show_env_info(i) # 场景信息测试
|
||||
139
zoo/demo/导航寻路.py
139
zoo/demo/导航寻路.py
|
|
@ -1,139 +0,0 @@
|
|||
#!/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 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)
|
||||
])
|
||||
|
||||
sim_client = GrabSim_pb2_grpc.GrabSimStub(channel)
|
||||
|
||||
'''
|
||||
初始化，卸载已经加载的关卡，清除所有机器人
|
||||
'''
|
||||
|
||||
|
||||
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("position:", walk_value)
|
||||
|
||||
if map_id == 11: # coffee
|
||||
v_list = [[0, 880], [250, 1200], [-55, 750], [70, -200]]
|
||||
else:
|
||||
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)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
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('------------ 场景操作 ------------')
|
||||
Observe(i)
|
||||
Reset(i)
|
||||
|
||||
print('------------ 导航移动 ------------')
|
||||
navigation_move(i, map_id)
|
||||
|
|
@ -1,44 +0,0 @@
|
|||
#!/usr/bin/env python3
|
||||
# -*- encoding: utf-8 -*-
|
||||
# enconding = utf8
|
||||
import sys
|
||||
import time
|
||||
import grpc
|
||||
|
||||
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
|
||||
|
||||
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 control_robot_action(scene_id=0, type=0, action=0, message="你好"):
|
||||
scene = sim_client.ControlRobot(GrabSim_pb2.ControlInfo(scene=scene_id, type=type, action=action, content=message))
|
||||
if (scene.info == "action success"):
|
||||
return True
|
||||
else:
|
||||
return False
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
map_id = 3 # 地图编号: 0：空房间 1：室内 2:咖啡厅1.0 3: 咖啡厅2.0 4:餐厅 5:养老院 6：会议室
|
||||
scene_num = 1 # 场景数量
|
||||
map_test(map_id, scene_num) # 场景加载测试
|
||||
time.sleep(5)
|
||||
|
||||
# 文字冒泡
|
||||
control_robot_action(0, 0, 1, "你好，欢迎光临")
|
||||
103
zoo/demo/物品操作.py
103
zoo/demo/物品操作.py
|
|
@ -1,103 +0,0 @@
|
|||
#!/usr/bin/env python3
|
||||
# -*- encoding: utf-8 -*-
|
||||
import sys
|
||||
import time
|
||||
import grpc
|
||||
|
||||
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
|
||||
|
||||
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 joint_test(scene_id=0):
|
||||
print('------------------joint_test----------------------')
|
||||
action_list = [[0, 0, 0, 0, 0, 30, 0, 0, 0, 0, 0, 0, 0, 0, 36.0, -39.37, 37.2, -92.4, 4.13, -0.62, 0.4],
|
||||
[0, 0, 0, 0, 0, 30, 0, 0, 0, 0, 0, 0, 0, 0, 36.0, -39.62, 34.75, -94.80, 3.22, -0.26, 0.85],
|
||||
[0, 0, 0, 0, 0, 30, 0, 0, 0, 0, 0, 0, 0, 0, 32.63, -32.80, 15.15, -110.70, 6.86, 2.36, 0.40],
|
||||
[0, 0, 0, 0, 0, 30, 0, 0, 0, 0, 0, 0, 0, 0, 28.18, -27.92, 6.75, -115.02, 9.46, 4.28, 1.35],
|
||||
[0, 0, 0, 0, 0, 30, 0, 0, 0, 0, 0, 0, 0, 0, 4.09, -13.15, -11.97, -107.35, 13.08, 8.58, 3.33]]
|
||||
|
||||
for value in action_list:
|
||||
action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.RotateJoints, values=value)
|
||||
scene = sim_client.Do(action)
|
||||
|
||||
for i in range(8, 21): # arm
|
||||
print(
|
||||
f"{scene.joints[i].name}:{scene.joints[i].angle} location:{scene.joints[i].location.X},{scene.joints[i].location.Y},{scene.joints[i].location.Z}"
|
||||
)
|
||||
print('')
|
||||
for i in range(5, 10): # Right hand
|
||||
print(
|
||||
f"{scene.fingers[i].name} angle:{scene.fingers[i].angle} location:{scene.fingers[i].location[0].X},{scene.fingers[i].location[0].Y},{scene.fingers[i].location[0].Z}"
|
||||
)
|
||||
print('----------------------------------------')
|
||||
time.sleep(0.03)
|
||||
time.sleep(1)
|
||||
|
||||
def gen_obj(scene_id, h=80):
|
||||
print('------------------gen objs----------------------')
|
||||
scene = sim_client.Observe(GrabSim_pb2.SceneID(value=scene_id))
|
||||
ginger_loc = [scene.location.X, scene.location.Y, scene.location.Z]
|
||||
|
||||
obj_list = [
|
||||
GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 90, y=ginger_loc[1] + 30, yaw=10, z=h, type=4),
|
||||
GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 80, y=ginger_loc[1] + 31, z=h, type=5),
|
||||
GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 33, y=ginger_loc[1] - 10.5, z=h+20, type=7),
|
||||
GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 70, y=ginger_loc[1] + 33, z=h, type=9),
|
||||
GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 60, y=ginger_loc[1] + 34, z=h, type=13)
|
||||
]
|
||||
scene = sim_client.AddObjects(GrabSim_pb2.ObjectList(objects=obj_list, scene=scene_id))
|
||||
print(scene.collision)
|
||||
time.sleep(5)
|
||||
|
||||
def remove_obj(scene_id=0, id_list=[1]):
|
||||
print('------------------remove objs----------------------')
|
||||
remove_obj_list = id_list
|
||||
scene = sim_client.RemoveObjects(GrabSim_pb2.RemoveList(IDs=remove_obj_list, scene=scene_id))
|
||||
print(f"remove objects {id_list}. current obj:")
|
||||
time.sleep(1)
|
||||
|
||||
def clean_obj(scene_id=0):
|
||||
print('------------------clean objs----------------------')
|
||||
scene = sim_client.CleanObjects(GrabSim_pb2.SceneID(value=scene_id))
|
||||
|
||||
def obj_test(scene_id=0):
|
||||
gen_obj(scene_id)
|
||||
# remove_obj(scene_id, id_list=[0])
|
||||
# clean_obj(scene_id)
|
||||
|
||||
def grasp_test(hand_id, obj_scene_id, scene_id=0):
|
||||
action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.Grasp, values=[hand_id, obj_scene_id])
|
||||
scene = sim_client.Do(action)
|
||||
|
||||
def release_test(hand_id, scene_id=0):
|
||||
action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.Release, values=[hand_id])
|
||||
scene = sim_client.Do(action)
|
||||
|
||||
if __name__ == '__main__':
|
||||
map_id = 3 # 地图编号: 3: 咖啡厅
|
||||
scene_num = 1 # 场景数量
|
||||
map_test(map_id, scene_num) # 场景加载测试
|
||||
time.sleep(5)
|
||||
|
||||
for i in range(scene_num):
|
||||
print("------------------", i, "----------------------")
|
||||
joint_test(i) # 关节控制测试
|
||||
obj_test(i) # 物品生成测试
|
||||
grasp_test(1, 2) # 抓取物品测试
|
||||
joint_test(i) # 关节控制测试
|
||||
release_test(1) # 释放物品测试
|
||||
|
|
@ -1,49 +0,0 @@
|
|||
#!/usr/bin/env python3
|
||||
# -*- encoding: utf-8 -*-
|
||||
import sys
|
||||
import time
|
||||
import grpc
|
||||
|
||||
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
|
||||
|
||||
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 get_camera(part, scene_id=0):
|
||||
action = GrabSim_pb2.CameraList(cameras=part, scene=scene_id)
|
||||
return sim_client.Capture(action)
|
||||
|
||||
def show_image(img_data):
|
||||
im = img_data.images[0]
|
||||
d = np.frombuffer(im.data, dtype=im.dtype).reshape((im.height, im.width, im.channels))
|
||||
plt.imshow(d, cmap="gray" if "depth" in im.name.lower() else None)
|
||||
plt.show()
|
||||
|
||||
def camera_test(scene_id=0):
|
||||
for camera_name in [GrabSim_pb2.CameraName.Head_Color, GrabSim_pb2.CameraName.Head_Depth, GrabSim_pb2.CameraName.Head_Segment]:
|
||||
img_data = get_camera([camera_name], scene_id)
|
||||
show_image(img_data)
|
||||
|
||||
if __name__ == '__main__':
|
||||
map_id = 3 # 地图编号: 0：空房间 1：室内 2:咖啡厅1.0 3: 咖啡厅2.0 4:餐厅 5:养老院 6：会议室
|
||||
scene_num = 1 # 场景数量
|
||||
map_test(map_id, scene_num) # 场景加载测试
|
||||
time.sleep(5)
|
||||
|
||||
for i in range(scene_num):
|
||||
print("------------------", i, "----------------------")
|
||||
camera_test(i) # 相机操作测试
|
||||
|
|
@ -1,86 +0,0 @@
|
|||
#!/usr/bin/env python3
|
||||
# -*- encoding: utf-8 -*-
|
||||
import sys
|
||||
import time
|
||||
import grpc
|
||||
|
||||
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
|
||||
|
||||
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 add_walker(scene_id=0):
|
||||
"""pose:表示行人的初始位置姿态"""
|
||||
print('------------------add walker----------------------')
|
||||
s = sim_client.Observe(GrabSim_pb2.SceneID(value=0))
|
||||
|
||||
walker_loc = [[0, 880], [250, 1200], [-55, 750], [70, -200]]
|
||||
walker_list = []
|
||||
for i in range(len(walker_loc)):
|
||||
loc = walker_loc[i] + [0, 600, 100]
|
||||
action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.WalkTo, values=loc)
|
||||
scene = sim_client.Do(action)
|
||||
print(scene.info)
|
||||
# 只有可达的位置才能成功初始化行人，显示unreachable的地方不能初始化行人
|
||||
walker_list.append(GrabSim_pb2.WalkerList.Walker(id=i, pose=GrabSim_pb2.Pose(X=loc[0], Y=loc[1], Yaw=90)))
|
||||
|
||||
scene = sim_client.AddWalker(GrabSim_pb2.WalkerList(walkers=walker_list, scene=scene_id))
|
||||
return scene
|
||||
|
||||
def control_walkers(scene_id=0):
|
||||
"""pose:表示行人的终止位置姿态"""
|
||||
s = sim_client.Observe(GrabSim_pb2.SceneID(value=scene_id))
|
||||
|
||||
walker_loc = [[-55, 750], [70, -200], [250, 1200], [0, 880]]
|
||||
controls = []
|
||||
for i in range(len(s.walkers)):
|
||||
loc = walker_loc[i]
|
||||
is_autowalk = False
|
||||
pose = GrabSim_pb2.Pose(X=loc[0], Y=loc[1], Yaw=180)
|
||||
controls.append(GrabSim_pb2.WalkerControls.WControl(id=i, autowalk=is_autowalk, speed=200, pose=pose))
|
||||
scene = sim_client.ControlWalkers(GrabSim_pb2.WalkerControls(controls=controls, scene=scene_id))
|
||||
time.sleep(10)
|
||||
return scene
|
||||
|
||||
def remove_walkers(scene_id=0):
|
||||
s = sim_client.Observe(GrabSim_pb2.SceneID(value=scene_id))
|
||||
scene = sim_client.RemoveWalkers(GrabSim_pb2.RemoveList(IDs=[1, 3], scene=scene_id))
|
||||
time.sleep(2)
|
||||
return
|
||||
|
||||
def clean_walkers(scene_id=0):
|
||||
scene = sim_client.CleanWalkers(GrabSim_pb2.SceneID(value=scene_id))
|
||||
return scene
|
||||
|
||||
def walker_test(scene_id=0):
|
||||
add_walker(scene_id)
|
||||
control_walkers(scene_id)
|
||||
print(sim_client.Observe(GrabSim_pb2.SceneID(value=scene_id)).walkers)
|
||||
remove_walkers(scene_id)
|
||||
print(sim_client.Observe(GrabSim_pb2.SceneID(value=scene_id)).walkers)
|
||||
clean_walkers(scene_id)
|
||||
return
|
||||
|
||||
if __name__ == '__main__':
|
||||
map_id = 3 # 地图编号: 3: 咖啡厅
|
||||
scene_num = 1 # 场景数量
|
||||
map_test(map_id, scene_num) # 场景加载测试
|
||||
time.sleep(5)
|
||||
|
||||
for i in range(scene_num):
|
||||
print("------------------", i, "----------------------")
|
||||
walker_test(i) # 行人控制测试
|
||||
|
|
@ -1 +0,0 @@
|
|||
conda install pytorch==1.11.0 torchvision==0.12.0 torchaudio==0.11.0 cudatoolkit=11.3 -c pytorch
|
||||
|
|
@ -1,4 +0,0 @@
|
|||
|
||||
from . import navigate
|
||||
from . import dstar_lite
|
||||
|
||||
|
|
@ -1,510 +0,0 @@
|
|||
'''
|
||||
基于两个D* lite的实现
|
||||
按照原始算法的伪代码
|
||||
自己写的D* lite
|
||||
'''
|
||||
|
||||
import math
|
||||
import queue
|
||||
from functools import partial
|
||||
import numpy as np
|
||||
import heapq
|
||||
|
||||
from matplotlib import pyplot as plt
|
||||
|
||||
|
||||
def diagonal_distance(start, end): #
|
||||
return max(abs(start[0] - end[0]), abs(start[1] - end[1]))
|
||||
|
||||
|
||||
def manhattan_distance(start, end): # 曼哈顿距离
|
||||
return abs(start[0] - end[0]) + abs(start[1] - end[1])
|
||||
|
||||
|
||||
def euclidean_distance(start, end): # 欧式距离
|
||||
# return np.linalg.norm(start-end)
|
||||
return math.sqrt((start[0] - end[0]) ** 2 + (start[1] - end[1]) ** 2)
|
||||
|
||||
|
||||
def heuristic(start, end, name='euclidean'):
|
||||
if name == 'diagonal':
|
||||
return diagonal_distance(start, end)
|
||||
elif name == 'euclidean':
|
||||
return euclidean_distance(start, end)
|
||||
elif name == 'manhattan':
|
||||
return manhattan_distance(start, end)
|
||||
else:
|
||||
raise Exception('Error heuristic name!')
|
||||
|
||||
|
||||
class Priority:
|
||||
'''
|
||||
优先级类
|
||||
'''
|
||||
|
||||
def __init__(self, k1, k2):
|
||||
self.k1 = k1
|
||||
self.k2 = k2
|
||||
|
||||
def __lt__(self, other): # 定义对象的 < 运算
|
||||
return self.k1 < other.k1 or (self.k1 == other.k1 and self.k2 < other.k2)
|
||||
|
||||
def __le__(self, other): # 定于对象的 <= 运算
|
||||
return self.k1 < other.k1 or (self.k1 == other.k1 and self.k2 <= other.k2)
|
||||
|
||||
|
||||
class Node:
|
||||
'''
|
||||
节点类
|
||||
'''
|
||||
|
||||
def __init__(self, pos: (int, int), priority: Priority):
|
||||
self.pos = pos # X Y
|
||||
self.priority = priority
|
||||
|
||||
def __lt__(self, other): # 定义对象的 < 运算
|
||||
return self.priority < other.priority
|
||||
|
||||
def __le__(self, other): # 定于对象的 <= 运算
|
||||
return self.priority <= other.priority
|
||||
|
||||
|
||||
class PriorityQueue:
|
||||
def __init__(self):
|
||||
self.queue = [] # 节点队列
|
||||
self.nodes = [] # 节点位置列表
|
||||
|
||||
def is_empty(self):
|
||||
# 队列判空
|
||||
return len(self.queue) == 0
|
||||
|
||||
def top(self):
|
||||
return self.queue[0].pos
|
||||
|
||||
def top_key(self):
|
||||
if self.is_empty():
|
||||
return Priority(float('inf'), float('inf'))
|
||||
return self.queue[0].priority
|
||||
|
||||
def pop(self):
|
||||
# 取出第一个节点
|
||||
node = heapq.heappop(self.queue)
|
||||
self.nodes.remove(node.pos)
|
||||
return node
|
||||
|
||||
def insert(self, pos, priority):
|
||||
# 创建并添加节点
|
||||
node = Node(pos, priority)
|
||||
heapq.heappush(self.queue, node)
|
||||
self.nodes.append(pos)
|
||||
|
||||
def remove(self, pos):
|
||||
# 删除指定节点并重新排序
|
||||
self.queue = [n for n in self.queue if n.pos != pos]
|
||||
heapq.heapify(self.queue) # 重排序
|
||||
self.nodes.remove(pos)
|
||||
|
||||
def update(self, pos, priority):
|
||||
# 更新指定位置的priority值
|
||||
for n in self.queue:
|
||||
if n.pos == pos:
|
||||
n.priority = priority
|
||||
break
|
||||
|
||||
|
||||
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实际身位半径
|
||||
):
|
||||
|
||||
# self.area_bounds = area
|
||||
self.map = map
|
||||
self.background = map
|
||||
self.X = map.shape[0]
|
||||
self.Y = map.shape[1]
|
||||
(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缩放后身位半径
|
||||
|
||||
# free:0, obs:1, dyna_obs:2
|
||||
self.idx_to_object = {
|
||||
0: "free",
|
||||
1: "obstacle",
|
||||
2: "dynamic obstacle"
|
||||
}
|
||||
self.object_to_idx = dict(zip(self.idx_to_object.values(), self.idx_to_object.keys()))
|
||||
self.object_to_cost = {
|
||||
"free": 0,
|
||||
"obstacle": float('inf'),
|
||||
"dynamic obstacle": 50
|
||||
}
|
||||
|
||||
self.compute_cost_map()
|
||||
|
||||
self.s_start = None # (int,int) 必须是元组(元组可以直接当作矩阵索引)
|
||||
self.s_goal = None # (int,int)
|
||||
self.s_last = None # (int,int)
|
||||
self.U = PriorityQueue()
|
||||
self.k_m = 0
|
||||
self.rhs = np.ones((self.X, self.Y)) * np.inf
|
||||
self.g = self.rhs.copy()
|
||||
self.path = []
|
||||
|
||||
def set_map(self, map_):
|
||||
'''
|
||||
设置map 和 cost_map
|
||||
'''
|
||||
self.map = map_
|
||||
self.X = map_.shape[0]
|
||||
self.Y = map_.shape[1]
|
||||
self.compute_cost_map()
|
||||
|
||||
def reset(self):
|
||||
'''
|
||||
(完成一次导航后)
|
||||
重置 1.环境变量
|
||||
2.dstar_lite变量
|
||||
'''
|
||||
# env reset
|
||||
self.map = self.background.copy()
|
||||
self.compute_cost_map()
|
||||
self.dyna_obs_list.clear()
|
||||
self.path.clear()
|
||||
# dstar_lite reset
|
||||
self.s_start = None
|
||||
self.s_goal = None
|
||||
self.s_last = None
|
||||
self.U = PriorityQueue()
|
||||
self.k_m = 0
|
||||
self.rhs = np.ones((self.X, self.Y)) * np.inf
|
||||
self.g = self.rhs.copy()
|
||||
|
||||
def calculate_key(self, s: (int, int)):
|
||||
'''
|
||||
计算 位置s 的 key1, key2
|
||||
:returns：
|
||||
Priority(k1, k2): 可比较的对象
|
||||
'''
|
||||
k1 = min(self.g[s], self.rhs[s]) + heuristic(self.s_start, s) + self.k_m
|
||||
k2 = min(self.g[s], self.rhs[s])
|
||||
return Priority(k1, k2)
|
||||
|
||||
def c(self, u: (int, int), v: (int, int), v_old=None) -> float:
|
||||
'''
|
||||
计算节点间的 路径代价 和 目标位置代价 (目标位置代价为0时采用路径代价)
|
||||
(因为是终点->起点的扩展方向，因此v是node，u是v扩展的neighbor)
|
||||
Args:
|
||||
u: from pos
|
||||
v: to pos
|
||||
v_old: 指定的v的类型
|
||||
'''
|
||||
if v_old:
|
||||
obj_cost = self.object_to_cost[v_old]
|
||||
else:
|
||||
obj_cost = self.cost_map[v]
|
||||
if obj_cost > 0:
|
||||
return obj_cost
|
||||
return heuristic(u, v)
|
||||
|
||||
def contain(self, u: (int, int)):
|
||||
'''
|
||||
判断 节点u 是否在队列中
|
||||
'''
|
||||
return u in self.U.nodes
|
||||
|
||||
def update_vertex(self, u: (int, int)):
|
||||
'''
|
||||
判定节点状态, 更新队列
|
||||
不一致且在队列 --> 更新key
|
||||
不一致且不在队列 --> 计算key并加入队列
|
||||
一致且在队列 --> 移出队列
|
||||
'''
|
||||
if self.g[u] != self.rhs[u] and self.contain(u):
|
||||
self.U.update(u, self.calculate_key(u))
|
||||
elif self.g[u] != self.rhs[u] and not self.contain(u):
|
||||
self.U.insert(u, self.calculate_key(u))
|
||||
elif self.g[u] == self.rhs[u] and self.contain(u):
|
||||
self.U.remove(u)
|
||||
|
||||
def compute_shortest_path(self):
|
||||
'''
|
||||
计算最短路径
|
||||
'''
|
||||
while self.U.top_key() < self.calculate_key(self.s_start) or self.rhs[self.s_start] > self.g[self.s_start]:
|
||||
u = self.U.top()
|
||||
k_old = self.U.top_key()
|
||||
k_new = self.calculate_key(u)
|
||||
if k_old < k_new:
|
||||
self.U.update(u, k_new)
|
||||
elif self.g[u] > self.rhs[u]: # 过一致
|
||||
self.g[u] = self.rhs[u]
|
||||
self.U.remove(u)
|
||||
pred = self.get_neighbors(u)
|
||||
for s in pred:
|
||||
if s != self.s_goal:
|
||||
self.rhs[s] = min(self.rhs[s], self.c(s, u) + self.g[u])
|
||||
self.update_vertex(s)
|
||||
else: # 欠一致
|
||||
g_old = self.g[u]
|
||||
self.g[u] = float('inf')
|
||||
pred = self.get_neighbors(u)
|
||||
for s in pred + [u]:
|
||||
if self.rhs[s] == self.c(s, u) + g_old:
|
||||
if s != self.s_goal:
|
||||
succ = self.get_neighbors(s)
|
||||
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):
|
||||
'''
|
||||
规划路径(实际实现)
|
||||
Args:
|
||||
dyna_obs: 动态障碍物位置列表
|
||||
step_num: 单次移动步数
|
||||
'''
|
||||
# 确保目标合法
|
||||
if not self.in_bounds_without_obstacle(s_goal):
|
||||
return None
|
||||
# 第一次规划需要初始化rhs并将goal加入队列，计算最短路径
|
||||
if self.s_goal is None:
|
||||
self.s_start = tuple(s_start)
|
||||
self.s_goal = tuple(s_goal)
|
||||
self.s_last = tuple(s_start)
|
||||
self.rhs[tuple(s_goal)] = 0
|
||||
self.U.insert(tuple(s_goal), Priority(k1=heuristic(tuple(s_start), tuple(s_goal)), k2=0))
|
||||
self.compute_shortest_path() # 计算最短路径
|
||||
self.path = self.get_path()
|
||||
# 后续规划只更新起点，直接使用原路径(去掉已走过部分)
|
||||
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:
|
||||
self.update_cost_map(changed_pos)
|
||||
self.update_edge_costs(changed_pos)
|
||||
# 若地图改变，重新计算最短路径
|
||||
self.compute_shortest_path()
|
||||
self.path = self.get_path()
|
||||
|
||||
# TODO: 误差抖动使robot没有到达路径上的点，导致新起点的rhs=∞，可能导致get_path失败 ( 当前版本没有该问题 )
|
||||
# assert (self.rhs[self.s_start] != float('inf')), "There is no known path!"
|
||||
# self.path = self.get_path(step_num)
|
||||
# # debug
|
||||
# if debug:
|
||||
# pass
|
||||
return self.path
|
||||
|
||||
def planning(self, s_start, s_goal, dyna_obs, step_num=None, 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])
|
||||
|
||||
self._planning(s_start, s_goal, dyna_obs, step_num, debug)
|
||||
|
||||
# 地图坐标->实际坐标
|
||||
path = [self.map2real(node) for node in self.path]
|
||||
return path
|
||||
|
||||
def get_path(self):
|
||||
'''
|
||||
得到路径
|
||||
Args:
|
||||
step_num: 路径步数 (None表示返回完整路径)
|
||||
return:
|
||||
path: [(x, y), ...]
|
||||
'''
|
||||
if self.s_start is None or self.s_goal == self.s_start:
|
||||
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])]
|
||||
path.append(cur)
|
||||
# else:
|
||||
# for i in range(step_num):
|
||||
# if cur == self.s_goal:
|
||||
# break
|
||||
# succ = self.get_neighbors(cur)
|
||||
# cur = succ[np.argmin([self.c(cur, s_) + self.g[s_] for s_ in succ])]
|
||||
# path.append(cur)
|
||||
return path
|
||||
|
||||
def in_bounds_without_obstacle(self, pos):
|
||||
'''
|
||||
判断位置在地图范围内 且 不是静态障碍物
|
||||
'''
|
||||
(x, y) = pos
|
||||
return 0 <= x < self.X and 0 <= y < self.Y and self.map[x, y] != self.object_to_idx["obstacle"]
|
||||
|
||||
def get_neighbors(self, pos, mode=8):
|
||||
'''
|
||||
获取邻居节点, 地图范围内
|
||||
'''
|
||||
(x_, y_) = pos
|
||||
# results = [(x_+1,y_), (x_-1,y_), (x_, y_+1), (x_,y_-1)]
|
||||
# if mode == 8:
|
||||
neighbors = [(x_ + 1, y_), (x_ - 1, y_), (x_, y_ + 1), (x_, y_ - 1), (x_ + 1, y_ + 1), (x_ - 1, y_ + 1),
|
||||
(x_ + 1, y_ - 1), (x_ - 1, y_ - 1)]
|
||||
neighbors = filter(self.in_bounds_without_obstacle, neighbors) # 确保位置在地图范围内 且 不是静态障碍物
|
||||
return list(neighbors)
|
||||
|
||||
def compute_cost_map(self):
|
||||
# 计算当前地图的cost_map
|
||||
self.cost_map = np.zeros(self.map.shape)
|
||||
for idx, obj in self.idx_to_object.items():
|
||||
self.cost_map[self.map == idx] = self.object_to_cost[obj]
|
||||
|
||||
def update_map(self, dyna_obs):
|
||||
'''
|
||||
更新地图 和 动态障碍物列表
|
||||
Args:
|
||||
dyna_obs: 当前动态障碍物位置列表 [(x,y), ...]
|
||||
return:
|
||||
update_obj: 改变的位置列表 [(x, y, obj_idx, obj_idx_old), ...]
|
||||
'''
|
||||
# dyna_obs没有变化 (集合set可以忽略元素在列表中的位置)
|
||||
if set(dyna_obs) == set(self.dyna_obs_list):
|
||||
return []
|
||||
|
||||
# 新旧dyna_obs占用位置列表
|
||||
old_obs_occupy = []
|
||||
new_obs_occupy = []
|
||||
for pos in self.dyna_obs_list:
|
||||
old_obs_occupy.extend(self.get_occupy_pos(pos))
|
||||
for pos in dyna_obs:
|
||||
new_obs_occupy.extend(self.get_occupy_pos(pos))
|
||||
old_obs_occupy = [pos for i, pos in enumerate(old_obs_occupy) if pos not in old_obs_occupy[:i]] # 去除重复位置
|
||||
new_obs_occupy = [pos for i, pos in enumerate(new_obs_occupy) if pos not in new_obs_occupy[:i]] # 去除重复位置
|
||||
|
||||
# 转变为free 和 转变为obs的位置列表
|
||||
changed_free = [pos for pos in old_obs_occupy if pos not in new_obs_occupy]
|
||||
changed_obs = [pos for pos in new_obs_occupy if pos not in old_obs_occupy]
|
||||
|
||||
# 更新地图，计算changed_pos
|
||||
changed_pos = []
|
||||
for (x, y) in changed_free:
|
||||
self.map[x, y] = self.object_to_idx['free']
|
||||
changed_pos.append((x, y, self.object_to_idx["free"], self.object_to_idx["dynamic obstacle"]))
|
||||
for (x, y) in changed_obs:
|
||||
self.map[x, y] = self.object_to_idx['dynamic obstacle']
|
||||
changed_pos.append((x, y, self.object_to_idx["dynamic obstacle"], self.object_to_idx["free"]))
|
||||
|
||||
# 更新动态障碍物列表
|
||||
self.dyna_obs_list = dyna_obs
|
||||
|
||||
return changed_pos
|
||||
|
||||
def get_occupy_pos(self, obs_pos):
|
||||
'''
|
||||
根据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))
|
||||
occupy_pos = filter(self.in_bounds_without_obstacle, occupy_pos) # 确保位置在地图范围内 且 不是静态障碍物
|
||||
return list(occupy_pos)
|
||||
|
||||
def update_cost_map(self, changed_pos):
|
||||
'''
|
||||
更新cost_map
|
||||
Args:
|
||||
changed_pos: 改变的位置列表 [x, y, idx]
|
||||
'''
|
||||
for (x, y, idx, _) in changed_pos:
|
||||
self.cost_map[x, y] = self.object_to_cost[self.idx_to_object[idx]]
|
||||
|
||||
def update_edge_costs(self, changed_pos):
|
||||
'''
|
||||
重新计算edge_cost，更新受影响节点的rhs
|
||||
Args:
|
||||
changed_pos: 改变的位置列表 [(x, y, obj_idx_new, obj_idx_old)]
|
||||
'''
|
||||
if not changed_pos: return
|
||||
self.k_m += heuristic(self.s_last, self.s_start, name='euclidean') # 使用欧式距离 更新km
|
||||
self.s_last = self.s_start
|
||||
for pos in changed_pos:
|
||||
v = (pos[0], pos[1]) # to pos
|
||||
v_old = self.idx_to_object[pos[3]] # 位置v的旧类型
|
||||
pred_v = self.get_neighbors(v)
|
||||
for u in pred_v:
|
||||
c_old = self.c(u, v, v_old=v_old)
|
||||
c_new = self.c(u, v)
|
||||
if c_old > c_new and u != self.s_goal:
|
||||
self.rhs[u] = min(self.rhs[u], self.c(u, v) + self.g[v])
|
||||
elif self.rhs[u] == c_old + self.g[v] and u != self.s_goal:
|
||||
succ_u = self.get_neighbors(u)
|
||||
self.rhs[u] = min([self.c(u, s_) + self.g[s_] for s_ in succ_u])
|
||||
self.update_vertex(u)
|
||||
|
||||
def map2real(self, pos):
|
||||
'''
|
||||
地图坐标->实际坐标
|
||||
'''
|
||||
x = pos[0] * self.scale_ratio + self.x_min
|
||||
y = pos[1] * self.scale_ratio + self.y_min
|
||||
return tuple((x, y))
|
||||
|
||||
def real2map(self, pos, approximation='round'):
|
||||
'''
|
||||
实际坐标->地图坐标
|
||||
'''
|
||||
if approximation == 'round': # 四舍五入
|
||||
x = round((pos[0] - self.x_min) / self.scale_ratio)
|
||||
y = round((pos[1] - self.y_min) / self.scale_ratio)
|
||||
elif approximation == 'low': # 向下取整
|
||||
x = math.floor((pos[0] - self.x_min) / self.scale_ratio)
|
||||
y = math.floor((pos[1] - self.y_min) / self.scale_ratio)
|
||||
else:
|
||||
raise Exception("Wrong approximation!")
|
||||
return tuple((x, y))
|
||||
|
||||
def draw_graph(self, step_num):
|
||||
# 清空当前figure内容，保留figure对象
|
||||
plt.clf()
|
||||
# for stopping simulation with the esc key. 按esc结束
|
||||
plt.gcf().canvas.mpl_connect(
|
||||
'key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
|
||||
# 缩放坐标偏移量
|
||||
offset = (self.x_min / self.scale_ratio, self.x_max / self.scale_ratio,
|
||||
self.y_min / self.scale_ratio, self.y_max / self.scale_ratio,)
|
||||
|
||||
# 画地图: X行Y列，第一行在下面
|
||||
# 范围: 横向Y[-80,290] 纵向X[-70,120]
|
||||
plt.imshow(self.map, cmap='binary', alpha=0.5, origin='lower',
|
||||
extent=(offset[2], offset[3],
|
||||
offset[0], offset[1]))
|
||||
# 画起点和目标
|
||||
plt.plot(self.s_start[1] + offset[2], self.s_start[0] + offset[0], "xr")
|
||||
plt.plot(self.s_goal[1] + offset[2], self.s_goal[0] + offset[0], "xr")
|
||||
|
||||
# 画搜索路径
|
||||
plt.plot([y + offset[2] for (x, y) in self.path],
|
||||
[x + offset[0] for (x, y) in self.path], "-g")
|
||||
|
||||
# 画移动路径
|
||||
next_step = min(step_num, len(self.path))
|
||||
# plt.plot([self.s_start[1] + offset[2], self.path[next_step-1][1] + offset[2]],
|
||||
# [self.s_start[0] + offset[0], self.path[next_step-1][0] + offset[0]], "-r")
|
||||
plt.plot([y + offset[2] for (x, y) in self.path[:next_step]],
|
||||
[x + offset[0] for (x, y) in self.path[:next_step]], "-r")
|
||||
|
||||
plt.xlabel('y', loc='right')
|
||||
plt.ylabel('x', loc='top')
|
||||
plt.grid(color='black', linestyle='-', linewidth=0.5)
|
||||
plt.pause(0.3)
|
||||
Binary file not shown.
|
|
@ -1,78 +0,0 @@
|
|||
#!/usr/bin/env python3
|
||||
# -*- encoding: utf-8 -*-
|
||||
import math
|
||||
import sys
|
||||
import time
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
from mpl_toolkits.axes_grid1 import make_axes_locatable
|
||||
from scene_utils import Scene
|
||||
|
||||
|
||||
from dstar_lite import DStarLite
|
||||
|
||||
class Navigator:
|
||||
'''
|
||||
导航类
|
||||
'''
|
||||
|
||||
def __init__(self, scene, area_range, map, scale_ratio=5):
|
||||
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_num = self.step_length // self.scale_ratio # 单次移动地图格数
|
||||
self.v = 200 # 速度
|
||||
self.step_time = self.step_length/self.v + 0.1 # 单步移动时长
|
||||
|
||||
|
||||
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):
|
||||
'''
|
||||
判断是否到达目标
|
||||
'''
|
||||
dis = np.linalg.norm(pos - goal)
|
||||
return dis < dis_limit
|
||||
|
||||
def reset_goal(self, goal:(float, float)):
|
||||
# TODO: 使目标可达
|
||||
# 目标在障碍物上：从目标开始方形向外扩展，直到找到可行点
|
||||
# 目标在地图外面：起点和目标连线最靠近目标的可行点
|
||||
pass
|
||||
|
||||
def navigate(self, goal: (float, float), animation=True):
|
||||
'''
|
||||
单次导航，直到到达目标
|
||||
'''
|
||||
|
||||
pos = np.array((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)
|
||||
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=' ')
|
||||
self.scene.walk_to(next_x, next_y, yaw, velocity=self.v)
|
||||
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))
|
||||
print('reach pos:', pos)
|
||||
|
||||
self.planner.reset() # 完成一轮导航，重置变量
|
||||
|
||||
if self.is_reached(pos, goal):
|
||||
print('The robot has achieved goal !!')
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -1,4 +0,0 @@
|
|||
### dstar_lite.py ——Dstar lite算法文件
|
||||
### navigate.py ——导航类
|
||||
### test.py ——测试文件
|
||||
### map_5.pkl ——离散化地图文件
|
||||
|
|
@ -1,72 +0,0 @@
|
|||
import os
|
||||
import pickle
|
||||
import time
|
||||
import random
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
|
||||
from scene_utils import Scene # TODO: 文件名改成Scene.py
|
||||
from navigate import Navigator
|
||||
|
||||
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
|
||||
file_name = 'map_5.pkl'
|
||||
if os.path.exists(file_name):
|
||||
with open(file_name, 'rb') as file:
|
||||
map = pickle.load(file)
|
||||
|
||||
Scene.init_world(1, 3)
|
||||
scene = Scene.Scene(sceneID=0)
|
||||
|
||||
navigator = Navigator(scene=scene, area_range=[-350, 600, -400, 1450], map=map)
|
||||
|
||||
'''场景1: 无行人环境 robot到达指定目标'''
|
||||
# goal = np.array((-100, 700))
|
||||
|
||||
|
||||
'''场景2: 静止行人环境 robot到达指定目标'''
|
||||
# scene.clean_walker()
|
||||
# scene.add_walker(50, 300, 0)
|
||||
# scene.add_walker(-50, 500, 0)
|
||||
# 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))
|
||||
|
||||
'''场景4: 行人自由移动 robot到达指定目标'''
|
||||
# # TODO: autowalk=True仿真器会闪退 ???
|
||||
# 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=True, speed=20, X=0, Y=0, Yaw=0)])
|
||||
# scene.control_walker([scene.walker_control_generator(walkerID=1, autowalk=True, speed=20, X=0, Y=0, Yaw=0)])
|
||||
# time.sleep(5)
|
||||
# goal = np.array((-100, 700))
|
||||
|
||||
navigator.navigate(goal, animation=False)
|
||||
|
||||
scene.clean_walker()
|
||||
print(scene.status.collision) # TODO: 不显示碰撞信息 ???
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -1,95 +0,0 @@
|
|||
|
||||
#叶结点
|
||||
class Leaf:
|
||||
def __init__(self,type,content,mincost):
|
||||
self.type=type
|
||||
self.content=content #conditionset or action
|
||||
self.parent=None
|
||||
self.parent_index=0
|
||||
self.mincost=mincost
|
||||
|
||||
# tick 叶节点，返回返回值以及对应的条件或行动对象self.content
|
||||
def tick(self,state):
|
||||
if self.type=='cond':
|
||||
if self.content <= state:
|
||||
return 'success',self.content
|
||||
else:
|
||||
return 'failure',self.content
|
||||
if self.type=='act':
|
||||
if self.content.pre<=state:
|
||||
return 'running',self.content #action
|
||||
else:
|
||||
return 'failure',self.content
|
||||
|
||||
def __str__(self):
|
||||
print( self.content)
|
||||
return ''
|
||||
|
||||
def print_nodes(self):
|
||||
print(self.content)
|
||||
|
||||
def count_size(self):
|
||||
return 1
|
||||
|
||||
|
||||
#可能包含控制结点的行为树
|
||||
class ControlBT:
|
||||
def __init__(self,type):
|
||||
self.type=type
|
||||
self.children=[]
|
||||
self.parent=None
|
||||
self.parent_index=0
|
||||
|
||||
|
||||
def add_child(self,subtree_list):
|
||||
for subtree in subtree_list:
|
||||
self.children.append(subtree)
|
||||
subtree.parent=self
|
||||
subtree.parent_index=len(self.children)-1
|
||||
|
||||
# tick行为树，根据不同控制结点逻辑tick子结点
|
||||
def tick(self,state):
|
||||
if len(self.children) < 1:
|
||||
print("error,no child")
|
||||
if self.type =='?':#选择结点，即或结点
|
||||
for child in self.children:
|
||||
val,obj=child.tick(state)
|
||||
if val=='success':
|
||||
return val,obj
|
||||
if val=='running':
|
||||
return val,obj
|
||||
return 'failure','?fails'
|
||||
if self.type =='>':#顺序结点，即与结点
|
||||
for child in self.children:
|
||||
val,obj=child.tick(state)
|
||||
if val=='failure':
|
||||
return val,obj
|
||||
if val=='running':
|
||||
return val,obj
|
||||
return 'success', '>success'
|
||||
if self.type =='act':#行动结点
|
||||
return self.children[0].tick(state)
|
||||
if self.type =='cond':#条件结点
|
||||
return self.children[0].tick(state)
|
||||
|
||||
def getFirstChild(self):
|
||||
return self.children[0]
|
||||
|
||||
def __str__(self):
|
||||
print(self.type+'\n')
|
||||
for child in self.children:
|
||||
print (child)
|
||||
return ''
|
||||
|
||||
def print_nodes(self):
|
||||
print(self.type)
|
||||
for child in self.children:
|
||||
child.print_nodes()
|
||||
|
||||
# 递归统计树中结点数
|
||||
def count_size(self):
|
||||
result=1
|
||||
for child in self.children:
|
||||
result+= child.count_size()
|
||||
return result
|
||||
|
||||
|
|
@ -1,59 +0,0 @@
|
|||
selector{
|
||||
cond At(Table,Coffee)
|
||||
selector{
|
||||
cond At(Robot,Table), Holding(Coffee)
|
||||
act PutDown(Table,Coffee)
|
||||
}
|
||||
selector{
|
||||
cond At(Robot,Coffee), NotHolding, At(Robot,Table)
|
||||
act PickUp(Coffee)
|
||||
}
|
||||
selector{
|
||||
cond Available(Table), Holding(Coffee)
|
||||
act MoveTo(Table)
|
||||
}
|
||||
selector{
|
||||
cond At(Robot,Coffee), At(Robot,Table), Holding(VacuumCup)
|
||||
act PutDown(Table,VacuumCup)
|
||||
}
|
||||
selector{
|
||||
cond At(Robot,CoffeeMachine), NotHolding, At(Robot,Table)
|
||||
act OpCoffeeMachine
|
||||
}
|
||||
selector{
|
||||
cond At(Robot,Coffee), Available(Table), NotHolding
|
||||
act PickUp(Coffee)
|
||||
}
|
||||
selector{
|
||||
cond At(Robot,CoffeeMachine), At(Robot,Table), Holding(VacuumCup)
|
||||
act PutDown(Table,VacuumCup)
|
||||
}
|
||||
selector{
|
||||
cond Available(Table), Available(Coffee), NotHolding
|
||||
act MoveTo(Coffee)
|
||||
}
|
||||
selector{
|
||||
cond Available(Table), At(Robot,CoffeeMachine), NotHolding
|
||||
act OpCoffeeMachine
|
||||
}
|
||||
selector{
|
||||
cond Available(Table), Available(Coffee), At(Robot,Table), Holding(VacuumCup)
|
||||
act PutDown(Table,VacuumCup)
|
||||
}
|
||||
selector{
|
||||
cond Available(Table), NotHolding, Available(CoffeeMachine)
|
||||
act MoveTo(CoffeeMachine)
|
||||
}
|
||||
selector{
|
||||
cond Available(Table), Available(Coffee), Holding(VacuumCup)
|
||||
act MoveTo(Table)
|
||||
}
|
||||
selector{
|
||||
cond Available(Table), Available(CoffeeMachine), At(Robot,Table), Holding(VacuumCup)
|
||||
act PutDown(Table,VacuumCup)
|
||||
}
|
||||
selector{
|
||||
cond Available(Table), Available(CoffeeMachine), Holding(VacuumCup)
|
||||
act MoveTo(Table)
|
||||
}
|
||||
}
|
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue