Merge remote-tracking branch 'origin/main'
This commit is contained in:
wuziji
commit
355f1fac7b
|
|
@ -20,6 +20,8 @@ MANIFEST
|
|||
MO-VLN/
|
||||
GLIP/
|
||||
|
||||
sub_task.ptml
|
||||
|
||||
# PyInstaller
|
||||
# Usually these files are written by a python script from a template
|
||||
# before PyInstaller builds the exe, so as to inject date/other infos into it.
|
||||
|
|
|
|||
|
|
@ -1,4 +1,3 @@
|
|||
|
||||
from . import navigate
|
||||
from . import dstar_lite
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,59 @@
|
|||
# !/usr/bin/env python3
|
||||
# -*- encoding: utf-8 -*-
|
||||
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
import pickle
|
||||
import os
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
def draw_grid_map(grid_map):
|
||||
# 生成新的地图图像
|
||||
plt.imshow(grid_map, cmap='binary', alpha=0.5, origin='lower') # 黑白网格
|
||||
|
||||
# 绘制坐标轴
|
||||
plt.xlabel('y', loc='right')
|
||||
plt.ylabel('x', loc='top')
|
||||
|
||||
# 显示网格线
|
||||
plt.grid(color='black', linestyle='-', linewidth=0.5)
|
||||
|
||||
# 显示图像
|
||||
plt.show()
|
||||
#plt.pause(0.01)
|
||||
|
||||
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
# control.init_world(scene_num=1, mapID=3)
|
||||
# scene = control.Scene(sceneID=0)
|
||||
#
|
||||
# X = int(950/5) # 采点数量
|
||||
# Y = int(1850/5)
|
||||
# map = np.zeros((X, Y))
|
||||
#
|
||||
# for x in range(X):
|
||||
# for y in range(Y):
|
||||
# if not scene.reachable_check(x*5-350, y*5-400, Yaw=0):
|
||||
# map[x, y] = 1
|
||||
# print(x, y)
|
||||
#
|
||||
#
|
||||
# file_name = 'map_5.pkl'
|
||||
# if not os.path.exists(file_name):
|
||||
# open(file_name, 'w').close()
|
||||
# with open(file_name, 'wb') as file:
|
||||
# pickle.dump(map, file)
|
||||
# print('保存成功')
|
||||
|
||||
|
||||
file_name = 'map_5.pkl'
|
||||
if os.path.exists(file_name):
|
||||
with open(file_name, 'rb') as file:
|
||||
map = pickle.load(file)
|
||||
draw_grid_map(map)
|
||||
|
|
@ -23,7 +23,8 @@ def manhattan_distance(start, end): # 曼哈顿距离
|
|||
|
||||
def euclidean_distance(start, end): # 欧式距离
|
||||
# return np.linalg.norm(start-end)
|
||||
return math.sqrt((start[0] - end[0]) ** 2 + (start[1] - end[1]) ** 2)
|
||||
# return math.sqrt((start[0] - end[0]) ** 2 + (start[1] - end[1]) ** 2)
|
||||
return math.hypot(start[0] - end[0], start[1] - end[1])
|
||||
|
||||
|
||||
def heuristic(start, end, name='euclidean'):
|
||||
|
|
@ -115,9 +116,9 @@ class PriorityQueue:
|
|||
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实际身位半径
|
||||
area_range, # [x_min, x_max, y_min, y_max] 实际坐标范围
|
||||
scale_ratio=5, # 地图缩放率
|
||||
dyna_obs_radius=30, # dyna_obs实际身位半径
|
||||
):
|
||||
|
||||
# self.area_bounds = area
|
||||
|
|
@ -128,7 +129,7 @@ class DStarLite:
|
|||
(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缩放后身位半径
|
||||
self.dyna_obs_radius = math.ceil(dyna_obs_radius / scale_ratio) # dyna_obs缩放后身位半径
|
||||
|
||||
# free:0, obs:1, dyna_obs:2
|
||||
self.idx_to_object = {
|
||||
|
|
@ -140,7 +141,7 @@ class DStarLite:
|
|||
self.object_to_cost = {
|
||||
"free": 0,
|
||||
"obstacle": float('inf'),
|
||||
"dynamic obstacle": 50
|
||||
"dynamic obstacle": 100
|
||||
}
|
||||
|
||||
self.compute_cost_map()
|
||||
|
|
@ -259,7 +260,7 @@ class DStarLite:
|
|||
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):
|
||||
def _planning(self, s_start, s_goal, dyna_obs, debug=False):
|
||||
'''
|
||||
规划路径(实际实现)
|
||||
Args:
|
||||
|
|
@ -268,7 +269,7 @@ class DStarLite:
|
|||
'''
|
||||
# 确保目标合法
|
||||
if not self.in_bounds_without_obstacle(s_goal):
|
||||
return None
|
||||
return []
|
||||
# 第一次规划需要初始化rhs并将goal加入队列，计算最短路径
|
||||
if self.s_goal is None:
|
||||
self.s_start = tuple(s_start)
|
||||
|
|
@ -281,7 +282,6 @@ class DStarLite:
|
|||
# 后续规划只更新起点，直接使用原路径(去掉已走过部分)
|
||||
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:
|
||||
|
|
@ -299,17 +299,16 @@ class DStarLite:
|
|||
# pass
|
||||
return self.path
|
||||
|
||||
def planning(self, s_start, s_goal, dyna_obs, step_num=None, debug=False):
|
||||
def planning(self, s_start, s_goal, dyna_obs, 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])
|
||||
dyna_obs = [self.real2map(obs) for obs in dyna_obs]
|
||||
|
||||
self._planning(s_start, s_goal, dyna_obs, step_num, debug)
|
||||
self._planning(s_start, s_goal, dyna_obs, debug)
|
||||
|
||||
# 地图坐标->实际坐标
|
||||
path = [self.map2real(node) for node in self.path]
|
||||
|
|
@ -319,7 +318,7 @@ class DStarLite:
|
|||
'''
|
||||
得到路径
|
||||
Args:
|
||||
step_num: 路径步数 (None表示返回完整路径)
|
||||
step_num: 路径步数
|
||||
return:
|
||||
path: [(x, y), ...]
|
||||
'''
|
||||
|
|
@ -327,7 +326,6 @@ class DStarLite:
|
|||
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])]
|
||||
|
|
@ -411,10 +409,13 @@ class DStarLite:
|
|||
根据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))
|
||||
# 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 = [(i, j) 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)
|
||||
if euclidean_distance((i, j), obs_pos) < self.dyna_obs_radius]
|
||||
|
||||
occupy_pos = filter(self.in_bounds_without_obstacle, occupy_pos) # 确保位置在地图范围内 且 不是静态障碍物
|
||||
return list(occupy_pos)
|
||||
|
||||
|
|
@ -507,4 +508,4 @@ class DStarLite:
|
|||
plt.xlabel('y', loc='right')
|
||||
plt.ylabel('x', loc='top')
|
||||
plt.grid(color='black', linestyle='-', linewidth=0.5)
|
||||
plt.pause(0.3)
|
||||
plt.pause(0.2)
|
||||
|
|
|
|||
|
|
@ -1,71 +1,80 @@
|
|||
#!/usr/bin/env python3
|
||||
# -*- encoding: utf-8 -*-
|
||||
import math
|
||||
import sys
|
||||
import time
|
||||
from dstar_lite import DStarLite, euclidean_distance
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
from mpl_toolkits.axes_grid1 import make_axes_locatable
|
||||
|
||||
|
||||
from robowaiter.algos.navigate.DstarLite.dstar_lite import DStarLite
|
||||
|
||||
class Navigator:
|
||||
'''
|
||||
导航类
|
||||
'''
|
||||
|
||||
def __init__(self, scene, area_range, map, scale_ratio=5):
|
||||
def __init__(self, scene, area_range, map, scale_ratio=5, step_length=150, velocity=150, react_radius=250):
|
||||
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.area_range = area_range # 地图实际坐标范围 xmin, xmax, ymin, ymax
|
||||
self.map = map # 缩放并离散化的地图 array(X,Y)
|
||||
self.scale_ratio = scale_ratio # 地图缩放率
|
||||
self.step_length = step_length # 步长(单次移动)
|
||||
self.step_num = self.step_length // self.scale_ratio # 单次移动地图格数
|
||||
self.v = 200 # 速度
|
||||
self.step_time = self.step_length/self.v + 0.1 # 单步移动时长
|
||||
|
||||
self.v = velocity # 速度
|
||||
self.react_radius = react_radius # robot反应半径
|
||||
|
||||
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):
|
||||
def is_reached(pos: (float, float), goal: (float, float), dis_limit=50):
|
||||
'''
|
||||
判断是否到达目标
|
||||
'''
|
||||
dis = np.linalg.norm(pos - goal)
|
||||
dis = math.hypot(pos[0]-goal[0], pos[1]-goal[1])
|
||||
# dis = np.linalg.norm(pos - goal)
|
||||
return dis < dis_limit
|
||||
|
||||
def reset_goal(self, goal:(float, float)):
|
||||
# TODO: 使目标可达
|
||||
# 目标在障碍物上：从目标开始方形向外扩展，直到找到可行点
|
||||
# 目标在地图外面：起点和目标连线最靠近目标的可行点
|
||||
pass
|
||||
@staticmethod
|
||||
def get_yaw(pos: (float, float), goal: (float, float)):
|
||||
'''
|
||||
得到移动方向
|
||||
'''
|
||||
return math.degrees(math.atan2(goal[0] - pos[0], -(goal[1] - pos[1])))
|
||||
|
||||
def legalize_goal(self, goal: (float, float)):
|
||||
'''
|
||||
TODO: 处理非法目标
|
||||
目标在障碍物上：从目标开始方形向外扩展，直到找到可行点
|
||||
目标在地图外面：起点和目标连线最靠近目标的可行点
|
||||
'''
|
||||
return goal
|
||||
|
||||
def navigate(self, goal: (float, float), animation=True):
|
||||
'''
|
||||
单次导航，直到到达目标
|
||||
'''
|
||||
if not self.scene.reachable_check(goal[0], goal[1], 0):
|
||||
goal = self.legalize_goal(goal)
|
||||
|
||||
pos = np.array((self.scene.status.location.X, self.scene.status.location.Y)) # 机器人当前: 位置 和 朝向
|
||||
yaw = self.scene.status.rotation.Yaw
|
||||
pos = (self.scene.status.location.X, self.scene.status.location.Y) # 机器人当前: 位置 和 朝向
|
||||
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)
|
||||
dyna_obs = [obs for obs in dyna_obs if euclidean_distance(obs, pos) < self.react_radius] # 过滤观测范围外的dyna_obs
|
||||
# 周围有dyna_obs则步长减半
|
||||
if dyna_obs:
|
||||
step_num = self.step_num // 2
|
||||
else:
|
||||
step_num = self.step_num
|
||||
path = self.planner.planning(pos, goal, dyna_obs)
|
||||
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=' ')
|
||||
scene_info = self.scene.walk_to(next_x, next_y, yaw, velocity=self.v)
|
||||
yaw = scene_info.rotation.Yaw
|
||||
|
||||
if animation:
|
||||
self.planner.draw_graph(self.step_num) # 画出搜索路径
|
||||
# time.sleep(self.step_time)
|
||||
pos = np.array((self.scene.status.location.X, self.scene.status.location.Y))
|
||||
self.planner.draw_graph(step_num) # 画出搜索路径
|
||||
next_step = min(step_num, len(path))
|
||||
next_pos = path[next_step - 1]
|
||||
print('plan pos:', next_pos, end=' ')
|
||||
yaw = self.get_yaw(pos, next_pos)
|
||||
self.scene.walk_to(next_pos[0], next_pos[1], yaw, velocity=self.v, dis_limit=10)
|
||||
# pos = (self.scene.status.location.X, self.scene.status.location.Y)
|
||||
# if self.is_reached(pos, next_pos):
|
||||
self.planner.path = self.planner.path[next_step - 1:] # 去除已走过的路径
|
||||
pos = (self.scene.status.location.X, self.scene.status.location.Y)
|
||||
print('reach pos:', pos)
|
||||
|
||||
self.planner.reset() # 完成一轮导航，重置变量
|
||||
|
|
@ -76,4 +85,3 @@ class Navigator:
|
|||
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -1,4 +1,52 @@
|
|||
### dstar_lite.py ——Dstar lite算法文件
|
||||
### navigate.py ——导航类
|
||||
### test.py ——测试文件
|
||||
### map_5.pkl ——离散化地图文件
|
||||
## 默认使用RRTStar+路径平滑
|
||||
|
||||
### apf.py: 势场法实现
|
||||
|
||||
### discretize_map.py: 地图离散化并压缩
|
||||
|
||||
### map_5.pkl: 地图文件(5倍压缩)
|
||||
|
||||
### navigate.py: 导航类
|
||||
|
||||
### pathsmoothing.py: 路径平滑
|
||||
|
||||
### rrt.py: RRT实现
|
||||
|
||||
### rrt_star.py: RRTStar 实现
|
||||
|
||||
### test.py: 测试文件
|
||||
|
||||
|
||||
|
||||
|
||||
## TODO
|
||||
|
||||
### 目标不合法
|
||||
#### 初始目标不合法
|
||||
目标在障碍物上：从目标开始方形向外扩展，直到找到可行点
|
||||
目标在地图外面：起点和目标连线最靠近目标的可行点
|
||||
对不合法的目标做单独处理，生成新的目标
|
||||
|
||||
#### 在移动过程中目标被占据
|
||||
给出目标但不会行移动，程序会继续运行，重新计算规划路径，给出新目标
|
||||
|
||||
|
||||
### 规划中断情况
|
||||
|
||||
|
||||
### 计算转向角
|
||||
`完成`
|
||||
|
||||
|
||||
### 有些本来可达的位置却无法走到（系统bug?）
|
||||
例如从初始位置(247, 500) 移动到(115, -10)，无法到达
|
||||
`(已解决)将dis_limit设为小值5而非0`
|
||||
#### 构型空间膨胀？？
|
||||
`不需要`
|
||||
|
||||
### 只考虑一定范围内的行人
|
||||
观测范围 / 反应半径
|
||||
|
||||
`完成`
|
||||
#### 观测范围内有行人步长要减小
|
||||
`完成`
|
||||
|
|
|
|||
|
|
@ -2,11 +2,12 @@ import os
|
|||
import pickle
|
||||
import time
|
||||
import random
|
||||
import math
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
|
||||
from robowaiter.scene.scene import Scene,init_world # TODO: 文件名改成Scene.py
|
||||
from robowaiter.scene import scene
|
||||
from navigate import Navigator
|
||||
|
||||
|
||||
|
|
@ -19,14 +20,13 @@ if __name__ == '__main__':
|
|||
with open(file_name, 'rb') as file:
|
||||
map = pickle.load(file)
|
||||
|
||||
init_world(1, 11)
|
||||
scene = Scene(sceneID=0)
|
||||
scene.init_world(1, 11)
|
||||
scene = scene.Scene(sceneID=0)
|
||||
|
||||
navigator = Navigator(scene=scene, area_range=[-350, 600, -400, 1450], map=map)
|
||||
navigator = Navigator(scene=scene, area_range=[-350, 600, -400, 1450], map=map)
|
||||
|
||||
'''场景1: 无行人环境 robot到达指定目标'''
|
||||
goal = np.array((-100, 700))
|
||||
|
||||
# goal = np.array((-100, 700))
|
||||
|
||||
'''场景2: 静止行人环境 robot到达指定目标'''
|
||||
# scene.clean_walker()
|
||||
|
|
@ -35,12 +35,17 @@ if __name__ == '__main__':
|
|||
# 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))
|
||||
scene.walk_to(100, 0, -90, dis_limit=10)
|
||||
scene.clean_walker()
|
||||
scene.add_walker(50, 300, 0)
|
||||
scene.add_walker(-50, 500, 0)
|
||||
scene.add_walker(0, 700, 0)
|
||||
scene.control_walker([scene.walker_control_generator(walkerID=0, autowalk=False, speed=50, X=-50, Y=600, Yaw=0)])
|
||||
scene.control_walker([scene.walker_control_generator(walkerID=1, autowalk=False, speed=50, X=100, Y=150, Yaw=0)])
|
||||
scene.control_walker([scene.walker_control_generator(walkerID=2, autowalk=False, speed=50, X=0, Y=0, Yaw=0)])
|
||||
|
||||
goal = (-100, 700)
|
||||
# goal = (-300)
|
||||
|
||||
'''场景4: 行人自由移动 robot到达指定目标'''
|
||||
# # TODO: autowalk=True仿真器会闪退 ???
|
||||
|
|
|
|||
|
|
@ -3,12 +3,14 @@ from robowaiter.behavior_lib._base.Behavior import Bahavior
|
|||
class Act(Bahavior):
|
||||
print_name_prefix = "act "
|
||||
type = 'Act'
|
||||
all_place = {'Bar', 'WaterTable', 'CoffeeTable', 'Bar2', 'Table1', 'Table2', 'Table3'}
|
||||
all_object = {'Coffee', 'Water', 'Dessert', 'Softdrink', 'BottledDrink', 'Yogurt', 'ADMilk', 'MilkDrink', 'Milk',
|
||||
'VacuumCup'}
|
||||
|
||||
def __init__(self,*args):
|
||||
super().__init__(*args)
|
||||
self.info = self.get_info(*args)
|
||||
|
||||
def get_conds(self):
|
||||
pre = set()
|
||||
add = set()
|
||||
de = set()
|
||||
return pre, add, de
|
||||
@classmethod
|
||||
def get_info(self,*arg):
|
||||
return None
|
||||
|
|
|
|||
|
|
@ -14,12 +14,28 @@ class Bahavior(ptree.behaviour.Behaviour):
|
|||
scene = None
|
||||
print_name_prefix = ""
|
||||
|
||||
@classmethod
|
||||
def get_ins_name(cls,*args):
|
||||
name = cls.__name__
|
||||
if len(args) > 0:
|
||||
ins_name = f'{name}({",".join(list(args))})'
|
||||
else:
|
||||
ins_name = f'{name}()'
|
||||
return ins_name
|
||||
|
||||
def __init__(self,*args):
|
||||
name = self.__class__.__name__
|
||||
if len(args)>0:
|
||||
name = f'{name}({",".join(list(args))})'
|
||||
self.name = name
|
||||
#get valid args
|
||||
# self.valid_arg_list = []
|
||||
# lines = self.valid_params.strip().splitlines()
|
||||
# for line in lines:
|
||||
# self.valid_arg_list.append((x.strip for x in line.split(",")))
|
||||
self.args = args
|
||||
|
||||
|
||||
super().__init__(self.name)
|
||||
|
||||
def _update(self) -> ptree.common.Status:
|
||||
|
|
@ -28,7 +44,9 @@ class Bahavior(ptree.behaviour.Behaviour):
|
|||
|
||||
@property
|
||||
def print_name(self):
|
||||
return f'{self.print_name_prefix}{self.name}'
|
||||
return f'{self.print_name_prefix}{self.get_ins_name(*self.args)}'
|
||||
|
||||
|
||||
|
||||
# let behavior node interact with the scene
|
||||
def set_scene(self, scene):
|
||||
|
|
|
|||
|
|
@ -0,0 +1,37 @@
|
|||
import py_trees as ptree
|
||||
from typing import Any
|
||||
from robowaiter.behavior_lib._base.Act import Act
|
||||
from robowaiter.behavior_lib._base.Behavior import Status
|
||||
|
||||
class Make(Act):
|
||||
can_be_expanded = True
|
||||
num_args = 1
|
||||
valid_args = (
|
||||
"Coffee","Water","Dessert"
|
||||
)
|
||||
|
||||
def __init__(self, *args):
|
||||
super().__init__(*args)
|
||||
self.target_obj = self.args[0]
|
||||
|
||||
|
||||
@classmethod
|
||||
def get_info(cls,arg):
|
||||
info = {}
|
||||
info["pre"]= {f'Holding(Nothing)'}
|
||||
info['del'] = set()
|
||||
if arg == "Coffee":
|
||||
info["add"]= {f'On(Coffee,CoffeeTable)'}
|
||||
elif arg == "Water":
|
||||
info["add"] = {f'On(Water,WaterTable)'}
|
||||
elif arg == "Dessert":
|
||||
info["add"] = {f'On(Dessert,Bar)'}
|
||||
return info
|
||||
|
||||
def _update(self) -> ptree.common.Status:
|
||||
op_type = 1
|
||||
self.scene.move_task_area(op_type)
|
||||
self.scene.op_task_execute(op_type)
|
||||
self.scene.state["condition_set"].union(self.info["add"])
|
||||
self.scene.state["condition_set"] -= self.info["del"]
|
||||
return Status.RUNNING
|
||||
|
|
@ -1,23 +0,0 @@
|
|||
import py_trees as ptree
|
||||
from typing import Any
|
||||
from robowaiter.behavior_lib._base.Act import Act
|
||||
from robowaiter.behavior_lib._base.Behavior import Status
|
||||
|
||||
class MakeCoffee(Act):
|
||||
|
||||
def __init__(self, *args):
|
||||
super().__init__(*args)
|
||||
|
||||
@property
|
||||
def cond_sets(self):
|
||||
pre = {"At(Robot,Bar)"}
|
||||
add = {"At(Coffee,Bar)"}
|
||||
de = {}
|
||||
return pre,add,de
|
||||
|
||||
def _update(self) -> ptree.common.Status:
|
||||
op_type = 1
|
||||
self.scene.move_task_area(op_type)
|
||||
self.scene.op_task_execute(op_type)
|
||||
self.scene.state["condition_set"].add("At(Coffee,Bar)")
|
||||
return Status.RUNNING
|
||||
|
|
@ -4,9 +4,23 @@ from robowaiter.behavior_lib._base.Act import Act
|
|||
from robowaiter.algos.navigate.DstarLite.navigate import Navigator
|
||||
|
||||
class MoveTo(Act):
|
||||
can_be_expanded = True
|
||||
num_args = 1
|
||||
valid_args = Act.all_object | Act.all_place
|
||||
valid_args.add('Customer')
|
||||
|
||||
def __init__(self, target_place):
|
||||
super().__init__(target_place)
|
||||
self.target_place = target_place
|
||||
|
||||
|
||||
@classmethod
|
||||
def get_info(self,arg):
|
||||
info = {}
|
||||
info["add"] = {f'At(Robot,{arg})'}
|
||||
info["del"] = {f'At(Robot,{place})' for place in self.valid_args if place != arg}
|
||||
return info
|
||||
|
||||
def __init__(self, *args):
|
||||
super().__init__(*args)
|
||||
|
||||
def _update(self) -> ptree.common.Status:
|
||||
# self.scene.test_move()
|
||||
|
|
|
|||
|
|
@ -6,7 +6,6 @@ class At(Cond):
|
|||
can_be_expanded = True
|
||||
num_params = 2
|
||||
valid_params = '''
|
||||
Coffee, Bar
|
||||
Robot, Bar
|
||||
'''
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,28 @@
|
|||
import py_trees as ptree
|
||||
from typing import Any
|
||||
from robowaiter.behavior_lib._base.Cond import Cond
|
||||
|
||||
class On(Cond):
|
||||
can_be_expanded = True
|
||||
num_params = 2
|
||||
valid_params = '''
|
||||
Robot, Bar
|
||||
'''
|
||||
|
||||
def __init__(self,*args):
|
||||
super().__init__(*args)
|
||||
|
||||
|
||||
def _update(self) -> ptree.common.Status:
|
||||
# if self.scene.status?
|
||||
arg_str = self.arg_str
|
||||
|
||||
if f'At({arg_str})' in self.scene.state["condition_set"]:
|
||||
return ptree.common.Status.SUCCESS
|
||||
else:
|
||||
return ptree.common.Status.FAILURE
|
||||
|
||||
# if self.scene.state['chat_list'] == []:
|
||||
# return ptree.common.Status.FAILURE
|
||||
# else:
|
||||
# return ptree.common.Status.SUCCESS
|
||||
|
|
@ -94,10 +94,13 @@ class OptBTExpAlgorithm:
|
|||
|
||||
# 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 c!=set():
|
||||
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
|
||||
else:
|
||||
subtree.add_child([copy.deepcopy(pair_node.act_leaf)])
|
||||
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,
|
||||
|
|
@ -183,12 +186,15 @@ class OptBTExpAlgorithm:
|
|||
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"
|
||||
if '(' not in child.content.name:
|
||||
self.ptml_string += 'act '+child.content.name+"()\n"
|
||||
else:
|
||||
self.ptml_string += 'act ' + child.content.name + "\n"
|
||||
elif isinstance(child, ControlBT):
|
||||
if parnode.type == '?':
|
||||
if child.type == '?':
|
||||
self.ptml_string += "selector{\n"
|
||||
self.dfs_ptml(parnode=child)
|
||||
elif parnode.type == '>':
|
||||
elif child.type == '>':
|
||||
self.ptml_string += "sequence{\n"
|
||||
self.dfs_ptml( parnode=child)
|
||||
self.ptml_string += '}\n'
|
||||
|
|
|
|||
|
|
@ -1,2 +1,4 @@
|
|||
OPENAI_API_BASE=
|
||||
#BACKEND_TYPE=webui
|
||||
#OPENAI_API_BASE=https://45.125.46.134:25344/v1/chat/completions
|
||||
OPENAI_API_BASE=https://45.125.46.134:25344/v1/chat/completions
|
||||
OPENAI_API_KEY=
|
||||
|
|
@ -170,7 +170,8 @@ class Agent:
|
|||
)
|
||||
else:
|
||||
# Standard non-function reply
|
||||
print("### Internal monologue: " + (response_message.content if response_message.content else ""))
|
||||
# print("### Internal monologue: " + (response_message.content if response_message.content else ""))
|
||||
print("### Internal monologue: " + (response_message['content'] if response_message['content'] else ""))
|
||||
messages.append(response_message)
|
||||
function_failed = None
|
||||
|
||||
|
|
@ -178,13 +179,37 @@ class Agent:
|
|||
|
||||
def step(self, user_message):
|
||||
input_message_sequence = self.messages + [{"role": "user", "content": user_message}]
|
||||
response = openai.ChatCompletion.create(model=self.model, messages=input_message_sequence,
|
||||
functions=self.functions_description, function_call="auto")
|
||||
|
||||
response_message = response.choices[0].message
|
||||
response_message_copy = response_message.copy()
|
||||
# 原来的通信方式
|
||||
# response = openai.ChatCompletion.create(model=self.model, messages=input_message_sequence,
|
||||
# functions=self.functions_description, function_call="auto")
|
||||
#
|
||||
# response_message = response.choices[0].message
|
||||
# response_message_copy = response_message.copy()
|
||||
|
||||
# ===我们的通信方式 "tools": self.functions_description 不起作用===
|
||||
import requests
|
||||
url = "https://45.125.46.134:25344/v1/chat/completions"
|
||||
headers = {"Content-Type": "application/json"}
|
||||
data = {
|
||||
"model": "RoboWaiter",
|
||||
"messages": input_message_sequence,
|
||||
# "functions":self.functions_description,
|
||||
# "function_call":"auto"
|
||||
# "function_call":self.functions_description
|
||||
"tools": self.functions_description
|
||||
}
|
||||
response = requests.post(url, headers=headers, json=data, verify=False)
|
||||
if response.status_code == 200:
|
||||
result = response.json()
|
||||
response_message = result['choices'][0]['message']
|
||||
else:
|
||||
response_message = "大模型请求失败:"+ str(response.status_code)
|
||||
response_message_copy = response_message
|
||||
# ===我们的通信方式 "tools": self.functions_description 不起作用===
|
||||
|
||||
|
||||
all_response_messages, function_failed = self.handle_ai_response(response_message)
|
||||
|
||||
assert "api_response" not in all_response_messages[0], f"api_response already in {all_response_messages[0]}"
|
||||
all_response_messages[0]["api_response"] = response_message_copy
|
||||
assert "api_args" not in all_response_messages[0], f"api_args already in {all_response_messages[0]}"
|
||||
|
|
|
|||
|
|
@ -0,0 +1,17 @@
|
|||
FUNCTIONS = [
|
||||
{
|
||||
"name": "get_current_weather",
|
||||
"description": "Get the current weather in a given location",
|
||||
"parameters": {
|
||||
"type": "object",
|
||||
"properties": {
|
||||
"location": {
|
||||
"type": "string",
|
||||
"description": "The city and state, e.g. San Francisco, CA",
|
||||
},
|
||||
"unit": {"type": "string"},
|
||||
},
|
||||
"required": ["location"],
|
||||
},
|
||||
}
|
||||
]
|
||||
|
|
@ -2,10 +2,14 @@ from dotenv import load_dotenv
|
|||
|
||||
load_dotenv()
|
||||
import utils
|
||||
from functions import FUNCTIONS
|
||||
# from functions import FUNCTIONS
|
||||
from functions_zh import FUNCTIONS
|
||||
from agent import Agent
|
||||
|
||||
|
||||
import urllib3
|
||||
urllib3.disable_warnings(urllib3.exceptions.InsecureRequestWarning)
|
||||
|
||||
def run_agent_loop(agent):
|
||||
while True:
|
||||
user_input = input("You: ")
|
||||
|
|
@ -20,6 +24,11 @@ if __name__ == "__main__":
|
|||
persona = utils.get_persona_text()
|
||||
human = utils.get_human_text()
|
||||
system = utils.get_system_text()
|
||||
|
||||
print("system:",system)
|
||||
print("persona:", persona)
|
||||
print("human:", human)
|
||||
|
||||
agent = Agent(model="gpt-3.5-turbo-16k-0613", system=system, functions_description=FUNCTIONS, persona_notes=persona,
|
||||
human_notes=human)
|
||||
run_agent_loop(agent)
|
||||
|
|
|
|||
|
|
@ -1 +1,2 @@
|
|||
{"测试VLM：做一杯咖啡": {"Answer": "测试VLM：做一杯咖啡", "Goal": "{\"At(Coffee,Bar)\"}"}, "测试VLN：前往桌子": {"Answer": "测试VLN：前往桌子", "Goal": "{\"At(Robot,Table)\"}"}, "测试AEM": {"Answer": "测试AEM", "Goal": "{\"EnvExplored()\"}"}}
|
||||
{"测试VLM：做一杯咖啡": {"Answer": "测试VLM：做一杯咖啡", "Goal": "{\"At(Coffee,Bar)\"}"}, "测试VLN：前往桌子": {"Answer": "测试VLN：前往桌子", "Goal": "{\"At(Robot,Table)\"}"}, "测试VLM：倒一杯水": {"Answer": "测试VLM：倒一杯水", "Goal": "{\"At(Water,WaterTable)\"}"}}
|
||||
|
||||
|
|
|
|||
|
|
@ -2,3 +2,4 @@ Question,Answer,Goal
|
|||
测试VLM：做一杯咖啡,测试VLM：做一杯咖啡,"{""At(Coffee,Bar)""}"
|
||||
测试VLN：前往桌子,测试VLN：前往桌子,"{""At(Robot,Table)""}"
|
||||
测试AEM,测试AEM,"{""EnvExplored()""}"
|
||||
测试VLM：倒一杯水,测试VLM：倒一杯水,"{""At(Water,WaterTable)""}"
|
||||
|
|
|
|||
|
|
|
@ -1,7 +1,9 @@
|
|||
import io
|
||||
import contextlib
|
||||
import os
|
||||
import importlib.util
|
||||
|
||||
from robowaiter.utils.bt.load import load_bt_from_ptml, find_node_by_name, print_tree_from_root
|
||||
from robowaiter.utils.bt.load import load_bt_from_ptml,find_node_by_name,print_tree_from_root
|
||||
from robowaiter.utils.bt.visitor import StatusVisitor
|
||||
|
||||
from robowaiter.behavior_tree.obtea.OptimalBTExpansionAlgorithm import Action # 调用最优行为树扩展算法
|
||||
|
|
@ -9,13 +11,13 @@ from robowaiter.behavior_tree.obtea.opt_bt_exp_main import BTOptExpInterface
|
|||
|
||||
from robowaiter.behavior_lib.act.DelSubTree import DelSubTree
|
||||
from robowaiter.behavior_lib._base.Sequence import Sequence
|
||||
|
||||
from robowaiter.utils.bt.load import load_behavior_tree_lib
|
||||
|
||||
class Robot(object):
|
||||
scene = None
|
||||
response_frequency = 1
|
||||
|
||||
def __init__(self, ptml_path, behavior_lib_path):
|
||||
def __init__(self,ptml_path,behavior_lib_path):
|
||||
self.ptml_path = ptml_path
|
||||
self.behavior_lib_path = behavior_lib_path
|
||||
|
||||
|
|
@ -24,12 +26,13 @@ class Robot(object):
|
|||
self.last_tick_output = ""
|
||||
self.action_list = None
|
||||
|
||||
def set_scene(self, scene):
|
||||
|
||||
def set_scene(self,scene):
|
||||
self.scene = scene
|
||||
|
||||
def load_BT(self):
|
||||
self.bt = load_bt_from_ptml(self.scene, self.ptml_path, self.behavior_lib_path)
|
||||
sub_task_place_holder = find_node_by_name(self.bt.root, "SubTaskPlaceHolder")
|
||||
self.bt = load_bt_from_ptml(self.scene, self.ptml_path,self.behavior_lib_path)
|
||||
sub_task_place_holder = find_node_by_name(self.bt.root,"SubTaskPlaceHolder")
|
||||
if sub_task_place_holder:
|
||||
sub_task_seq = sub_task_place_holder.parent
|
||||
sub_task_seq.children.pop()
|
||||
|
|
@ -38,10 +41,16 @@ class Robot(object):
|
|||
self.bt_visitor = StatusVisitor()
|
||||
self.bt.visitors.append(self.bt_visitor)
|
||||
|
||||
def expand_sub_task_tree(self, goal):
|
||||
|
||||
def expand_sub_task_tree(self,goal):
|
||||
if self.action_list is None:
|
||||
self.action_list = self.collect_action_nodes()
|
||||
print(f"首次运行行为树扩展算法，收集到{len(self.action_list)}个有效动作")
|
||||
print("\n--------------------")
|
||||
print(f"首次运行行为树扩展算法，收集到{len(self.action_list)}个有效动作:")
|
||||
for a in self.action_list:
|
||||
print(a.name)
|
||||
print("--------------------\n")
|
||||
|
||||
|
||||
algo = BTOptExpInterface(self.action_list)
|
||||
|
||||
|
|
@ -52,7 +61,7 @@ class Robot(object):
|
|||
with open(file_path, 'w') as file:
|
||||
file.write(ptml_string)
|
||||
|
||||
sub_task_bt = load_bt_from_ptml(self.scene, file_path, self.behavior_lib_path)
|
||||
sub_task_bt = load_bt_from_ptml(self.scene, file_path,self.behavior_lib_path)
|
||||
|
||||
# 加入删除子树的节点
|
||||
seq = Sequence(name="Sequence", memory=False)
|
||||
|
|
@ -65,15 +74,30 @@ class Robot(object):
|
|||
print("当前行为树为：")
|
||||
print_tree_from_root(self.bt.root)
|
||||
|
||||
# 获取所有action的pre,add,del列表
|
||||
def collect_action_nodes(self):
|
||||
action_list = [
|
||||
Action(name='MakeCoffee()', pre={'At(Robot,CoffeeMachine)'},
|
||||
add={'At(Coffee,Bar)'}, del_set=set(), cost=1),
|
||||
Action(name='MoveTo(Table)', pre={'At(Robot,Bar)'},
|
||||
add={'At(Robot,Table)'}, del_set=set(), cost=1),
|
||||
Action(name='ExploreEnv()', pre={'At(Robot,Bar)'},
|
||||
add={'EnvExplored()'}, del_set=set(), cost=1),
|
||||
]
|
||||
action_list = []
|
||||
behavior_dict = load_behavior_tree_lib()
|
||||
for cls in behavior_dict["act"].values():
|
||||
if cls.can_be_expanded:
|
||||
if cls.num_args == 0:
|
||||
action_list.append(Action(name=cls.get_ins_name(),**cls.get_info()))
|
||||
if cls.num_args == 1:
|
||||
for arg in cls.valid_args:
|
||||
action_list.append(Action(name=cls.get_ins_name(arg), **cls.get_info(arg)))
|
||||
if cls.num_args > 1:
|
||||
for args in cls.valid_args:
|
||||
action_list.append(Action(name=cls.get_ins_name(*args),**cls.get_info(*args)))
|
||||
|
||||
print(action_list)
|
||||
# action_list = [
|
||||
# Action(name='MakeCoffee', pre={'At(Robot,CoffeeMachine)'},
|
||||
# add={'At(Coffee,Bar)'}, del_set=set(), cost=1),
|
||||
# Action(name='MoveTo(Table)', pre={'At(Robot,Bar)'},
|
||||
# add={'At(Robot,Table)'}, del_set=set(), cost=1),
|
||||
# Action(name='ExploreEnv()', pre=set(),
|
||||
# add={'EnvExplored()'}, del_set=set(), cost=1),
|
||||
# ]
|
||||
return action_list
|
||||
|
||||
def step(self):
|
||||
|
|
@ -92,6 +116,5 @@ class Robot(object):
|
|||
print("\n")
|
||||
self.last_tick_output = bt_output
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
pass
|
||||
pass
|
||||
|
|
@ -409,10 +409,11 @@ class Scene:
|
|||
return True
|
||||
|
||||
def gen_obj(self,h=100):
|
||||
# 4;冰红(盒) 5;酸奶 7:保温杯 9;冰红(瓶) 13:代语词典
|
||||
# 4;冰红(盒) 5;酸奶 7:保温杯 9;冰红(瓶) 13:代语词典 14:cake 61:甜牛奶
|
||||
type= 9 #9
|
||||
scene = stub.Observe(GrabSim_pb2.SceneID(value=self.sceneID))
|
||||
ginger_loc = [scene.location.X, scene.location.Y, scene.location.Z]
|
||||
obj_list = [GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 50, y=ginger_loc[1] - 40, z = h, roll=0, pitch=0, yaw=0, type=9)]
|
||||
obj_list = [GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 50, y=ginger_loc[1] - 40, z = h, roll=0, pitch=0, yaw=0, type=type)]
|
||||
scene = stub.AddObjects(GrabSim_pb2.ObjectList(objects=obj_list, scene=self.sceneID))
|
||||
time.sleep(1.0)
|
||||
|
||||
|
|
|
|||
|
|
@ -12,12 +12,13 @@ class SceneVLM(Scene):
|
|||
# 在这里加入场景中发生的事件， (事件发生的时间，事件函数)
|
||||
self.event_list = [
|
||||
(5, self.create_chat_event("测试VLM：做一杯咖啡")),
|
||||
# (5, self.create_chat_event("测试VLM：倒一杯水")),
|
||||
]
|
||||
|
||||
def _reset(self):
|
||||
pass
|
||||
|
||||
def _run(self, op_type=2):
|
||||
def _run(self, op_type=7):
|
||||
# 共17个操作
|
||||
# "制作咖啡","倒水","夹点心","拖地","擦桌子","开筒灯","搬椅子", # 1-7
|
||||
# "关筒灯","开大厅灯","关大厅灯","关闭窗帘","打开窗帘", # 8-12
|
||||
|
|
@ -29,8 +30,8 @@ class SceneVLM(Scene):
|
|||
# self.gen_obj()
|
||||
# self.op_task_execute(op_type, obj_id=0)
|
||||
# # 原始吧台处:[247.0, 520.0, 100.0], 空调开关旁吧台:[240.0, 40.0, 70.0], 水杯桌:[-70.0, 500.0, 107]
|
||||
# # 桌子1:[-55.0, 0.0, 107],桌子1:[-55.0, 150.0, 107]
|
||||
# elif op_type == 17: self.op_task_execute(op_type, release_pos=[-55.0, 150.0, 107])
|
||||
# # 桌子1:[-55.0, 0.0, 107],桌子2:[-55.0, 150.0, 107]
|
||||
# elif op_type == 17: self.op_task_execute(op_type, release_pos=[247.0, 520.0, 100.0])#[-55.0, 150.0, 107]
|
||||
# else:
|
||||
# self.move_task_area(op_type)
|
||||
# self.op_task_execute(op_type)
|
||||
|
|
|
|||
|
|
@ -2,11 +2,4 @@ import os
|
|||
|
||||
from robowaiter.utils import *
|
||||
from robowaiter.utils import *
|
||||
|
||||
|
||||
|
||||
|
||||
def get_root_path():
|
||||
return os.path.abspath(
|
||||
os.path.join(__file__, "../../..")
|
||||
)
|
||||
from robowaiter.utils.basic import get_root_path
|
||||
|
|
@ -0,0 +1,6 @@
|
|||
import os
|
||||
|
||||
def get_root_path():
|
||||
return os.path.abspath(
|
||||
os.path.join(__file__, "../../..")
|
||||
)
|
||||
|
|
@ -1,10 +1,12 @@
|
|||
import py_trees as ptree
|
||||
from robowaiter.behavior_tree.ptml import ptmlCompiler
|
||||
|
||||
import os
|
||||
import importlib.util
|
||||
from robowaiter.utils.basic import get_root_path
|
||||
|
||||
def load_bt_from_ptml(scene, ptml_path, behavior_lib_path):
|
||||
ptml_bt = ptmlCompiler.load(scene, ptml_path, behavior_lib_path)
|
||||
bt = ptree.trees.BehaviourTree(ptml_bt)
|
||||
bt = ptree.trees.BehaviourTree(ptml_bt)
|
||||
|
||||
with open(ptml_path, 'r') as f:
|
||||
ptml = f.read()
|
||||
|
|
@ -29,7 +31,6 @@ def print_tree_from_root(node, indent=0):
|
|||
for child in node.children:
|
||||
print_tree_from_root(child, indent + 1)
|
||||
|
||||
|
||||
def find_node_by_name(tree, name):
|
||||
"""
|
||||
Find a node in the behavior tree with the specified name.
|
||||
|
|
@ -47,6 +48,42 @@ def find_node_by_name(tree, name):
|
|||
return result
|
||||
return None
|
||||
|
||||
|
||||
|
||||
def get_classes_from_folder(folder_path):
|
||||
cls_dict = {}
|
||||
for filename in os.listdir(folder_path):
|
||||
if filename.endswith('.py'):
|
||||
# 构建模块的完整路径
|
||||
module_path = os.path.join(folder_path, filename)
|
||||
# 获取模块名（不含.py扩展名）
|
||||
module_name = os.path.splitext(filename)[0]
|
||||
|
||||
# 动态导入模块
|
||||
spec = importlib.util.spec_from_file_location(module_name, module_path)
|
||||
module = importlib.util.module_from_spec(spec)
|
||||
spec.loader.exec_module(module)
|
||||
|
||||
# 获取模块中定义的所有类
|
||||
for name, obj in module.__dict__.items():
|
||||
if isinstance(obj, type):
|
||||
cls_dict[module_name] = obj
|
||||
|
||||
return cls_dict
|
||||
|
||||
|
||||
def load_behavior_tree_lib():
|
||||
root_path = get_root_path()
|
||||
type_list = ["act","cond"]
|
||||
behavior_dict = {}
|
||||
for type in type_list:
|
||||
path = os.path.join(root_path,"robowaiter","behavior_lib",type)
|
||||
behavior_dict[type] = get_classes_from_folder(path)
|
||||
|
||||
return behavior_dict
|
||||
|
||||
if __name__ == '__main__':
|
||||
print(load_behavior_tree_lib())
|
||||
# class BehaviorTree(ptree):
|
||||
# def __init__(self):
|
||||
# super().__init__()
|
||||
# super().__init__()
|
||||
|
|
@ -1,7 +1,3 @@
|
|||
selector{
|
||||
cond At(Coffee,Bar)
|
||||
selector{
|
||||
cond At(Robot,CoffeeMachine)
|
||||
act MakeCoffee()
|
||||
}
|
||||
cond EnvExplored()
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue