427 lines
15 KiB
Python
427 lines
15 KiB
Python
"""
|
|
行为树叶子节点
|
|
包括条件节点、动作节点、等待节点等
|
|
提供多种叶子节点实现
|
|
"""
|
|
|
|
import time
|
|
import random
|
|
import math
|
|
from typing import Optional, Callable, Any, Union
|
|
from ..core.behavior_tree import BTNode, NodeStatus, BTNodeConfig
|
|
|
|
class ConditionNode(BTNode):
|
|
"""
|
|
条件节点
|
|
检查条件是否满足
|
|
支持多种条件检查方式和缓存机制
|
|
"""
|
|
|
|
def __init__(self, config: Union[BTNodeConfig, str] = "Condition",
|
|
condition_func: Optional[Callable[[Any], bool]] = None,
|
|
cache_result: bool = False, # 是否缓存结果
|
|
cache_duration: float = 0.0): # 缓存持续时间（秒）
|
|
super().__init__(config)
|
|
self.condition_func = condition_func
|
|
self.cache_result = cache_result
|
|
self.cache_duration = cache_duration
|
|
self.cached_result: Optional[bool] = None
|
|
self.cache_time: float = 0.0
|
|
|
|
def execute(self, blackboard) -> NodeStatus:
|
|
"""执行条件检查"""
|
|
current_time = time.time()
|
|
|
|
# 检查缓存
|
|
if (self.cache_result and
|
|
self.cached_result is not None and
|
|
(self.cache_duration <= 0 or
|
|
current_time - self.cache_time < self.cache_duration)):
|
|
# 使用缓存结果
|
|
return NodeStatus.SUCCESS if self.cached_result else NodeStatus.FAILURE
|
|
|
|
# 执行条件检查
|
|
result = self._evaluate_condition(blackboard)
|
|
|
|
# 更新缓存
|
|
if self.cache_result:
|
|
self.cached_result = result
|
|
self.cache_time = current_time
|
|
|
|
return NodeStatus.SUCCESS if result else NodeStatus.FAILURE
|
|
|
|
def _evaluate_condition(self, blackboard) -> bool:
|
|
"""评估条件"""
|
|
if self.condition_func:
|
|
try:
|
|
return bool(self.condition_func(blackboard))
|
|
except Exception as e:
|
|
print(f"条件节点执行出错: {e}")
|
|
return False
|
|
else:
|
|
# 如果没有提供条件函数，默认返回成功
|
|
return True
|
|
|
|
def invalidate_cache(self) -> None:
|
|
"""使缓存失效"""
|
|
self.cached_result = None
|
|
self.cache_time = 0.0
|
|
|
|
def reset(self) -> None:
|
|
"""重置节点状态"""
|
|
super().reset()
|
|
self.invalidate_cache()
|
|
|
|
class ActionNode(BTNode):
|
|
"""
|
|
动作节点
|
|
执行具体行为
|
|
支持异步执行和进度跟踪
|
|
"""
|
|
|
|
def __init__(self, config: Union[BTNodeConfig, str] = "Action",
|
|
action_func: Optional[Callable[[Any], Any]] = None,
|
|
async_action: bool = False, # 是否异步执行
|
|
progress_callback: Optional[Callable[[float], None]] = None): # 进度回调
|
|
super().__init__(config)
|
|
self.action_func = action_func
|
|
self.async_action = async_action
|
|
self.progress_callback = progress_callback
|
|
self.is_running = False
|
|
self.start_time = 0.0
|
|
self.estimated_duration = 0.0 # 估计执行时间
|
|
|
|
def execute(self, blackboard) -> NodeStatus:
|
|
"""执行动作"""
|
|
current_time = time.time()
|
|
|
|
# 如果是异步动作且正在运行
|
|
if self.async_action and self.is_running:
|
|
# 检查动作是否完成
|
|
if self._is_async_action_complete(blackboard):
|
|
self.is_running = False
|
|
self.start_time = 0.0
|
|
return NodeStatus.SUCCESS
|
|
else:
|
|
# 更新进度
|
|
if self.progress_callback:
|
|
progress = self._calculate_progress(current_time)
|
|
try:
|
|
self.progress_callback(progress)
|
|
except Exception as e:
|
|
print(f"进度回调执行出错: {e}")
|
|
return NodeStatus.RUNNING
|
|
|
|
# 执行动作
|
|
if self.action_func:
|
|
try:
|
|
# 记录开始时间
|
|
self.start_time = current_time
|
|
|
|
if self.async_action:
|
|
# 异步执行
|
|
result = self._start_async_action(blackboard)
|
|
if result == NodeStatus.RUNNING:
|
|
self.is_running = True
|
|
return NodeStatus.RUNNING
|
|
else:
|
|
self.start_time = 0.0
|
|
return result
|
|
else:
|
|
# 同步执行
|
|
result = self.action_func(blackboard)
|
|
self.start_time = 0.0
|
|
|
|
if result is True:
|
|
return NodeStatus.SUCCESS
|
|
elif result is False:
|
|
return NodeStatus.FAILURE
|
|
elif result == NodeStatus.RUNNING:
|
|
return NodeStatus.RUNNING
|
|
else:
|
|
# 默认返回成功
|
|
return NodeStatus.SUCCESS
|
|
|
|
except Exception as e:
|
|
print(f"动作节点执行出错: {e}")
|
|
self.is_running = False
|
|
self.start_time = 0.0
|
|
return NodeStatus.FAILURE
|
|
else:
|
|
# 如果没有提供动作函数，默认返回成功
|
|
return NodeStatus.SUCCESS
|
|
|
|
def _start_async_action(self, blackboard) -> NodeStatus:
|
|
"""启动异步动作"""
|
|
# 这里应该启动异步操作，例如创建线程或任务
|
|
# 为了简化，我们假设异步动作立即开始并需要继续运行
|
|
return NodeStatus.RUNNING
|
|
|
|
def _is_async_action_complete(self, blackboard) -> bool:
|
|
"""检查异步动作是否完成"""
|
|
# 这里应该检查异步操作是否完成
|
|
# 为了简化，我们随机决定是否完成
|
|
if self.estimated_duration > 0:
|
|
current_time = time.time()
|
|
elapsed = current_time - self.start_time
|
|
return elapsed >= self.estimated_duration
|
|
else:
|
|
# 随机完成（用于演示）
|
|
return random.random() < 0.1 # 10%概率完成
|
|
|
|
def _calculate_progress(self, current_time: float) -> float:
|
|
"""计算进度"""
|
|
if self.estimated_duration > 0:
|
|
elapsed = current_time - self.start_time
|
|
progress = min(1.0, elapsed / self.estimated_duration)
|
|
return max(0.0, progress)
|
|
else:
|
|
return 0.0
|
|
|
|
def reset(self) -> None:
|
|
"""重置节点状态"""
|
|
super().reset()
|
|
self.is_running = False
|
|
self.start_time = 0.0
|
|
|
|
class WaitNode(ActionNode):
|
|
"""
|
|
等待节点
|
|
等待指定时间后返回成功
|
|
支持随机等待时间和进度回调
|
|
"""
|
|
|
|
def __init__(self, config: Union[BTNodeConfig, str] = "Wait",
|
|
duration: float = 1.0,
|
|
random_variance: float = 0.0, # 随机变化范围（秒）
|
|
progress_callback: Optional[Callable[[float], None]] = None):
|
|
super().__init__(config, None, False, progress_callback)
|
|
self.base_duration = duration
|
|
self.random_variance = random_variance
|
|
self.actual_duration = duration # 实际等待时间
|
|
self.start_time = 0.0
|
|
|
|
def execute(self, blackboard) -> NodeStatus:
|
|
"""执行等待"""
|
|
current_time = time.time()
|
|
|
|
# 如果是第一次执行，计算实际等待时间
|
|
if self.start_time == 0.0:
|
|
self.start_time = current_time
|
|
# 添加随机变化
|
|
if self.random_variance > 0:
|
|
variance = random.uniform(-self.random_variance, self.random_variance)
|
|
self.actual_duration = max(0.0, self.base_duration + variance)
|
|
else:
|
|
self.actual_duration = self.base_duration
|
|
|
|
# 检查是否等待完成
|
|
elapsed = current_time - self.start_time
|
|
if elapsed >= self.actual_duration:
|
|
# 等待完成，重置开始时间
|
|
self.start_time = 0.0
|
|
return NodeStatus.SUCCESS
|
|
else:
|
|
# 更新进度
|
|
if self.progress_callback:
|
|
progress = min(1.0, elapsed / self.actual_duration)
|
|
try:
|
|
self.progress_callback(progress)
|
|
except Exception as e:
|
|
print(f"进度回调执行出错: {e}")
|
|
return NodeStatus.RUNNING
|
|
|
|
def reset(self) -> None:
|
|
"""重置节点状态"""
|
|
super().reset()
|
|
self.start_time = 0.0
|
|
self.actual_duration = self.base_duration
|
|
|
|
class SubtreeNode(BTNode):
|
|
"""
|
|
子树节点
|
|
执行另一个行为树作为子树
|
|
"""
|
|
|
|
def __init__(self, config: Union[BTNodeConfig, str] = "Subtree",
|
|
subtree: Optional['BehaviorTree'] = None):
|
|
super().__init__(config)
|
|
self.subtree = subtree
|
|
self.is_running = False
|
|
|
|
def execute(self, blackboard) -> NodeStatus:
|
|
"""执行子树"""
|
|
if not self.subtree:
|
|
return NodeStatus.FAILURE
|
|
|
|
# 设置子树的黑板
|
|
self.subtree.set_blackboard(blackboard)
|
|
|
|
# 执行子树
|
|
try:
|
|
result = self.subtree.run()
|
|
return result
|
|
except Exception as e:
|
|
print(f"子树执行出错: {e}")
|
|
return NodeStatus.FAILURE
|
|
|
|
def set_subtree(self, subtree: 'BehaviorTree') -> None:
|
|
"""设置子树"""
|
|
self.subtree = subtree
|
|
|
|
def get_subtree(self) -> Optional['BehaviorTree']:
|
|
"""获取子树"""
|
|
return self.subtree
|
|
|
|
class ProbabilityNode(ActionNode):
|
|
"""
|
|
概率节点
|
|
根据概率决定执行结果
|
|
"""
|
|
|
|
def __init__(self, config: Union[BTNodeConfig, str] = "Probability",
|
|
success_probability: float = 0.5, # 成功概率
|
|
action_func: Optional[Callable[[Any], Any]] = None):
|
|
super().__init__(config, action_func)
|
|
self.success_probability = max(0.0, min(1.0, success_probability))
|
|
self.has_executed = False
|
|
self.result = NodeStatus.FAILURE
|
|
|
|
def execute(self, blackboard) -> NodeStatus:
|
|
"""执行概率动作"""
|
|
# 如果已经执行过，返回之前的结果
|
|
if self.has_executed:
|
|
return self.result
|
|
|
|
# 执行实际动作（如果提供了）
|
|
if self.action_func:
|
|
try:
|
|
action_result = self.action_func(blackboard)
|
|
# 如果动作返回了具体状态，使用该状态
|
|
if action_result == NodeStatus.SUCCESS:
|
|
self.result = NodeStatus.SUCCESS
|
|
elif action_result == NodeStatus.FAILURE:
|
|
self.result = NodeStatus.FAILURE
|
|
elif action_result == NodeStatus.RUNNING:
|
|
return NodeStatus.RUNNING
|
|
except Exception as e:
|
|
print(f"概率节点动作执行出错: {e}")
|
|
self.result = NodeStatus.FAILURE
|
|
else:
|
|
# 根据概率决定结果
|
|
if random.random() < self.success_probability:
|
|
self.result = NodeStatus.SUCCESS
|
|
else:
|
|
self.result = NodeStatus.FAILURE
|
|
|
|
self.has_executed = True
|
|
return self.result
|
|
|
|
def reset(self) -> None:
|
|
"""重置节点状态"""
|
|
super().reset()
|
|
self.has_executed = False
|
|
self.result = NodeStatus.FAILURE
|
|
|
|
class CounterNode(ConditionNode):
|
|
"""
|
|
计数器节点
|
|
根据执行次数决定结果
|
|
"""
|
|
|
|
def __init__(self, config: Union[BTNodeConfig, str] = "Counter",
|
|
target_count: int = 1, # 目标计数
|
|
reset_on_target: bool = True, # 达到目标后是否重置
|
|
condition_func: Optional[Callable[[Any], bool]] = None):
|
|
super().__init__(config, condition_func)
|
|
self.target_count = max(1, target_count)
|
|
self.reset_on_target = reset_on_target
|
|
self.current_count = 0
|
|
|
|
def _evaluate_condition(self, blackboard) -> bool:
|
|
"""评估条件"""
|
|
# 首先检查提供的条件函数
|
|
if self.condition_func:
|
|
try:
|
|
if not self.condition_func(blackboard):
|
|
return False
|
|
except Exception as e:
|
|
print(f"计数器节点条件检查出错: {e}")
|
|
return False
|
|
|
|
# 增加计数
|
|
self.current_count += 1
|
|
|
|
# 检查是否达到目标计数
|
|
result = self.current_count >= self.target_count
|
|
|
|
# 如果达到目标且需要重置，重置计数器
|
|
if result and self.reset_on_target:
|
|
self.current_count = 0
|
|
|
|
return result
|
|
|
|
def reset(self) -> None:
|
|
"""重置节点状态"""
|
|
super().reset()
|
|
self.current_count = 0
|
|
|
|
class TimeoutNode(ActionNode):
|
|
"""
|
|
超时节点
|
|
限制动作执行时间
|
|
"""
|
|
|
|
def __init__(self, config: Union[BTNodeConfig, str] = "Timeout",
|
|
action_func: Optional[Callable[[Any], Any]] = None,
|
|
timeout: float = 5.0): # 超时时间（秒）
|
|
super().__init__(config, action_func)
|
|
self.timeout = timeout
|
|
self.start_time = 0.0
|
|
|
|
def execute(self, blackboard) -> NodeStatus:
|
|
"""执行带超时的动作"""
|
|
current_time = time.time()
|
|
|
|
# 记录开始时间
|
|
if self.start_time == 0.0:
|
|
self.start_time = current_time
|
|
|
|
# 检查是否超时
|
|
if current_time - self.start_time >= self.timeout:
|
|
# 超时，返回失败
|
|
self.start_time = 0.0
|
|
return NodeStatus.FAILURE
|
|
|
|
# 执行动作
|
|
if self.action_func:
|
|
try:
|
|
result = self.action_func(blackboard)
|
|
|
|
# 如果动作完成，重置开始时间
|
|
if result != NodeStatus.RUNNING:
|
|
self.start_time = 0.0
|
|
|
|
if result is True:
|
|
return NodeStatus.SUCCESS
|
|
elif result is False:
|
|
return NodeStatus.FAILURE
|
|
elif result == NodeStatus.RUNNING:
|
|
return NodeStatus.RUNNING
|
|
else:
|
|
# 默认返回成功
|
|
self.start_time = 0.0
|
|
return NodeStatus.SUCCESS
|
|
except Exception as e:
|
|
print(f"超时节点动作执行出错: {e}")
|
|
self.start_time = 0.0
|
|
return NodeStatus.FAILURE
|
|
else:
|
|
# 如果没有提供动作函数，默认返回成功
|
|
self.start_time = 0.0
|
|
return NodeStatus.SUCCESS
|
|
|
|
def reset(self) -> None:
|
|
"""重置节点状态"""
|
|
super().reset()
|
|
self.start_time = 0.0 |