书接上回的py_trees快速实践，写了一个机器人沿着拓扑路径循迹移动，最后到达目标点后，执行一个任务动作的行为树。在行为树中，增加了在每个tick检查机器人电量的逻辑。在电量低于一定阈值时，会中断当前任务并触发充电动作。这个逻辑体现了行为树响应性(Reactive)的特点，希望对学习行为树的同学有一点参考价值。
下面直接给出相应的代码：
#!/usr/bin/python3
# coding: utf-8
# robot_mission_behaviors.py
from py_trees import behaviour,behaviours, blackboard, common, meta, composites, display, decorators, idioms, trees, visitors
import functools
import operator
import sys
import time
import threading
class Plan(behaviour.Behaviour):
def __init__(self, name):
super(Plan,self).__init__(name)
self.blackboard = self.attach_blackboard_client()
self.blackboard.register_key(key="goal_list", access=common.Access.WRITE)
self.blackboard.register_key(key="new_goal_activate", access=common.Access.WRITE)
def initialise(self) -> None:
pass
def update(self) -> common.Status:
# path planing
planned_path = [1,2,3,4,5,6,7,8]
succeed = True
self.blackboard.goal_list = planned_path
if succeed:
self.feedback_message = "path plan succeed."
self.blackboard.new_goal_activate = True
return common.Status.SUCCESS
else:
self.feedback_message = "path plan failed!"
self.blackboard.new_goal_activate = False
return common.Status.FAILURE
class SendNavGoal(behaviour.Behaviour):
def __init__(self, name):
super(SendNavGoal, self).__init__(name)
self.blackboard = self.attach_blackboard_client()
self.blackboard.register_key(key="new_goal_activate", access=common.Access.WRITE)
self.blackboard.register_key(key="goal_list", access=common.Access.READ)
self.blackboard.register_key(key="nav_return_code", access=common.Access.WRITE)
def initialise(self) -> None:
pass
def send_nav_goal(self,callback):
# send nav_point
pass
def nav_gaol_cb(self,error_code):
self.blackboard.nav_return_code = error_code
def update(self) -> common.Status:
if self.blackboard.new_goal_activate and self.blackboard.goal_list:
goal = self.blackboard.goal_list.pop(0)
print("-*-*- Go to path: {}".format(goal))
#self.thread = threading.Thread(target=self.send_nav_goal,args=(self.nav_gaol_cb))
#self.thread.setDaemon(True)
#self.thread.start()
#self.blackboard.new_goal_activate = False
#self.feedback_message = "send nav goal."
return common.Status.SUCCESS
class CheckPowerStatus(behaviour.Behaviour):
def __init__(self, name):
super(CheckPowerStatus, self).__init__(name)
self.count = 3
def initialise(self) -> None:
pass
def update(self) -> common.Status:
# check power status
if self.count == 0:
power_ok = False
else:
power_ok = True
self.count -= 1
if power_ok:
return common.Status.SUCCESS
else:
return common.Status.FAILURE
class SetGoalToCharge(behaviour.Behaviour):
def __init__(self, name):
super(SetGoalToCharge,self).__init__(name)
self.blackboard = self.attach_blackboard_client()
self.blackboard.register_key(key="goal_list", access=common.Access.WRITE)
self.blackboard.register_key(key="new_goal_activate", access=common.Access.WRITE)
def initialise(self) -> None:
pass
def update(self) -> common.Status:
# charge re-plan
self.blackboard.goal_list = [100,99,98,97,96,95]
return common.Status.SUCCESS
class QueryNavStatus(behaviour.Behaviour):
def __init__(self, name):
super(QueryNavStatus, self).__init__(name)
self.blackboard = self.attach_blackboard_client()
def initialise(self) -> None:
pass
def update(self) -> common.Status:
# ask_nav_statsus
status = 0
if status == 0:
self.feedback_message = "Nav succeed."
return common.Status.SUCCESS
elif status == 0:
return common.Status.RUNNING
else:
return common.Status.FAILURE
class QueryNavCallbackBlackboard(behaviour.Behaviour):
def __init__(self, name):
super(QueryNavCallbackBlackboard, self).__init__(name)
self.blackboard = self.attach_blackboard_client()
self.blackboard.register_key(key="nav_return_code", access=common.Access.READ)
def initialise(self) -> None:
pass
def update(self) -> common.Status:
# check_nav_cb_balckboard
status = 1
if status == 0:
self.feedback_message = "Nav succeed."
return common.Status.SUCCESS
elif status == 0:
return common.Status.RUNNING
else:
return common.Status.FAILURE
class PtzAction(behaviour.Behaviour):
def __init__(self, name):
super(PtzAction, self).__init__(name)
def initialise(self) -> None:
pass
def update(self) -> common.Status:
# ptz_ctrl & take_picture & save_picture & update
self.feedback_message = "Run Action succeed."
return common.Status.SUCCESS
def create_root() -> behaviour.Behaviour:
root = composites.Sequence(name="RobotMission", memory=True)
plan = Plan(name="Plan")
seq1 = composites.Sequence(name="Sequence", memory=False)
# PowerChecking
power_check = composites.Selector(name="PowerChecking", memory=False)
check_power_status = CheckPowerStatus(name="PowerStatusOK")
set_goal_to_charge=decorators.OneShot(name="OneShot",child=SetGoalToCharge(name="SetGoalToCharge"),policy=common.OneShotPolicy.ON_COMPLETION)
power_check.add_children([check_power_status, set_goal_to_charge])
nav_sequence = composites.Sequence(name="NavSequence", memory=False)
# GoalSelector
new_goal_selector = composites.Selector(name="GoalSelector", memory=False)
check_new_goal_variable =decorators.Inverter(
name = "No New Goal?",
child = behaviours.CheckBlackboardVariableValue(
name="CheckNewGoalActivate",
check=common.ComparisonExpression(variable="new_goal_activate", value=True, operator=operator.eq)
)
)
send_nav_goal = SendNavGoal(name="SendNavGoal")
new_goal_selector.add_children([check_new_goal_variable, send_nav_goal])
# NavSelector
nav_selector = composites.Selector(name="NavSelector", memory=False)
query_nav_status = decorators.RunningIsFailure(
name="RunningIsFailure",
child=QueryNavStatus(name="QueryNavStatus")
)
query_nav_cb_blackboard = decorators.Timeout(
name="TimeOut",
duration=20.0,
child=QueryNavCallbackBlackboard(name="QueryNavCallbackBlackboard")
)
nav_selector.add_children([query_nav_status, query_nav_cb_blackboard])
# GoalFinishChecking
goal_finish = composites.Selector(name="GoalFinishChecking", memory=False)
check_goal_list_empty = behaviours.CheckBlackboardVariableValue(
name="CheckGoalListEmpty",
check=common.ComparisonExpression(variable="goal_list", value=[], operator=operator.eq)
)
set_goal_activate= decorators.SuccessIsRunning(
name="SuccessIsRunning",
child = behaviours.SetBlackboardVariable(
name="ActivateNewGoal",
variable_name="new_goal_activate",
variable_value=True,
overwrite=True
)
)
goal_finish.add_children([check_goal_list_empty, set_goal_activate])
nav_sequence.add_children([new_goal_selector, nav_selector, goal_finish])
ptz_action = PtzAction(name="PtzAction")
seq1.add_children([power_check, nav_sequence, ptz_action])
root.add_children([plan, seq1])
return root
def pre_tick_handler(behaviour_tree: trees.BehaviourTree) -> None:
print("\n--------- Run %s ---------\n" % behaviour_tree.count)
def post_tick_handler(
snapshot_visitor: visitors.SnapshotVisitor,
behaviour_tree: trees.BehaviourTree,
) -> None:
"""Print an ascii tree with the current snapshot status."""
print(
"\n"
+ display.unicode_tree(
root=behaviour_tree.root,
visited=snapshot_visitor.visited,
previously_visited=snapshot_visitor.previously_visited,
)
)
if __name__ == "__main__":
root = create_root()
render = False
if render:
display.render_dot_tree(root,with_blackboard_variables=True)
sys.exit()
root.setup_with_descendants()
behaviour_tree = trees.BehaviourTree(root)
behaviour_tree.add_pre_tick_handler(pre_tick_handler)
behaviour_tree.visitors.append(visitors.DebugVisitor())
snapshot_visitor = visitors.SnapshotVisitor()
behaviour_tree.add_post_tick_handler(
functools.partial(post_tick_handler, snapshot_visitor)
)
behaviour_tree.visitors.append(snapshot_visitor)
succeed = False
while not succeed:
try:
behaviour_tree.tick()
print("behaviour_tree status: ",root.status)
succeed = behaviour_tree.root.status == common.Status.SUCCESS
time.sleep(1.0)
except KeyboardInterrupt:
break
print("\n")
根据代码，py_trees会基于pydot自动生成行为树的可视化模型，如下所示：
运行程序，反映行为树流转的日志如下：
:~$ python3 robot_mission_behaviors.py
--------- Run 0 ---------
-*-*- Go to path: 1
{-} RobotMission [*]
--> Plan [✓] -- path plan succeed.
[-] Sequence [*]
[o] PowerChecking [✓]
--> PowerStatusOK [✓]
-^- OneShot
--> SetGoalToCharge
[-] NavSequence [*]
[o] GoalSelector [✓]
-^- No New Goal? [✕] -- success -> failure
--> CheckNewGoalActivate [✓] -- 'new_goal_activate' comparison succeeded [v: True][e: True]
--> SendNavGoal [✓]
[o] NavSelector [✓]
-^- RunningIsFailure [✓] -- Nav succeed.
--> QueryNavStatus [✓] -- Nav succeed.
-^- TimeOut
--> QueryNavCallbackBlackboard
[o] GoalFinishChecking [*]
--> CheckGoalListEmpty [✕] -- 'goal_list' comparison failed [v: [2, 3, 4, 5, 6, 7, 8]][e: []]
-^- SuccessIsRunning [*] -- success is running []
--> ActivateNewGoal [✓]
--> PtzAction
behaviour_tree status: Status.RUNNING
--------- Run 1 ---------
-*-*- Go to path: 2
{-} RobotMission [*]
--> Plan
[-] Sequence [*]
[o] PowerChecking [✓]
--> PowerStatusOK [✓]
-^- OneShot
--> SetGoalToCharge
[-] NavSequence [*]
[o] GoalSelector [✓]
-^- No New Goal? [✕] -- success -> failure
--> CheckNewGoalActivate [✓] -- 'new_goal_activate' comparison succeeded [v: True][e: True]
--> SendNavGoal [✓]
[o] NavSelector [✓]
-^- RunningIsFailure [✓] -- Nav succeed.
--> QueryNavStatus [✓] -- Nav succeed.
-^- TimeOut
--> QueryNavCallbackBlackboard
[o] GoalFinishChecking [*]
--> CheckGoalListEmpty [✕] -- 'goal_list' comparison failed [v: [3, 4, 5, 6, 7, 8]][e: []]
-^- SuccessIsRunning [*] -- success is running []
--> ActivateNewGoal [✓]
--> PtzAction
behaviour_tree status: Status.RUNNING
--------- Run 2 ---------
-*-*- Go to path: 3
{-} RobotMission [*]
--> Plan
[-] Sequence [*]
[o] PowerChecking [✓]
--> PowerStatusOK [✓]
-^- OneShot
--> SetGoalToCharge
[-] NavSequence [*]
[o] GoalSelector [✓]
-^- No New Goal? [✕] -- success -> failure
--> CheckNewGoalActivate [✓] -- 'new_goal_activate' comparison succeeded [v: True][e: True]
--> SendNavGoal [✓]
[o] NavSelector [✓]
-^- RunningIsFailure [✓] -- Nav succeed.
--> QueryNavStatus [✓] -- Nav succeed.
-^- TimeOut
--> QueryNavCallbackBlackboard
[o] GoalFinishChecking [*]
--> CheckGoalListEmpty [✕] -- 'goal_list' comparison failed [v: [4, 5, 6, 7, 8]][e: []]
-^- SuccessIsRunning [*] -- success is running []
--> ActivateNewGoal [✓]
--> PtzAction
behaviour_tree status: Status.RUNNING
--------- Run 3 ---------
-*-*- Go to path: 100
{-} RobotMission [*]
--> Plan
[-] Sequence [*]
[o] PowerChecking [✓]
--> PowerStatusOK [✕]
-^- OneShot [✓] -- oneshot completed
--> SetGoalToCharge [✓]
[-] NavSequence [*]
[o] GoalSelector [✓]
-^- No New Goal? [✕] -- success -> failure
--> CheckNewGoalActivate [✓] -- 'new_goal_activate' comparison succeeded [v: True][e: True]
--> SendNavGoal [✓]
[o] NavSelector [✓]
-^- RunningIsFailure [✓] -- Nav succeed.
--> QueryNavStatus [✓] -- Nav succeed.
-^- TimeOut
--> QueryNavCallbackBlackboard
[o] GoalFinishChecking [*]
--> CheckGoalListEmpty [✕] -- 'goal_list' comparison failed [v: [99, 98, 97, 96, 95]][e: []]
-^- SuccessIsRunning [*] -- success is running []
--> ActivateNewGoal [✓]
--> PtzAction
behaviour_tree status: Status.RUNNING
--------- Run 4 ---------
-*-*- Go to path: 99
{-} RobotMission [*]
--> Plan
[-] Sequence [*]
[o] PowerChecking [✓]
--> PowerStatusOK [✕]
-^- OneShot [✓] -- oneshot completed
--> SetGoalToCharge
[-] NavSequence [*]
[o] GoalSelector [✓]
-^- No New Goal? [✕] -- success -> failure
--> CheckNewGoalActivate [✓] -- 'new_goal_activate' comparison succeeded [v: True][e: True]
--> SendNavGoal [✓]
[o] NavSelector [✓]
-^- RunningIsFailure [✓] -- Nav succeed.
--> QueryNavStatus [✓] -- Nav succeed.
-^- TimeOut
--> QueryNavCallbackBlackboard
[o] GoalFinishChecking [*]
--> CheckGoalListEmpty [✕] -- 'goal_list' comparison failed [v: [98, 97, 96, 95]][e: []]
-^- SuccessIsRunning [*] -- success is running []
--> ActivateNewGoal [✓]
--> PtzAction
behaviour_tree status: Status.RUNNING
--------- Run 5 ---------
-*-*- Go to path: 98
{-} RobotMission [*]
--> Plan
[-] Sequence [*]
[o] PowerChecking [✓]
--> PowerStatusOK [✕]
-^- OneShot [✓] -- oneshot completed
--> SetGoalToCharge
[-] NavSequence [*]
[o] GoalSelector [✓]
-^- No New Goal? [✕] -- success -> failure
--> CheckNewGoalActivate [✓] -- 'new_goal_activate' comparison succeeded [v: True][e: True]
--> SendNavGoal [✓]
[o] NavSelector [✓]
-^- RunningIsFailure [✓] -- Nav succeed.
--> QueryNavStatus [✓] -- Nav succeed.
-^- TimeOut
--> QueryNavCallbackBlackboard
[o] GoalFinishChecking [*]
--> CheckGoalListEmpty [✕] -- 'goal_list' comparison failed [v: [97, 96, 95]][e: []]
-^- SuccessIsRunning [*] -- success is running []
--> ActivateNewGoal [✓]
--> PtzAction
behaviour_tree status: Status.RUNNING
--------- Run 6 ---------
-*-*- Go to path: 97
{-} RobotMission [*]
--> Plan
[-] Sequence [*]
[o] PowerChecking [✓]
--> PowerStatusOK [✕]
-^- OneShot [✓] -- oneshot completed
--> SetGoalToCharge
[-] NavSequence [*]
[o] GoalSelector [✓]
-^- No New Goal? [✕] -- success -> failure
--> CheckNewGoalActivate [✓] -- 'new_goal_activate' comparison succeeded [v: True][e: True]
--> SendNavGoal [✓]
[o] NavSelector [✓]
-^- RunningIsFailure [✓] -- Nav succeed.
--> QueryNavStatus [✓] -- Nav succeed.
-^- TimeOut
--> QueryNavCallbackBlackboard
[o] GoalFinishChecking [*]
--> CheckGoalListEmpty [✕] -- 'goal_list' comparison failed [v: [96, 95]][e: []]
-^- SuccessIsRunning [*] -- success is running []
--> ActivateNewGoal [✓]
--> PtzAction
behaviour_tree status: Status.RUNNING
--------- Run 7 ---------
-*-*- Go to path: 96
{-} RobotMission [*]
--> Plan
[-] Sequence [*]
[o] PowerChecking [✓]
--> PowerStatusOK [✕]
-^- OneShot [✓] -- oneshot completed
--> SetGoalToCharge
[-] NavSequence [*]
[o] GoalSelector [✓]
-^- No New Goal? [✕] -- success -> failure
--> CheckNewGoalActivate [✓] -- 'new_goal_activate' comparison succeeded [v: True][e: True]
--> SendNavGoal [✓]
[o] NavSelector [✓]
-^- RunningIsFailure [✓] -- Nav succeed.
--> QueryNavStatus [✓] -- Nav succeed.
-^- TimeOut
--> QueryNavCallbackBlackboard
[o] GoalFinishChecking [*]
--> CheckGoalListEmpty [✕] -- 'goal_list' comparison failed [v: [95]][e: []]
-^- SuccessIsRunning [*] -- success is running []
--> ActivateNewGoal [✓]
--> PtzAction
behaviour_tree status: Status.RUNNING
--------- Run 8 ---------
-*-*- Go to path: 95
{-} RobotMission [✓]
--> Plan
[-] Sequence [✓]
[o] PowerChecking [✓]
--> PowerStatusOK [✕]
-^- OneShot [✓] -- oneshot completed
--> SetGoalToCharge
[-] NavSequence [✓]
[o] GoalSelector [✓]
-^- No New Goal? [✕] -- success -> failure
--> CheckNewGoalActivate [✓] -- 'new_goal_activate' comparison succeeded [v: True][e: True]
--> SendNavGoal [✓]
[o] NavSelector [✓]
-^- RunningIsFailure [✓] -- Nav succeed.
--> QueryNavStatus [✓] -- Nav succeed.
-^- TimeOut
--> QueryNavCallbackBlackboard
[o] GoalFinishChecking [✓]
--> CheckGoalListEmpty [✓] -- 'goal_list' comparison succeeded [v: []][e: []]
-^- SuccessIsRunning [-]
--> ActivateNewGoal
--> PtzAction [✓] -- Run Action succeed.
behaviour_tree status: Status.SUCCESS
观察日志可以看到，机器人最开始沿着[1,2, 3, 4, 5, 6, 7, 8]这个拓扑路径依次执行。但是在第3个tick，机器人发现电量不足，在当前位置重新规划了[100,99,98,97,96,95]的路径去充电。最终沿着这个路径运行到达充电点。
一些tips:
1. 采用OneShot装饰器，可以确保充电重规划动作只执行一次。
2. 行为树根节点的Sequence设置为有memory，确保第一次路径规划只做一次。
3. 如果不强调Reactive，即不希望中途检查和打断，则可把电量检查移到根节点下面，此时任务的表达类似于状态机。
4. 如果程序中没有发生电量不足，机器人将沿着原来规划的路径走到底，并执行任务动作。而本文充电打断后，tick也会流转到任务动作执行，读者可以思考下该如何处理:-》。
1790
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
