重构代码，将tasks移到外层
This commit is contained in:
ChenXL97
parent
0b8ca87c08
commit
6183649218
44
README.md
44
README.md
|
|
@ -14,45 +14,29 @@ pip install -e .
|
||||||
|
|
||||||
### 快速入门
|
### 快速入门
|
||||||
1. 安装UE及Harix插件，打开默认项目并运行
|
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
|
||||||
Robot是机器人类，包括从ptml加载行为树的方法，以及执行行为树的方法等
|
Robot是机器人类，包括从ptml加载行为树的方法，以及执行行为树的方法等
|
||||||
|
|
||||||
|
|
||||||
## task_map
|
## tasks
|
||||||
task_map是任务字典，通过任务缩写来返回相应的场景类。
|
tasks文件夹中存放的场景定义及运行代码。
|
||||||
|
|
||||||
| 缩写 | 任务 |
|
| 缩写 | 任务 |
|
||||||
|----|---------|
|
|---------------------|-------------|
|
||||||
| AEM | 主动探索和记忆 |
|
| AEM | 主动探索和记忆 |
|
||||||
| GQA | 具身多轮对话 |
|
| GQA | 具身多轮对话 |
|
||||||
| VLN | 视觉语言导航 |
|
| VLN | 视觉语言导航 |
|
||||||
| VLM | 视觉语言操作 |
|
| VLM | 视觉语言操作 |
|
||||||
| OT | 复杂开放任务 |
|
| OT | 复杂开放任务 |
|
||||||
| AT | 自主任务 |
|
| AT | 自主任务 |
|
||||||
|
| CafeDailyOperations | 整体展示：咖啡厅的一天 |
|
||||||
|
| Interact | 命令行自由交互 |
|
||||||
|
|
||||||
|
|
||||||
## Scene
|
## Scene
|
||||||
|
|
|
||||||
|
|
@ -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):
|
def set_scene(self, scene=None):
|
||||||
if scene:
|
if scene:
|
||||||
self.scene = scene
|
self.scene = scene
|
||||||
|
|
|
||||||
|
|
@ -9,7 +9,7 @@ ok, 有需要您再找我。
|
||||||
做一杯咖啡
|
做一杯咖啡
|
||||||
好的，我马上做咖啡
|
好的，我马上做咖啡
|
||||||
create_sub_task
|
create_sub_task
|
||||||
{"goal":"On(Coffee,CoffeeTable)"}00
|
{"goal":"On(Coffee,CoffeeTable)"}
|
||||||
|
|
||||||
不用了。
|
不用了。
|
||||||
好的，您有需要再跟我说
|
好的，您有需要再跟我说
|
||||||
|
|
|
||||||
|
|
@ -306,9 +306,7 @@ class Scene:
|
||||||
|
|
||||||
return set_sub_task
|
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):
|
def new_set_goal(self,goal):
|
||||||
self.state['chat_list'].append(("Goal",goal))
|
self.state['chat_list'].append(("Goal",goal))
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -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()
|
|
||||||
|
|
@ -1,17 +1,10 @@
|
||||||
selector
|
sequence{
|
||||||
sequence
|
cond Holding(Nothing)
|
||||||
cond CustomerChatting()
|
sequence{
|
||||||
act DealChat()
|
cond Is(Curtain,Off)
|
||||||
sequence
|
act Turn(Curtain,On)
|
||||||
cond HasSubTask()
|
}
|
||||||
sequence
|
sequence{
|
||||||
act SubTaskPlaceHolder()
|
cond Is(AC,Off)
|
||||||
sequence
|
act Turn(AC,On)
|
||||||
cond FocusingCustomer()
|
}}
|
||||||
act ServeCustomer()
|
|
||||||
sequence
|
|
||||||
cond NewCustomerComing()
|
|
||||||
selector
|
|
||||||
cond At(Robot,Bar)
|
|
||||||
act MoveTo(Bar)
|
|
||||||
act GreetCustomer()
|
|
||||||
|
|
@ -2,27 +2,28 @@ selector
|
||||||
{
|
{
|
||||||
sequence
|
sequence
|
||||||
{
|
{
|
||||||
cond CustomerChatting()
|
cond Chatting()
|
||||||
act DealChat()
|
act DealChat()
|
||||||
|
} sequence
|
||||||
} sequence
|
|
||||||
{
|
{
|
||||||
cond HasSubTask()
|
cond HasSubTask()
|
||||||
sequence
|
sequence
|
||||||
{
|
{
|
||||||
act SubTaskPlaceHolder()
|
act SubTaskPlaceHolder()
|
||||||
|
} } sequence
|
||||||
} sequence
|
{
|
||||||
cond FocusingCustomer()
|
cond FocusingCustomer()
|
||||||
act ServeCustomer()
|
act ServeCustomer()
|
||||||
|
} sequence
|
||||||
} sequence
|
|
||||||
{
|
{
|
||||||
cond NewCustomerComing()
|
cond NewCustomer()
|
||||||
selector
|
selector
|
||||||
{
|
{
|
||||||
cond At(Robot,Bar)
|
cond At(Robot,Bar)
|
||||||
act MoveTo(Bar)
|
act MoveTo(Bar)
|
||||||
|
} act GreetCustomer()
|
||||||
} act GreetCustomer()
|
} sequence
|
||||||
}}
|
{
|
||||||
|
cond AnomalyDetected()
|
||||||
|
act ResolveAnomaly()
|
||||||
|
}}
|
||||||
|
|
@ -3,37 +3,19 @@ ordering=out;
|
||||||
graph [fontname="times-roman"];
|
graph [fontname="times-roman"];
|
||||||
node [fontname="times-roman"];
|
node [fontname="times-roman"];
|
||||||
edge [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];
|
"c9a8671e-b0b8-4587-a4de-a094fd678334" [fillcolor=orange, fontcolor=black, fontsize=20, height=0.01, label=Selector, shape=octagon, 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];
|
"bc597ff6-b3cb-4573-9dea-944fdb38d045" [fillcolor=yellow, fontcolor=black, fontsize=20, label="Holding(Nothing)", shape=ellipse, style=filled];
|
||||||
"a3ac082f-ad46-4cc9-86b4-3e76c3c564d3" -> "717a1290-56bc-41e1-9b4b-e6df93d40e64";
|
"c9a8671e-b0b8-4587-a4de-a094fd678334" -> "bc597ff6-b3cb-4573-9dea-944fdb38d045";
|
||||||
"b3236b13-d5f2-4643-ae7c-6f27bd4ab6f2" [fillcolor=yellow, fontcolor=black, fontsize=20, label="CustomerChatting()", shape=ellipse, style=filled];
|
"a8e26121-f5cc-4ba5-b98e-86bd84de06d6" [fillcolor=orange, fontcolor=black, fontsize=20, height=0.01, label=Sequence, shape=octagon, style=filled, width=0.01];
|
||||||
"717a1290-56bc-41e1-9b4b-e6df93d40e64" -> "b3236b13-d5f2-4643-ae7c-6f27bd4ab6f2";
|
"c9a8671e-b0b8-4587-a4de-a094fd678334" -> "a8e26121-f5cc-4ba5-b98e-86bd84de06d6";
|
||||||
"28015225-b7a4-4f38-b26f-4dbe5eea4154" [fillcolor=lawngreen, fontcolor=black, fontsize=20, label="DealChat()", shape=box, style=filled];
|
"676d8bda-2c30-44a5-bca2-ade68761403a" [fillcolor=yellow, fontcolor=black, fontsize=20, label="Is(Curtain,Off)", shape=ellipse, style=filled];
|
||||||
"717a1290-56bc-41e1-9b4b-e6df93d40e64" -> "28015225-b7a4-4f38-b26f-4dbe5eea4154";
|
"a8e26121-f5cc-4ba5-b98e-86bd84de06d6" -> "676d8bda-2c30-44a5-bca2-ade68761403a";
|
||||||
"ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e" [fillcolor=orange, fontcolor=black, fontsize=20, height=0.01, label=Sequence, shape=octagon, style=filled, width=0.01];
|
"c3d096cf-f848-46a3-9497-5398614798e8" [fillcolor=lawngreen, fontcolor=black, fontsize=20, label="Turn(Curtain,On)", shape=box, style=filled];
|
||||||
"a3ac082f-ad46-4cc9-86b4-3e76c3c564d3" -> "ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e";
|
"a8e26121-f5cc-4ba5-b98e-86bd84de06d6" -> "c3d096cf-f848-46a3-9497-5398614798e8";
|
||||||
"2f77f976-499c-4601-9d8c-86dd80d66dfa" [fillcolor=yellow, fontcolor=black, fontsize=20, label="HasSubTask()", shape=ellipse, style=filled];
|
"400b4104-ff96-4d64-89b2-49dc6e4e3cbd" [fillcolor=orange, fontcolor=black, fontsize=20, height=0.01, label=Sequence, shape=octagon, style=filled, width=0.01];
|
||||||
"ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e" -> "2f77f976-499c-4601-9d8c-86dd80d66dfa";
|
"c9a8671e-b0b8-4587-a4de-a094fd678334" -> "400b4104-ff96-4d64-89b2-49dc6e4e3cbd";
|
||||||
"2428792c-3896-443d-8744-f5e286644fad" [fillcolor=orange, fontcolor=black, fontsize=20, height=0.01, label=Sequence, shape=octagon, style=filled, width=0.01];
|
"3fc48afc-1cef-45f5-826d-2571d7979a2f" [fillcolor=yellow, fontcolor=black, fontsize=20, label="Is(AC,Off)", shape=ellipse, style=filled];
|
||||||
"ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e" -> "2428792c-3896-443d-8744-f5e286644fad";
|
"400b4104-ff96-4d64-89b2-49dc6e4e3cbd" -> "3fc48afc-1cef-45f5-826d-2571d7979a2f";
|
||||||
"4bb7a9c3-521b-408f-b403-1fd25d54b192" [fillcolor=lawngreen, fontcolor=black, fontsize=20, label="SubTaskPlaceHolder()", shape=box, style=filled];
|
"47af4df0-8811-45a5-95e2-f0fc9458ddfd" [fillcolor=lawngreen, fontcolor=black, fontsize=20, label="Turn(AC,On)", shape=box, style=filled];
|
||||||
"2428792c-3896-443d-8744-f5e286644fad" -> "4bb7a9c3-521b-408f-b403-1fd25d54b192";
|
"400b4104-ff96-4d64-89b2-49dc6e4e3cbd" -> "47af4df0-8811-45a5-95e2-f0fc9458ddfd";
|
||||||
"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";
|
|
||||||
}
|
}
|
||||||
|
|
|
||||||
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)
|
<!-- Generated by graphviz version 9.0.0 (20230911.1827)
|
||||||
-->
|
-->
|
||||||
<!-- Title: pastafarianism Pages: 1 -->
|
<!-- Title: pastafarianism Pages: 1 -->
|
||||||
<svg width="1699pt" height="417pt"
|
<svg width="716pt" height="220pt"
|
||||||
viewBox="0.00 0.00 1699.49 416.87" xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink">
|
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 412.87)">
|
<g id="graph0" class="graph" transform="scale(1 1) rotate(0) translate(4 215.87)">
|
||||||
<title>pastafarianism</title>
|
<title>pastafarianism</title>
|
||||||
<polygon fill="white" stroke="none" points="-4,4 -4,-412.87 1695.49,-412.87 1695.49,4 -4,4"/>
|
<polygon fill="white" stroke="none" points="-4,4 -4,-215.87 711.7,-215.87 711.7,4 -4,4"/>
|
||||||
<!-- a3ac082f-ad46-4cc9-86b4-3e76c3c564d3 -->
|
<!-- c9a8671e-b0b8-4587-a4de-a094fd678334 -->
|
||||||
<g id="node1" class="node">
|
<g id="node1" class="node">
|
||||||
<title>a3ac082f-ad46-4cc9-86b4-3e76c3c564d3</title>
|
<title>c9a8671e-b0b8-4587-a4de-a094fd678334</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"/>
|
<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="495.87" y="-370.24" font-family="Times New Roman,serif" font-size="20.00">Selector</text>
|
<text text-anchor="middle" x="319.2" y="-180.57" font-family="Times New Roman,serif" font-size="20.00">Selector</text>
|
||||||
</g>
|
</g>
|
||||||
<!-- 717a1290-56bc-41e1-9b4b-e6df93d40e64 -->
|
<!-- bc597ff6-b3cb-4573-9dea-944fdb38d045 -->
|
||||||
<g id="node2" class="node">
|
<g id="node2" class="node">
|
||||||
<title>717a1290-56bc-41e1-9b4b-e6df93d40e64</title>
|
<title>bc597ff6-b3cb-4573-9dea-944fdb38d045</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"/>
|
<ellipse fill="yellow" stroke="black" cx="114.2" cy="-104.11" rx="114.2" ry="22.1"/>
|
||||||
<text text-anchor="middle" x="322.87" y="-279.07" font-family="Times New Roman,serif" font-size="20.00">Sequence</text>
|
<text text-anchor="middle" x="114.2" y="-96.74" font-family="Times New Roman,serif" font-size="20.00">Holding(Nothing)</text>
|
||||||
</g>
|
</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">
|
<g id="edge1" class="edge">
|
||||||
<title>a3ac082f-ad46-4cc9-86b4-3e76c3c564d3->717a1290-56bc-41e1-9b4b-e6df93d40e64</title>
|
<title>c9a8671e-b0b8-4587-a4de-a094fd678334->bc597ff6-b3cb-4573-9dea-944fdb38d045</title>
|
||||||
<path fill="none" stroke="black" d="M461.27,-358.78C435.8,-345.66 400.81,-327.62 372.38,-312.97"/>
|
<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="374.07,-309.9 363.58,-308.43 370.87,-316.13 374.07,-309.9"/>
|
<polygon fill="black" stroke="black" points="174.86,-125.56 164.27,-125.1 172.26,-132.06 174.86,-125.56"/>
|
||||||
</g>
|
</g>
|
||||||
<!-- ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e -->
|
<!-- a8e26121-f5cc-4ba5-b98e-86bd84de06d6 -->
|
||||||
<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 -->
|
|
||||||
<g id="node3" class="node">
|
<g id="node3" class="node">
|
||||||
<title>b3236b13-d5f2-4643-ae7c-6f27bd4ab6f2</title>
|
<title>a8e26121-f5cc-4ba5-b98e-86bd84de06d6</title>
|
||||||
<ellipse fill="yellow" stroke="black" cx="125.87" cy="-202.61" rx="125.87" ry="22.1"/>
|
<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="125.87" y="-195.24" font-family="Times New Roman,serif" font-size="20.00">CustomerChatting()</text>
|
<text text-anchor="middle" x="319.2" y="-96.74" font-family="Times New Roman,serif" font-size="20.00">Sequence</text>
|
||||||
</g>
|
</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">
|
<g id="edge2" class="edge">
|
||||||
<title>717a1290-56bc-41e1-9b4b-e6df93d40e64->b3236b13-d5f2-4643-ae7c-6f27bd4ab6f2</title>
|
<title>c9a8671e-b0b8-4587-a4de-a094fd678334->a8e26121-f5cc-4ba5-b98e-86bd84de06d6</title>
|
||||||
<path fill="none" stroke="black" d="M278.16,-266.88C250.24,-255.28 213.99,-240.22 183.86,-227.7"/>
|
<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="185.46,-224.58 174.88,-223.97 182.77,-231.04 185.46,-224.58"/>
|
<polygon fill="black" stroke="black" points="322.7,-139.92 319.2,-129.92 315.7,-139.92 322.7,-139.92"/>
|
||||||
</g>
|
</g>
|
||||||
<!-- 28015225-b7a4-4f38-b26f-4dbe5eea4154 -->
|
<!-- 400b4104-ff96-4d64-89b2-49dc6e4e3cbd -->
|
||||||
<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 -->
|
|
||||||
<g id="node6" class="node">
|
<g id="node6" class="node">
|
||||||
<title>2f77f976-499c-4601-9d8c-86dd80d66dfa</title>
|
<title>400b4104-ff96-4d64-89b2-49dc6e4e3cbd</title>
|
||||||
<ellipse fill="yellow" stroke="black" cx="486.87" cy="-202.61" rx="92.45" ry="22.1"/>
|
<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="486.87" y="-195.24" font-family="Times New Roman,serif" font-size="20.00">HasSubTask()</text>
|
<text text-anchor="middle" x="490.2" y="-96.74" font-family="Times New Roman,serif" font-size="20.00">Sequence</text>
|
||||||
</g>
|
</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">
|
<g id="edge5" class="edge">
|
||||||
<title>ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e->2f77f976-499c-4601-9d8c-86dd80d66dfa</title>
|
<title>c9a8671e-b0b8-4587-a4de-a094fd678334->400b4104-ff96-4d64-89b2-49dc6e4e3cbd</title>
|
||||||
<path fill="none" stroke="black" d="M626.96,-266.26C600.92,-254.61 567.48,-239.66 539.8,-227.28"/>
|
<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="541.44,-224.18 530.88,-223.3 538.58,-230.57 541.44,-224.18"/>
|
<polygon fill="black" stroke="black" points="440.29,-132.88 447.8,-125.4 437.27,-126.56 440.29,-132.88"/>
|
||||||
</g>
|
</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">
|
<g id="node7" class="node">
|
||||||
<title>2428792c-3896-443d-8744-f5e286644fad</title>
|
<title>3fc48afc-1cef-45f5-826d-2571d7979a2f</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"/>
|
<ellipse fill="yellow" stroke="black" cx="490.2" cy="-22.1" rx="72.3" ry="22.1"/>
|
||||||
<text text-anchor="middle" x="669.87" y="-195.24" font-family="Times New Roman,serif" font-size="20.00">Sequence</text>
|
<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>
|
</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">
|
<g id="edge6" class="edge">
|
||||||
<title>ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e->2428792c-3896-443d-8744-f5e286644fad</title>
|
<title>400b4104-ff96-4d64-89b2-49dc6e4e3cbd->3fc48afc-1cef-45f5-826d-2571d7979a2f</title>
|
||||||
<path fill="none" stroke="black" d="M669.87,-262.22C669.87,-254.79 669.87,-246.39 669.87,-238.33"/>
|
<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="673.37,-238.42 669.87,-228.42 666.37,-238.42 673.37,-238.42"/>
|
<polygon fill="black" stroke="black" points="493.7,-56.06 490.2,-46.06 486.7,-56.06 493.7,-56.06"/>
|
||||||
</g>
|
</g>
|
||||||
<!-- 64b2362a-d99e-4e99-8772-ad1419e53a2e -->
|
<!-- 47af4df0-8811-45a5-95e2-f0fc9458ddfd -->
|
||||||
<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 -->
|
|
||||||
<g id="node8" class="node">
|
<g id="node8" class="node">
|
||||||
<title>4bb7a9c3-521b-408f-b403-1fd25d54b192</title>
|
<title>47af4df0-8811-45a5-95e2-f0fc9458ddfd</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"/>
|
<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="473.87" y="-104.07" font-family="Times New Roman,serif" font-size="20.00">SubTaskPlaceHolder()</text>
|
<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>
|
</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">
|
<g id="edge7" class="edge">
|
||||||
<title>2428792c-3896-443d-8744-f5e286644fad->4bb7a9c3-521b-408f-b403-1fd25d54b192</title>
|
<title>400b4104-ff96-4d64-89b2-49dc6e4e3cbd->47af4df0-8811-45a5-95e2-f0fc9458ddfd</title>
|
||||||
<path fill="none" stroke="black" d="M627.82,-182.48C596.86,-168.4 554.7,-149.22 522.33,-134.49"/>
|
<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="524.12,-131.46 513.57,-130.51 521.22,-137.83 524.12,-131.46"/>
|
<polygon fill="black" stroke="black" points="602.15,-48.89 609.41,-41.17 598.92,-42.68 602.15,-48.89"/>
|
||||||
</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"/>
|
|
||||||
</g>
|
</g>
|
||||||
</g>
|
</g>
|
||||||
</svg>
|
</svg>
|
||||||
|
|
|
||||||
|
Before Width: | Height: | Size: 15 KiB After Width: | Height: | Size: 7.1 KiB |
|
|
@ -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
|
map_ratio = self.map_ratio
|
||||||
db = DBSCAN(eps=map_ratio, min_samples=int(map_ratio / 2))
|
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):
|
if os.path.exists(file_name):
|
||||||
with open(file_name, 'rb') as file:
|
with open(file_name, 'rb') as file:
|
||||||
map = pickle.load(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}",
|
{"id": f"{i}", "name": f"{cur_objs[i].name}", "location": f"{cur_objs[i].location}",
|
||||||
"height": f"{cur_objs[i].location.Z}"})
|
"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)
|
json.dump(obj_json_data, file)
|
||||||
|
|
||||||
# for i in range(-350, 600):
|
# for i in range(-350, 600):
|
||||||
|
|
@ -1,10 +1,5 @@
|
||||||
"""
|
"""
|
||||||
人提出请求，机器人完成任务
|
交互式场景，输入
|
||||||
1. 做咖啡（固定动画）：接收到做咖啡指令、走到咖啡机、拿杯子、操作咖啡机、取杯子、送到客人桌子上
|
|
||||||
2. 倒水
|
|
||||||
3. 夹点心
|
|
||||||
|
|
||||||
具体描述：设计一套点单规则（如菜单包含咖啡、水、点心等），按照规则拟造随机的订单。在收到订单后，通过大模型让机器人输出合理的备餐计划，并尝试在模拟环境中按照这个规划实现任务。
|
|
||||||
|
|
||||||
"""
|
"""
|
||||||
|
|
||||||
|
|
@ -27,9 +22,9 @@ class SubScene(Scene):
|
||||||
question = input("请输入指令：")
|
question = input("请输入指令：")
|
||||||
if question[-1] == ")":
|
if question[-1] == ")":
|
||||||
print(f"设置目标:{question}")
|
print(f"设置目标:{question}")
|
||||||
self.set_goal(question)()
|
self.new_set_goal(question)
|
||||||
else:
|
else:
|
||||||
self.state['chat_list'].append(question)
|
self.customer_say("System",question)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -3,8 +3,6 @@
|
||||||
|
|
||||||
测试导航算法的动态避障(Obstacle Detection and Avoidance, ODA)能力
|
测试导航算法的动态避障(Obstacle Detection and Avoidance, ODA)能力
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
"""
|
"""
|
||||||
import os
|
import os
|
||||||
import pickle
|
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)
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
@ -1,210 +0,0 @@
|
||||||
import copy
|
|
||||||
import random
|
|
||||||
from robowaiter.behavior_tree.obtea.BehaviorTree import Leaf,ControlBT
|
|
||||||
|
|
||||||
class CondActPair:
|
|
||||||
def __init__(self, cond_leaf,act_leaf):
|
|
||||||
self.cond_leaf = cond_leaf
|
|
||||||
self.act_leaf = act_leaf
|
|
||||||
|
|
||||||
#定义行动类，行动包括前提、增加和删除影响
|
|
||||||
class Action:
|
|
||||||
def __init__(self,name='anonymous action',pre=set(),add=set(),del_set=set(),cost=1):
|
|
||||||
self.pre=copy.deepcopy(pre)
|
|
||||||
self.add=copy.deepcopy(add)
|
|
||||||
self.del_set=copy.deepcopy(del_set)
|
|
||||||
self.name=name
|
|
||||||
self.cost=cost
|
|
||||||
|
|
||||||
def __str__(self):
|
|
||||||
return self.name
|
|
||||||
|
|
||||||
#生成随机状态
|
|
||||||
def generate_random_state(num):
|
|
||||||
result = set()
|
|
||||||
for i in range(0,num):
|
|
||||||
if random.random()>0.5:
|
|
||||||
result.add(i)
|
|
||||||
return result
|
|
||||||
#从状态和行动生成后继状态
|
|
||||||
def state_transition(state,action):
|
|
||||||
if not action.pre <= state:
|
|
||||||
print ('error: action not applicable')
|
|
||||||
return state
|
|
||||||
new_state=(state | action.add) - action.del_set
|
|
||||||
return new_state
|
|
||||||
|
|
||||||
|
|
||||||
#本文所提出的完备规划算法
|
|
||||||
class OptBTExpAlgorithm:
|
|
||||||
def __init__(self,verbose=False):
|
|
||||||
self.bt = None
|
|
||||||
self.nodes=[]
|
|
||||||
self.traversed=[]
|
|
||||||
self.mounted=[]
|
|
||||||
self.conditions=[]
|
|
||||||
self.conditions_index=[]
|
|
||||||
self.verbose=verbose
|
|
||||||
|
|
||||||
def clear(self):
|
|
||||||
self.bt = None
|
|
||||||
self.nodes = []
|
|
||||||
self.traversed = []
|
|
||||||
self.conditions = []
|
|
||||||
self.conditions_index = []
|
|
||||||
|
|
||||||
#运行规划算法，从初始状态、目标状态和可用行动，计算行为树self.bt
|
|
||||||
def run_algorithm(self,goal,actions):
|
|
||||||
if self.verbose:
|
|
||||||
print("\n算法开始！")
|
|
||||||
|
|
||||||
|
|
||||||
self.bt = ControlBT(type='cond')
|
|
||||||
# 初始行为树只包含目标条件
|
|
||||||
gc_node = Leaf(type='cond', content=goal,mincost=0) # 为了统一，都成对出现
|
|
||||||
ga_node = Leaf(type='act', content=None, mincost=0)
|
|
||||||
subtree = ControlBT(type='?')
|
|
||||||
subtree.add_child([copy.deepcopy(gc_node)]) # 子树首先保留所扩展结
|
|
||||||
self.bt.add_child([subtree])
|
|
||||||
|
|
||||||
# self.conditions.append(goal)
|
|
||||||
cond_anc_pair = CondActPair(cond_leaf=gc_node,act_leaf=ga_node)
|
|
||||||
self.nodes.append(copy.deepcopy(cond_anc_pair)) # the set of explored but unexpanded conditions
|
|
||||||
self.traversed = [goal] # the set of expanded conditions
|
|
||||||
|
|
||||||
|
|
||||||
while len(self.nodes)!=0:
|
|
||||||
|
|
||||||
# Find the condition for the shortest cost path
|
|
||||||
pair_node = None
|
|
||||||
min_cost = float ('inf')
|
|
||||||
index= -1
|
|
||||||
for i,cond_anc_pair in enumerate(self.nodes):
|
|
||||||
if cond_anc_pair.cond_leaf.mincost < min_cost:
|
|
||||||
min_cost = cond_anc_pair.cond_leaf.mincost
|
|
||||||
pair_node = copy.deepcopy(cond_anc_pair)
|
|
||||||
index = i
|
|
||||||
break
|
|
||||||
|
|
||||||
if self.verbose:
|
|
||||||
print("选择扩展条件结点：",pair_node.cond_leaf.content)
|
|
||||||
# Update self.nodes and self.traversed
|
|
||||||
self.nodes.pop(index) # the set of explored but unexpanded conditions. self.nodes.remove(pair_node)
|
|
||||||
c = pair_node.cond_leaf.content # 子树所扩展结点对应的条件（一个文字的set）
|
|
||||||
|
|
||||||
# Mount the action node and extend BT. T = Eapand(T,c,A(c))
|
|
||||||
if c!=goal:
|
|
||||||
sequence_structure = ControlBT(type='>')
|
|
||||||
sequence_structure.add_child(
|
|
||||||
[copy.deepcopy(pair_node.cond_leaf), copy.deepcopy(pair_node.act_leaf)])
|
|
||||||
subtree.add_child([copy.deepcopy(sequence_structure)]) # subtree 是回不断变化的，它的父亲是self.bt
|
|
||||||
if self.verbose:
|
|
||||||
print("完成扩展 a_node= %s,对应的新条件 c_attr= %s,mincost=%d" \
|
|
||||||
% (cond_anc_pair.act_leaf.content.name, cond_anc_pair.cond_leaf.content,
|
|
||||||
cond_anc_pair.cond_leaf.mincost))
|
|
||||||
|
|
||||||
if self.verbose:
|
|
||||||
print("遍历所有动作, 寻找符合条件的动作")
|
|
||||||
# 遍历所有动作, 寻找符合条件的动作
|
|
||||||
current_mincost = pair_node.cond_leaf.mincost # 当前的最短路径是多少
|
|
||||||
|
|
||||||
for i in range(0, len(actions)):
|
|
||||||
if not c & ((actions[i].pre | actions[i].add) - actions[i].del_set) <= set():
|
|
||||||
if (c - actions[i].del_set) == c:
|
|
||||||
if self.verbose:
|
|
||||||
print("———— 满足条件可以扩展")
|
|
||||||
c_attr = (actions[i].pre | c) - actions[i].add
|
|
||||||
|
|
||||||
# 剪枝操作,现在的条件是以前扩展过的条件的超集
|
|
||||||
valid = True
|
|
||||||
for j in self.traversed: # 剪枝操作
|
|
||||||
if j <= c_attr:
|
|
||||||
valid = False
|
|
||||||
if self.verbose:
|
|
||||||
print("———— --被剪枝")
|
|
||||||
break
|
|
||||||
|
|
||||||
if valid:
|
|
||||||
# 把符合条件的动作节点都放到列表里
|
|
||||||
if self.verbose:
|
|
||||||
print("———— -- %s 符合条件放入列表" % actions[i].name)
|
|
||||||
c_attr_node = Leaf(type='cond', content=c_attr, mincost=current_mincost + actions[i].cost)
|
|
||||||
a_attr_node = Leaf(type='act', content=actions[i], mincost=current_mincost + actions[i].cost)
|
|
||||||
cond_anc_pair = CondActPair(cond_leaf=c_attr_node, act_leaf=a_attr_node)
|
|
||||||
self.nodes.append(copy.deepcopy(cond_anc_pair)) # condition node list
|
|
||||||
self.traversed.append(c_attr) # 重点 the set of expanded conditions
|
|
||||||
|
|
||||||
if self.verbose:
|
|
||||||
print("算法结束！\n")
|
|
||||||
return True
|
|
||||||
|
|
||||||
def print_solution(self):
|
|
||||||
print("========= BT ==========") # 树的bfs遍历
|
|
||||||
nodes_ls = []
|
|
||||||
nodes_ls.append(self.bt)
|
|
||||||
while len(nodes_ls) != 0:
|
|
||||||
parnode = nodes_ls[0]
|
|
||||||
print("Parrent:", parnode.type)
|
|
||||||
for child in parnode.children:
|
|
||||||
if isinstance(child, Leaf):
|
|
||||||
print("---- Leaf:", child.content)
|
|
||||||
elif isinstance(child, ControlBT):
|
|
||||||
print("---- ControlBT:", child.type)
|
|
||||||
nodes_ls.append(child)
|
|
||||||
print()
|
|
||||||
nodes_ls.pop(0)
|
|
||||||
print("========= BT ==========\n")
|
|
||||||
|
|
||||||
# 返回所有能到达目标状态的初始状态
|
|
||||||
def get_all_state_leafs(self):
|
|
||||||
state_leafs=[]
|
|
||||||
|
|
||||||
nodes_ls = []
|
|
||||||
nodes_ls.append(self.bt)
|
|
||||||
while len(nodes_ls) != 0:
|
|
||||||
parnode = nodes_ls[0]
|
|
||||||
for child in parnode.children:
|
|
||||||
if isinstance(child, Leaf):
|
|
||||||
if child.type == "cond":
|
|
||||||
state_leafs.append(child.content)
|
|
||||||
elif isinstance(child, ControlBT):
|
|
||||||
nodes_ls.append(child)
|
|
||||||
nodes_ls.pop(0)
|
|
||||||
|
|
||||||
return state_leafs
|
|
||||||
|
|
||||||
|
|
||||||
# 树的dfs
|
|
||||||
def dfs_ptml(self,parnode):
|
|
||||||
for child in parnode.children:
|
|
||||||
if isinstance(child, Leaf):
|
|
||||||
if child.type == 'cond':
|
|
||||||
self.ptml_string += "cond "
|
|
||||||
c_set_str = ', '.join(map(str, child.content)) + "\n"
|
|
||||||
self.ptml_string += c_set_str
|
|
||||||
elif child.type == 'act':
|
|
||||||
self.ptml_string += 'act ' + child.content.name + "\n"
|
|
||||||
elif isinstance(child, ControlBT):
|
|
||||||
if parnode.type == '?':
|
|
||||||
self.ptml_string += "selector{\n"
|
|
||||||
self.dfs_ptml(parnode=child)
|
|
||||||
elif parnode.type == '>':
|
|
||||||
self.ptml_string += "sequence{\n"
|
|
||||||
self.dfs_ptml( parnode=child)
|
|
||||||
self.ptml_string += '}\n'
|
|
||||||
|
|
||||||
|
|
||||||
def get_ptml(self):
|
|
||||||
self.ptml_string = "selector{\n"
|
|
||||||
self.dfs_ptml(self.bt.children[0])
|
|
||||||
self.ptml_string += '}\n'
|
|
||||||
return self.ptml_string
|
|
||||||
|
|
||||||
|
|
||||||
def save_ptml_file(self,file_name):
|
|
||||||
self.ptml_string = "selector{\n"
|
|
||||||
self.dfs_ptml(self.bt.children[0])
|
|
||||||
self.ptml_string += '}\n'
|
|
||||||
with open(f'./{file_name}.ptml', 'w') as file:
|
|
||||||
file.write(self.ptml_string)
|
|
||||||
return self.ptml_string
|
|
||||||
Binary file not shown.
|
Before Width: | Height: | Size: 54 KiB |
|
|
@ -1,125 +0,0 @@
|
||||||
|
|
||||||
|
|
||||||
## 代码说明
|
|
||||||
|
|
||||||
### 1. `BehaviorTree.py` 实现行为树叶子结点和非叶子结点的定义
|
|
||||||
|
|
||||||
- **Leaf**：表示叶节点，可以是动作（`act`）或条件（`cond`）。
|
|
||||||
- **ControlBT**：代表可能包含控制节点的行为树。它们可以是选择器（`?`）、序列（`>`）、动作节点（`act`）或条件节点（`cond`）。
|
|
||||||
- 上述两个类都包含 `tick` 方法。
|
|
||||||
|
|
||||||
### 2. `OptimalBTExpansionAlgorithm.py` 实现最优行为树扩展算法
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
定义行动类
|
|
||||||
```python
|
|
||||||
#定义行动类，行动包括前提、增加和删除影响
|
|
||||||
class Action:
|
|
||||||
def __init__(self,name='anonymous action',pre=set(),add=set(),del_set=set(),cost=1):
|
|
||||||
self.pre=copy.deepcopy(pre)
|
|
||||||
self.add=copy.deepcopy(add)
|
|
||||||
self.del_set=copy.deepcopy(del_set)
|
|
||||||
self.name=name
|
|
||||||
self.cost=cost
|
|
||||||
|
|
||||||
def __str__(self):
|
|
||||||
return self.name
|
|
||||||
```
|
|
||||||
|
|
||||||
调用算法
|
|
||||||
```python
|
|
||||||
algo = OptBTExpAlgorithm(verbose=True)
|
|
||||||
algo.clear()
|
|
||||||
algo.run_algorithm(start, goal, actions) # 使用算法得到行为树在 algo.bt
|
|
||||||
algo.print_solution() # 打印行为树
|
|
||||||
val, obj = algo.bt.tick(state) # 执行行为树
|
|
||||||
algo.save_ptml_file("bt.ptml") # 保存行为树为 ptml 文件
|
|
||||||
```
|
|
||||||
|
|
||||||
### 3. **`tools.py`** 实现打印数据、行为树测试等模块
|
|
||||||
|
|
||||||
使用方法
|
|
||||||
|
|
||||||
```python
|
|
||||||
print_action_data_table(goal,start,actions) # 打印所有变量
|
|
||||||
|
|
||||||
# 行为树鲁棒性测试，随机生成规划问题
|
|
||||||
# 设置生成规划问题集的超参数：文字数、解深度、迭代次数
|
|
||||||
seed=1
|
|
||||||
literals_num=10
|
|
||||||
depth = 10
|
|
||||||
iters= 10
|
|
||||||
BTTest(seed=seed,literals_num=literals_num,depth=depth,iters=iters)
|
|
||||||
```
|
|
||||||
|
|
||||||
### 4. `example.py` 中设计规划案例 goals, start, actions
|
|
||||||
|
|
||||||
```python
|
|
||||||
def MoveBtoB ():
|
|
||||||
actions=[]
|
|
||||||
a = Action(name="Move(b,ab)")
|
|
||||||
a.pre={'Free(ab)','WayClear'}
|
|
||||||
a.add={'At(b,ab)'}
|
|
||||||
a.del_set= {'Free(ab)','At(b,pb)'}
|
|
||||||
a.cost = 1
|
|
||||||
actions.append(a)
|
|
||||||
|
|
||||||
a=Action(name="Move(s,ab)")
|
|
||||||
a.pre={'Free(ab)'}
|
|
||||||
a.add={'Free(ab)','WayClear'}
|
|
||||||
a.del_set={'Free(ab)','At(s,ps)'}
|
|
||||||
a.cost = 1
|
|
||||||
actions.append(a)
|
|
||||||
|
|
||||||
a=Action(name="Move(s,as)")
|
|
||||||
a.pre={'Free(as)'}
|
|
||||||
a.add={'At(s,ps)','WayClear'}
|
|
||||||
a.del_set={'Free(as)','At(s,ps)'}
|
|
||||||
a.cost = 1
|
|
||||||
actions.append(a)
|
|
||||||
|
|
||||||
start = {'Free(ab)','Free(as)','At(b,pb)','At(s,ps)'}
|
|
||||||
goal= {'At(b,ab)'}
|
|
||||||
return goal,start,actions
|
|
||||||
```
|
|
||||||
|
|
||||||
### 5. `opt_bt_exp_main.py` 为主函数，在此演示如何调用最优行为树扩展算法得到完全扩展最优行为树
|
|
||||||
|
|
||||||
初始化的时候：传入 actions (包含 pre,add,del,cost).
|
|
||||||
调用的时候，传入 goal 状态集合 (set类型)，返回完全最优扩展行为树的 ptml 形式 (string类型)
|
|
||||||
|
|
||||||
```python
|
|
||||||
actions=[
|
|
||||||
Action(name='PutDown(Table,Coffee)', pre={'Holding(Coffee)','At(Robot,Table)'}, add={'At(Table,Coffee)','NotHolding'}, del_set={'Holding(Coffee)'}, cost=1)
|
|
||||||
…………
|
|
||||||
]
|
|
||||||
algo = BTOptExpInterface(actions)
|
|
||||||
|
|
||||||
goal = {'At(Table,Coffee)'}
|
|
||||||
ptml_string = algo.process(goal,start)
|
|
||||||
print(ptml_string)
|
|
||||||
|
|
||||||
```
|
|
||||||
两种检测方法，用于检测当前状态 `start` 能否到达目标状态 `goal`
|
|
||||||
|
|
||||||
```python
|
|
||||||
# 判断初始状态能否到达目标状态
|
|
||||||
start = {'At(Robot,Bar)', 'Holding(VacuumCup)', 'Available(Table)', 'Available(CoffeeMachine)','Available(FrontDesk)'}
|
|
||||||
|
|
||||||
# 方法一：算法返回所有可能的初始状态，在里面看看有没有对应的初始状态
|
|
||||||
right_bt = algo.find_all_leaf_states_contain_start(start)
|
|
||||||
if not right_bt:
|
|
||||||
print("ERROR1: The current state cannot reach the goal state!")
|
|
||||||
else:
|
|
||||||
print("Right1: The current state can reach the goal state!")
|
|
||||||
|
|
||||||
# 方法二：预先跑一边行为树，看能否到达目标状态
|
|
||||||
right_bt2 = algo.run_bt_from_start(goal,start)
|
|
||||||
if not right_bt2:
|
|
||||||
print("ERROR2: The current state cannot reach the goal state!")
|
|
||||||
else:
|
|
||||||
print("Right2: The current state can reach the goal state!")
|
|
||||||
|
|
||||||
```
|
|
||||||
|
|
||||||
|
|
@ -1,174 +0,0 @@
|
||||||
|
|
||||||
from zoo.opt_bt_expansion.OptimalBTExpansionAlgorithm import Action
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
def MakeCoffee():
|
|
||||||
actions=[
|
|
||||||
Action(name='Put(Table,Coffee)', pre={'Holding(Coffee)','At(Table)'}, add={'At(Table,Coffee)','NotHolding'}, del_set={'Holding(Coffee)'}, cost=1),
|
|
||||||
Action(name='Put(Table,VacuumCup)', pre={'Holding(VacuumCup)','At(Table)'}, add={'At(Table,VacuumCup)','NotHolding'}, del_set={'Holding(VacuumCup)'}, cost=1),
|
|
||||||
|
|
||||||
Action(name='Grasp(Coffee)', pre={'NotHolding','At(Coffee)'}, add={'Holding(Coffee)'}, del_set={'NotHolding'}, cost=1),
|
|
||||||
|
|
||||||
Action(name='MoveTo(Table)', pre={'Exist(Table)'}, add={'At(Table)'}, del_set={'At(FrontDesk)','At(Coffee)','At(CoffeeMachine)'}, cost=1),
|
|
||||||
Action(name='MoveTo(Coffee)', pre={'Exist(Coffee)'}, add={'At(Coffee)'}, del_set={'At(FrontDesk)','At(Table)','At(CoffeeMachine)'}, cost=1),
|
|
||||||
Action(name='MoveTo(CoffeeMachine)', pre={'Exist(CoffeeMachine)'}, add={'At(CoffeeMachine)'}, del_set={'At(FrontDesk)','At(Coffee)','At(Table)'}, cost=1),
|
|
||||||
|
|
||||||
Action(name='OpCoffeeMachine', pre={'At(CoffeeMachine)','NotHolding'}, add={'Exist(Coffee)','At(Coffee)'}, del_set=set(), cost=1),
|
|
||||||
]
|
|
||||||
|
|
||||||
start = {'At(FrontDesk)','Holding(VacuumCup)','Exist(Table)','Exist(CoffeeMachine)','Exist(FrontDesk)'}
|
|
||||||
goal = {'At(Table,Coffee)'}
|
|
||||||
return goal,start,actions
|
|
||||||
|
|
||||||
# 本例子中，将 VacuumCup 放到 FrontDesk，比 MoveTo(Table) 再 Put(Table,VacuumCup) 的 cost 要小
|
|
||||||
def MakeCoffeeCost():
|
|
||||||
actions=[
|
|
||||||
Action(name='PutDown(Table,Coffee)', pre={'Holding(Coffee)','At(Robot,Table)'}, add={'At(Table,Coffee)','NotHolding'}, del_set={'Holding(Coffee)'}, cost=1),
|
|
||||||
Action(name='PutDown(Table,VacuumCup)', pre={'Holding(VacuumCup)','At(Robot,Table)'}, add={'At(Table,VacuumCup)','NotHolding'}, del_set={'Holding(VacuumCup)'}, cost=1),
|
|
||||||
|
|
||||||
Action(name='PickUp(Coffee)', pre={'NotHolding','At(Robot,Coffee)'}, add={'Holding(Coffee)'}, del_set={'NotHolding'}, cost=1),
|
|
||||||
|
|
||||||
Action(name='MoveTo(Table)', pre={'Available(Table)'}, add={'At(Robot,Table)'}, del_set={'At(Robot,FrontDesk)','At(Robot,Coffee)','At(Robot,CoffeeMachine)'}, cost=1),
|
|
||||||
Action(name='MoveTo(Coffee)', pre={'Available(Coffee)'}, add={'At(Robot,Coffee)'}, del_set={'At(Robot,FrontDesk)','At(Robot,Table)','At(Robot,CoffeeMachine)'}, cost=1),
|
|
||||||
Action(name='MoveTo(CoffeeMachine)', pre={'Available(CoffeeMachine)'}, add={'At(Robot,CoffeeMachine)'}, del_set={'At(Robot,FrontDesk)','At(Robot,Coffee)','At(Robot,Table)'}, cost=1),
|
|
||||||
|
|
||||||
Action(name='OpCoffeeMachine', pre={'At(Robot,CoffeeMachine)','NotHolding'}, add={'Available(Coffee)','At(Robot,Coffee)'}, del_set=set(), cost=1),
|
|
||||||
]
|
|
||||||
|
|
||||||
start = {'At(Robot,Bar)','Holding(VacuumCup)','Available(Table)','Available(CoffeeMachine)','Available(FrontDesk)'}
|
|
||||||
goal = {'At(Table,Coffee)'}
|
|
||||||
|
|
||||||
return goal,start,actions
|
|
||||||
|
|
||||||
|
|
||||||
# test
|
|
||||||
def Test():
|
|
||||||
actions=[
|
|
||||||
Action(name='a1', pre={6}, add={0,2,4}, del_set={1,5}, cost=1),
|
|
||||||
Action(name='a2', pre=set(), add={0,1}, del_set=set(), cost=1),
|
|
||||||
Action(name='a3', pre={1,6}, add={0,2,3,5}, del_set={1,6}, cost=1),
|
|
||||||
Action(name='a4', pre={0,2,3}, add={4,5}, del_set={0,6}, cost=1),
|
|
||||||
Action(name='a5', pre={0,1,4}, add={2,3,6}, del_set={0}, cost=1),
|
|
||||||
]
|
|
||||||
|
|
||||||
start = {1,2,6}
|
|
||||||
goal={0,1,2,4,6}
|
|
||||||
return goal,start,actions
|
|
||||||
|
|
||||||
# def Test():
|
|
||||||
# actions=[
|
|
||||||
# Action(name='a1', pre={2}, add={1}, del_set=set(), cost=1),
|
|
||||||
# Action(name='a2', pre=set(), add={1}, del_set={0,2}, cost=1),
|
|
||||||
# Action(name='a3', pre={1}, add=set(), del_set={0,2}, cost=1),
|
|
||||||
# Action(name='a4', pre=set(), add={0}, del_set=set(), cost=1),
|
|
||||||
# Action(name='a5', pre={1}, add={0,2}, del_set={1}, cost=1),
|
|
||||||
# Action(name='a6', pre={1}, add=set(), del_set={0,1,2}, cost=1),
|
|
||||||
# Action(name='a7', pre={1}, add={2}, del_set={0, 2}, cost=1),
|
|
||||||
# ]
|
|
||||||
#
|
|
||||||
# start = {1,2}
|
|
||||||
# goal={0,1}
|
|
||||||
# return goal,start,actions
|
|
||||||
|
|
||||||
|
|
||||||
# todo: 最原始的例子
|
|
||||||
def MoveBtoB_num ():
|
|
||||||
actions=[]
|
|
||||||
a = Action(name='a1')
|
|
||||||
a.pre={1,4}
|
|
||||||
a.add={"c_goal"}
|
|
||||||
a.del_set={1,4}
|
|
||||||
a.cost = 1
|
|
||||||
actions.append(a)
|
|
||||||
|
|
||||||
a=Action(name='a2')
|
|
||||||
a.pre={1,2,3}
|
|
||||||
a.add={"c_goal"}
|
|
||||||
a.del_set={1,2,3}
|
|
||||||
a.cost = 1
|
|
||||||
actions.append(a)
|
|
||||||
|
|
||||||
a=Action(name='a3')
|
|
||||||
a.pre={1,2}
|
|
||||||
a.add={4}
|
|
||||||
a.del_set={2}
|
|
||||||
a.cost = 1
|
|
||||||
actions.append(a)
|
|
||||||
|
|
||||||
a=Action(name='a4')
|
|
||||||
a.pre={"c_start"}
|
|
||||||
a.add={1,2,3}
|
|
||||||
a.del_set={"c_start",4}
|
|
||||||
a.cost = 1
|
|
||||||
actions.append(a)
|
|
||||||
|
|
||||||
start = {"c_start"}
|
|
||||||
goal={"c_goal"}
|
|
||||||
return goal,start,actions
|
|
||||||
|
|
||||||
|
|
||||||
# todo: 最原始的例子
|
|
||||||
def MoveBtoB ():
|
|
||||||
actions=[]
|
|
||||||
a = Action(name="Move(b,ab)") #'movebtob'
|
|
||||||
a.pre={'Free(ab)','WayClear'} #{1,2}
|
|
||||||
a.add={'At(b,ab)'} #{3}
|
|
||||||
a.del_set= {'Free(ab)','At(b,pb)'} #{1,4}
|
|
||||||
a.cost = 1
|
|
||||||
actions.append(a)
|
|
||||||
|
|
||||||
a=Action(name="Move(s,ab)") #'moveatob'
|
|
||||||
a.pre={'Free(ab)'} #{1}
|
|
||||||
a.add={'Free(ab)','WayClear'} #{5,2}
|
|
||||||
a.del_set={'Free(ab)','At(s,ps)'} #{1,6}
|
|
||||||
a.cost = 1
|
|
||||||
actions.append(a)
|
|
||||||
|
|
||||||
a=Action(name="Move(s,as)") #'moveatoa'
|
|
||||||
a.pre={'Free(as)'} #{7}
|
|
||||||
a.add={'At(s,ps)','WayClear'} #{8,2}
|
|
||||||
a.del_set={'Free(as)','At(s,ps)'} #{7,6}
|
|
||||||
a.cost = 1
|
|
||||||
actions.append(a)
|
|
||||||
|
|
||||||
start = {'Free(ab)','Free(as)','At(b,pb)','At(s,ps)'} #{1,7,4,6}
|
|
||||||
goal= {'At(b,ab)'} #{3}
|
|
||||||
return goal,start,actions
|
|
||||||
|
|
||||||
|
|
||||||
# 小蔡师兄论文里的例子
|
|
||||||
def Cond2BelongsToCond3():
|
|
||||||
actions=[]
|
|
||||||
a = Action(name='a1')
|
|
||||||
a.pre={1,4}
|
|
||||||
a.add={"c_goal"}
|
|
||||||
a.del_set={1,4}
|
|
||||||
a.cost = 1
|
|
||||||
actions.append(a)
|
|
||||||
|
|
||||||
a=Action(name='a2')
|
|
||||||
a.pre={1,2,3}
|
|
||||||
a.add={"c_goal"}
|
|
||||||
a.del_set={1,2,3}
|
|
||||||
a.cost = 100
|
|
||||||
actions.append(a)
|
|
||||||
|
|
||||||
a=Action(name='a3')
|
|
||||||
a.pre={1,2}
|
|
||||||
a.add={4}
|
|
||||||
a.del_set={2}
|
|
||||||
a.cost = 1
|
|
||||||
actions.append(a)
|
|
||||||
|
|
||||||
a=Action(name='a4')
|
|
||||||
a.pre={"c_start"}
|
|
||||||
a.add={1,2,3}
|
|
||||||
a.del_set={"c_start",4}
|
|
||||||
a.cost = 1
|
|
||||||
actions.append(a)
|
|
||||||
|
|
||||||
start = {"c_start"}
|
|
||||||
goal={"c_goal"}
|
|
||||||
return goal,start,actions
|
|
||||||
|
|
||||||
|
|
@ -1,118 +0,0 @@
|
||||||
from zoo.opt_bt_expansion.OptimalBTExpansionAlgorithm import Action,OptBTExpAlgorithm,state_transition # 调用最优行为树扩展算法
|
|
||||||
from opt_bt_expansion.examples import *
|
|
||||||
|
|
||||||
|
|
||||||
# 封装好的主接口
|
|
||||||
class BTOptExpInterface:
|
|
||||||
def __init__(self, action_list):
|
|
||||||
"""
|
|
||||||
Initialize the BTOptExpansion with a list of actions.
|
|
||||||
:param action_list: A list of actions to be used in the behavior tree.
|
|
||||||
"""
|
|
||||||
# self.actions = []
|
|
||||||
# for act in action_list:
|
|
||||||
# a = Action(name=act.name)
|
|
||||||
# a.pre=act['pre']
|
|
||||||
# a.add=act['add']
|
|
||||||
# a.del_set= act['del_set']
|
|
||||||
# a.cost = 1
|
|
||||||
# self.actions.append(a)
|
|
||||||
self.actions = action_list
|
|
||||||
self.has_processed = False
|
|
||||||
|
|
||||||
def process(self, goal):
|
|
||||||
"""
|
|
||||||
Process the input sets and return a string result.
|
|
||||||
:param input_set: The set of goal states and the set of initial states.
|
|
||||||
:return: A PTML string representing the outcome of the behavior tree.
|
|
||||||
"""
|
|
||||||
self.goal = goal
|
|
||||||
self.algo = OptBTExpAlgorithm(verbose=False)
|
|
||||||
self.algo.clear()
|
|
||||||
self.algo.run_algorithm(self.goal, self.actions) # 调用算法得到行为树保存至 algo.bt
|
|
||||||
self.ptml_string = self.algo.get_ptml()
|
|
||||||
self.has_processed = True
|
|
||||||
# algo.print_solution() # print behavior tree
|
|
||||||
|
|
||||||
return self.ptml_string
|
|
||||||
|
|
||||||
# 方法一：查找所有初始状态是否包含当前状态
|
|
||||||
def find_all_leaf_states_contain_start(self,start):
|
|
||||||
if not self.has_processed:
|
|
||||||
raise RuntimeError("The process method must be called before find_all_leaf_states_contain_start!")
|
|
||||||
# 返回所有能到达目标状态的初始状态
|
|
||||||
state_leafs = self.algo.get_all_state_leafs()
|
|
||||||
for state in state_leafs:
|
|
||||||
if start >= state:
|
|
||||||
return True
|
|
||||||
return False
|
|
||||||
|
|
||||||
# 方法二：模拟跑一遍行为树，看 start 能够通过执行一系列动作到达 goal
|
|
||||||
def run_bt_from_start(self,goal,start):
|
|
||||||
if not self.has_processed:
|
|
||||||
raise RuntimeError("The process method must be called before run_bt_from_start!")
|
|
||||||
# 检查是否能到达目标
|
|
||||||
right_bt = True
|
|
||||||
state = start
|
|
||||||
steps = 0
|
|
||||||
val, obj = self.algo.bt.tick(state)
|
|
||||||
while val != 'success' and val != 'failure':
|
|
||||||
state = state_transition(state, obj)
|
|
||||||
val, obj = self.algo.bt.tick(state)
|
|
||||||
if (val == 'failure'):
|
|
||||||
# print("bt fails at step", steps)
|
|
||||||
right_bt = False
|
|
||||||
steps += 1
|
|
||||||
if not goal <= state:
|
|
||||||
# print("wrong solution", steps)
|
|
||||||
right_bt = False
|
|
||||||
else:
|
|
||||||
pass
|
|
||||||
# print("right solution", steps)
|
|
||||||
return right_bt
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__' :
|
|
||||||
|
|
||||||
# todo: Example Cafe
|
|
||||||
# todo: Define goal, start, actions
|
|
||||||
actions=[
|
|
||||||
Action(name='PutDown(Table,Coffee)', pre={'Holding(Coffee)','At(Robot,Table)'}, add={'At(Table,Coffee)','NotHolding'}, del_set={'Holding(Coffee)'}, cost=1),
|
|
||||||
Action(name='PutDown(Table,VacuumCup)', pre={'Holding(VacuumCup)','At(Robot,Table)'}, add={'At(Table,VacuumCup)','NotHolding'}, del_set={'Holding(VacuumCup)'}, cost=1),
|
|
||||||
|
|
||||||
Action(name='PickUp(Coffee)', pre={'NotHolding','At(Robot,Coffee)'}, add={'Holding(Coffee)'}, del_set={'NotHolding'}, cost=1),
|
|
||||||
|
|
||||||
Action(name='MoveTo(Table)', pre={'Available(Table)'}, add={'At(Robot,Table)'}, del_set={'At(Robot,FrontDesk)','At(Robot,Coffee)','At(Robot,CoffeeMachine)'}, cost=1),
|
|
||||||
Action(name='MoveTo(Coffee)', pre={'Available(Coffee)'}, add={'At(Robot,Coffee)'}, del_set={'At(Robot,FrontDesk)','At(Robot,Table)','At(Robot,CoffeeMachine)'}, cost=1),
|
|
||||||
Action(name='MoveTo(CoffeeMachine)', pre={'Available(CoffeeMachine)'}, add={'At(Robot,CoffeeMachine)'}, del_set={'At(Robot,FrontDesk)','At(Robot,Coffee)','At(Robot,Table)'}, cost=1),
|
|
||||||
|
|
||||||
Action(name='OpCoffeeMachine', pre={'At(Robot,CoffeeMachine)','NotHolding'}, add={'Available(Coffee)','At(Robot,Coffee)'}, del_set=set(), cost=1),
|
|
||||||
]
|
|
||||||
algo = BTOptExpInterface(actions)
|
|
||||||
|
|
||||||
|
|
||||||
goal = {'At(Table,Coffee)'}
|
|
||||||
ptml_string = algo.process(goal)
|
|
||||||
print(ptml_string)
|
|
||||||
|
|
||||||
file_name = "MakeCoffee"
|
|
||||||
with open(f'./{file_name}.ptml', 'w') as file:
|
|
||||||
file.write(ptml_string)
|
|
||||||
|
|
||||||
|
|
||||||
# 判断初始状态能否到达目标状态
|
|
||||||
start = {'At(Robot,Bar)', 'Holding(VacuumCup)', 'Available(Table)', 'Available(CoffeeMachine)','Available(FrontDesk)'}
|
|
||||||
# 方法一：算法返回所有可能的初始状态，在里面看看有没有对应的初始状态
|
|
||||||
right_bt = algo.find_all_leaf_states_contain_start(start)
|
|
||||||
if not right_bt:
|
|
||||||
print("ERROR1: The current state cannot reach the goal state!")
|
|
||||||
else:
|
|
||||||
print("Right1: The current state can reach the goal state!")
|
|
||||||
# 方法二：预先跑一边行为树，看能否到达目标状态
|
|
||||||
right_bt2 = algo.run_bt_from_start(goal,start)
|
|
||||||
if not right_bt2:
|
|
||||||
print("ERROR2: The current state cannot reach the goal state!")
|
|
||||||
else:
|
|
||||||
print("Right2: The current state can reach the goal state!")
|
|
||||||
|
|
@ -1,167 +0,0 @@
|
||||||
|
|
||||||
|
|
||||||
from tabulate import tabulate
|
|
||||||
import numpy as np
|
|
||||||
import random
|
|
||||||
from zoo.opt_bt_expansion.OptimalBTExpansionAlgorithm import Action,OptBTExpAlgorithm
|
|
||||||
import time
|
|
||||||
|
|
||||||
|
|
||||||
def print_action_data_table(goal,start,actions):
|
|
||||||
data = []
|
|
||||||
for a in actions:
|
|
||||||
data.append([a.name , a.pre , a.add , a.del_set , a.cost])
|
|
||||||
data.append(["Goal" ,goal ," " ,"Start" ,start])
|
|
||||||
print(tabulate(data, headers=["Name", "Pre", "Add" ,"Del" ,"Cost"], tablefmt="fancy_grid")) # grid plain simple github fancy_grid
|
|
||||||
|
|
||||||
|
|
||||||
# 从状态随机生成一个行动
|
|
||||||
def generate_from_state(act,state,num):
|
|
||||||
for i in range(0,num):
|
|
||||||
if i in state:
|
|
||||||
if random.random() >0.5:
|
|
||||||
act.pre.add(i)
|
|
||||||
if random.random() >0.5:
|
|
||||||
act.del_set.add(i)
|
|
||||||
continue
|
|
||||||
if random.random() > 0.5:
|
|
||||||
act.add.add(i)
|
|
||||||
continue
|
|
||||||
if random.random() >0.5:
|
|
||||||
act.del_set.add(i)
|
|
||||||
|
|
||||||
def print_action(act):
|
|
||||||
print (act.pre)
|
|
||||||
print(act.add)
|
|
||||||
print(act.del_set)
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
#行为树测试代码
|
|
||||||
def BTTest(seed=1,literals_num=10,depth=10,iters=10,total_count=1000):
|
|
||||||
print("============= BT Test ==============")
|
|
||||||
random.seed(seed)
|
|
||||||
# 设置生成规划问题集的超参数：文字数、解深度、迭代次数
|
|
||||||
literals_num=literals_num
|
|
||||||
depth = depth
|
|
||||||
iters= iters
|
|
||||||
total_tree_size = []
|
|
||||||
total_action_num = []
|
|
||||||
total_state_num = []
|
|
||||||
total_steps_num=[]
|
|
||||||
#fail_count=0
|
|
||||||
#danger_count=0
|
|
||||||
success_count =0
|
|
||||||
failure_count = 0
|
|
||||||
planning_time_total = 0.0
|
|
||||||
# 实验1000次
|
|
||||||
for count in range (total_count):
|
|
||||||
|
|
||||||
action_num = 1
|
|
||||||
|
|
||||||
# 生成一个规划问题，包括随机的状态和行动，以及目标状态
|
|
||||||
states = []
|
|
||||||
actions = []
|
|
||||||
start = generate_random_state(literals_num)
|
|
||||||
state = start
|
|
||||||
states.append(state)
|
|
||||||
#print (state)
|
|
||||||
for i in range (0,depth):
|
|
||||||
a = Action()
|
|
||||||
generate_from_state(a,state,literals_num)
|
|
||||||
if not a in actions:
|
|
||||||
a.name = "a"+str(action_num)
|
|
||||||
action_num+=1
|
|
||||||
actions.append(a)
|
|
||||||
state = state_transition(state,a)
|
|
||||||
if state in states:
|
|
||||||
pass
|
|
||||||
else:
|
|
||||||
states.append(state)
|
|
||||||
#print(state)
|
|
||||||
|
|
||||||
goal = states[-1]
|
|
||||||
state = start
|
|
||||||
for i in range (0,iters):
|
|
||||||
a = Action()
|
|
||||||
generate_from_state(a,state,literals_num)
|
|
||||||
if not a in actions:
|
|
||||||
a.name = "a"+str(action_num)
|
|
||||||
action_num+=1
|
|
||||||
actions.append(a)
|
|
||||||
state = state_transition(state,a)
|
|
||||||
if state in states:
|
|
||||||
pass
|
|
||||||
else:
|
|
||||||
states.append(state)
|
|
||||||
state = random.sample(states,1)[0]
|
|
||||||
|
|
||||||
# 选择测试本文算法btalgorithm，或对比算法weakalgorithm
|
|
||||||
algo = OptBTExpAlgorithm()
|
|
||||||
#algo = Weakalgorithm()
|
|
||||||
start_time = time.time()
|
|
||||||
# print_action_data_table(goal, start, list(actions))
|
|
||||||
if algo.run_algorithm(start, goal, actions):#运行算法，规划后行为树为algo.bt
|
|
||||||
total_tree_size.append( algo.bt.count_size()-1)
|
|
||||||
# algo.print_solution() # 打印行为树
|
|
||||||
else:
|
|
||||||
print ("error")
|
|
||||||
end_time = time.time()
|
|
||||||
planning_time_total += (end_time-start_time)
|
|
||||||
|
|
||||||
#开始从初始状态运行行为树，测试
|
|
||||||
state=start
|
|
||||||
steps=0
|
|
||||||
val, obj = algo.bt.tick(state)#tick行为树，obj为所运行的行动
|
|
||||||
while val !='success' and val !='failure':#运行直到行为树成功或失败
|
|
||||||
state = state_transition(state,obj)
|
|
||||||
val, obj = algo.bt.tick(state)
|
|
||||||
if(val == 'failure'):
|
|
||||||
print("bt fails at step",steps)
|
|
||||||
steps+=1
|
|
||||||
if(steps>=500):#至多运行500步
|
|
||||||
break
|
|
||||||
if not goal <= state:#错误解，目标条件不在执行后状态满足
|
|
||||||
#print ("wrong solution",steps)
|
|
||||||
failure_count+=1
|
|
||||||
|
|
||||||
else:#正确解，满足目标条件
|
|
||||||
#print ("right solution",steps)
|
|
||||||
success_count+=1
|
|
||||||
total_steps_num.append(steps)
|
|
||||||
algo.clear()
|
|
||||||
total_action_num.append(len(actions))
|
|
||||||
total_state_num.append(len(states))
|
|
||||||
|
|
||||||
print ("success:",success_count,"failure:",failure_count)#算法成功和失败次数
|
|
||||||
print("Total Tree Size: mean=",np.mean(total_tree_size), "std=",np.std(total_tree_size, ddof=1))#1000次测试树大小
|
|
||||||
print ("Total Steps Num: mean=",np.mean(total_steps_num),"std=",np.std(total_steps_num,ddof=1))
|
|
||||||
print ("Average number of states:",np.mean(total_state_num))#1000次问题的平均状态数
|
|
||||||
print ("Average number of actions",np.mean(total_action_num))#1000次问题的平均行动数
|
|
||||||
print("Planning Time Total:",planning_time_total,planning_time_total/1000.0)
|
|
||||||
print("============ End BT Test ===========")
|
|
||||||
|
|
||||||
# xiao cai
|
|
||||||
# success: 1000 failure: 0
|
|
||||||
# Total Tree Size: mean= 35.303 std= 29.71336526001515
|
|
||||||
# Total Steps Num: mean= 1.898 std= 0.970844240101644
|
|
||||||
# Average number of states: 20.678
|
|
||||||
# Average number of actions 20.0
|
|
||||||
# Planning Time Total: 0.6280641555786133 0.0006280641555786133
|
|
||||||
|
|
||||||
# our start
|
|
||||||
# success: 1000 failure: 0
|
|
||||||
# Total Tree Size: mean= 17.945 std= 12.841997192488865
|
|
||||||
# Total Steps Num: mean= 1.785 std= 0.8120556843187752
|
|
||||||
# Average number of states: 20.678
|
|
||||||
# Average number of actions 20.0
|
|
||||||
# Planning Time Total: 1.4748523235321045 0.0014748523235321046
|
|
||||||
|
|
||||||
# our
|
|
||||||
# success: 1000 failure: 0
|
|
||||||
# Total Tree Size: mean= 48.764 std= 20.503626574406358
|
|
||||||
# Total Steps Num: mean= 1.785 std= 0.8120556843187752
|
|
||||||
# Average number of states: 20.678
|
|
||||||
# Average number of actions 20.0
|
|
||||||
# Planning Time Total: 3.3271877765655518 0.0033271877765655516
|
|
||||||
|
|
||||||
Loading…
Reference in New Issue