导航修改

robowaiter/algos/navigator/navigate.py

@@ -7,7 +7,7 @@ import pickle
 
 import matplotlib.pyplot as plt
 import numpy as np
-from robowaiter.scene import scene
+# from robowaiter.scene import scene
 
 from robowaiter.algos.navigator.dstar_lite import DStarLite, euclidean_distance
 

robowaiter/behavior_lib/cond/NeedExplore.py

@@ -2,17 +2,18 @@ import py_trees as ptree
 from typing import Any
 from robowaiter.behavior_lib._base.Cond import Cond
 
-class EnvExplored(Cond):
+class NeedExplore(Cond):
     def __init__(self):
         super().__init__()
 
     def _update(self) -> ptree.common.Status:
-        arg_str = self.arg_str
+        # arg_str = self.arg_str
-
+        #
-        if f'EnvExplored()' not in self.scene.state["condition_set"]:
+        # if f'EnvExplored()' not in self.scene.state["condition_set"]:
-            return ptree.common.Status.SUCCESS
+        #     return ptree.common.Status.SUCCESS
-        else:
+        # else:
-            return ptree.common.Status.FAILURE
+        #     return ptree.common.Status.FAILURE
+        return ptree.common.Status.FAILURE
 
 
         # if self.scene.status?

robowaiter/robot/Default.ptml

@@ -15,6 +15,9 @@ selector
             cond At(Robot,Bar)
             act MoveTo(Bar)
         act GreetCustomer()
+    sequence
+        cond NeedExplore()
+        act ExploreEnv()
     sequence
         cond AnomalyDetected()
         act ResolveAnomaly()

robowaiter/robot/Default_bracket.ptml

@@ -23,6 +23,10 @@ selector
             act MoveTo(Bar)
         }        act GreetCustomer()
     }    sequence
+{
+        cond NeedExplore()
+        act ExploreEnv()
+    }    sequence
 {
         cond AnomalyDetected()
         act ResolveAnomaly()

robowaiter/utils/draw_bt/Default.ptml

@@ -1,10 +1,23 @@
-sequence{
+selector
-cond Holding(Nothing)
+    sequence
-sequence{
+        cond Chatting()
-cond Is(Curtain,Off)
+        act DealChat()
-act Turn(Curtain,On)
+    sequence
-}
+        cond HasSubTask()
-sequence{
+        sequence
-cond Is(AC,Off)
+            act SubTaskPlaceHolder()
-act Turn(AC,On)
+    sequence
-}}
+        cond FocusingCustomer()
+        act ServeCustomer()
+    sequence
+        cond NewCustomer()
+        selector
+            cond At(Robot,Bar)
+            act MoveTo(Bar)
+        act GreetCustomer()
+    sequence
+        cond NeedExplore()
+        act ExploreEnv()
+    sequence
+        cond AnomalyDetected()
+        act ResolveAnomaly()

robowaiter/utils/draw_bt/Default_bracket.ptml

@@ -23,6 +23,10 @@ selector
             act MoveTo(Bar)
         }        act GreetCustomer()
     }    sequence
+{
+        cond NeedExplore()
+        act ExploreEnv()
+    }    sequence
 {
         cond AnomalyDetected()
         act ResolveAnomaly()

robowaiter/utils/draw_bt/draw_bt.py

@@ -2,9 +2,8 @@
 # from robowaiter.behavior_tree.ptml.ptmlCompiler import load
 
 import os
-from robowaiter.robot.robot import Robot
-from robowaiter.utils.bt.draw import render_dot_tree
 from robowaiter.utils.basic import get_root_path
+from robowaiter.utils.bt.draw import render_dot_tree
 from robowaiter.utils.bt.load import load_bt_from_ptml
 
 if __name__ == '__main__':