Merge remote-tracking branch 'origin/main'

.gitignore

@@ -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.

robowaiter/algos/navigate/DstarLite/__init__.py

@@ -1,4 +1,3 @@
-
 from . import navigate
 from . import dstar_lite
 

robowaiter/algos/navigate/DstarLite/discretize_map.py

@@ -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)

robowaiter/algos/navigate/DstarLite/dstar_lite.py

@@ -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'):
@@ -117,7 +118,7 @@ class DStarLite:
                  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实际身位半径
+                 dyna_obs_radius=30,  # dyna_obs实际身位半径
                  ):
 
         # self.area_bounds = area
@@ -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 = [self.real2map(obs) for obs in dyna_obs]
-            dyna_obs[i] = self.real2map(dyna_obs[i])
 
-        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 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):
-            for j in range(y-self.dyna_obs_radius, y+self.dyna_obs_radius+1):
+        #         occupy_pos.append((i, j))
-                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)

robowaiter/algos/navigate/DstarLite/navigate.py

@@ -1,71 +1,80 @@
 #!/usr/bin/env python3
 # -*- encoding: utf-8 -*-
 import math
-import sys
+from dstar_lite import DStarLite, euclidean_distance
-import time
 
-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.step_length = step_length  # 步长(单次移动)
         self.step_num = self.step_length // self.scale_ratio  # 单次移动地图格数
-        self.v = 200                                          # 速度
+        self.v = velocity  # 速度
-        self.step_time = self.step_length/self.v + 0.1        # 单步移动时长
+        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)):
+    @staticmethod
-        # TODO: 使目标可达
+    def get_yaw(pos: (float, float), goal: (float, float)):
-        # 目标在障碍物上：从目标开始方形向外扩展，直到找到可行点
+        '''
-        # 目标在地图外面：起点和目标连线最靠近目标的可行点
+            得到移动方向
-        pass
+        '''
+        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))  # 机器人当前: 位置 和 朝向
+        pos = (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)
+            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)  # 画出搜索路径
+                    self.planner.draw_graph(step_num)  # 画出搜索路径
-                # time.sleep(self.step_time)
+                next_step = min(step_num, len(path))
-            pos = np.array((self.scene.status.location.X, self.scene.status.location.Y))
+                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:
 
 
 
-

robowaiter/algos/navigate/DstarLite/readme.md

@@ -1,4 +1,52 @@
-### dstar_lite.py ——Dstar lite算法文件 
+## 默认使用RRTStar+路径平滑
-### navigate.py ——导航类
+
-### test.py ——测试文件
+### apf.py: 势场法实现
-### map_5.pkl ——离散化地图文件
+
+### 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`
+####  构型空间膨胀？？
+`不需要`
+
+### 只考虑一定范围内的行人
+观测范围 / 反应半径
+
+`完成`
+#### 观测范围内有行人步长要减小
+`完成`

robowaiter/algos/navigate/DstarLite/test.py

@@ -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.init_world(1, 11)
-    scene = Scene(sceneID=0)
+    scene = scene.Scene(sceneID=0)
 
     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.walk_to(100, 0, -90, dis_limit=10)
-    # scene.add_walker(50, 300, 0)
+    scene.clean_walker()
-    # scene.add_walker(-50, 500, 0)
+    scene.add_walker(50, 300, 0)
-    # scene.control_walker([scene.walker_control_generator(walkerID=0, autowalk=False, speed=20, X=-50, Y=600, Yaw=0)])
+    scene.add_walker(-50, 500, 0)
-    # scene.control_walker([scene.walker_control_generator(walkerID=1, autowalk=False, speed=20, X=100, Y=150, Yaw=0)])
+    scene.add_walker(0, 700, 0)
-    # goal = np.array((-100, 700))
+    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仿真器会闪退 ???

robowaiter/behavior_lib/_base/Act.py

