From 91638a36bb4576ecf3f099693ca9b8b2b7e349a2 Mon Sep 17 00:00:00 2001
From: Aglargil <1252223935@qq.com>
Date: Mon, 3 Mar 2025 16:29:53 +0800
Subject: [PATCH 1/4] feat: add behavior tree
---
MissionPlanning/BehaviorTree/behavior_tree.py | 605 ++++++++++++++++++
1 file changed, 605 insertions(+)
create mode 100644 MissionPlanning/BehaviorTree/behavior_tree.py
diff --git a/MissionPlanning/BehaviorTree/behavior_tree.py b/MissionPlanning/BehaviorTree/behavior_tree.py
new file mode 100644
index 0000000000..c43d316783
--- /dev/null
+++ b/MissionPlanning/BehaviorTree/behavior_tree.py
@@ -0,0 +1,605 @@
+"""
+Behavior Tree
+
+author: Wang Zheng (@Aglargil)
+
+Ref:
+
+- [Behavior Tree](https://en.wikipedia.org/wiki/Behavior_tree_(artificial_intelligence,_robotics_and_control))
+"""
+
+import time
+import xml.etree.ElementTree as ET
+
+
+class Status:
+ SUCCESS = "success"
+ FAILURE = "failure"
+ RUNNING = "running"
+
+
+class NodeType:
+ CONTROL_NODE = "ControlNode"
+ ACTION_NODE = "ActionNode"
+ DECORATOR_NODE = "DecoratorNode"
+
+
+class Node:
+ """
+ Base class for all nodes in a behavior tree.
+ """
+
+ def __init__(self, name):
+ self.name = name
+ self.status = None
+
+ def tick(self) -> Status:
+ """
+ Tick the node.
+
+ Returns:
+ Status: The status of the node.
+ """
+ raise ValueError("Node is not implemented")
+
+ def tick_and_set_status(self) -> Status:
+ """
+ Tick the node and set the status.
+
+ Returns:
+ Status: The status of the node.
+ """
+ self.status = self.tick()
+ return self.status
+
+
+class ControlNode(Node):
+ """
+ Base class for all control nodes in a behavior tree.
+
+ Control nodes manage the execution flow of their child nodes according to specific rules.
+ They typically have multiple children and determine which children to execute and in what order.
+ """
+
+ def __init__(self, name):
+ super().__init__(name)
+ self.children = []
+ self.type = NodeType.CONTROL_NODE
+
+ def not_set_children_raise_error(self):
+ if len(self.children) == 0:
+ raise ValueError("Children are not set")
+
+
+class SequenceNode(ControlNode):
+ """
+ Executes child nodes in sequence until one fails or all succeed.
+
+ Returns:
+ - Returns FAILURE if any child returns FAILURE
+ - Returns SUCCESS when all children have succeeded
+ - Returns RUNNING when a child is still running or when moving to the next child
+
+ Example:
+ <Sequence>
+ <Action1 />
+ <Action2 />
+ </Sequence>
+ """
+
+ def __init__(self, name):
+ super().__init__(name)
+ self.current_child_index = 0
+
+ def tick(self) -> Status:
+ self.not_set_children_raise_error()
+
+ if self.current_child_index >= len(self.children):
+ return Status.SUCCESS
+ status = self.children[self.current_child_index].tick_and_set_status()
+ if status == Status.FAILURE:
+ return Status.FAILURE
+ elif status == Status.SUCCESS:
+ self.current_child_index += 1
+ return Status.RUNNING
+ elif status == Status.RUNNING:
+ return Status.RUNNING
+ else:
+ raise ValueError("Unknown status")
+
+
+class SelectorNode(ControlNode):
+ """
+ Executes child nodes in sequence until one succeeds or all fail.
+
+ Returns:
+ - Returns SUCCESS if any child returns SUCCESS
+ - Returns FAILURE when all children have failed
+ - Returns RUNNING when a child is still running or when moving to the next child
+
+ Example:
+ <Selector>
+ <Action1 />
+ <Action2 />
+ </Selector>
+ """
+
+ def __init__(self, name):
+ super().__init__(name)
+ self.current_child_index = 0
+
+ def tick(self) -> Status:
+ self.not_set_children_raise_error()
+
+ if self.current_child_index >= len(self.children):
+ return Status.FAILURE
+ status = self.children[self.current_child_index].tick_and_set_status()
+ if status == Status.SUCCESS:
+ return Status.SUCCESS
+ elif status == Status.FAILURE:
+ self.current_child_index += 1
+ return Status.RUNNING
+ elif status == Status.RUNNING:
+ return Status.RUNNING
+ else:
+ raise ValueError("Unknown status")
+
+
+class WhileDoElseNode(ControlNode):
+ """
+ Conditional execution node with three parts: condition, do, and optional else.
+
+ Returns:
+ - First executes the condition node (child[0])
+ - If condition succeeds, executes and returns result of do node (child[1])
+ - If condition fails, executes and returns result of else node (child[2]) if present
+ - Otherwise returns SUCCESS when condition fails with no else node
+
+ Example:
+ <WhileDoElse>
+ <Condition />
+ <Do />
+ <Else />
+ </WhileDoElse>
+ """
+
+ def __init__(self, name):
+ super().__init__(name)
+
+ def tick(self) -> Status:
+ if len(self.children) != 3 and len(self.children) != 2:
+ raise ValueError("WhileDoElseNode must have exactly 3 or 2 children")
+
+ condition_node = self.children[0]
+ do_node = self.children[1]
+ else_node = self.children[2] if len(self.children) == 3 else None
+
+ condition_status = condition_node.tick_and_set_status()
+ if condition_status == Status.SUCCESS:
+ return do_node.tick_and_set_status()
+ elif condition_status == Status.FAILURE:
+ return (
+ else_node.tick_and_set_status()
+ if else_node is not None
+ else Status.SUCCESS
+ )
+ else:
+ raise ValueError("Unknown status")
+
+
+class ActionNode(Node):
+ """
+ Base class for all action nodes in a behavior tree.
+
+ Action nodes are responsible for performing specific tasks or actions.
+ They do not have children and are typically used to execute logic or operations.
+ """
+
+ def __init__(self, name):
+ super().__init__(name)
+ self.type = NodeType.ACTION_NODE
+
+
+class SleepNode(ActionNode):
+ """
+ Sleep node that sleeps for a specified duration.
+
+ Returns:
+ - Returns SUCCESS after the specified duration has passed
+ - Returns RUNNING if the duration has not yet passed
+
+ Example:
+ <Sleep sec="1.5" />
+ """
+
+ def __init__(self, name, duration):
+ super().__init__(name)
+ self.duration = duration
+ self.start_time = None
+
+ def tick(self) -> Status:
+ if self.start_time is None:
+ self.start_time = time.time()
+ if time.time() - self.start_time > self.duration:
+ return Status.SUCCESS
+ return Status.RUNNING
+
+
+class EchoNode(ActionNode):
+ """
+ Echo node that prints a message to the console.
+
+ Returns:
+ - Returns SUCCESS after the message has been printed
+
+ Example:
+ <Echo message="Hello, World!" />
+ """
+
+ def __init__(self, name, message):
+ super().__init__(name)
+ self.message = message
+
+ def tick(self) -> Status:
+ print(self.name, self.message)
+ return Status.SUCCESS
+
+
+class DecoratorNode(Node):
+ """
+ Base class for all decorator nodes in a behavior tree.
+
+ Decorator nodes modify the behavior of their child node.
+ They must have a single child and can alter the status of the child node.
+ """
+
+ def __init__(self, name):
+ super().__init__(name)
+ self.type = NodeType.DECORATOR_NODE
+ self.child = None
+
+ def not_set_child_raise_error(self):
+ if self.child is None:
+ raise ValueError("Child is not set")
+
+
+class InverterNode(DecoratorNode):
+ """
+ Inverter node that inverts the status of its child node.
+
+ Returns:
+ - Returns SUCCESS if the child returns FAILURE
+ - Returns FAILURE if the child returns SUCCESS
+ - Returns RUNNING if the child returns RUNNING
+ Example:
+ <Inverter>
+ <Action />
+ </Inverter>
+ """
+
+ def __init__(self, name):
+ super().__init__(name)
+
+ def tick(self) -> Status:
+ self.not_set_child_raise_error()
+ status = self.child.tick_and_set_status()
+ return Status.SUCCESS if status == Status.FAILURE else Status.FAILURE
+
+
+class TimeoutNode(DecoratorNode):
+ """
+ Timeout node that fails if the child node takes too long to execute
+
+ Returns:
+ - FAILURE: If the timeout duration has been exceeded
+ - Child's status: Otherwise, passes through the status of the child node
+
+ Example:
+ <Timeout sec="1.5">
+ <Action />
+ </Timeout>
+ """
+
+ def __init__(self, name, timeout):
+ super().__init__(name)
+ self.timeout = timeout
+ self.start_time = None
+
+ def tick(self) -> Status:
+ self.not_set_child_raise_error()
+ if self.start_time is None:
+ self.start_time = time.time()
+ if time.time() - self.start_time > self.timeout:
+ return Status.FAILURE
+ return self.child.tick_and_set_status()
+
+
+class DelayNode(DecoratorNode):
+ """
+ Delay node that delays the execution of its child node for a specified duration.
+
+ Returns:
+ - Returns RUNNING if the duration has not yet passed
+ - Returns child's status after the duration has passed
+
+ Example:
+ <Delay sec="1.5">
+ <Action />
+ </Delay>
+ """
+
+ def __init__(self, name, delay):
+ super().__init__(name)
+ self.delay = delay
+ self.start_time = None
+
+ def tick(self) -> Status:
+ self.not_set_child_raise_error()
+ if self.start_time is None:
+ self.start_time = time.time()
+ if time.time() - self.start_time > self.delay:
+ return self.child.tick_and_set_status()
+ return Status.RUNNING
+
+
+class ForceSuccessNode(DecoratorNode):
+ """
+ ForceSuccess node that always returns SUCCESS.
+
+ Returns:
+ - Returns RUNNING if the child returns RUNNING
+ - Returns SUCCESS if the child returns SUCCESS or FAILURE
+ """
+
+ def __init__(self, name):
+ super().__init__(name)
+
+ def tick(self) -> Status:
+ self.not_set_child_raise_error()
+ status = self.child.tick_and_set_status()
+ if status == Status.FAILURE:
+ return Status.SUCCESS
+ return status
+
+
+class ForceFailureNode(DecoratorNode):
+ """
+ ForceFailure node that always returns FAILURE.
+
+ Returns:
+ - Returns RUNNING if the child returns RUNNING
+ - Returns FAILURE if the child returns SUCCESS or FAILURE
+ """
+
+ def __init__(self, name):
+ super().__init__(name)
+
+ def tick(self) -> Status:
+ self.not_set_child_raise_error()
+ status = self.child.tick_and_set_status()
+ if status == Status.SUCCESS:
+ return Status.FAILURE
+ return status
+
+
+class BehaviorTree:
+ """
+ Behavior tree class that manages the execution of a behavior tree.
+ """
+
+ def __init__(self, root):
+ self.root = root
+
+ def tick(self):
+ """
+ Tick once on the behavior tree.
+ """
+ self.root.tick_and_set_status()
+
+ def tick_while_running(self, interval=1.0, enable_print=True):
+ """
+ Tick the behavior tree while it is running.
+
+ Args:
+ interval (float, optional): The interval between ticks. Defaults to 1.0.
+ enable_print (bool, optional): Whether to print the behavior tree. Defaults to True.
+ """
+ while self.root.tick_and_set_status() == Status.RUNNING:
+ if enable_print:
+ self.print_tree()
+ if interval is not None:
+ time.sleep(interval)
+ if enable_print:
+ self.print_tree()
+
+ def to_text(self, root, indent=0):
+ """
+ Recursively convert the behavior tree to a text representation.
+ """
+ current_text = ""
+ if root.status == Status.RUNNING:
+ # yellow
+ current_text = "\033[93m" + root.name + "\033[0m"
+ elif root.status == Status.SUCCESS:
+ # green
+ current_text = "\033[92m" + root.name + "\033[0m"
+ elif root.status == Status.FAILURE:
+ # red
+ current_text = "\033[91m" + root.name + "\033[0m"
+ else:
+ current_text = root.name
+ if root.type == NodeType.CONTROL_NODE:
+ current_text = " " * indent + "[" + current_text + "]\n"
+ for child in root.children:
+ current_text += self.to_text(child, indent + 2)
+ elif root.type == NodeType.DECORATOR_NODE:
+ current_text = " " * indent + "(" + current_text + ")\n"
+ current_text += self.to_text(root.child, indent + 2)
+ elif root.type == NodeType.ACTION_NODE:
+ current_text = " " * indent + "<" + current_text + ">\n"
+ return current_text
+
+ def print_tree(self):
+ """
+ Print the behavior tree.
+
+ Node types:
+ Action: <Action>
+ Decorator: (Decorator)
+ Control: [Control]
+
+ Colors:
+ Yellow: RUNNING
+ Green: SUCCESS
+ Red: FAILURE
+ """
+ text = self.to_text(self.root)
+ text = text.strip()
+ print("\033[94m" + "Behavior Tree" + "\033[0m")
+ print(text)
+ print("\033[94m" + "Behavior Tree" + "\033[0m")
+
+
+class BehaviorTreeFactory:
+ """
+ Factory class for creating behavior trees from XML strings.
+ """
+
+ def __init__(self):
+ self.node_builders = {}
+ # Control nodes
+ self.register_node_builder(
+ "Sequence",
+ lambda node: SequenceNode(node.attrib.get("name", SequenceNode.__name__)),
+ )
+ self.register_node_builder(
+ "Selector",
+ lambda node: SelectorNode(node.attrib.get("name", SelectorNode.__name__)),
+ )
+ self.register_node_builder(
+ "WhileDoElse",
+ lambda node: WhileDoElseNode(
+ node.attrib.get("name", WhileDoElseNode.__name__)
+ ),
+ )
+ # Decorator nodes
+ self.register_node_builder(
+ "Inverter",
+ lambda node: InverterNode(node.attrib.get("name", InverterNode.__name__)),
+ )
+ self.register_node_builder(
+ "Timeout",
+ lambda node: TimeoutNode(
+ node.attrib.get("name", SelectorNode.__name__),
+ float(node.attrib["sec"]),
+ ),
+ )
+ self.register_node_builder(
+ "Delay",
+ lambda node: DelayNode(
+ node.attrib.get("name", DelayNode.__name__),
+ float(node.attrib["sec"]),
+ ),
+ )
+ # Action nodes
+ self.register_node_builder(
+ "Sleep",
+ lambda node: SleepNode(
+ node.attrib.get("name", SleepNode.__name__),
+ float(node.attrib["sec"]),
+ ),
+ )
+ self.register_node_builder(
+ "Echo",
+ lambda node: EchoNode(
+ node.attrib.get("name", EchoNode.__name__),
+ node.attrib["message"],
+ ),
+ )
+
+ def register_node_builder(self, node_name, builder):
+ """
+ Register a builder for a node
+
+ Args:
+ node_name (str): The name of the node.
+ builder (function): The builder function.
+ """
+ self.node_builders[node_name] = builder
+
+ def build_node(self, node):
+ """
+ Build a node from an XML element.
+
+ Args:
+ node (Element): The XML element to build the node from.
+
+ Returns:
+ BehaviorTree Node: the built node
+ """
+ if node.tag in self.node_builders:
+ root = self.node_builders[node.tag](node)
+ if root.type == NodeType.CONTROL_NODE:
+ if len(node) <= 0:
+ raise ValueError("Control node must have children")
+ for child in node:
+ root.children.append(self.build_node(child))
+ elif root.type == NodeType.DECORATOR_NODE:
+ if len(node) != 1:
+ raise ValueError("Decorator node must have exactly one child")
+ root.child = self.build_node(node[0])
+ elif root.type == NodeType.ACTION_NODE:
+ if len(node) != 0:
+ raise ValueError("Action node must have no children")
+ return root
+ else:
+ raise ValueError(f"Unknown node type: {node.tag}")
+
+ def build_tree(self, xml_string):
+ """
+ Build a behavior tree from an XML string.
+
+ Args:
+ xml_string (str): The XML string containing the behavior tree.
+
+ Returns:
+ BehaviorTree: The behavior tree.
+ """
+ xml_tree = ET.fromstring(xml_string)
+ root = self.build_node(xml_tree)
+ return BehaviorTree(root)
+
+ def build_tree_from_file(self, file_path):
+ """
+ Build a behavior tree from a file.
+
+ Args:
+ file_path (str): The path to the file containing the behavior tree.
+
+ Returns:
+ BehaviorTree: The behavior tree.
+ """
+ with open(file_path) as file:
+ xml_string = file.read()
+ return self.build_tree(xml_string)
+
+
+xml_string = """
+ <Sequence name="Sequence">
+ <Echo name="Echo0" message="Hello, World0!" />
+ <Delay name="Delay" sec="1.5">
+ <Echo name="Echo1" message="Hello, World1!" />
+ </Delay>
+ <Echo name="Echo2" message="Hello, World2!" />
+ </Sequence>
+ """
+
+
+def main():
+ factory = BehaviorTreeFactory()
+ tree = factory.build_tree(xml_string)
+ tree.tick_while_running()
+
+
+if __name__ == "__main__":
+ main()
From 984a5959d8a5cd13ee39b8420f8cc61e49d0528a Mon Sep 17 00:00:00 2001
From: Aglargil <1252223935@qq.com>
Date: Tue, 4 Mar 2025 16:38:27 +0800
Subject: [PATCH 2/4] feat: add behavior tree test
---
MissionPlanning/BehaviorTree/behavior_tree.py | 83 +++++-
.../BehaviorTree/robot_behavior_case.py | 247 ++++++++++++++++++
.../BehaviorTree/robot_behavior_tree.xml | 57 ++++
tests/test_behavior_tree.py | 207 +++++++++++++++
4 files changed, 580 insertions(+), 14 deletions(-)
create mode 100644 MissionPlanning/BehaviorTree/robot_behavior_case.py
create mode 100644 MissionPlanning/BehaviorTree/robot_behavior_tree.xml
create mode 100644 tests/test_behavior_tree.py
diff --git a/MissionPlanning/BehaviorTree/behavior_tree.py b/MissionPlanning/BehaviorTree/behavior_tree.py
index c43d316783..76c8cfa53e 100644
--- a/MissionPlanning/BehaviorTree/behavior_tree.py
+++ b/MissionPlanning/BehaviorTree/behavior_tree.py
@@ -52,6 +52,18 @@ def tick_and_set_status(self) -> Status:
self.status = self.tick()
return self.status
+ def reset(self):
+ """
+ Reset the node.
+ """
+ self.status = None
+
+ def reset_children(self):
+ """
+ Reset the children of the node.
+ """
+ pass
+
class ControlNode(Node):
"""
@@ -70,6 +82,10 @@ def not_set_children_raise_error(self):
if len(self.children) == 0:
raise ValueError("Children are not set")
+ def reset_children(self):
+ for child in self.children:
+ child.reset()
+
class SequenceNode(ControlNode):
"""
@@ -95,9 +111,11 @@ def tick(self) -> Status:
self.not_set_children_raise_error()
if self.current_child_index >= len(self.children):
+ self.reset_children()
return Status.SUCCESS
status = self.children[self.current_child_index].tick_and_set_status()
if status == Status.FAILURE:
+ self.reset_children()
return Status.FAILURE
elif status == Status.SUCCESS:
self.current_child_index += 1
@@ -132,9 +150,11 @@ def tick(self) -> Status:
self.not_set_children_raise_error()
if self.current_child_index >= len(self.children):
+ self.reset_children()
return Status.FAILURE
status = self.children[self.current_child_index].tick_and_set_status()
if status == Status.SUCCESS:
+ self.reset_children()
return Status.SUCCESS
elif status == Status.FAILURE:
self.current_child_index += 1
@@ -151,9 +171,9 @@ class WhileDoElseNode(ControlNode):
Returns:
- First executes the condition node (child[0])
- - If condition succeeds, executes and returns result of do node (child[1])
- - If condition fails, executes and returns result of else node (child[2]) if present
- - Otherwise returns SUCCESS when condition fails with no else node
+ - If condition succeeds, executes do node (child[1]) and returns RUNNING
+ - If condition fails, executes else node (child[2]) if present and returns result of else node
+ - If condition fails and there is no else node, returns SUCCESS
Example:
<WhileDoElse>
@@ -176,13 +196,24 @@ def tick(self) -> Status:
condition_status = condition_node.tick_and_set_status()
if condition_status == Status.SUCCESS:
- return do_node.tick_and_set_status()
+ do_node.tick_and_set_status()
+ return Status.RUNNING
elif condition_status == Status.FAILURE:
- return (
- else_node.tick_and_set_status()
- if else_node is not None
- else Status.SUCCESS
- )
+ if else_node is not None:
+ else_status = else_node.tick_and_set_status()
+ if else_status == Status.SUCCESS:
+ self.reset_children()
+ return Status.SUCCESS
+ elif else_status == Status.FAILURE:
+ self.reset_children()
+ return Status.FAILURE
+ elif else_status == Status.RUNNING:
+ return Status.RUNNING
+ else:
+ raise ValueError("Unknown status")
+ else:
+ self.reset_children()
+ return Status.SUCCESS
else:
raise ValueError("Unknown status")
@@ -262,6 +293,9 @@ def not_set_child_raise_error(self):
if self.child is None:
raise ValueError("Child is not set")
+ def reset_children(self):
+ self.child.reset()
+
class InverterNode(DecoratorNode):
"""
@@ -311,6 +345,7 @@ def tick(self) -> Status:
self.start_time = time.time()
if time.time() - self.start_time > self.timeout:
return Status.FAILURE
+ print(f"{self.name} is running")
return self.child.tick_and_set_status()
@@ -396,12 +431,18 @@ def tick(self):
"""
self.root.tick_and_set_status()
- def tick_while_running(self, interval=1.0, enable_print=True):
+ def reset(self):
+ """
+ Reset the behavior tree.
+ """
+ self.root.reset()
+
+ def tick_while_running(self, interval=None, enable_print=True):
"""
Tick the behavior tree while it is running.
Args:
- interval (float, optional): The interval between ticks. Defaults to 1.0.
+ interval (float, optional): The interval between ticks. Defaults to None.
enable_print (bool, optional): Whether to print the behavior tree. Defaults to True.
"""
while self.root.tick_and_set_status() == Status.RUNNING:
@@ -501,6 +542,18 @@ def __init__(self):
float(node.attrib["sec"]),
),
)
+ self.register_node_builder(
+ "ForceSuccess",
+ lambda node: ForceSuccessNode(
+ node.attrib.get("name", ForceSuccessNode.__name__)
+ ),
+ )
+ self.register_node_builder(
+ "ForceFailure",
+ lambda node: ForceFailureNode(
+ node.attrib.get("name", ForceFailureNode.__name__)
+ ),
+ )
# Action nodes
self.register_node_builder(
"Sleep",
@@ -541,16 +594,18 @@ def build_node(self, node):
root = self.node_builders[node.tag](node)
if root.type == NodeType.CONTROL_NODE:
if len(node) <= 0:
- raise ValueError("Control node must have children")
+ raise ValueError(f"{root.name} Control node must have children")
for child in node:
root.children.append(self.build_node(child))
elif root.type == NodeType.DECORATOR_NODE:
if len(node) != 1:
- raise ValueError("Decorator node must have exactly one child")
+ raise ValueError(
+ f"{root.name} Decorator node must have exactly one child"
+ )
root.child = self.build_node(node[0])
elif root.type == NodeType.ACTION_NODE:
if len(node) != 0:
- raise ValueError("Action node must have no children")
+ raise ValueError(f"{root.name} Action node must have no children")
return root
else:
raise ValueError(f"Unknown node type: {node.tag}")
diff --git a/MissionPlanning/BehaviorTree/robot_behavior_case.py b/MissionPlanning/BehaviorTree/robot_behavior_case.py
new file mode 100644
index 0000000000..6c39aa76b2
--- /dev/null
+++ b/MissionPlanning/BehaviorTree/robot_behavior_case.py
@@ -0,0 +1,247 @@
+"""
+Robot Behavior Tree Case
+
+This file demonstrates how to use a behavior tree to control robot behavior.
+"""
+
+from behavior_tree import (
+ BehaviorTreeFactory,
+ Status,
+ ActionNode,
+)
+import time
+import random
+import os
+
+
+class CheckBatteryNode(ActionNode):
+ """
+ Node to check robot battery level
+
+ If battery level is below threshold, returns FAILURE, otherwise returns SUCCESS
+ """
+
+ def __init__(self, name, threshold=20):
+ super().__init__(name)
+ self.threshold = threshold
+ self.battery_level = 100 # Initial battery level is 100%
+
+ def tick(self):
+ # Simulate battery level decreasing
+ self.battery_level -= random.randint(1, 5)
+ print(f"Current battery level: {self.battery_level}%")
+
+ if self.battery_level <= self.threshold:
+ return Status.FAILURE
+ return Status.SUCCESS
+
+
+class ChargeBatteryNode(ActionNode):
+ """
+ Node to charge the robot's battery
+ """
+
+ def __init__(self, name, charge_rate=10):
+ super().__init__(name)
+ self.charge_rate = charge_rate
+ self.charging_time = 0
+
+ def tick(self):
+ # Simulate charging process
+ if self.charging_time == 0:
+ print("Starting to charge...")
+
+ self.charging_time += 1
+ charge_amount = self.charge_rate * self.charging_time
+
+ if charge_amount >= 100:
+ print("Charging complete! Battery level: 100%")
+ self.charging_time = 0
+ return Status.SUCCESS
+ else:
+ print(f"Charging in progress... Battery level: {min(charge_amount, 100)}%")
+ return Status.RUNNING
+
+
+class MoveToPositionNode(ActionNode):
+ """
+ Node to move to a specified position
+ """
+
+ def __init__(self, name, position, move_duration=2):
+ super().__init__(name)
+ self.position = position
+ self.move_duration = move_duration
+ self.start_time = None
+
+ def tick(self):
+ if self.start_time is None:
+ self.start_time = time.time()
+ print(f"Starting movement to position {self.position}")
+
+ elapsed_time = time.time() - self.start_time
+
+ if elapsed_time >= self.move_duration:
+ print(f"Arrived at position {self.position}")
+ self.start_time = None
+ return Status.SUCCESS
+ else:
+ print(
+ f"Moving to position {self.position}... {int(elapsed_time / self.move_duration * 100)}% complete"
+ )
+ return Status.RUNNING
+
+
+class DetectObstacleNode(ActionNode):
+ """
+ Node to detect obstacles
+ """
+
+ def __init__(self, name, obstacle_probability=0.3):
+ super().__init__(name)
+ self.obstacle_probability = obstacle_probability
+
+ def tick(self):
+ # Use random probability to simulate obstacle detection
+ if random.random() < self.obstacle_probability:
+ print("Obstacle detected!")
+ return Status.SUCCESS
+ else:
+ print("No obstacle detected")
+ return Status.FAILURE
+
+
+class AvoidObstacleNode(ActionNode):
+ """
+ Node to avoid obstacles
+ """
+
+ def __init__(self, name, avoid_duration=1.5):
+ super().__init__(name)
+ self.avoid_duration = avoid_duration
+ self.start_time = None
+
+ def tick(self):
+ if self.start_time is None:
+ self.start_time = time.time()
+ print("Starting obstacle avoidance...")
+
+ elapsed_time = time.time() - self.start_time
+
+ if elapsed_time >= self.avoid_duration:
+ print("Obstacle avoidance complete")
+ self.start_time = None
+ return Status.SUCCESS
+ else:
+ print("Avoiding obstacle...")
+ return Status.RUNNING
+
+
+class PerformTaskNode(ActionNode):
+ """
+ Node to perform a specific task
+ """
+
+ def __init__(self, name, task_name, task_duration=3):
+ super().__init__(name)
+ self.task_name = task_name
+ self.task_duration = task_duration
+ self.start_time = None
+
+ def tick(self):
+ if self.start_time is None:
+ self.start_time = time.time()
+ print(f"Starting task: {self.task_name}")
+
+ elapsed_time = time.time() - self.start_time
+
+ if elapsed_time >= self.task_duration:
+ print(f"Task complete: {self.task_name}")
+ self.start_time = None
+ return Status.SUCCESS
+ else:
+ print(
+ f"Performing task: {self.task_name}... {int(elapsed_time / self.task_duration * 100)}% complete"
+ )
+ return Status.RUNNING
+
+
+def create_robot_behavior_tree():
+ """
+ Create robot behavior tree
+ """
+
+ factory = BehaviorTreeFactory()
+
+ # Register custom nodes
+ factory.register_node_builder(
+ "CheckBattery",
+ lambda node: CheckBatteryNode(
+ node.attrib.get("name", "CheckBattery"),
+ int(node.attrib.get("threshold", "20")),
+ ),
+ )
+
+ factory.register_node_builder(
+ "ChargeBattery",
+ lambda node: ChargeBatteryNode(
+ node.attrib.get("name", "ChargeBattery"),
+ int(node.attrib.get("charge_rate", "10")),
+ ),
+ )
+
+ factory.register_node_builder(
+ "MoveToPosition",
+ lambda node: MoveToPositionNode(
+ node.attrib.get("name", "MoveToPosition"),
+ node.attrib.get("position", "Unknown Position"),
+ float(node.attrib.get("move_duration", "2")),
+ ),
+ )
+
+ factory.register_node_builder(
+ "DetectObstacle",
+ lambda node: DetectObstacleNode(
+ node.attrib.get("name", "DetectObstacle"),
+ float(node.attrib.get("obstacle_probability", "0.3")),
+ ),
+ )
+
+ factory.register_node_builder(
+ "AvoidObstacle",
+ lambda node: AvoidObstacleNode(
+ node.attrib.get("name", "AvoidObstacle"),
+ float(node.attrib.get("avoid_duration", "1.5")),
+ ),
+ )
+
+ factory.register_node_builder(
+ "PerformTask",
+ lambda node: PerformTaskNode(
+ node.attrib.get("name", "PerformTask"),
+ node.attrib.get("task_name", "Unknown Task"),
+ float(node.attrib.get("task_duration", "3")),
+ ),
+ )
+ # Read XML from file
+ xml_path = os.path.join(os.path.dirname(__file__), "robot_behavior_tree.xml")
+ return factory.build_tree_from_file(xml_path)
+
+
+def main():
+ """
+ Main function: Create and run the robot behavior tree
+ """
+ print("Creating robot behavior tree...")
+ tree = create_robot_behavior_tree()
+
+ print("\nStarting robot behavior tree execution...\n")
+ # Run for a period of time or until completion
+
+ tree.tick_while_running(interval=0.01)
+
+ print("\nBehavior tree execution complete!")
+
+
+if __name__ == "__main__":
+ main()
diff --git a/MissionPlanning/BehaviorTree/robot_behavior_tree.xml b/MissionPlanning/BehaviorTree/robot_behavior_tree.xml
new file mode 100644
index 0000000000..0bca76a3ff
--- /dev/null
+++ b/MissionPlanning/BehaviorTree/robot_behavior_tree.xml
@@ -0,0 +1,57 @@
+<Selector name="Robot Main Controller">
+ <!-- Charge battery when power is low -->
+ <Sequence name="Battery Management">
+ <Inverter name="Low Battery Detection">
+ <CheckBattery name="Check Battery" threshold="30" />
+ </Inverter>
+ <Echo name="Low Battery Warning" message="Battery level low! Charging needed" />
+ <ChargeBattery name="Charge Battery" charge_rate="20" />
+ </Sequence>
+ <!-- Main task sequence -->
+ <Sequence name="Patrol Task">
+ <Echo name="Start Task" message="Starting patrol task" />
+ <!-- Move to position A -->
+ <Sequence name="Move to Position A">
+ <MoveToPosition name="Move to A" position="A" move_duration="2" />
+ <!-- Handle obstacles -->
+ <Selector name="Obstacle Handling A">
+ <Sequence name="Obstacle Present">
+ <DetectObstacle name="Detect Obstacle" obstacle_probability="0.3" />
+ <AvoidObstacle name="Avoid Obstacle" avoid_duration="1.5" />
+ </Sequence>
+ <Echo name="No Obstacle" message="Path clear" />
+ </Selector>
+ <PerformTask name="Position A Task" task_name="Check Device Status" task_duration="2" />
+ </Sequence>
+ <!-- Move to position B -->
+ <Sequence name="Move to Position B">
+ <Delay name="Short Wait" sec="1">
+ <Echo name="Prepare Movement" message="Preparing to move to next position" />
+ </Delay>
+ <MoveToPosition name="Move to B" position="B" move_duration="3" />
+ <!-- Handle obstacles with timeout -->
+ <Timeout name="Limited Time Obstacle Handling" sec="2">
+ <Sequence name="Obstacle Present">
+ <DetectObstacle name="Detect Obstacle" obstacle_probability="0.4" />
+ <AvoidObstacle name="Avoid Obstacle" avoid_duration="1.8" />
+ </Sequence>
+ </Timeout>
+ <PerformTask name="Position B Task" task_name="Data Collection" task_duration="2.5" />
+ </Sequence>
+ <!-- Move to position C -->
+ <WhileDoElse name="Conditional Move to C">
+ <CheckBattery name="Check Sufficient Battery" threshold="50" />
+ <Sequence name="Perform Position C Task">
+ <MoveToPosition name="Move to C" position="C" move_duration="2.5" />
+ <ForceSuccess name="Ensure Completion">
+ <PerformTask name="Position C Task" task_name="Environment Monitoring"
+ task_duration="2" />
+ </ForceSuccess>
+ </Sequence>
+ <Echo name="Skip Position C" message="Insufficient power, skipping position C task" />
+ </WhileDoElse>
+ <Echo name="Complete Patrol" message="Patrol task completed, returning to charging station" />
+ <MoveToPosition name="Return to Charging Station" position="Charging Station"
+ move_duration="4" />
+ </Sequence>
+</Selector>
\ No newline at end of file
diff --git a/tests/test_behavior_tree.py b/tests/test_behavior_tree.py
new file mode 100644
index 0000000000..0898690448
--- /dev/null
+++ b/tests/test_behavior_tree.py
@@ -0,0 +1,207 @@
+import pytest
+import conftest
+
+from MissionPlanning.BehaviorTree.behavior_tree import (
+ BehaviorTreeFactory,
+ Status,
+ ActionNode,
+)
+
+
+def test_sequence_node1():
+ xml_string = """
+ <Sequence>
+ <Echo name="Echo 1" message="Hello, World1!" />
+ <Echo name="Echo 2" message="Hello, World2!" />
+ <ForceFailure name="Force Failure">
+ <Echo name="Echo 3" message="Hello, World3!" />
+ </ForceFailure>
+ </Sequence>
+ """
+ bt_factory = BehaviorTreeFactory()
+ bt = bt_factory.build_tree(xml_string)
+ bt.tick()
+ assert bt.root.status == Status.RUNNING
+ assert bt.root.children[0].status == Status.SUCCESS
+ assert bt.root.children[1].status is None
+ assert bt.root.children[2].status is None
+ bt.tick()
+ bt.tick()
+ assert bt.root.status == Status.FAILURE
+ assert bt.root.children[0].status is None
+ assert bt.root.children[1].status is None
+ assert bt.root.children[2].status is None
+
+
+def test_sequence_node2():
+ xml_string = """
+ <Sequence>
+ <Echo name="Echo 1" message="Hello, World1!" />
+ <Echo name="Echo 2" message="Hello, World2!" />
+ <ForceSuccess name="Force Success">
+ <Echo name="Echo 3" message="Hello, World3!" />
+ </ForceSuccess>
+ </Sequence>
+ """
+ bt_factory = BehaviorTreeFactory()
+ bt = bt_factory.build_tree(xml_string)
+ bt.tick_while_running()
+ assert bt.root.status == Status.SUCCESS
+ assert bt.root.children[0].status is None
+ assert bt.root.children[1].status is None
+ assert bt.root.children[2].status is None
+
+
+def test_selector_node1():
+ xml_string = """
+ <Selector>
+ <ForceFailure name="Force Failure">
+ <Echo name="Echo 1" message="Hello, World1!" />
+ </ForceFailure>
+ <Echo name="Echo 2" message="Hello, World2!" />
+ <Echo name="Echo 3" message="Hello, World3!" />
+ </Selector>
+ """
+ bt_factory = BehaviorTreeFactory()
+ bt = bt_factory.build_tree(xml_string)
+ bt.tick()
+ assert bt.root.status == Status.RUNNING
+ assert bt.root.children[0].status == Status.FAILURE
+ assert bt.root.children[1].status is None
+ assert bt.root.children[2].status is None
+ bt.tick()
+ assert bt.root.status == Status.SUCCESS
+ assert bt.root.children[0].status is None
+ assert bt.root.children[1].status is None
+ assert bt.root.children[2].status is None
+
+
+def test_selector_node2():
+ xml_string = """
+ <Selector>
+ <ForceFailure name="Force Success">
+ <Echo name="Echo 1" message="Hello, World1!" />
+ </ForceFailure>
+ <ForceFailure name="Force Failure">
+ <Echo name="Echo 2" message="Hello, World2!" />
+ </ForceFailure>
+ </Selector>
+ """
+ bt_factory = BehaviorTreeFactory()
+ bt = bt_factory.build_tree(xml_string)
+ bt.tick_while_running()
+ assert bt.root.status == Status.FAILURE
+ assert bt.root.children[0].status is None
+ assert bt.root.children[1].status is None
+
+
+def test_while_do_else_node():
+ xml_string = """
+ <WhileDoElse>
+ <Count name="Count" count_threshold="3" />
+ <Echo name="Echo 1" message="Hello, World1!" />
+ <Echo name="Echo 2" message="Hello, World2!" />
+ </WhileDoElse>
+ """
+
+ class CountNode(ActionNode):
+ def __init__(self, name, count_threshold):
+ super().__init__(name)
+ self.count = 0
+ self.count_threshold = count_threshold
+
+ def tick(self):
+ self.count += 1
+ if self.count >= self.count_threshold:
+ return Status.FAILURE
+ else:
+ return Status.SUCCESS
+
+ bt_factory = BehaviorTreeFactory()
+ bt_factory.register_node_builder(
+ "Count",
+ lambda node: CountNode(
+ node.attrib.get("name", CountNode.__name__),
+ int(node.attrib["count_threshold"]),
+ ),
+ )
+ bt = bt_factory.build_tree(xml_string)
+ bt.tick()
+ assert bt.root.status == Status.RUNNING
+ assert bt.root.children[0].status == Status.SUCCESS
+ assert bt.root.children[1].status is Status.SUCCESS
+ assert bt.root.children[2].status is None
+ bt.tick()
+ assert bt.root.status == Status.RUNNING
+ assert bt.root.children[0].status == Status.SUCCESS
+ assert bt.root.children[1].status is Status.SUCCESS
+ assert bt.root.children[2].status is None
+ bt.tick()
+ assert bt.root.status == Status.SUCCESS
+ assert bt.root.children[0].status is None
+ assert bt.root.children[1].status is None
+ assert bt.root.children[2].status is None
+
+
+def test_node_children():
+ # ControlNode Must have children
+ xml_string = """
+ <Sequence>
+ </Sequence>
+ """
+ bt_factory = BehaviorTreeFactory()
+ with pytest.raises(ValueError):
+ bt_factory.build_tree(xml_string)
+
+ # DecoratorNode Must have child
+ xml_string = """
+ <Inverter>
+ </Inverter>
+ """
+ with pytest.raises(ValueError):
+ bt_factory.build_tree(xml_string)
+
+ # DecoratorNode Must have only one child
+ xml_string = """
+ <Inverter>
+ <Echo name="Echo 1" message="Hello, World1!" />
+ <Echo name="Echo 2" message="Hello, World2!" />
+ </Inverter>
+ """
+ with pytest.raises(ValueError):
+ bt_factory.build_tree(xml_string)
+
+ # ActionNode Must have no children
+ xml_string = """
+ <Echo name="Echo 1" message="Hello, World1!">
+ <Echo name="Echo 2" message="Hello, World2!" />
+ </Echo>
+ """
+ with pytest.raises(ValueError):
+ bt_factory.build_tree(xml_string)
+
+ # WhileDoElse Must have exactly 2 or 3 children
+ xml_string = """
+ <WhileDoElse>
+ <Echo name="Echo 1" message="Hello, World1!" />
+ </WhileDoElse>
+ """
+ with pytest.raises(ValueError):
+ bt = bt_factory.build_tree(xml_string)
+ bt.tick()
+
+ xml_string = """
+ <WhileDoElse>
+ <Echo name="Echo 1" message="Hello, World1!" />
+ <Echo name="Echo 2" message="Hello, World2!" />
+ <Echo name="Echo 3" message="Hello, World3!" />
+ <Echo name="Echo 4" message="Hello, World4!" />
+ </WhileDoElse>
+ """
+ with pytest.raises(ValueError):
+ bt = bt_factory.build_tree(xml_string)
+ bt.tick()
+
+
+if __name__ == "__main__":
+ conftest.run_this_test(__file__)
From 596a80ed9426ff962bafb2863f024693b8fa81f2 Mon Sep 17 00:00:00 2001
From: Aglargil <1252223935@qq.com>
Date: Wed, 5 Mar 2025 00:34:38 +0800
Subject: [PATCH 3/4] feat: add behavior tree doc
---
MissionPlanning/BehaviorTree/behavior_tree.py | 133 +++++++++++-------
.../behavior_tree/behavior_tree_main.rst | 102 ++++++++++++++
.../behavior_tree/robot_behavior_case.svg | 22 +++
.../mission_planning_main.rst | 1 +
4 files changed, 206 insertions(+), 52 deletions(-)
create mode 100644 docs/modules/13_mission_planning/behavior_tree/behavior_tree_main.rst
create mode 100644 docs/modules/13_mission_planning/behavior_tree/robot_behavior_case.svg
diff --git a/MissionPlanning/BehaviorTree/behavior_tree.py b/MissionPlanning/BehaviorTree/behavior_tree.py
index 76c8cfa53e..f796f25d69 100644
--- a/MissionPlanning/BehaviorTree/behavior_tree.py
+++ b/MissionPlanning/BehaviorTree/behavior_tree.py
@@ -92,15 +92,17 @@ class SequenceNode(ControlNode):
Executes child nodes in sequence until one fails or all succeed.
Returns:
- - Returns FAILURE if any child returns FAILURE
- - Returns SUCCESS when all children have succeeded
- - Returns RUNNING when a child is still running or when moving to the next child
+ - Returns FAILURE if any child returns FAILURE
+ - Returns SUCCESS when all children have succeeded
+ - Returns RUNNING when a child is still running or when moving to the next child
Example:
- <Sequence>
- <Action1 />
- <Action2 />
- </Sequence>
+ .. code-block:: xml
+
+ <Sequence>
+ <Action1 />
+ <Action2 />
+ </Sequence>
"""
def __init__(self, name):
@@ -131,15 +133,17 @@ class SelectorNode(ControlNode):
Executes child nodes in sequence until one succeeds or all fail.
Returns:
- - Returns SUCCESS if any child returns SUCCESS
- - Returns FAILURE when all children have failed
- - Returns RUNNING when a child is still running or when moving to the next child
+ - Returns SUCCESS if any child returns SUCCESS
+ - Returns FAILURE when all children have failed
+ - Returns RUNNING when a child is still running or when moving to the next child
- Example:
- <Selector>
- <Action1 />
- <Action2 />
- </Selector>
+ Examples:
+ .. code-block:: xml
+
+ <Selector>
+ <Action1 />
+ <Action2 />
+ </Selector>
"""
def __init__(self, name):
@@ -170,17 +174,19 @@ class WhileDoElseNode(ControlNode):
Conditional execution node with three parts: condition, do, and optional else.
Returns:
- - First executes the condition node (child[0])
- - If condition succeeds, executes do node (child[1]) and returns RUNNING
- - If condition fails, executes else node (child[2]) if present and returns result of else node
- - If condition fails and there is no else node, returns SUCCESS
+ First executes the condition node (child[0])
+ If condition succeeds, executes do node (child[1]) and returns RUNNING
+ If condition fails, executes else node (child[2]) if present and returns result of else node
+ If condition fails and there is no else node, returns SUCCESS
Example:
- <WhileDoElse>
- <Condition />
- <Do />
- <Else />
- </WhileDoElse>
+ .. code-block:: xml
+
+ <WhileDoElse>
+ <Condition />
+ <Do />
+ <Else />
+ </WhileDoElse>
"""
def __init__(self, name):
@@ -236,11 +242,13 @@ class SleepNode(ActionNode):
Sleep node that sleeps for a specified duration.
Returns:
- - Returns SUCCESS after the specified duration has passed
- - Returns RUNNING if the duration has not yet passed
+ Returns SUCCESS after the specified duration has passed
+ Returns RUNNING if the duration has not yet passed
Example:
- <Sleep sec="1.5" />
+ .. code-block:: xml
+
+ <Sleep sec="1.5" />
"""
def __init__(self, name, duration):
@@ -261,10 +269,12 @@ class EchoNode(ActionNode):
Echo node that prints a message to the console.
Returns:
- - Returns SUCCESS after the message has been printed
+ Returns SUCCESS after the message has been printed
Example:
- <Echo message="Hello, World!" />
+ .. code-block:: xml
+
+ <Echo message="Hello, World!" />
"""
def __init__(self, name, message):
@@ -302,13 +312,16 @@ class InverterNode(DecoratorNode):
Inverter node that inverts the status of its child node.
Returns:
- - Returns SUCCESS if the child returns FAILURE
- - Returns FAILURE if the child returns SUCCESS
- - Returns RUNNING if the child returns RUNNING
- Example:
- <Inverter>
- <Action />
- </Inverter>
+ - Returns SUCCESS if the child returns FAILURE
+ - Returns FAILURE if the child returns SUCCESS
+ - Returns RUNNING if the child returns RUNNING
+
+ Examples:
+ .. code-block:: xml
+
+ <Inverter>
+ <Action />
+ </Inverter>
"""
def __init__(self, name):
@@ -325,13 +338,15 @@ class TimeoutNode(DecoratorNode):
Timeout node that fails if the child node takes too long to execute
Returns:
- - FAILURE: If the timeout duration has been exceeded
- - Child's status: Otherwise, passes through the status of the child node
+ - FAILURE: If the timeout duration has been exceeded
+ - Child's status: Otherwise, passes through the status of the child node
Example:
- <Timeout sec="1.5">
- <Action />
- </Timeout>
+ .. code-block:: xml
+
+ <Timeout sec="1.5">
+ <Action />
+ </Timeout>
"""
def __init__(self, name, timeout):
@@ -354,13 +369,15 @@ class DelayNode(DecoratorNode):
Delay node that delays the execution of its child node for a specified duration.
Returns:
- - Returns RUNNING if the duration has not yet passed
- - Returns child's status after the duration has passed
+ - Returns RUNNING if the duration has not yet passed
+ - Returns child's status after the duration has passed
Example:
- <Delay sec="1.5">
- <Action />
- </Delay>
+ .. code-block:: xml
+
+ <Delay sec="1.5">
+ <Action />
+ </Delay>
"""
def __init__(self, name, delay):
@@ -382,8 +399,8 @@ class ForceSuccessNode(DecoratorNode):
ForceSuccess node that always returns SUCCESS.
Returns:
- - Returns RUNNING if the child returns RUNNING
- - Returns SUCCESS if the child returns SUCCESS or FAILURE
+ - Returns RUNNING if the child returns RUNNING
+ - Returns SUCCESS if the child returns SUCCESS or FAILURE
"""
def __init__(self, name):
@@ -402,8 +419,8 @@ class ForceFailureNode(DecoratorNode):
ForceFailure node that always returns FAILURE.
Returns:
- - Returns RUNNING if the child returns RUNNING
- - Returns FAILURE if the child returns SUCCESS or FAILURE
+ - Returns RUNNING if the child returns RUNNING
+ - Returns FAILURE if the child returns SUCCESS or FAILURE
"""
def __init__(self, name):
@@ -484,12 +501,12 @@ def print_tree(self):
"""
Print the behavior tree.
- Node types:
+ Node print format:
Action: <Action>
Decorator: (Decorator)
Control: [Control]
- Colors:
+ Node status colors:
Yellow: RUNNING
Green: SUCCESS
Red: FAILURE
@@ -577,6 +594,18 @@ def register_node_builder(self, node_name, builder):
Args:
node_name (str): The name of the node.
builder (function): The builder function.
+
+ Example:
+ .. code-block:: python
+
+ factory = BehaviorTreeFactory()
+ factory.register_node_builder(
+ "MyNode",
+ lambda node: MyNode(
+ node.attrib.get("name", MyNode.__name__),
+ node.attrib["my_param"],
+ ),
+ )
"""
self.node_builders[node_name] = builder
diff --git a/docs/modules/13_mission_planning/behavior_tree/behavior_tree_main.rst b/docs/modules/13_mission_planning/behavior_tree/behavior_tree_main.rst
new file mode 100644
index 0000000000..0096ccbc7e
--- /dev/null
+++ b/docs/modules/13_mission_planning/behavior_tree/behavior_tree_main.rst
@@ -0,0 +1,102 @@
+Behavior Tree
+-------------
+
+Behavior Tree is a modular, hierarchical decision model that is widely used in robot control, and game development.
+
+Core Concepts
+~~~~~~~~~~~~~
+
+Control Node
+++++++++++++
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.ControlNode
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.SequenceNode
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.SelectorNode
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.WhileDoElseNode
+
+Action Node
+++++++++++++
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.ActionNode
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.EchoNode
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.SleepNode
+
+Decorator Node
+++++++++++++++
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.DecoratorNode
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.InverterNode
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.TimeoutNode
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.DelayNode
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.ForceSuccessNode
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.ForceFailureNode
+
+Behavior Tree Factory
++++++++++++++++++++++
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.BehaviorTreeFactory
+ :members:
+
+Behavior Tree
++++++++++++++
+
+.. autoclass:: MissionPlanning.BehaviorTree.behavior_tree.BehaviorTree
+ :members:
+
+Example
+~~~~~~~
+
+Visualize the behavior tree by `xml-tree-visual <https://xml-tree-visual.vercel.app/>`_.
+
+.. image:: ./robot_behavior_case.svg
+
+Print the behavior tree
+
+.. code-block:: text
+
+ Behavior Tree
+ [Robot Main Controller]
+ [Battery Management]
+ (Low Battery Detection)
+ <Check Battery>
+ <Low Battery Warning>
+ <Charge Battery>
+ [Patrol Task]
+ <Start Task>
+ [Move to Position A]
+ <Move to A>
+ [Obstacle Handling A]
+ [Obstacle Present]
+ <Detect Obstacle>
+ <Avoid Obstacle>
+ <No Obstacle>
+ <Position A Task>
+ [Move to Position B]
+ (Short Wait)
+ <Prepare Movement>
+ <Move to B>
+ (Limited Time Obstacle Handling)
+ [Obstacle Present]
+ <Detect Obstacle>
+ <Avoid Obstacle>
+ <Position B Task>
+ [Conditional Move to C]
+ <Check Sufficient Battery>
+ [Perform Position C Task]
+ <Move to C>
+ (Ensure Completion)
+ <Position C Task>
+ <Skip Position C>
+ <Complete Patrol>
+ <Return to Charging Station>
+ Behavior Tree
diff --git a/docs/modules/13_mission_planning/behavior_tree/robot_behavior_case.svg b/docs/modules/13_mission_planning/behavior_tree/robot_behavior_case.svg
new file mode 100644
index 0000000000..a3d43aed52
--- /dev/null
+++ b/docs/modules/13_mission_planning/behavior_tree/robot_behavior_case.svg
@@ -0,0 +1,22 @@
+<svg width="2870" height="795" id="tree-svg" style="overflow: visible;" xmlns="http://www.w3.org/2000/svg" viewBox="-1560 -65 4380 810"><style xmlns="http://www.w3.org/1999/xhtml">
+ .node rect {
+ fill: #fff;
+ stroke-width: 1px;
+ }
+ .node text {
+ font: 12px sans-serif;
+ }
+ .node-attributes {
+ font: 10px sans-serif;
+ fill: #666;
+ }
+ .link {
+ fill: none;
+ stroke: #999;
+ stroke-width: 1px;
+ }
+ line {
+ stroke: #444;
+ stroke-width: 2px;
+ }
+ </style><g transform="translate(75,112) scale(0.8)"><path class="link" d="M0,0C0,60,-1125,60,-1125,120"/><path class="link" d="M0,0C0,60,1125,60,1125,120"/><path class="link" d="M-1125,120C-1125,180,-1395,180,-1395,240"/><path class="link" d="M-1125,120C-1125,180,-1125,180,-1125,240"/><path class="link" d="M-1125,120C-1125,180,-855,180,-855,240"/><path class="link" d="M1125,120C1125,180,-405,180,-405,240"/><path class="link" d="M1125,120C1125,180,-135,180,-135,240"/><path class="link" d="M1125,120C1125,180,990,180,990,240"/><path class="link" d="M1125,120C1125,180,2115,180,2115,240"/><path class="link" d="M1125,120C1125,180,2385,180,2385,240"/><path class="link" d="M1125,120C1125,180,2655,180,2655,240"/><path class="link" d="M-1395,240C-1395,300,-1395,300,-1395,360"/><path class="link" d="M-135,240C-135,300,-405,300,-405,360"/><path class="link" d="M-135,240C-135,300,-135,300,-135,360"/><path class="link" d="M-135,240C-135,300,135,300,135,360"/><path class="link" d="M990,240C990,300,585,300,585,360"/><path class="link" d="M990,240C990,300,855,300,855,360"/><path class="link" d="M990,240C990,300,1125,300,1125,360"/><path class="link" d="M990,240C990,300,1395,300,1395,360"/><path class="link" d="M2115,240C2115,300,1845,300,1845,360"/><path class="link" d="M2115,240C2115,300,2115,300,2115,360"/><path class="link" d="M2115,240C2115,300,2385,300,2385,360"/><path class="link" d="M-135,360C-135,420,-270,420,-270,480"/><path class="link" d="M-135,360C-135,420,0,420,0,480"/><path class="link" d="M585,360C585,420,585,420,585,480"/><path class="link" d="M1125,360C1125,420,1125,420,1125,480"/><path class="link" d="M2115,360C2115,420,1980,420,1980,480"/><path class="link" d="M2115,360C2115,420,2250,420,2250,480"/><path class="link" d="M-270,480C-270,540,-405,540,-405,600"/><path class="link" d="M-270,480C-270,540,-135,540,-135,600"/><path class="link" d="M1125,480C1125,540,990,540,990,600"/><path class="link" d="M1125,480C1125,540,1260,540,1260,600"/><path class="link" d="M2250,480C2250,540,2250,540,2250,600"/><g class="node" transform="translate(0,0)"><rect x="-115" y="-15" width="230" height="50" stroke="#4A90E2" style="stroke: rgb(74, 144, 226);"/><text dy="5" text-anchor="middle">Selector</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Robot Main Controller</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(-1125,120)"><rect x="-115" y="-15" width="230" height="50" stroke="#4A90E2" style="stroke: rgb(74, 144, 226);"/><text dy="5" text-anchor="middle">Sequence</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Battery Management</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(1125,120)"><rect x="-115" y="-15" width="230" height="50" stroke="#4A90E2" style="stroke: rgb(74, 144, 226);"/><text dy="5" text-anchor="middle">Sequence</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Patrol Task</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(-1395,240)"><rect x="-115" y="-15" width="230" height="50" stroke="#F5A623" style="stroke: rgb(245, 166, 35);"/><text dy="5" text-anchor="middle">Inverter</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Low Battery Detection</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(-1125,240)"><rect x="-115" y="-15" width="230" height="90" stroke="#D0021B" style="stroke: rgb(208, 2, 27);"/><text dy="5" text-anchor="middle">Echo</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Low Battery Warning</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">message: Battery level low!</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="65" x="-105" text-anchor="start">Charging needed</text><line x1="-105" y1="70" x2="105" y2="70" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(-855,240)"><rect x="-115" y="-15" width="230" height="70" stroke="#90EE90" style="stroke: rgb(144, 238, 144);"/><text dy="5" text-anchor="middle">ChargeBattery</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Charge Battery</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">charge_rate: 20</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(-405,240)"><rect x="-115" y="-15" width="230" height="70" stroke="#D0021B" style="stroke: rgb(208, 2, 27);"/><text dy="5" text-anchor="middle">Echo</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Start Task</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">message: Starting patrol task</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(-135,240)"><rect x="-115" y="-15" width="230" height="50" stroke="#4A90E2" style="stroke: rgb(74, 144, 226);"/><text dy="5" text-anchor="middle">Sequence</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Move to Position A</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(990,240)"><rect x="-115" y="-15" width="230" height="50" stroke="#4A90E2" style="stroke: rgb(74, 144, 226);"/><text dy="5" text-anchor="middle">Sequence</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Move to Position B</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(2115,240)"><rect x="-115" y="-15" width="230" height="50" stroke="#4A90E2" style="stroke: rgb(74, 144, 226);"/><text dy="5" text-anchor="middle">WhileDoElse</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Conditional Move to C</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(2385,240)"><rect x="-115" y="-15" width="230" height="110" stroke="#D0021B" style="stroke: rgb(208, 2, 27);"/><text dy="5" text-anchor="middle">Echo</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Complete Patrol</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">message: Patrol task</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="65" x="-105" text-anchor="start">completed, returning to</text><line x1="-105" y1="70" x2="105" y2="70" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="85" x="-105" text-anchor="start">charging station</text><line x1="-105" y1="90" x2="105" y2="90" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(2655,240)"><rect x="-115" y="-15" width="230" height="110" stroke="#90EE90" style="stroke: rgb(144, 238, 144);"/><text dy="5" text-anchor="middle">MoveToPosition</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Return to Charging</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">Station</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="65" x="-105" text-anchor="start">position: Charging Station</text><line x1="-105" y1="70" x2="105" y2="70" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="85" x="-105" text-anchor="start">move_duration: 4</text><line x1="-105" y1="90" x2="105" y2="90" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(-1395,360)"><rect x="-115" y="-15" width="230" height="70" stroke="#90EE90" style="stroke: rgb(144, 238, 144);"/><text dy="5" text-anchor="middle">CheckBattery</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Check Battery</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">threshold: 30</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(-405,360)"><rect x="-115" y="-15" width="230" height="90" stroke="#90EE90" style="stroke: rgb(144, 238, 144);"/><text dy="5" text-anchor="middle">MoveToPosition</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Move to A</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">position: A</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="65" x="-105" text-anchor="start">move_duration: 2</text><line x1="-105" y1="70" x2="105" y2="70" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(-135,360)"><rect x="-115" y="-15" width="230" height="50" stroke="#4A90E2" style="stroke: rgb(74, 144, 226);"/><text dy="5" text-anchor="middle">Selector</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Obstacle Handling A</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(135,360)"><rect x="-115" y="-15" width="230" height="90" stroke="#90EE90" style="stroke: rgb(144, 238, 144);"/><text dy="5" text-anchor="middle">PerformTask</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Position A Task</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">task_name: Check Device Status</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="65" x="-105" text-anchor="start">task_duration: 2</text><line x1="-105" y1="70" x2="105" y2="70" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(585,360)"><rect x="-115" y="-15" width="230" height="70" stroke="#F5A623" style="stroke: rgb(245, 166, 35);"/><text dy="5" text-anchor="middle">Delay</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Short Wait</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">sec: 1</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(855,360)"><rect x="-115" y="-15" width="230" height="90" stroke="#90EE90" style="stroke: rgb(144, 238, 144);"/><text dy="5" text-anchor="middle">MoveToPosition</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Move to B</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">position: B</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="65" x="-105" text-anchor="start">move_duration: 3</text><line x1="-105" y1="70" x2="105" y2="70" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(1125,360)"><rect x="-115" y="-15" width="230" height="90" stroke="#F5A623" style="stroke: rgb(245, 166, 35);"/><text dy="5" text-anchor="middle">Timeout</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Limited Time Obstacle</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">Handling</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="65" x="-105" text-anchor="start">sec: 2</text><line x1="-105" y1="70" x2="105" y2="70" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(1395,360)"><rect x="-115" y="-15" width="230" height="90" stroke="#90EE90" style="stroke: rgb(144, 238, 144);"/><text dy="5" text-anchor="middle">PerformTask</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Position B Task</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">task_name: Data Collection</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="65" x="-105" text-anchor="start">task_duration: 2.5</text><line x1="-105" y1="70" x2="105" y2="70" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(1845,360)"><rect x="-115" y="-15" width="230" height="70" stroke="#90EE90" style="stroke: rgb(144, 238, 144);"/><text dy="5" text-anchor="middle">CheckBattery</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Check Sufficient Battery</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">threshold: 50</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(2115,360)"><rect x="-115" y="-15" width="230" height="50" stroke="#4A90E2" style="stroke: rgb(74, 144, 226);"/><text dy="5" text-anchor="middle">Sequence</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Perform Position C Task</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(2385,360)"><rect x="-115" y="-15" width="230" height="90" stroke="#D0021B" style="stroke: rgb(208, 2, 27);"/><text dy="5" text-anchor="middle">Echo</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Skip Position C</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">message: Insufficient power,</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="65" x="-105" text-anchor="start">skipping position C task</text><line x1="-105" y1="70" x2="105" y2="70" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(-270,480)"><rect x="-115" y="-15" width="230" height="50" stroke="#4A90E2" style="stroke: rgb(74, 144, 226);"/><text dy="5" text-anchor="middle">Sequence</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Obstacle Present</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(0,480)"><rect x="-115" y="-15" width="230" height="70" stroke="#D0021B" style="stroke: rgb(208, 2, 27);"/><text dy="5" text-anchor="middle">Echo</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: No Obstacle</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">message: Path clear</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(585,480)"><rect x="-115" y="-15" width="230" height="90" stroke="#D0021B" style="stroke: rgb(208, 2, 27);"/><text dy="5" text-anchor="middle">Echo</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Prepare Movement</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">message: Preparing to move to</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="65" x="-105" text-anchor="start">next position</text><line x1="-105" y1="70" x2="105" y2="70" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(1125,480)"><rect x="-115" y="-15" width="230" height="50" stroke="#4A90E2" style="stroke: rgb(74, 144, 226);"/><text dy="5" text-anchor="middle">Sequence</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Obstacle Present</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(1980,480)"><rect x="-115" y="-15" width="230" height="90" stroke="#90EE90" style="stroke: rgb(144, 238, 144);"/><text dy="5" text-anchor="middle">MoveToPosition</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Move to C</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">position: C</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="65" x="-105" text-anchor="start">move_duration: 2.5</text><line x1="-105" y1="70" x2="105" y2="70" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(2250,480)"><rect x="-115" y="-15" width="230" height="50" stroke="#F5A623" style="stroke: rgb(245, 166, 35);"/><text dy="5" text-anchor="middle">ForceSuccess</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Ensure Completion</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(-405,600)"><rect x="-115" y="-15" width="230" height="70" stroke="#90EE90" style="stroke: rgb(144, 238, 144);"/><text dy="5" text-anchor="middle">DetectObstacle</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Detect Obstacle</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">obstacle_probability: 0.3</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(-135,600)"><rect x="-115" y="-15" width="230" height="70" stroke="#90EE90" style="stroke: rgb(144, 238, 144);"/><text dy="5" text-anchor="middle">AvoidObstacle</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Avoid Obstacle</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">avoid_duration: 1.5</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(990,600)"><rect x="-115" y="-15" width="230" height="70" stroke="#90EE90" style="stroke: rgb(144, 238, 144);"/><text dy="5" text-anchor="middle">DetectObstacle</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Detect Obstacle</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">obstacle_probability: 0.4</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(1260,600)"><rect x="-115" y="-15" width="230" height="70" stroke="#90EE90" style="stroke: rgb(144, 238, 144);"/><text dy="5" text-anchor="middle">AvoidObstacle</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Avoid Obstacle</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">avoid_duration: 1.8</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/></g><g class="node" transform="translate(2250,600)"><rect x="-115" y="-15" width="230" height="110" stroke="#90EE90" style="stroke: rgb(144, 238, 144);"/><text dy="5" text-anchor="middle">PerformTask</text><line x1="-105" y1="10" x2="105" y2="10" stroke="#444" stroke-width="2"/><text class="node-attributes" dy="25" x="-105" text-anchor="start">name: Position C Task</text><line x1="-105" y1="30" x2="105" y2="30" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="45" x="-105" text-anchor="start">task_name: Environment</text><line x1="-105" y1="50" x2="105" y2="50" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="65" x="-105" text-anchor="start">Monitoring</text><line x1="-105" y1="70" x2="105" y2="70" stroke="#ddd" stroke-width="1"/><text class="node-attributes" dy="85" x="-105" text-anchor="start">task_duration: 2</text><line x1="-105" y1="90" x2="105" y2="90" stroke="#ddd" stroke-width="1"/></g></g></svg>
\ No newline at end of file
diff --git a/docs/modules/13_mission_planning/mission_planning_main.rst b/docs/modules/13_mission_planning/mission_planning_main.rst
index 385e62f68e..c35eacd8d5 100644
--- a/docs/modules/13_mission_planning/mission_planning_main.rst
+++ b/docs/modules/13_mission_planning/mission_planning_main.rst
@@ -10,3 +10,4 @@ Mission planning includes tools such as finite state machines and behavior trees
:caption: Contents
state_machine/state_machine
+ behavior_tree/behavior_tree
From 0bf975d7db3f9753696cd8359dddc66d3508280b Mon Sep 17 00:00:00 2001
From: Aglargil <1252223935@qq.com>
Date: Sat, 8 Mar 2025 17:58:37 +0800
Subject: [PATCH 4/4] feat: add behavior tree update
---
MissionPlanning/BehaviorTree/behavior_tree.py | 5 +++--
.../13_mission_planning/behavior_tree/behavior_tree_main.rst | 4 +++-
2 files changed, 6 insertions(+), 3 deletions(-)
diff --git a/MissionPlanning/BehaviorTree/behavior_tree.py b/MissionPlanning/BehaviorTree/behavior_tree.py
index f796f25d69..59f4c713f1 100644
--- a/MissionPlanning/BehaviorTree/behavior_tree.py
+++ b/MissionPlanning/BehaviorTree/behavior_tree.py
@@ -10,15 +10,16 @@
import time
import xml.etree.ElementTree as ET
+from enum import Enum
-class Status:
+class Status(Enum):
SUCCESS = "success"
FAILURE = "failure"
RUNNING = "running"
-class NodeType:
+class NodeType(Enum):
CONTROL_NODE = "ControlNode"
ACTION_NODE = "ActionNode"
DECORATOR_NODE = "DecoratorNode"
diff --git a/docs/modules/13_mission_planning/behavior_tree/behavior_tree_main.rst b/docs/modules/13_mission_planning/behavior_tree/behavior_tree_main.rst
index 0096ccbc7e..ae3e16da81 100644
--- a/docs/modules/13_mission_planning/behavior_tree/behavior_tree_main.rst
+++ b/docs/modules/13_mission_planning/behavior_tree/behavior_tree_main.rst
@@ -1,7 +1,9 @@
Behavior Tree
-------------
-Behavior Tree is a modular, hierarchical decision model that is widely used in robot control, and game development.
+Behavior Tree is a modular, hierarchical decision model that is widely used in robot control, and game development.
+It present some similarities to hierarchical state machines with the key difference that the main building block of a behavior is a task rather than a state.
+Behavior Tree have been shown to generalize several other control architectures (https://ieeexplore.ieee.org/document/7790863)
Core Concepts
~~~~~~~~~~~~~