diff --git a/robowaiter/algos/navigator/navigate.py b/robowaiter/algos/navigator/navigate.py index ddad6bc..c165962 100644 --- a/robowaiter/algos/navigator/navigate.py +++ b/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 diff --git a/robowaiter/behavior_lib/cond/EnvExplored.py b/robowaiter/behavior_lib/cond/NeedExplore.py similarity index 57% rename from robowaiter/behavior_lib/cond/EnvExplored.py rename to robowaiter/behavior_lib/cond/NeedExplore.py index a42471d..4e31e3b 100644 --- a/robowaiter/behavior_lib/cond/EnvExplored.py +++ b/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 - - if f'EnvExplored()' not in self.scene.state["condition_set"]: - return ptree.common.Status.SUCCESS - else: - return ptree.common.Status.FAILURE + # arg_str = self.arg_str + # + # if f'EnvExplored()' not in self.scene.state["condition_set"]: + # return ptree.common.Status.SUCCESS + # else: + # return ptree.common.Status.FAILURE + return ptree.common.Status.FAILURE # if self.scene.status? diff --git a/robowaiter/robot/Default.ptml b/robowaiter/robot/Default.ptml index d97954b..e6363f5 100644 --- a/robowaiter/robot/Default.ptml +++ b/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() diff --git a/robowaiter/robot/Default_bracket.ptml b/robowaiter/robot/Default_bracket.ptml index 844db49..dace919 100644 --- a/robowaiter/robot/Default_bracket.ptml +++ b/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() diff --git a/robowaiter/scene/outputs/current_bt.png b/robowaiter/scene/outputs/current_bt.png index c382351..76d2910 100644 Binary files a/robowaiter/scene/outputs/current_bt.png and b/robowaiter/scene/outputs/current_bt.png differ diff --git a/robowaiter/utils/draw_bt/Default.ptml b/robowaiter/utils/draw_bt/Default.ptml index 55a6f88..e6363f5 100644 --- a/robowaiter/utils/draw_bt/Default.ptml +++ b/robowaiter/utils/draw_bt/Default.ptml @@ -1,10 +1,23 @@ -sequence{ -cond Holding(Nothing) -sequence{ -cond Is(Curtain,Off) -act Turn(Curtain,On) -} -sequence{ -cond Is(AC,Off) -act Turn(AC,On) -}} \ No newline at end of file +selector + sequence + cond Chatting() + act DealChat() + sequence + cond HasSubTask() + sequence + act SubTaskPlaceHolder() + 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() diff --git a/robowaiter/utils/draw_bt/Default_bracket.ptml b/robowaiter/utils/draw_bt/Default_bracket.ptml index 844db49..dace919 100644 --- a/robowaiter/utils/draw_bt/Default_bracket.ptml +++ b/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() diff --git a/robowaiter/utils/draw_bt/draw_bt.py b/robowaiter/utils/draw_bt/draw_bt.py index 4cbb292..43a7078 100644 --- a/robowaiter/utils/draw_bt/draw_bt.py +++ b/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__': diff --git a/robowaiter/utils/draw_bt/test.png b/robowaiter/utils/draw_bt/test.png index 15a3183..514391c 100644 Binary files a/robowaiter/utils/draw_bt/test.png and b/robowaiter/utils/draw_bt/test.png differ diff --git a/robowaiter/utils/draw_bt/test.zip b/robowaiter/utils/draw_bt/test.zip new file mode 100644 index 0000000..eee42a8 Binary files /dev/null and b/robowaiter/utils/draw_bt/test.zip differ