Merge branch 'main' of https://github.com/HPCL-EI/RoboWaiter

# Conflicts:
#	robowaiter/scene/scene.py

robowaiter/behavior_lib/_base/Behavior.py

@@ -29,8 +29,7 @@ class Bahavior(ptree.behaviour.Behaviour):
         'Bar2': (240.0, 40.0, 70.0),
         'WaterTable':(-70.0, 500.0, 107),
         'CoffeeTable':(250.0, 310.0, 100.0),
-        'Table1': (340.0, 900.0, 98.0),
+        'Table1': (340.0, 900.0, 99.0),
-        # 'Table1': (345.0, 895.0, 98.0),
         'Table2': (-55.0, 0.0, 107),
         'Table3':(-55.0, 150.0, 107)
     }

robowaiter/behavior_lib/act/ExploreEnv.py

@@ -3,7 +3,8 @@ from typing import Any
 from robowaiter.behavior_lib._base.Act import Act
 
 class ExploreEnv(Act):
-    can_be_expanded = True
+    # can_be_expanded = True
+    can_be_expanded = False
     num_args=0
     valid_args=()
 
@@ -20,8 +21,6 @@ class ExploreEnv(Act):
 
     def _update(self) -> ptree.common.Status:
         # explore algorithm
-
-
         self.scene.state["condition_set"]|= self.info["add"]
 
         return ptree.common.Status.RUNNING

robowaiter/behavior_lib/act/GreatCustomer.py

@@ -0,0 +1,27 @@
+import py_trees as ptree
+from robowaiter.behavior_lib._base.Act import Act
+from robowaiter.algos.navigator.navigate import Navigator
+
+class GreatCustomer(Act):
+    can_be_expanded = False
+    num_args = 0
+    valid_args = ()
+
+    def __init__(self, *args):
+        super().__init__(*args)
+
+    @classmethod
+    def get_info(cls):
+        info = {}
+        info['pre'] = set()
+        info["add"] = set()
+        info["del_set"] = set()
+        info['cost']=0
+        return info
+
+    def _update(self) -> ptree.common.Status:
+
+        goal = Act.place_xyz_dic['Bar']
+        self.scene.walk_to(goal[0]-5,goal[1], 180, 180, 0)
+        self.scene.chat_bubble("欢迎光临！请问有什么可以帮您？")
+        return ptree.common.Status.RUNNING

robowaiter/behavior_lib/act/Make.py

@@ -34,6 +34,7 @@ class Make(Act):
             info["add"] |= {f'On({arg},WaterTable)'}
         elif arg == cls.valid_args[2]:
             info["add"] |= {f'On({arg},Bar)'}
+        info['cost'] = 10
         return info
 
     def _update(self) -> ptree.common.Status:

robowaiter/behavior_lib/act/MoveTo.py

@@ -21,7 +21,7 @@ class MoveTo(Act):
             info['pre'] |= {f'Exist({arg})'}
         info["add"] = {f'At(Robot,{arg})'}
         info["del_set"] = {f'At(Robot,{place})' for place in cls.valid_args if place != arg}
-        info['cost']=10
+        info['cost']=5
         return info
 
 

robowaiter/behavior_lib/act/PutDown.py

@@ -23,7 +23,7 @@ class PutDown(Act):
         info["add"] = {f'Holding(Nothing)',f'On({arg[0]},{arg[1]})'}
         info["del_set"] = {f'Holding({arg[0]})'}
 
-        info['cost'] = 100
+        info['cost'] = 1
         return info
 
 

robowaiter/behavior_lib/cond/DetectCustomer.py

@@ -0,0 +1,31 @@
+import py_trees as ptree
+from typing import Any
+from robowaiter.behavior_lib._base.Cond import Cond
+import itertools
+
+class DetectCustomer(Cond):
+    can_be_expanded = False
+    num_params = 0
+    valid_args = ()
+
+    def __init__(self,*args):
+        super().__init__(*args)
+
+
+    def _update(self) -> ptree.common.Status:
+
+        # 获取customer的位置
+        # bar (247.0, 520.0, 100.0)
+        close_to_bar = False
+        scene = self.scene.status
+        for walker in scene.walkers:
+            x, y, yaw = walker.pose.X, walker.pose.Y, walker.pose.Yaw
+            # 到达一定区域就打招呼
+            if y >= 450 and y <= 620 and x >= 40 and x <= 100 and yaw>=-10 and yaw <=10:
+                close_to_bar = True
+                break
+
+        if close_to_bar:
+            return ptree.common.Status.SUCCESS
+        else:
+            return ptree.common.Status.FAILURE

robowaiter/robot/Default.ptml

@@ -14,5 +14,14 @@ selector
             act SubTaskPlaceHolder()
         }
     }
-
+    sequence
+    {
+        selector
+        {
+            cond At(Robot,Bar)
+            act MoveTo(Bar)
+        }
+        cond DetectCustomer()
+        act GreatCustomer
+    }
 }

robowaiter/robot/robot.py

@@ -37,6 +37,7 @@ class Robot(object):
     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()")

robowaiter/scene/scene.py

@@ -1,3 +1,4 @@
+import pickle
 import sys
 import time
 import grpc
@@ -9,8 +10,9 @@ import math
 from robowaiter.proto import GrabSim_pb2
 from robowaiter.proto import GrabSim_pb2_grpc
 