@@ -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):
+    @classmethod
-        pre = set()
+    def get_info(self,*arg):
-        add = set()
+        return None
-        de = set()
-        return pre, add, de

robowaiter/behavior_lib/_base/Behavior.py

@@ -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):

robowaiter/behavior_lib/act/Make.py

@@ -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

robowaiter/behavior_lib/act/MakeCoffee.py

@@ -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

robowaiter/behavior_lib/act/MoveTo.py

@@ -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()

robowaiter/behavior_lib/cond/At.py

@@ -6,7 +6,6 @@ class At(Cond):
     can_be_expanded = True
     num_params = 2
     valid_params = '''
-        Coffee, Bar
         Robot, Bar
     '''
 

robowaiter/behavior_lib/cond/On.py

@@ -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

robowaiter/behavior_tree/obtea/OptimalBTExpansionAlgorithm.py

@@ -94,10 +94,13 @@ class OptBTExpAlgorithm:
 
             # Mount the action node and extend BT. T = Eapand(T,c,A(c))
             if c!=goal:
+                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':
+                    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'

robowaiter/llm_client/MemGPT_simple/.env

@@ -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=

robowaiter/llm_client/MemGPT_simple/agent.py

@@ -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]}"

robowaiter/llm_client/MemGPT_simple/functions_zh.py

@@ -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"],
+        },
+    }
+]

robowaiter/llm_client/MemGPT_simple/main.py

@@ -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)

robowaiter/llm_client/data/test_questions.txt

@@ -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)\"}"}}
+

robowaiter/llm_client/data_raw/test_questions.csv

@@ -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)""}"

robowaiter/robot/robot.py

@@ -1,5 +1,7 @@
 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.visitor import StatusVisitor
@@ -9,7 +11,7 @@ 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
@@ -24,6 +26,7 @@ class Robot(object):
         self.last_tick_output = ""
         self.action_list = None
 
+
     def set_scene(self,scene):
         self.scene = scene
 
@@ -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):
         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)
 
@@ -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_list = []
-            Action(name='MakeCoffee()', pre={'At(Robot,CoffeeMachine)'},
+        behavior_dict = load_behavior_tree_lib()
-                   add={'At(Coffee,Bar)'}, del_set=set(), cost=1),
+        for cls in behavior_dict["act"].values():
-            Action(name='MoveTo(Table)', pre={'At(Robot,Bar)'},
+            if cls.can_be_expanded:
-                   add={'At(Robot,Table)'}, del_set=set(), cost=1),
+                if cls.num_args == 0:
-            Action(name='ExploreEnv()', pre={'At(Robot,Bar)'},
+                    action_list.append(Action(name=cls.get_ins_name(),**cls.get_info()))
-                   add={'EnvExplored()'}, del_set=set(), cost=1),
+                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

robowaiter/scene/scene.py

@@ -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)
 

robowaiter/scene/tasks/VLM.py

@@ -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]
+        # # 桌子1:[-55.0, 0.0, 107],桌子2:[-55.0, 150.0, 107]
-        # elif op_type == 17: self.op_task_execute(op_type, release_pos=[-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)

robowaiter/utils/__init__.py

@@ -2,11 +2,4 @@ import os
 
 from robowaiter.utils import *
 from robowaiter.utils import *
-
+from robowaiter.utils.basic import get_root_path
-
-
-
-def get_root_path():
-    return os.path.abspath(
-        os.path.join(__file__, "../../..")
-    )

robowaiter/utils/basic.py

@@ -0,0 +1,6 @@
+import os
+
+def get_root_path():
+    return os.path.abspath(
+        os.path.join(__file__, "../../..")
+    )

robowaiter/utils/bt/load.py

@@ -1,6 +1,8 @@
 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)
@@ -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__()

sub_task.ptml

@@ -1,7 +1,3 @@
 selector{
-cond At(Coffee,Bar)
+cond EnvExplored()
-selector{
-cond At(Robot,CoffeeMachine)
-act MakeCoffee()
-}
 }