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

robowaiter/scene/scene.py

@@ -70,6 +70,11 @@ class Scene:
         self.robot = robot
         self.sub_task_seq = None
 
+        # myx op
+        self.op_dialog = ["","制作咖啡","倒水","夹点心","拖地","擦桌子","关闭窗帘","关筒灯","开大厅灯","搬椅子","打开窗帘","关大厅灯","开筒灯"]
+        self.op_act_num = [0,3,4,6,3,2,0,0,0,1,0,0,0]
+        self.op_v_list = [[[0.0,0.0]],[[250.0, 310.0]],[[-70.0, 480.0]],[[250.0, 630.0]],[[260.0, 1120.0]],[[300.0, -220.0]],[[0.0, -70.0]]]
+
 
     def _reset(self):
         # 场景自定义的reset
@@ -320,3 +325,42 @@ class Scene:
     def animation_reset(self):
         stub.ControlRobot(GrabSim_pb2.ControlInfo(scene=self.sceneID, type=0, action=0))
 
+    def control_robot_action(self, type=0, action=0, message="你好"):
+        scene = stub.ControlRobot(
+            GrabSim_pb2.ControlInfo(
+                scene=self.sceneID, type=type, action=action, content=message
+            )
+        )
+        if str(scene.info).find("Action Success") > -1:
+            print(scene.info)
+            return True
+        else:
+            print(scene.info)
+            return False
+
+    def op_task_execute(self,task_type):
+        self.control_robot_action(0, 1, "开始"+self.op_dialog[task_type])   # 开始制作咖啡
+        result = self.control_robot_action(task_type, 1)    #
+        self.control_robot_action(0, 2)
+        if result:
+            if self.op_act_num[task_type]>0:
+                for i in range(2,2+self.op_act_num[task_type]):
+                    self.control_robot_action(task_type,i)
+                    self.control_robot_action(0, 2)
+                self.control_robot_action(0, 1, "成功"+self.op_dialog[task_type])
+        else:
+            self.control_robot_action(0, 1, self.op_dialog[task_type]+"失败")
+
+    def move_task_area(self,op_type=0):
+        scene = stub.Observe(GrabSim_pb2.SceneID(value=self.sceneID))
+
+        walk_value = [scene.location.X, scene.location.Y, scene.rotation.Yaw]
+        print("------------------move_task_area----------------------")
+        print("position:", walk_value,"开始任务:",self.op_dialog[op_type])
+        for walk_v in self.op_v_list[op_type]:
+            walk_v = walk_v + [scene.rotation.Yaw, 60, 0]
+            action = GrabSim_pb2.Action(
+                scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v
+            )
+            scene = stub.Do(action)
+

robowaiter/scene/tasks/VLM.py

@@ -6,34 +6,17 @@
 import time
 from robowaiter.scene.scene import Scene
 
-
 class SceneVLM(Scene):
     def __init__(self, robot):
         super().__init__(robot)
 
     def _reset(self):
-        self.reset_sim()
+        pass
 
-        self.add_walker(1085, 2630, 220)
-        self.control_walker([self.walker_control_generator(0, False, 100, 755, 1900, 180)])
+    def _run(self, op_type=1):
 
-    def _run(self):
-        # 空调操作
-        self.walk_to(950, 1260, 90)  # 没法转向？
-        # todo: 手臂操作
-        time.sleep(5)
-        self.walk_to(947, 1900, 0)
-        
-        # 物品挪动
-        # todo: 视觉导航至目标点，操作手臂至可抓位置
-        """
-        scene.grasp(1, your_objectID)
-        """
-
-        # todo: 视觉导航至目标点，找准释放位置
-        """
-        scene.release(1)
-        """
+        self.move_task_area(op_type)
+        self.op_task_execute(op_type)
 
     def _step(self):
         pass