-
+import os
-
+from robowaiter.utils import get_root_path
+root_path = get_root_path()
 
 channel = grpc.insecure_channel(
     "localhost:30001",
@@ -110,6 +112,9 @@ class Scene:
         self.all_frontier_list = set()
         self.semantic_map = semantic_map
         self.auto_map = np.ones((800, 1550))
+        self.filename = os.path.join(root_path, 'robowaiter/proto/map_1.pkl')
+        with open(self.filename, 'rb') as file:
+            self.map_file = pickle.load(file)
 
 
     def reset(self):
@@ -176,8 +181,8 @@ class Scene:
     def reset_sim(self):
         # reset world
         init_world()
-        
         stub.Reset(GrabSim_pb2.ResetParams(scene=self.sceneID))
+        stub.CleanWalkers(GrabSim_pb2.SceneID(value=self.sceneID))
 
     def _reset(self):
         # 场景自定义的reset
@@ -709,4 +714,13 @@ class Scene:
         ginger_x, ginger_y, ginger_z = [int(scene.location.X), int(scene.location.Y),100]
         return math.sqrt((ginger_x - objx) ** 2 + (ginger_y - objy) ** 2 + (ginger_z - objz) ** 2)
 
+    # 根据map文件判断是否可达
+    def reachable(self, pos):
+        x, y = self.real2map(pos[0], pos[1])
+        if self.map_file[x, y] == 0:
+            return True
+        else:
+            return False
+
+
 

robowaiter/scene/tasks/VLM/VLM2.py

@@ -11,28 +11,11 @@ class SceneVLM(Scene):
         super().__init__(robot)
         # 在这里加入场景中发生的事件， (事件发生的时间，事件函数)
         self.event_list = [
-            # (5, self.create_chat_event("测试VLM：做一杯咖啡")),
-            # (5, self.create_chat_event("测试VLM：倒一杯水")),
-            # (5, self.create_chat_event("测试VLM：开空调")),
-            # (5, self.create_chat_event("测试VLM：关空调")),
-            # (5, self.create_chat_event("测试VLM：开大厅灯")),
-            # (5, self.create_chat_event("测试VLM：拖地")),
-            # (7, self.create_chat_event("测试VLM：擦桌子")),
-            # (5, self.create_chat_event("测试VLM：整理椅子")),
-            # (5, self.create_chat_event("测试VLM：把冰红茶放到Table2")),
-            # (5, self.create_chat_event("测试VLM：关大厅灯"))
-            # (5, self.create_chat_event("测试VLM：做一杯咖啡放到吧台上")),
-            # (5, self.create_chat_event("测试VLM：做一杯咖啡放到水杯桌上并倒水")),
-            # (8, self.create_chat_event("测试VLN：前往1号桌")),
 
         ]
 
     def _reset(self):
-
+        self.gen_obj()
-        # self.gen_obj(type=5)
-        # self.gen_obj(type=9)
-        # self.op_task_execute(op_type=16, obj_id=0)
-        # self.move_task_area(op_type=4)
         pass
 
     def _run(self, op_type=10):
@@ -42,17 +25,13 @@ class SceneVLM(Scene):
         # 带领行人去有太阳的地方
         # 行人说 有点热
         # 好的，这就去开空调
-
+        scene = self.add_walkers([[47, 920]])
-        scene = self.add_walkers([[47, 920],[70,-200]])
         self.control_walker(
-            [self.walker_control_generator(walkerID=0, autowalk=False, speed=200, X=47, Y=520, Yaw=0)])
+            [self.walker_control_generator(walkerID=0, autowalk=False, speed=200, X=60, Y=520, Yaw=0)])
 
         cont = scene.walkers[0].name+":请问可以带我去空位上嘛？我想晒太阳"
         self.control_robot_action(0,3,cont)
 
-        # self.clean_walker()
-
-
         # 共17个操作
         # "制作咖啡","倒水","夹点心","拖地","擦桌子","开筒灯","搬椅子",    # 1-7
         # "关筒灯","开大厅灯","关大厅灯","关闭窗帘","打开窗帘",            # 8-12
@@ -73,39 +52,6 @@ class SceneVLM(Scene):
         #     self.move_task_area(op_type, release_pos=pos)
         #     self.op_task_execute(op_type, release_pos=pos)   # [325.0, 860.0, 100]
 
-        # 流程测试
-        # 抓握放置:抓吧台前生成的酸奶，放到抹布桌上
-        self.gen_obj()
-        # self.move_task_area(16, obj_id=0)
-        # self.op_task_execute(16, obj_id=0)
-        # pos = [340.0, 900.0, 99.0]
-        # self.move_task_area(17, release_pos=pos)
-        # self.op_task_execute(17, release_pos=pos)
-        #
-        # # 做咖啡:做完的咖啡放到水杯桌上
-        # self.move_task_area(1)
-        # self.op_task_execute(1)
-        #
-        # self.find_obj("CoffeeCup")
-        #
-        # self.move_task_area(16, obj_id=275)
-        # self.op_task_execute(16, obj_id=275)
-        # pos = [-70.0, 500.0, 107]
-        # self.move_task_area(17, release_pos=pos)
-        # self.op_task_execute(17, release_pos=pos)
-        #
-        # # 倒水:倒完的水放到旁边桌子上
-        # self.move_task_area(2)
-        # self.op_task_execute(2)
-
-        #
-        # self.move_task_area(16, obj_id=190)
-        # self.op_task_execute(16, obj_id=190)
-        # pos = [-55.0, 0.0, 107]
-        # self.move_task_area(17, release_pos=pos)
-        # self.op_task_execute(17, release_pos=pos)
-
-        # self.test_yaw()
 
         pass