use crate::{BehaviorNode, BoxedBehavior, NodeStatus, TreeStatusSnapshot};
use async_trait::async_trait;
use mecha10_core::config::{controller_mode, ControllerMode};
use mecha10_core::Context;
use rand::rngs::StdRng;
use rand::{RngExt, SeedableRng};
use std::time::{Duration, Instant};
use tracing::{debug, info, warn};
#[derive(Debug)]
pub struct SequenceNode {
children: Vec<BoxedBehavior>,
current_index: usize,
last_status: Option<NodeStatus>,
}
impl SequenceNode {
pub fn new(children: Vec<BoxedBehavior>) -> Self {
Self {
children,
current_index: 0,
last_status: None,
}
}
pub fn add_child(&mut self, child: BoxedBehavior) {
self.children.push(child);
}
pub fn len(&self) -> usize {
self.children.len()
}
pub fn is_empty(&self) -> bool {
self.children.is_empty()
}
}
#[async_trait]
impl BehaviorNode for SequenceNode {
async fn tick(&mut self, ctx: &Context) -> anyhow::Result<NodeStatus> {
if self.children.is_empty() {
self.last_status = Some(NodeStatus::Success);
return Ok(NodeStatus::Success);
}
let total_children = self.children.len();
while self.current_index < total_children {
debug!(
"Sequence: ticking child {} of {}",
self.current_index + 1,
total_children
);
let child = &mut self.children[self.current_index];
match child.tick(ctx).await? {
NodeStatus::Success => {
self.current_index += 1;
}
NodeStatus::Failure => {
debug!("Sequence: child {} failed", self.current_index + 1);
self.last_status = Some(NodeStatus::Failure);
return Ok(NodeStatus::Failure);
}
NodeStatus::Running => {
self.last_status = Some(NodeStatus::Running);
return Ok(NodeStatus::Running);
}
}
}
debug!("Sequence: all children succeeded");
self.last_status = Some(NodeStatus::Success);
Ok(NodeStatus::Success)
}
crate::reset_index_field!(current_index);
fn name(&self) -> &str {
"sequence"
}
fn children_mut(&mut self) -> Vec<&mut dyn BehaviorNode> {
crate::boxed_children_mut(self.children.iter_mut())
}
fn active_capability(&self) -> Option<&str> {
self.children
.get(self.current_index)
.and_then(|c| c.active_capability())
}
fn status_snapshot(&self) -> TreeStatusSnapshot {
TreeStatusSnapshot {
name: self.name().to_string(),
status: self.last_status,
active_capability: self.active_capability().map(String::from),
children: self.children.iter().map(|c| c.status_snapshot()).collect(),
}
}
}
#[derive(Debug)]
pub struct SelectOrNode {
children: Vec<BoxedBehavior>,
current_index: usize,
last_status: Option<NodeStatus>,
}
impl SelectOrNode {
pub fn new(children: Vec<BoxedBehavior>) -> Self {
Self {
children,
current_index: 0,
last_status: None,
}
}
pub fn add_child(&mut self, child: BoxedBehavior) {
self.children.push(child);
}
pub fn len(&self) -> usize {
self.children.len()
}
pub fn is_empty(&self) -> bool {
self.children.is_empty()
}
}
#[async_trait]
impl BehaviorNode for SelectOrNode {
async fn tick(&mut self, ctx: &Context) -> anyhow::Result<NodeStatus> {
if self.children.is_empty() {
self.last_status = Some(NodeStatus::Failure);
return Ok(NodeStatus::Failure);
}
let total_children = self.children.len();
while self.current_index < total_children {
debug!(
"Selector: ticking child {} of {}",
self.current_index + 1,
total_children
);
let child = &mut self.children[self.current_index];
match child.tick(ctx).await? {
NodeStatus::Success => {
debug!("Selector: child {} succeeded", self.current_index + 1);
self.last_status = Some(NodeStatus::Success);
return Ok(NodeStatus::Success);
}
NodeStatus::Failure => {
warn!("Selector: child {} failed, trying next", self.current_index + 1);
self.current_index += 1;
}
NodeStatus::Running => {
self.last_status = Some(NodeStatus::Running);
return Ok(NodeStatus::Running);
}
}
}
debug!("Selector: all children failed");
self.last_status = Some(NodeStatus::Failure);
Ok(NodeStatus::Failure)
}
crate::reset_index_field!(current_index);
fn name(&self) -> &str {
"selector"
}
fn children_mut(&mut self) -> Vec<&mut dyn BehaviorNode> {
crate::boxed_children_mut(self.children.iter_mut())
}
fn active_capability(&self) -> Option<&str> {
self.children
.get(self.current_index)
.and_then(|c| c.active_capability())
}
fn status_snapshot(&self) -> TreeStatusSnapshot {
TreeStatusSnapshot {
name: self.name().to_string(),
status: self.last_status,
active_capability: self.active_capability().map(String::from),
children: self.children.iter().map(|c| c.status_snapshot()).collect(),
}
}
}
#[derive(Debug)]
pub struct WeightedSelectNode {
children: Vec<BoxedBehavior>,
weights: Vec<f64>,
tried: Vec<bool>,
current_index: Option<usize>,
rng: StdRng,
last_status: Option<NodeStatus>,
}
impl WeightedSelectNode {
pub fn new(weighted_children: Vec<(f64, BoxedBehavior)>) -> Self {
Self::with_rng(weighted_children, StdRng::from_rng(&mut rand::rng()))
}
pub fn with_rng(weighted_children: Vec<(f64, BoxedBehavior)>, rng: StdRng) -> Self {
let mut weights = Vec::with_capacity(weighted_children.len());
let mut children = Vec::with_capacity(weighted_children.len());
for (weight, child) in weighted_children {
weights.push(weight.max(0.0));
children.push(child);
}
let tried = vec![false; children.len()];
Self {
children,
weights,
tried,
current_index: None,
rng,
last_status: None,
}
}
pub fn len(&self) -> usize {
self.children.len()
}
pub fn is_empty(&self) -> bool {
self.children.is_empty()
}
fn pick_next(&mut self) -> Option<usize> {
let candidates: Vec<usize> = (0..self.children.len())
.filter(|&i| !self.tried[i] && self.weights[i] > 0.0)
.collect();
let total: f64 = candidates.iter().map(|&i| self.weights[i]).sum();
if candidates.is_empty() || total <= 0.0 {
return None;
}
let mut draw = self.rng.random::<f64>() * total;
for &i in &candidates {
draw -= self.weights[i];
if draw <= 0.0 {
return Some(i);
}
}
candidates.last().copied()
}
}
#[async_trait]
impl BehaviorNode for WeightedSelectNode {
async fn tick(&mut self, ctx: &Context) -> anyhow::Result<NodeStatus> {
if self.children.is_empty() {
self.last_status = Some(NodeStatus::Failure);
return Ok(NodeStatus::Failure);
}
loop {
let idx = match self.current_index {
Some(idx) => idx,
None => match self.pick_next() {
Some(idx) => idx,
None => {
debug!("WeightedSelect: no eligible child remains, failing");
self.tried.iter_mut().for_each(|t| *t = false);
self.last_status = Some(NodeStatus::Failure);
return Ok(NodeStatus::Failure);
}
},
};
self.current_index = Some(idx);
self.tried[idx] = true;
let child = &mut self.children[idx];
match child.tick(ctx).await? {
NodeStatus::Success => {
debug!("WeightedSelect: child {} succeeded", idx);
self.current_index = None;
self.tried.iter_mut().for_each(|t| *t = false);
self.last_status = Some(NodeStatus::Success);
return Ok(NodeStatus::Success);
}
NodeStatus::Failure => {
debug!("WeightedSelect: child {} failed, rolling again", idx);
self.current_index = None;
}
NodeStatus::Running => {
self.last_status = Some(NodeStatus::Running);
return Ok(NodeStatus::Running);
}
}
}
}
fn reset_self(&mut self) {
self.current_index = None;
self.tried.iter_mut().for_each(|t| *t = false);
}
fn name(&self) -> &str {
"weighted_select"
}
fn children_mut(&mut self) -> Vec<&mut dyn BehaviorNode> {
crate::boxed_children_mut(self.children.iter_mut())
}
fn active_capability(&self) -> Option<&str> {
self.current_index
.and_then(|i| self.children.get(i))
.and_then(|c| c.active_capability())
}
fn status_snapshot(&self) -> TreeStatusSnapshot {
TreeStatusSnapshot {
name: self.name().to_string(),
status: self.last_status,
active_capability: self.active_capability().map(String::from),
children: self.children.iter().map(|c| c.status_snapshot()).collect(),
}
}
}
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum ParallelPolicy {
RequireAll,
RequireOne,
RequireN(usize),
}
#[derive(Debug)]
pub struct ParallelNode {
children: Vec<BoxedBehavior>,
policy: ParallelPolicy,
child_statuses: Vec<NodeStatus>,
last_status: Option<NodeStatus>,
}
impl ParallelNode {
pub fn new(children: Vec<BoxedBehavior>, policy: ParallelPolicy) -> Self {
let child_statuses = vec![NodeStatus::Running; children.len()];
Self {
children,
policy,
child_statuses,
last_status: None,
}
}
pub fn add_child(&mut self, child: BoxedBehavior) {
self.children.push(child);
self.child_statuses.push(NodeStatus::Running);
}
pub fn len(&self) -> usize {
self.children.len()
}
pub fn is_empty(&self) -> bool {
self.children.is_empty()
}
}
#[async_trait]
impl BehaviorNode for ParallelNode {
async fn tick(&mut self, ctx: &Context) -> anyhow::Result<NodeStatus> {
if self.children.is_empty() {
self.last_status = Some(NodeStatus::Success);
return Ok(NodeStatus::Success);
}
for (i, child) in self.children.iter_mut().enumerate() {
if self.child_statuses[i].is_running() {
let status = child.tick(ctx).await?;
self.child_statuses[i] = status;
debug!("Parallel: child {} status: {}", i + 1, status);
}
}
let success_count = self.child_statuses.iter().filter(|s| s.is_success()).count();
let failure_count = self.child_statuses.iter().filter(|s| s.is_failure()).count();
let _running_count = self.child_statuses.iter().filter(|s| s.is_running()).count();
let status = match self.policy {
ParallelPolicy::RequireAll => {
if failure_count > 0 {
debug!("Parallel (RequireAll): at least one child failed");
NodeStatus::Failure
} else if success_count == self.children.len() {
debug!("Parallel (RequireAll): all children succeeded");
NodeStatus::Success
} else {
NodeStatus::Running
}
}
ParallelPolicy::RequireOne => {
if success_count > 0 {
debug!("Parallel (RequireOne): at least one child succeeded");
NodeStatus::Success
} else if failure_count == self.children.len() {
debug!("Parallel (RequireOne): all children failed");
NodeStatus::Failure
} else {
NodeStatus::Running
}
}
ParallelPolicy::RequireN(n) => {
if success_count >= n {
debug!("Parallel (RequireN({})): {} children succeeded", n, success_count);
NodeStatus::Success
} else if (self.children.len() - failure_count) < n {
debug!("Parallel (RequireN({})): not enough children can succeed", n);
NodeStatus::Failure
} else {
NodeStatus::Running
}
}
};
self.last_status = Some(status);
Ok(status)
}
crate::reset_child_statuses_field!(child_statuses);
fn name(&self) -> &str {
"parallel"
}
fn children_mut(&mut self) -> Vec<&mut dyn BehaviorNode> {
crate::boxed_children_mut(self.children.iter_mut())
}
fn active_capability(&self) -> Option<&str> {
self.children
.iter()
.zip(self.child_statuses.iter())
.find(|(_, status)| status.is_running())
.and_then(|(child, _)| child.active_capability())
}
fn status_snapshot(&self) -> TreeStatusSnapshot {
TreeStatusSnapshot {
name: self.name().to_string(),
status: self.last_status,
active_capability: self.active_capability().map(String::from),
children: self.children.iter().map(|c| c.status_snapshot()).collect(),
}
}
}
#[derive(Debug)]
pub struct WatchNode {
children: Vec<BoxedBehavior>,
policy: ParallelPolicy,
last_active: Option<usize>,
last_status: Option<NodeStatus>,
}
impl WatchNode {
pub fn new(children: Vec<BoxedBehavior>, policy: ParallelPolicy) -> Self {
Self {
children,
policy,
last_active: None,
last_status: None,
}
}
pub fn add_child(&mut self, child: BoxedBehavior) {
self.children.push(child);
}
pub fn len(&self) -> usize {
self.children.len()
}
pub fn is_empty(&self) -> bool {
self.children.is_empty()
}
}
#[async_trait]
impl BehaviorNode for WatchNode {
async fn tick(&mut self, ctx: &Context) -> anyhow::Result<NodeStatus> {
if self.children.is_empty() {
self.last_active = None;
self.last_status = Some(NodeStatus::Failure);
return Ok(NodeStatus::Failure);
}
let mut statuses = Vec::with_capacity(self.children.len());
for child in &mut self.children {
statuses.push(child.tick(ctx).await?);
}
self.last_active = statuses.iter().position(|s| s.is_running());
let success_count = statuses.iter().filter(|s| s.is_success()).count();
let failure_count = statuses.iter().filter(|s| s.is_failure()).count();
let status = match self.policy {
ParallelPolicy::RequireAll => {
if failure_count > 0 {
debug!("Watch (RequireAll): at least one child failed");
NodeStatus::Failure
} else if success_count == self.children.len() {
debug!("Watch (RequireAll): all children succeeded");
NodeStatus::Success
} else {
NodeStatus::Running
}
}
ParallelPolicy::RequireOne => {
if success_count > 0 {
debug!("Watch (RequireOne): at least one child succeeded");
NodeStatus::Success
} else if failure_count == self.children.len() {
debug!("Watch (RequireOne): all children failed");
NodeStatus::Failure
} else {
NodeStatus::Running
}
}
ParallelPolicy::RequireN(n) => {
if success_count >= n {
debug!("Watch (RequireN({})): {} children succeeded", n, success_count);
NodeStatus::Success
} else if (self.children.len() - failure_count) < n {
debug!("Watch (RequireN({})): not enough children can succeed", n);
NodeStatus::Failure
} else {
NodeStatus::Running
}
}
};
self.last_status = Some(status);
Ok(status)
}
fn name(&self) -> &str {
"watch"
}
fn children_mut(&mut self) -> Vec<&mut dyn BehaviorNode> {
crate::boxed_children_mut(self.children.iter_mut())
}
fn active_capability(&self) -> Option<&str> {
self.last_active
.and_then(|i| self.children.get(i))
.and_then(|child| child.active_capability())
}
fn status_snapshot(&self) -> TreeStatusSnapshot {
TreeStatusSnapshot {
name: self.name().to_string(),
status: self.last_status,
active_capability: self.active_capability().map(String::from),
children: self.children.iter().map(|c| c.status_snapshot()).collect(),
}
}
}
#[derive(Debug)]
pub struct SubsumptionLayer {
pub priority: u8,
pub behavior: BoxedBehavior,
pub name: Option<String>,
}
impl SubsumptionLayer {
mecha10_core::new_with_defaults!(
pub fn new(priority: u8, behavior: BoxedBehavior) -> Self {
name: None
}
);
mecha10_core::builder_setter!(into_some, with_name, name, impl Into<String>);
}
#[derive(Debug)]
pub struct SubsumptionNode {
pub layers: Vec<SubsumptionLayer>,
last_winning_layer: Option<usize>,
last_status: Option<NodeStatus>,
}
impl SubsumptionNode {
pub fn new() -> Self {
Self {
layers: Vec::new(),
last_winning_layer: None,
last_status: None,
}
}
pub fn add_layer(mut self, priority: u8, behavior: BoxedBehavior) -> Self {
self.layers.push(SubsumptionLayer::new(priority, behavior));
self.layers.sort_by_key(|b| std::cmp::Reverse(b.priority));
self
}
}
impl Default for SubsumptionNode {
fn default() -> Self {
Self::new()
}
}
#[async_trait]
impl BehaviorNode for SubsumptionNode {
async fn tick(&mut self, ctx: &Context) -> anyhow::Result<NodeStatus> {
if self.layers.is_empty() {
self.last_winning_layer = None;
self.last_status = Some(NodeStatus::Failure);
return Ok(NodeStatus::Failure);
}
for (i, layer) in self.layers.iter_mut().enumerate() {
let layer_name = layer.name.as_deref().unwrap_or("unnamed");
debug!(
"Subsumption: trying layer '{}' (priority {})",
layer_name, layer.priority
);
let status = layer.behavior.tick(ctx).await?;
match status {
NodeStatus::Success | NodeStatus::Running => {
info!(
"Subsumption: layer '{}' (priority {}) taking control with status {}",
layer_name, layer.priority, status
);
self.last_winning_layer = Some(i);
self.last_status = Some(status);
return Ok(status);
}
NodeStatus::Failure => {
debug!("Subsumption: layer '{}' failed, trying next layer", layer_name);
continue;
}
}
}
debug!("Subsumption: all layers failed");
self.last_winning_layer = None;
self.last_status = Some(NodeStatus::Failure);
Ok(NodeStatus::Failure)
}
fn name(&self) -> &str {
"subsumption"
}
crate::children_mut_via_wrapped_behavior!(layers, layer);
fn active_capability(&self) -> Option<&str> {
self.last_winning_layer
.and_then(|i| self.layers.get(i))
.and_then(|layer| layer.behavior.active_capability())
}
fn status_snapshot(&self) -> TreeStatusSnapshot {
TreeStatusSnapshot {
name: self.name().to_string(),
status: self.last_status,
active_capability: self.active_capability().map(String::from),
children: self.layers.iter().map(|l| l.behavior.status_snapshot()).collect(),
}
}
}
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum ControllerModeKind {
Locomotion,
Joints,
}
#[derive(Debug, Clone, PartialEq, Eq)]
pub enum ConditionKind {
ControllerModeIs(ControllerModeKind),
}
impl ConditionKind {
fn evaluate(&self) -> bool {
match self {
ConditionKind::ControllerModeIs(kind) => {
let current = match controller_mode() {
ControllerMode::Locomotion => ControllerModeKind::Locomotion,
ControllerMode::Joints { .. } => ControllerModeKind::Joints,
};
current == *kind
}
}
}
}
#[derive(Debug)]
pub struct ConditionNode {
condition: ConditionKind,
child: BoxedBehavior,
last_status: Option<NodeStatus>,
}
impl ConditionNode {
pub fn new(condition: ConditionKind, child: BoxedBehavior) -> Self {
Self {
condition,
child,
last_status: None,
}
}
}
#[async_trait]
impl BehaviorNode for ConditionNode {
async fn tick(&mut self, ctx: &Context) -> anyhow::Result<NodeStatus> {
let status = if self.condition.evaluate() {
self.child.tick(ctx).await?
} else {
debug!(
"Condition: predicate {:?} is false, returning Failure without ticking child",
self.condition
);
NodeStatus::Failure
};
self.last_status = Some(status);
Ok(status)
}
fn name(&self) -> &str {
"condition"
}
fn children_mut(&mut self) -> Vec<&mut dyn BehaviorNode> {
vec![self.child.as_mut()]
}
fn active_capability(&self) -> Option<&str> {
self.child.active_capability()
}
fn status_snapshot(&self) -> TreeStatusSnapshot {
TreeStatusSnapshot {
name: self.name().to_string(),
status: self.last_status,
active_capability: self.active_capability().map(String::from),
children: vec![self.child.status_snapshot()],
}
}
}
#[derive(Debug)]
pub struct InverterNode {
child: BoxedBehavior,
last_status: Option<NodeStatus>,
}
impl InverterNode {
pub fn new(child: BoxedBehavior) -> Self {
Self {
child,
last_status: None,
}
}
}
#[async_trait]
impl BehaviorNode for InverterNode {
async fn tick(&mut self, ctx: &Context) -> anyhow::Result<NodeStatus> {
let status = match self.child.tick(ctx).await? {
NodeStatus::Success => NodeStatus::Failure,
NodeStatus::Failure => NodeStatus::Success,
NodeStatus::Running => NodeStatus::Running,
};
self.last_status = Some(status);
Ok(status)
}
fn name(&self) -> &str {
"inverter"
}
fn children_mut(&mut self) -> Vec<&mut dyn BehaviorNode> {
vec![self.child.as_mut()]
}
fn active_capability(&self) -> Option<&str> {
self.child.active_capability()
}
fn status_snapshot(&self) -> TreeStatusSnapshot {
TreeStatusSnapshot {
name: self.name().to_string(),
status: self.last_status,
active_capability: self.active_capability().map(String::from),
children: vec![self.child.status_snapshot()],
}
}
}
#[derive(Debug)]
pub struct ForceSuccessNode {
child: BoxedBehavior,
last_status: Option<NodeStatus>,
}
impl ForceSuccessNode {
pub fn new(child: BoxedBehavior) -> Self {
Self {
child,
last_status: None,
}
}
}
#[async_trait]
impl BehaviorNode for ForceSuccessNode {
async fn tick(&mut self, ctx: &Context) -> anyhow::Result<NodeStatus> {
let status = match self.child.tick(ctx).await? {
NodeStatus::Running => NodeStatus::Running,
NodeStatus::Success | NodeStatus::Failure => NodeStatus::Success,
};
self.last_status = Some(status);
Ok(status)
}
fn name(&self) -> &str {
"force_success"
}
fn children_mut(&mut self) -> Vec<&mut dyn BehaviorNode> {
vec![self.child.as_mut()]
}
fn active_capability(&self) -> Option<&str> {
self.child.active_capability()
}
fn status_snapshot(&self) -> TreeStatusSnapshot {
TreeStatusSnapshot {
name: self.name().to_string(),
status: self.last_status,
active_capability: self.active_capability().map(String::from),
children: vec![self.child.status_snapshot()],
}
}
}
#[derive(Debug)]
pub struct ForceFailureNode {
child: BoxedBehavior,
last_status: Option<NodeStatus>,
}
impl ForceFailureNode {
pub fn new(child: BoxedBehavior) -> Self {
Self {
child,
last_status: None,
}
}
}
#[async_trait]
impl BehaviorNode for ForceFailureNode {
async fn tick(&mut self, ctx: &Context) -> anyhow::Result<NodeStatus> {
let status = match self.child.tick(ctx).await? {
NodeStatus::Running => NodeStatus::Running,
NodeStatus::Success | NodeStatus::Failure => NodeStatus::Failure,
};
self.last_status = Some(status);
Ok(status)
}
fn name(&self) -> &str {
"force_failure"
}
fn children_mut(&mut self) -> Vec<&mut dyn BehaviorNode> {
vec![self.child.as_mut()]
}
fn active_capability(&self) -> Option<&str> {
self.child.active_capability()
}
fn status_snapshot(&self) -> TreeStatusSnapshot {
TreeStatusSnapshot {
name: self.name().to_string(),
status: self.last_status,
active_capability: self.active_capability().map(String::from),
children: vec![self.child.status_snapshot()],
}
}
}
#[derive(Debug)]
pub struct RepeatNode {
child: BoxedBehavior,
count: Option<u32>,
completed: u32,
last_status: Option<NodeStatus>,
}
impl RepeatNode {
pub fn new(child: BoxedBehavior, count: Option<u32>) -> Self {
Self {
child,
count,
completed: 0,
last_status: None,
}
}
}
#[async_trait]
impl BehaviorNode for RepeatNode {
async fn tick(&mut self, ctx: &Context) -> anyhow::Result<NodeStatus> {
if let Some(count) = self.count {
if self.completed >= count {
self.last_status = Some(NodeStatus::Success);
return Ok(NodeStatus::Success);
}
}
let status = match self.child.tick(ctx).await? {
NodeStatus::Success => {
self.completed += 1;
self.child.reset().await?;
match self.count {
Some(count) if self.completed >= count => NodeStatus::Success,
_ => NodeStatus::Running,
}
}
NodeStatus::Failure => NodeStatus::Failure,
NodeStatus::Running => NodeStatus::Running,
};
self.last_status = Some(status);
Ok(status)
}
fn reset_self(&mut self) {
self.completed = 0;
}
fn name(&self) -> &str {
"repeat"
}
fn children_mut(&mut self) -> Vec<&mut dyn BehaviorNode> {
vec![self.child.as_mut()]
}
fn active_capability(&self) -> Option<&str> {
self.child.active_capability()
}
fn status_snapshot(&self) -> TreeStatusSnapshot {
TreeStatusSnapshot {
name: self.name().to_string(),
status: self.last_status,
active_capability: self.active_capability().map(String::from),
children: vec![self.child.status_snapshot()],
}
}
}
#[derive(Debug)]
pub struct RetryNode {
child: BoxedBehavior,
max_attempts: u32,
attempts: u32,
last_status: Option<NodeStatus>,
}
impl RetryNode {
pub fn new(child: BoxedBehavior, max_attempts: u32) -> Self {
Self {
child,
max_attempts: max_attempts.max(1),
attempts: 0,
last_status: None,
}
}
}
#[async_trait]
impl BehaviorNode for RetryNode {
async fn tick(&mut self, ctx: &Context) -> anyhow::Result<NodeStatus> {
let status = match self.child.tick(ctx).await? {
NodeStatus::Success => NodeStatus::Success,
NodeStatus::Running => NodeStatus::Running,
NodeStatus::Failure => {
self.attempts += 1;
if self.attempts >= self.max_attempts {
debug!("Retry: child failed after {} attempt(s), giving up", self.attempts);
NodeStatus::Failure
} else {
debug!(
"Retry: child failed on attempt {} of {}, retrying",
self.attempts, self.max_attempts
);
self.child.reset().await?;
NodeStatus::Running
}
}
};
self.last_status = Some(status);
Ok(status)
}
fn reset_self(&mut self) {
self.attempts = 0;
}
fn name(&self) -> &str {
"retry"
}
fn children_mut(&mut self) -> Vec<&mut dyn BehaviorNode> {
vec![self.child.as_mut()]
}
fn active_capability(&self) -> Option<&str> {
self.child.active_capability()
}
fn status_snapshot(&self) -> TreeStatusSnapshot {
TreeStatusSnapshot {
name: self.name().to_string(),
status: self.last_status,
active_capability: self.active_capability().map(String::from),
children: vec![self.child.status_snapshot()],
}
}
}
#[derive(Debug)]
pub struct TimeoutNode {
child: BoxedBehavior,
duration: Duration,
start_time: Option<Instant>,
last_status: Option<NodeStatus>,
}
impl TimeoutNode {
pub fn new(child: BoxedBehavior, duration: Duration) -> Self {
Self {
child,
duration,
start_time: None,
last_status: None,
}
}
}
#[async_trait]
impl BehaviorNode for TimeoutNode {
async fn tick(&mut self, ctx: &Context) -> anyhow::Result<NodeStatus> {
let now = Instant::now();
let start = *self.start_time.get_or_insert(now);
if now.duration_since(start) >= self.duration {
warn!("Timeout: child did not resolve within {:?}, failing", self.duration);
self.start_time = None;
self.child.reset().await?;
self.last_status = Some(NodeStatus::Failure);
return Ok(NodeStatus::Failure);
}
let status = self.child.tick(ctx).await?;
if status.is_complete() {
self.start_time = None;
}
self.last_status = Some(status);
Ok(status)
}
fn reset_self(&mut self) {
self.start_time = None;
}
fn name(&self) -> &str {
"timeout"
}
fn children_mut(&mut self) -> Vec<&mut dyn BehaviorNode> {
vec![self.child.as_mut()]
}
fn active_capability(&self) -> Option<&str> {
self.child.active_capability()
}
fn status_snapshot(&self) -> TreeStatusSnapshot {
TreeStatusSnapshot {
name: self.name().to_string(),
status: self.last_status,
active_capability: self.active_capability().map(String::from),
children: vec![self.child.status_snapshot()],
}
}
}
#[derive(Debug)]
pub struct CooldownNode {
child: BoxedBehavior,
cooldown: Duration,
cooldown_until: Option<Instant>,
last_status: Option<NodeStatus>,
}
impl CooldownNode {
pub fn new(child: BoxedBehavior, cooldown: Duration) -> Self {
Self {
child,
cooldown,
cooldown_until: None,
last_status: None,
}
}
}
#[async_trait]
impl BehaviorNode for CooldownNode {
async fn tick(&mut self, ctx: &Context) -> anyhow::Result<NodeStatus> {
if let Some(until) = self.cooldown_until {
if Instant::now() < until {
debug!("Cooldown: still cooling down, returning Failure without ticking child");
self.last_status = Some(NodeStatus::Failure);
return Ok(NodeStatus::Failure);
}
self.cooldown_until = None;
}
let status = self.child.tick(ctx).await?;
if status.is_complete() {
self.cooldown_until = Some(Instant::now() + self.cooldown);
}
self.last_status = Some(status);
Ok(status)
}
fn reset_self(&mut self) {
self.cooldown_until = None;
}
fn name(&self) -> &str {
"cooldown"
}
fn children_mut(&mut self) -> Vec<&mut dyn BehaviorNode> {
vec![self.child.as_mut()]
}
fn active_capability(&self) -> Option<&str> {
self.child.active_capability()
}
fn status_snapshot(&self) -> TreeStatusSnapshot {
TreeStatusSnapshot {
name: self.name().to_string(),
status: self.last_status,
active_capability: self.active_capability().map(String::from),
children: vec![self.child.status_snapshot()],
}
}
}
#[cfg(test)]
mod tests {
use super::*;
#[derive(Debug, Default)]
struct DummyBehavior;
#[async_trait]
impl BehaviorNode for DummyBehavior {
async fn tick(&mut self, _ctx: &Context) -> anyhow::Result<NodeStatus> {
unimplemented!("not exercised by these tests")
}
}
#[derive(Debug)]
struct CapabilityBehavior(&'static str);
#[async_trait]
impl BehaviorNode for CapabilityBehavior {
async fn tick(&mut self, _ctx: &Context) -> anyhow::Result<NodeStatus> {
unimplemented!("not exercised by these tests")
}
fn active_capability(&self) -> Option<&str> {
Some(self.0)
}
}
#[test]
fn sequence_children_mut_exposes_every_child() {
let mut node = SequenceNode::new(vec![Box::new(DummyBehavior), Box::new(DummyBehavior)]);
assert_eq!(node.children_mut().len(), 2);
}
#[tokio::test]
async fn sequence_reset_rewinds_current_index() {
let mut node = SequenceNode::new(vec![Box::new(DummyBehavior)]);
node.current_index = 1;
node.reset().await.unwrap();
assert_eq!(node.current_index, 0);
}
#[test]
fn selector_children_mut_exposes_every_child() {
let mut node = SelectOrNode::new(vec![Box::new(DummyBehavior), Box::new(DummyBehavior)]);
assert_eq!(node.children_mut().len(), 2);
}
#[tokio::test]
async fn selector_reset_rewinds_current_index() {
let mut node = SelectOrNode::new(vec![Box::new(DummyBehavior)]);
node.current_index = 1;
node.reset().await.unwrap();
assert_eq!(node.current_index, 0);
}
#[test]
fn parallel_children_mut_exposes_every_child() {
let mut node = ParallelNode::new(
vec![Box::new(DummyBehavior), Box::new(DummyBehavior)],
ParallelPolicy::RequireAll,
);
assert_eq!(node.children_mut().len(), 2);
}
#[tokio::test]
async fn parallel_reset_clears_child_statuses_back_to_running() {
let mut node = ParallelNode::new(vec![Box::new(DummyBehavior)], ParallelPolicy::RequireAll);
node.child_statuses[0] = NodeStatus::Success;
node.reset().await.unwrap();
assert_eq!(node.child_statuses[0], NodeStatus::Running);
}
#[test]
fn sequence_forwards_the_current_childs_active_capability() {
let node = SequenceNode::new(vec![Box::new(CapabilityBehavior("rl")), Box::new(DummyBehavior)]);
assert_eq!(node.active_capability(), Some("rl"));
}
#[test]
fn sequence_has_no_active_capability_once_past_the_declaring_child() {
let mut node = SequenceNode::new(vec![Box::new(CapabilityBehavior("rl")), Box::new(DummyBehavior)]);
node.current_index = 1;
assert_eq!(node.active_capability(), None);
}
#[test]
fn sequence_status_snapshot_is_none_before_any_tick() {
let node = SequenceNode::new(vec![Box::new(DummyBehavior)]);
let snapshot = node.status_snapshot();
assert_eq!(snapshot.status, None);
assert_eq!(snapshot.name, "sequence");
assert_eq!(
snapshot.children.len(),
1,
"children must still be recursed into pre-tick"
);
}
#[tokio::test]
#[ignore = "Requires Redis infrastructure"]
async fn sequence_status_snapshot_matches_the_last_tick_result_and_recurses_into_children() {
let ctx = crate::test_helpers::create_test_context("sequence_status_snapshot").await;
let mut node = SequenceNode::new(vec![Box::new(StatusAndCapabilityBehavior {
status: NodeStatus::Success,
capability: "rl",
})]);
let status = node.tick(&ctx).await.unwrap();
assert_eq!(status, NodeStatus::Success);
let snapshot = node.status_snapshot();
assert_eq!(snapshot.status, Some(status));
assert_eq!(snapshot.name, "sequence");
assert_eq!(
snapshot.active_capability, None,
"the only child already succeeded and was advanced past, so no capability is active"
);
assert_eq!(snapshot.children.len(), 1);
}
#[test]
fn selector_forwards_the_current_childs_active_capability() {
let node = SelectOrNode::new(vec![Box::new(CapabilityBehavior("rl")), Box::new(DummyBehavior)]);
assert_eq!(node.active_capability(), Some("rl"));
}
#[test]
fn parallel_forwards_the_first_running_childs_active_capability() {
let mut node = ParallelNode::new(
vec![Box::new(CapabilityBehavior("rl")), Box::new(DummyBehavior)],
ParallelPolicy::RequireAll,
);
node.child_statuses[0] = NodeStatus::Success;
assert_eq!(
node.active_capability(),
None,
"a non-running child's capability must not be reported"
);
}
#[derive(Debug)]
struct SequencedStatusBehavior {
statuses: Vec<NodeStatus>,
index: usize,
tick_count: usize,
capability: Option<&'static str>,
}
impl SequencedStatusBehavior {
fn new(statuses: Vec<NodeStatus>) -> Self {
Self {
statuses,
index: 0,
tick_count: 0,
capability: None,
}
}
fn with_capability(statuses: Vec<NodeStatus>, capability: &'static str) -> Self {
Self {
capability: Some(capability),
..Self::new(statuses)
}
}
}
#[async_trait]
impl BehaviorNode for SequencedStatusBehavior {
async fn tick(&mut self, _ctx: &Context) -> anyhow::Result<NodeStatus> {
self.tick_count += 1;
let status = self.statuses[self.index.min(self.statuses.len() - 1)];
if self.index + 1 < self.statuses.len() {
self.index += 1;
}
Ok(status)
}
fn active_capability(&self) -> Option<&str> {
self.capability
}
}
#[test]
fn watch_children_mut_exposes_every_child() {
let mut node = WatchNode::new(
vec![Box::new(DummyBehavior), Box::new(DummyBehavior)],
ParallelPolicy::RequireOne,
);
assert_eq!(node.children_mut().len(), 2);
}
#[tokio::test]
#[ignore = "Requires Redis infrastructure"]
async fn watch_is_empty_returns_failure() {
let ctx = crate::test_helpers::create_test_context("watch_empty").await;
let mut node = WatchNode::new(vec![], ParallelPolicy::RequireOne);
assert_eq!(node.tick(&ctx).await.unwrap(), NodeStatus::Failure);
}
#[tokio::test]
#[ignore = "Requires Redis infrastructure"]
async fn watch_re_ticks_every_child_every_call_and_reflects_the_latest_statuses() {
let ctx = crate::test_helpers::create_test_context("watch_re_ticks").await;
let mut node = WatchNode::new(
vec![Box::new(SequencedStatusBehavior::new(vec![
NodeStatus::Failure,
NodeStatus::Running,
NodeStatus::Failure,
]))],
ParallelPolicy::RequireOne,
);
assert_eq!(node.tick(&ctx).await.unwrap(), NodeStatus::Failure);
assert_eq!(node.tick(&ctx).await.unwrap(), NodeStatus::Running);
assert_eq!(
node.tick(&ctx).await.unwrap(),
NodeStatus::Failure,
"a ParallelNode would have frozen on the first Failure and never re-ticked the child"
);
}
#[tokio::test]
#[ignore = "Requires Redis infrastructure"]
async fn watch_forwards_the_currently_running_childs_active_capability() {
let ctx = crate::test_helpers::create_test_context("watch_capability").await;
let mut node = WatchNode::new(
vec![Box::new(SequencedStatusBehavior::with_capability(
vec![NodeStatus::Running, NodeStatus::Failure],
"instance_track",
))],
ParallelPolicy::RequireOne,
);
node.tick(&ctx).await.unwrap();
assert_eq!(node.active_capability(), Some("instance_track"));
node.tick(&ctx).await.unwrap();
assert_eq!(
node.active_capability(),
None,
"a child that was running last tick and is not running this tick must not keep reporting its capability"
);
}
#[test]
fn subsumption_children_mut_exposes_every_layer_behavior() {
let mut node = SubsumptionNode::new()
.add_layer(10, Box::new(DummyBehavior))
.add_layer(5, Box::new(DummyBehavior));
assert_eq!(node.children_mut().len(), 2);
}
#[derive(Debug)]
struct StatusAndCapabilityBehavior {
status: NodeStatus,
capability: &'static str,
}
#[async_trait]
impl BehaviorNode for StatusAndCapabilityBehavior {
async fn tick(&mut self, _ctx: &Context) -> anyhow::Result<NodeStatus> {
Ok(self.status)
}
fn active_capability(&self) -> Option<&str> {
Some(self.capability)
}
}
#[tokio::test]
#[ignore = "Requires Redis infrastructure"]
async fn subsumption_active_capability_forwards_the_winning_layers_capability() {
let ctx = crate::test_helpers::create_test_context("subsumption_capability").await;
let mut node = SubsumptionNode::new()
.add_layer(
10,
Box::new(StatusAndCapabilityBehavior {
status: NodeStatus::Failure,
capability: "instance_track",
}),
)
.add_layer(
0,
Box::new(StatusAndCapabilityBehavior {
status: NodeStatus::Running,
capability: "patrol",
}),
);
assert_eq!(node.active_capability(), None, "no tick has happened yet");
let status = node.tick(&ctx).await.unwrap();
assert_eq!(
status,
NodeStatus::Running,
"the high-priority layer fails, so the low layer wins"
);
assert_eq!(
node.active_capability(),
Some("patrol"),
"active_capability must forward the winning (low-priority) layer's capability, not the failed high-priority one"
);
}
#[tokio::test]
#[ignore = "Requires Redis infrastructure"]
async fn subsumption_active_capability_none_when_all_layers_fail() {
let ctx = crate::test_helpers::create_test_context("subsumption_capability_all_fail").await;
let mut node =
SubsumptionNode::new().add_layer(10, Box::new(SequencedStatusBehavior::new(vec![NodeStatus::Failure])));
node.tick(&ctx).await.unwrap();
assert_eq!(node.active_capability(), None);
}
#[derive(Debug)]
struct ToggleableBehavior {
engaged: std::sync::Arc<std::sync::atomic::AtomicBool>,
}
#[async_trait]
impl BehaviorNode for ToggleableBehavior {
async fn tick(&mut self, _ctx: &Context) -> anyhow::Result<NodeStatus> {
Ok(if self.engaged.load(std::sync::atomic::Ordering::SeqCst) {
NodeStatus::Running
} else {
NodeStatus::Failure
})
}
}
#[derive(Debug)]
struct CountedLegBehavior {
running_ticks_remaining: usize,
tick_count: std::sync::Arc<std::sync::atomic::AtomicUsize>,
}
#[async_trait]
impl BehaviorNode for CountedLegBehavior {
async fn tick(&mut self, _ctx: &Context) -> anyhow::Result<NodeStatus> {
self.tick_count.fetch_add(1, std::sync::atomic::Ordering::SeqCst);
if self.running_ticks_remaining == 0 {
Ok(NodeStatus::Success)
} else {
self.running_ticks_remaining -= 1;
Ok(NodeStatus::Running)
}
}
}
#[tokio::test]
#[ignore = "Requires Redis infrastructure"]
async fn subsumption_preserves_lower_layer_progress_across_an_interrupt() {
use std::sync::atomic::{AtomicBool, AtomicUsize, Ordering};
use std::sync::Arc;
let ctx = crate::test_helpers::create_test_context("subsumption_resume").await;
let engaged = Arc::new(AtomicBool::new(false));
let leg1_ticks = Arc::new(AtomicUsize::new(0));
let leg2_ticks = Arc::new(AtomicUsize::new(0));
let watch = WatchNode::new(
vec![Box::new(ToggleableBehavior {
engaged: engaged.clone(),
})],
ParallelPolicy::RequireOne,
);
let sequence = SequenceNode::new(vec![
Box::new(CountedLegBehavior {
running_ticks_remaining: 2,
tick_count: leg1_ticks.clone(),
}),
Box::new(CountedLegBehavior {
running_ticks_remaining: 2,
tick_count: leg2_ticks.clone(),
}),
]);
let mut subsumption = SubsumptionNode::new()
.add_layer(100, Box::new(watch))
.add_layer(0, Box::new(sequence));
assert_eq!(subsumption.tick(&ctx).await.unwrap(), NodeStatus::Running);
assert_eq!(leg1_ticks.load(Ordering::SeqCst), 1);
assert_eq!(subsumption.tick(&ctx).await.unwrap(), NodeStatus::Running);
assert_eq!(leg1_ticks.load(Ordering::SeqCst), 2);
assert_eq!(subsumption.tick(&ctx).await.unwrap(), NodeStatus::Running);
assert_eq!(leg1_ticks.load(Ordering::SeqCst), 3);
assert_eq!(leg2_ticks.load(Ordering::SeqCst), 1);
engaged.store(true, Ordering::SeqCst);
for _ in 0..3 {
assert_eq!(
subsumption.tick(&ctx).await.unwrap(),
NodeStatus::Running,
"the instance_track layer wins while engaged"
);
}
assert_eq!(
leg2_ticks.load(Ordering::SeqCst),
1,
"the main layer must be frozen (not reset, not advanced) while the interrupt layer is engaged"
);
engaged.store(false, Ordering::SeqCst);
let status = subsumption.tick(&ctx).await.unwrap();
assert_eq!(
status,
NodeStatus::Running,
"handback must surface the Sequence's own status, not Success/Failure from the ceding interrupt layer"
);
assert_eq!(
leg1_ticks.load(Ordering::SeqCst),
3,
"leg1 must not be re-ticked - the sequence was not reset"
);
assert_eq!(
leg2_ticks.load(Ordering::SeqCst),
2,
"the Sequence resumes ticking leg2 (not leg1) - progress was preserved, not reset"
);
}
#[tokio::test]
#[ignore = "Requires Redis infrastructure"]
async fn emergency_stop_layer_preempts_both_instance_track_and_the_main_layer_and_resumes_on_clear() {
use std::sync::atomic::{AtomicBool, AtomicUsize, Ordering};
use std::sync::Arc;
let ctx = crate::test_helpers::create_test_context("subsumption_estop_resume").await;
let estop_active = Arc::new(AtomicBool::new(false));
let instance_track_engaged = Arc::new(AtomicBool::new(false));
let leg1_ticks = Arc::new(AtomicUsize::new(0));
let leg2_ticks = Arc::new(AtomicUsize::new(0));
let estop_watch = WatchNode::new(
vec![Box::new(ToggleableBehavior {
engaged: estop_active.clone(),
})],
ParallelPolicy::RequireOne,
);
let instance_track_watch = WatchNode::new(
vec![Box::new(ToggleableBehavior {
engaged: instance_track_engaged.clone(),
})],
ParallelPolicy::RequireOne,
);
let main_sequence = SequenceNode::new(vec![
Box::new(CountedLegBehavior {
running_ticks_remaining: 2,
tick_count: leg1_ticks.clone(),
}),
Box::new(CountedLegBehavior {
running_ticks_remaining: 2,
tick_count: leg2_ticks.clone(),
}),
]);
let mut subsumption = SubsumptionNode::new()
.add_layer(255, Box::new(estop_watch))
.add_layer(100, Box::new(instance_track_watch))
.add_layer(0, Box::new(main_sequence));
assert_eq!(subsumption.tick(&ctx).await.unwrap(), NodeStatus::Running);
assert_eq!(leg1_ticks.load(Ordering::SeqCst), 1);
instance_track_engaged.store(true, Ordering::SeqCst);
assert_eq!(subsumption.tick(&ctx).await.unwrap(), NodeStatus::Running);
assert_eq!(
leg1_ticks.load(Ordering::SeqCst),
1,
"main layer frozen while instance_track is engaged"
);
estop_active.store(true, Ordering::SeqCst);
for _ in 0..3 {
assert_eq!(
subsumption.tick(&ctx).await.unwrap(),
NodeStatus::Running,
"the emergency-stop layer must win even over an already-engaged instance_track layer"
);
}
assert_eq!(
leg1_ticks.load(Ordering::SeqCst),
1,
"main layer must stay frozen while e-stop is active"
);
instance_track_engaged.store(false, Ordering::SeqCst);
assert_eq!(
subsumption.tick(&ctx).await.unwrap(),
NodeStatus::Running,
"e-stop still active, so it keeps control even after instance_track clears"
);
assert_eq!(leg1_ticks.load(Ordering::SeqCst), 1, "main layer still frozen");
estop_active.store(false, Ordering::SeqCst);
let status = subsumption.tick(&ctx).await.unwrap();
assert_eq!(
status,
NodeStatus::Running,
"handback surfaces the Sequence's own status once e-stop clears"
);
assert_eq!(
leg1_ticks.load(Ordering::SeqCst),
2,
"leg1 resumes from where it left off (1 prior tick), not reset to 0"
);
assert_eq!(leg2_ticks.load(Ordering::SeqCst), 0, "leg2 not yet reached");
}
#[derive(Debug)]
struct FixedStatusBehavior(NodeStatus);
#[async_trait]
impl BehaviorNode for FixedStatusBehavior {
async fn tick(&mut self, _ctx: &Context) -> anyhow::Result<NodeStatus> {
Ok(self.0)
}
}
#[tokio::test]
#[ignore = "Requires Redis infrastructure"]
async fn inverter_flips_success_and_failure_but_passes_running_through() {
let ctx = crate::test_helpers::create_test_context("inverter").await;
let mut success = InverterNode::new(Box::new(FixedStatusBehavior(NodeStatus::Success)));
assert_eq!(success.tick(&ctx).await.unwrap(), NodeStatus::Failure);
let mut failure = InverterNode::new(Box::new(FixedStatusBehavior(NodeStatus::Failure)));
assert_eq!(failure.tick(&ctx).await.unwrap(), NodeStatus::Success);
let mut running = InverterNode::new(Box::new(FixedStatusBehavior(NodeStatus::Running)));
assert_eq!(running.tick(&ctx).await.unwrap(), NodeStatus::Running);
}
#[tokio::test]
#[ignore = "Requires Redis infrastructure"]
async fn force_success_reports_success_even_when_child_fails() {
let ctx = crate::test_helpers::create_test_context("force_success").await;
let mut node = ForceSuccessNode::new(Box::new(FixedStatusBehavior(NodeStatus::Failure)));
assert_eq!(node.tick(&ctx).await.unwrap(), NodeStatus::Success);
let mut running = ForceSuccessNode::new(Box::new(FixedStatusBehavior(NodeStatus::Running)));
assert_eq!(running.tick(&ctx).await.unwrap(), NodeStatus::Running);
}
#[tokio::test]
#[ignore = "Requires Redis infrastructure"]
async fn force_failure_reports_failure_even_when_child_succeeds() {
let ctx = crate::test_helpers::create_test_context("force_failure").await;
let mut node = ForceFailureNode::new(Box::new(FixedStatusBehavior(NodeStatus::Success)));
assert_eq!(node.tick(&ctx).await.unwrap(), NodeStatus::Failure);
let mut running = ForceFailureNode::new(Box::new(FixedStatusBehavior(NodeStatus::Running)));
assert_eq!(running.tick(&ctx).await.unwrap(), NodeStatus::Running);
}
#[tokio::test]
#[ignore = "Requires Redis infrastructure"]
async fn repeat_with_count_succeeds_after_n_successful_runs() {
let ctx = crate::test_helpers::create_test_context("repeat_count").await;
let mut node = RepeatNode::new(Box::new(FixedStatusBehavior(NodeStatus::Success)), Some(3));
assert_eq!(node.tick(&ctx).await.unwrap(), NodeStatus::Running);
assert_eq!(node.tick(&ctx).await.unwrap(), NodeStatus::Running);
assert_eq!(node.tick(&ctx).await.unwrap(), NodeStatus::Success);
}
#[tokio::test]
#[ignore = "Requires Redis infrastructure"]
async fn repeat_stops_immediately_on_child_failure() {
let ctx = crate::test_helpers::create_test_context("repeat_failure").await;
let mut node = RepeatNode::new(Box::new(FixedStatusBehavior(NodeStatus::Failure)), None);
assert_eq!(node.tick(&ctx).await.unwrap(), NodeStatus::Failure);
}
#[tokio::test]
#[ignore = "Requires Redis infrastructure"]
async fn retry_succeeds_once_an_attempt_within_the_budget_succeeds() {
let ctx = crate::test_helpers::create_test_context("retry_succeeds").await;
let mut node = RetryNode::new(
Box::new(SequencedStatusBehavior::new(vec![
NodeStatus::Failure,
NodeStatus::Failure,
NodeStatus::Success,
])),
3,
);
assert_eq!(node.tick(&ctx).await.unwrap(), NodeStatus::Running);
assert_eq!(node.tick(&ctx).await.unwrap(), NodeStatus::Running);
assert_eq!(node.tick(&ctx).await.unwrap(), NodeStatus::Success);
}
#[tokio::test]
#[ignore = "Requires Redis infrastructure"]
async fn retry_fails_once_every_attempt_is_exhausted() {
let ctx = crate::test_helpers::create_test_context("retry_exhausted").await;
let mut node = RetryNode::new(Box::new(FixedStatusBehavior(NodeStatus::Failure)), 2);
assert_eq!(node.tick(&ctx).await.unwrap(), NodeStatus::Running);
assert_eq!(node.tick(&ctx).await.unwrap(), NodeStatus::Failure);
}
#[tokio::test]
#[ignore = "Requires Redis infrastructure"]
async fn timeout_passes_through_a_child_that_resolves_before_the_deadline() {
let ctx = crate::test_helpers::create_test_context("timeout_resolves").await;
let mut node = TimeoutNode::new(
Box::new(FixedStatusBehavior(NodeStatus::Success)),
Duration::from_secs(60),
);
assert_eq!(node.tick(&ctx).await.unwrap(), NodeStatus::Success);
}
#[tokio::test]
#[ignore = "Requires Redis infrastructure"]
async fn timeout_fails_once_the_child_has_been_running_too_long() {
let ctx = crate::test_helpers::create_test_context("timeout_expires").await;
let mut node = TimeoutNode::new(Box::new(FixedStatusBehavior(NodeStatus::Running)), Duration::ZERO);
assert_eq!(node.tick(&ctx).await.unwrap(), NodeStatus::Failure);
}
static CONTROLLER_ENV_LOCK: tokio::sync::Mutex<()> = tokio::sync::Mutex::const_new(());
fn clear_controller_env() {
std::env::remove_var(mecha10_core::config::CAPABILITIES_ENV_VAR);
std::env::remove_var(mecha10_core::config::ACTUATION_JOINT_NAMES_ENV_VAR);
}
#[test]
fn controller_mode_is_evaluates_against_the_current_controller_mode() {
let _guard = CONTROLLER_ENV_LOCK.blocking_lock();
clear_controller_env();
assert!(ConditionKind::ControllerModeIs(ControllerModeKind::Locomotion).evaluate());
assert!(!ConditionKind::ControllerModeIs(ControllerModeKind::Joints).evaluate());
std::env::set_var(mecha10_core::config::CAPABILITIES_ENV_VAR, r#"["joints"]"#);
assert!(ConditionKind::ControllerModeIs(ControllerModeKind::Joints).evaluate());
assert!(!ConditionKind::ControllerModeIs(ControllerModeKind::Locomotion).evaluate());
clear_controller_env();
}
#[tokio::test]
#[ignore = "Requires Redis infrastructure"]
async fn condition_ticks_child_and_forwards_its_status_when_the_predicate_holds() {
let _guard = CONTROLLER_ENV_LOCK.lock().await;
clear_controller_env();
let ctx = crate::test_helpers::create_test_context("condition_true").await;
let mut node = ConditionNode::new(
ConditionKind::ControllerModeIs(ControllerModeKind::Locomotion),
Box::new(FixedStatusBehavior(NodeStatus::Running)),
);
assert_eq!(
node.tick(&ctx).await.unwrap(),
NodeStatus::Running,
"a true predicate must tick the child and forward its status unchanged"
);
clear_controller_env();
}
#[tokio::test]
#[ignore = "Requires Redis infrastructure"]
async fn condition_returns_failure_without_ticking_child_when_the_predicate_is_false() {
let _guard = CONTROLLER_ENV_LOCK.lock().await;
clear_controller_env();
std::env::set_var(mecha10_core::config::CAPABILITIES_ENV_VAR, r#"["joints"]"#);
let ctx = crate::test_helpers::create_test_context("condition_false").await;
let mut node = ConditionNode::new(
ConditionKind::ControllerModeIs(ControllerModeKind::Locomotion),
Box::new(DummyBehavior),
);
assert_eq!(node.tick(&ctx).await.unwrap(), NodeStatus::Failure);
clear_controller_env();
}
#[test]
fn condition_children_mut_exposes_the_child() {
let mut node = ConditionNode::new(
ConditionKind::ControllerModeIs(ControllerModeKind::Joints),
Box::new(DummyBehavior),
);
assert_eq!(node.children_mut().len(), 1);
}
#[test]
fn condition_forwards_its_childs_active_capability() {
let node = ConditionNode::new(
ConditionKind::ControllerModeIs(ControllerModeKind::Locomotion),
Box::new(CapabilityBehavior("sweep")),
);
assert_eq!(node.active_capability(), Some("sweep"));
}
#[test]
fn weighted_select_children_mut_exposes_every_child() {
let mut node = WeightedSelectNode::new(vec![
(1.0, Box::new(DummyBehavior) as BoxedBehavior),
(1.0, Box::new(DummyBehavior) as BoxedBehavior),
]);
assert_eq!(node.children_mut().len(), 2);
}
#[tokio::test]
#[ignore = "Requires Redis infrastructure"]
async fn weighted_select_never_picks_a_zero_weight_child() {
let ctx = crate::test_helpers::create_test_context("weighted_select_zero_weight").await;
let mut node = WeightedSelectNode::with_rng(
vec![
(1.0, Box::new(FixedStatusBehavior(NodeStatus::Failure)) as BoxedBehavior),
(0.0, Box::new(DummyBehavior) as BoxedBehavior),
],
StdRng::seed_from_u64(42),
);
assert_eq!(
node.tick(&ctx).await.unwrap(),
NodeStatus::Failure,
"the only eligible (positive-weight) child fails, and the zero-weight one must not be tried"
);
}
#[tokio::test]
#[ignore = "Requires Redis infrastructure"]
async fn weighted_select_does_not_reroll_away_from_a_running_child() {
use std::sync::atomic::{AtomicUsize, Ordering};
use std::sync::Arc;
let ctx = crate::test_helpers::create_test_context("weighted_select_sticky").await;
let leg_a_ticks = Arc::new(AtomicUsize::new(0));
let leg_b_ticks = Arc::new(AtomicUsize::new(0));
let mut node = WeightedSelectNode::with_rng(
vec![
(
1.0,
Box::new(CountedLegBehavior {
running_ticks_remaining: 1,
tick_count: leg_a_ticks.clone(),
}) as BoxedBehavior,
),
(
1.0,
Box::new(CountedLegBehavior {
running_ticks_remaining: 1,
tick_count: leg_b_ticks.clone(),
}) as BoxedBehavior,
),
],
StdRng::seed_from_u64(7),
);
assert_eq!(node.tick(&ctx).await.unwrap(), NodeStatus::Running);
let a_after_first = leg_a_ticks.load(Ordering::SeqCst);
let b_after_first = leg_b_ticks.load(Ordering::SeqCst);
assert_eq!(
a_after_first + b_after_first,
1,
"exactly one child should have been ticked"
);
assert_eq!(node.tick(&ctx).await.unwrap(), NodeStatus::Success);
assert_eq!(
leg_a_ticks.load(Ordering::SeqCst) + leg_b_ticks.load(Ordering::SeqCst),
2,
"the same child that was Running must be re-ticked (reaching 2 ticks), not the other \
one (which must remain untouched at 0)"
);
assert!(
leg_a_ticks.load(Ordering::SeqCst) == 0 || leg_b_ticks.load(Ordering::SeqCst) == 0,
"the child that was never chosen must never be ticked at all"
);
}
#[tokio::test]
#[ignore = "Requires Redis infrastructure"]
async fn weighted_select_forwards_the_current_childs_active_capability() {
let ctx = crate::test_helpers::create_test_context("weighted_select_capability").await;
let mut node = WeightedSelectNode::with_rng(
vec![(
1.0,
Box::new(StatusAndCapabilityBehavior {
status: NodeStatus::Running,
capability: "patrol",
}) as BoxedBehavior,
)],
StdRng::seed_from_u64(1),
);
assert_eq!(node.active_capability(), None, "no tick has happened yet");
assert_eq!(node.tick(&ctx).await.unwrap(), NodeStatus::Running);
assert_eq!(node.active_capability(), Some("patrol"));
}
#[tokio::test]
async fn weighted_select_reset_clears_current_index_and_tried() {
let mut node = WeightedSelectNode::with_rng(
vec![
(1.0, Box::new(DummyBehavior) as BoxedBehavior),
(1.0, Box::new(DummyBehavior) as BoxedBehavior),
],
StdRng::seed_from_u64(3),
);
node.current_index = Some(1);
node.tried[0] = true;
node.reset().await.unwrap();
assert_eq!(node.current_index, None);
assert!(node.tried.iter().all(|t| !t));
}
#[derive(Debug)]
struct CountingFixedStatusBehavior {
status: NodeStatus,
tick_count: std::sync::Arc<std::sync::atomic::AtomicUsize>,
}
#[async_trait]
impl BehaviorNode for CountingFixedStatusBehavior {
async fn tick(&mut self, _ctx: &Context) -> anyhow::Result<NodeStatus> {
self.tick_count.fetch_add(1, std::sync::atomic::Ordering::SeqCst);
Ok(self.status)
}
}
#[tokio::test]
#[ignore = "Requires Redis infrastructure"]
async fn cooldown_blocks_reentry_immediately_after_a_terminal_result() {
use std::sync::atomic::{AtomicUsize, Ordering};
use std::sync::Arc;
let ctx = crate::test_helpers::create_test_context("cooldown_blocks").await;
let tick_count = Arc::new(AtomicUsize::new(0));
let mut node = CooldownNode::new(
Box::new(CountingFixedStatusBehavior {
status: NodeStatus::Success,
tick_count: tick_count.clone(),
}),
Duration::from_secs(60),
);
assert_eq!(node.tick(&ctx).await.unwrap(), NodeStatus::Success);
assert_eq!(tick_count.load(Ordering::SeqCst), 1);
assert_eq!(
node.tick(&ctx).await.unwrap(),
NodeStatus::Failure,
"within the cooldown window, the node must fail without ticking the child"
);
assert_eq!(
tick_count.load(Ordering::SeqCst),
1,
"the child must not have been ticked again while cooling down"
);
}
#[tokio::test]
#[ignore = "Requires Redis infrastructure"]
async fn cooldown_expires_and_ticking_resumes() {
use std::sync::atomic::{AtomicUsize, Ordering};
use std::sync::Arc;
let ctx = crate::test_helpers::create_test_context("cooldown_expires").await;
let tick_count = Arc::new(AtomicUsize::new(0));
let mut node = CooldownNode::new(
Box::new(CountingFixedStatusBehavior {
status: NodeStatus::Success,
tick_count: tick_count.clone(),
}),
Duration::ZERO,
);
assert_eq!(node.tick(&ctx).await.unwrap(), NodeStatus::Success);
assert_eq!(tick_count.load(Ordering::SeqCst), 1);
assert_eq!(
node.tick(&ctx).await.unwrap(),
NodeStatus::Success,
"once the cooldown window elapses, ticking must resume normally"
);
assert_eq!(tick_count.load(Ordering::SeqCst), 2);
}
#[tokio::test]
#[ignore = "Requires Redis infrastructure"]
async fn cooldown_does_not_start_while_the_child_is_running() {
use std::sync::atomic::{AtomicUsize, Ordering};
use std::sync::Arc;
let ctx = crate::test_helpers::create_test_context("cooldown_running").await;
let tick_count = Arc::new(AtomicUsize::new(0));
let mut node = CooldownNode::new(
Box::new(CountingFixedStatusBehavior {
status: NodeStatus::Running,
tick_count: tick_count.clone(),
}),
Duration::from_secs(60),
);
assert_eq!(node.tick(&ctx).await.unwrap(), NodeStatus::Running);
assert_eq!(tick_count.load(Ordering::SeqCst), 1);
assert_eq!(
node.tick(&ctx).await.unwrap(),
NodeStatus::Running,
"Running must not start the cooldown clock, so the child must be ticked again"
);
assert_eq!(tick_count.load(Ordering::SeqCst), 2);
}
#[test]
fn cooldown_children_mut_exposes_the_child() {
let mut node = CooldownNode::new(Box::new(DummyBehavior), Duration::from_secs(1));
assert_eq!(node.children_mut().len(), 1);
}
#[test]
fn cooldown_forwards_its_childs_active_capability() {
let node = CooldownNode::new(Box::new(CapabilityBehavior("sweep")), Duration::from_secs(1));
assert_eq!(node.active_capability(), Some("sweep"));
}
#[test]
fn cooldown_reset_clears_cooldown_until() {
let mut node = CooldownNode::new(Box::new(DummyBehavior), Duration::from_secs(1));
node.cooldown_until = Some(Instant::now() + Duration::from_secs(60));
node.reset_self();
assert_eq!(node.cooldown_until, None);
}
}