mecha10_behavior_runtime/
actions.rs

1//! Built-in action nodes for behavior trees
2//!
3//! This module provides common action nodes that can be used in behavior tree compositions.
4//! All nodes implement the `BehaviorNode` trait and can be registered in the NodeRegistry.
5
6use crate::{BehaviorNode, BoxedBehavior, Context, NodeRegistry, NodeStatus};
7use async_trait::async_trait;
8use mecha10_core::actuator::Twist;
9use mecha10_core::topics::Topic;
10use serde::{Deserialize, Serialize};
11use serde_json::Value;
12use std::time::{Duration, Instant};
13
14/// Register all built-in action nodes with a registry
15///
16/// This function registers the following nodes:
17/// - `wander`: WanderNode
18/// - `move`: MoveNode
19/// - `sensor_check`: SensorCheckNode
20/// - `timer`: TimerNode
21/// - `idle`: IdleNode
22/// - `pause`: PauseNode
23/// - `stop`: StopNode
24/// - `plan_path`: PlanPathNode
25/// - `follow_path`: FollowPathNode
26/// - `check_arrival`: CheckArrivalNode
27///
28/// # Example
29///
30/// ```rust
31/// use mecha10_behavior_runtime::{NodeRegistry, actions};
32///
33/// let mut registry = NodeRegistry::new();
34/// actions::register_builtin_actions(&mut registry);
35/// ```
36pub fn register_builtin_actions(registry: &mut NodeRegistry) {
37    registry.register("wander", |config: Value| -> anyhow::Result<BoxedBehavior> {
38        Ok(Box::new(WanderNode::from_json(config)?))
39    });
40
41    registry.register("move", |config: Value| -> anyhow::Result<BoxedBehavior> {
42        Ok(Box::new(MoveNode::from_json(config)?))
43    });
44
45    registry.register("sensor_check", |config: Value| -> anyhow::Result<BoxedBehavior> {
46        Ok(Box::new(SensorCheckNode::from_json(config)?))
47    });
48
49    registry.register("timer", |config: Value| -> anyhow::Result<BoxedBehavior> {
50        Ok(Box::new(TimerNode::from_json(config)?))
51    });
52
53    registry.register("idle", |config: Value| -> anyhow::Result<BoxedBehavior> {
54        Ok(Box::new(IdleNode::from_json(config)?))
55    });
56
57    registry.register("pause", |config: Value| -> anyhow::Result<BoxedBehavior> {
58        Ok(Box::new(PauseNode::from_json(config)?))
59    });
60
61    registry.register("stop", |config: Value| -> anyhow::Result<BoxedBehavior> {
62        Ok(Box::new(StopNode::from_json(config)?))
63    });
64
65    registry.register("plan_path", |config: Value| -> anyhow::Result<BoxedBehavior> {
66        Ok(Box::new(PlanPathNode::from_json(config)?))
67    });
68
69    registry.register("follow_path", |config: Value| -> anyhow::Result<BoxedBehavior> {
70        Ok(Box::new(FollowPathNode::from_json(config)?))
71    });
72
73    registry.register("check_arrival", |config: Value| -> anyhow::Result<BoxedBehavior> {
74        Ok(Box::new(CheckArrivalNode::from_json(config)?))
75    });
76}
77
78/// WanderNode - Generates random movement commands
79///
80/// This node publishes random velocity commands to make a robot wander around.
81/// It periodically changes direction to create natural wandering behavior.
82///
83/// # Configuration
84///
85/// ```json
86/// {
87///   "type": "wander",
88///   "config": {
89///     "topic": "/motor/cmd_vel",
90///     "linear_speed": 0.3,
91///     "angular_speed": 0.5,
92///     "change_interval_secs": 3.0
93///   }
94/// }
95/// ```
96#[derive(Debug)]
97pub struct WanderNode {
98    topic: String,
99    linear_speed: f64,
100    angular_speed: f64,
101    change_interval: Duration,
102    last_change: Option<Instant>,
103    current_angular: f64,
104}
105
106#[derive(Debug, Serialize, Deserialize)]
107struct WanderConfig {
108    topic: String,
109    #[serde(default = "default_linear_speed")]
110    linear_speed: f64,
111    #[serde(default = "default_angular_speed")]
112    angular_speed: f64,
113    #[serde(default = "default_change_interval")]
114    change_interval_secs: f64,
115}
116
117fn default_linear_speed() -> f64 {
118    0.3
119}
120
121fn default_angular_speed() -> f64 {
122    0.5
123}
124
125fn default_change_interval() -> f64 {
126    3.0
127}
128
129impl WanderNode {
130    /// Create a new WanderNode from JSON configuration
131    pub fn from_json(config: Value) -> anyhow::Result<Self> {
132        let cfg: WanderConfig = serde_json::from_value(config)?;
133        Ok(Self {
134            topic: cfg.topic,
135            linear_speed: cfg.linear_speed,
136            angular_speed: cfg.angular_speed,
137            change_interval: Duration::from_secs_f64(cfg.change_interval_secs),
138            last_change: None,
139            current_angular: 0.0,
140        })
141    }
142}
143
144#[async_trait]
145impl BehaviorNode for WanderNode {
146    async fn tick(&mut self, ctx: &Context) -> anyhow::Result<NodeStatus> {
147        let now = Instant::now();
148
149        // Change direction periodically
150        if self.last_change.is_none() || now.duration_since(self.last_change.unwrap()) >= self.change_interval {
151            // Generate random angular velocity between -angular_speed and +angular_speed
152            self.current_angular = (rand::random::<f64>() * 2.0 - 1.0) * self.angular_speed;
153            self.last_change = Some(now);
154        }
155
156        // Publish velocity command
157        let twist = Twist {
158            linear: self.linear_speed as f32,
159            angular: self.current_angular as f32,
160        };
161
162        // Leak topic path for 'static lifetime
163        let topic_static: &'static str = Box::leak(self.topic.clone().into_boxed_str());
164        let topic = Topic::<Twist>::new(topic_static);
165
166        ctx.publish_to(topic, &twist).await?;
167        Ok(NodeStatus::Running)
168    }
169
170    fn name(&self) -> &str {
171        "WanderNode"
172    }
173}
174
175/// MoveNode - Executes a velocity command
176///
177/// Publishes a fixed velocity command to control robot movement.
178///
179/// # Configuration
180///
181/// ```json
182/// {
183///   "type": "move",
184///   "config": {
185///     "topic": "/motor/cmd_vel",
186///     "linear": 0.5,
187///     "angular": 0.0,
188///     "duration_secs": 2.0
189///   }
190/// }
191/// ```
192#[derive(Debug)]
193pub struct MoveNode {
194    topic: String,
195    linear: f64,
196    angular: f64,
197    duration: Option<Duration>,
198    start_time: Option<Instant>,
199}
200
201#[derive(Debug, Serialize, Deserialize)]
202struct MoveConfig {
203    topic: String,
204    linear: f64,
205    angular: f64,
206    duration_secs: Option<f64>,
207}
208
209impl MoveNode {
210    /// Create a new MoveNode from JSON configuration
211    pub fn from_json(config: Value) -> anyhow::Result<Self> {
212        let cfg: MoveConfig = serde_json::from_value(config)?;
213        Ok(Self {
214            topic: cfg.topic,
215            linear: cfg.linear,
216            angular: cfg.angular,
217            duration: cfg.duration_secs.map(Duration::from_secs_f64),
218            start_time: None,
219        })
220    }
221}
222
223#[async_trait]
224impl BehaviorNode for MoveNode {
225    async fn tick(&mut self, ctx: &Context) -> anyhow::Result<NodeStatus> {
226        let now = Instant::now();
227
228        // Initialize start time on first tick
229        if self.start_time.is_none() {
230            self.start_time = Some(now);
231        }
232
233        // Check if duration has elapsed
234        if let Some(duration) = self.duration {
235            if now.duration_since(self.start_time.unwrap()) >= duration {
236                return Ok(NodeStatus::Success);
237            }
238        }
239
240        // Publish velocity command
241        let twist = Twist {
242            linear: self.linear as f32,
243            angular: self.angular as f32,
244        };
245
246        // Leak topic path for 'static lifetime
247        let topic_static: &'static str = Box::leak(self.topic.clone().into_boxed_str());
248        let topic = Topic::<Twist>::new(topic_static);
249
250        ctx.publish_to(topic, &twist).await?;
251
252        Ok(NodeStatus::Running)
253    }
254
255    async fn reset(&mut self) -> anyhow::Result<()> {
256        self.start_time = None;
257        Ok(())
258    }
259
260    fn name(&self) -> &str {
261        "MoveNode"
262    }
263}
264
265/// SensorCheckNode - Checks sensor state
266///
267/// Subscribes to a sensor topic and checks if a condition is met.
268///
269/// # Configuration
270///
271/// ```json
272/// {
273///   "type": "sensor_check",
274///   "config": {
275///     "topic": "/sensors/obstacle",
276///     "field": "distance",
277///     "operator": "less_than",
278///     "threshold": 0.5
279///   }
280/// }
281/// ```
282#[derive(Debug)]
283pub struct SensorCheckNode {
284    #[allow(dead_code)] // TODO: Implement sensor subscription
285    topic: String,
286    field: String,
287    operator: ComparisonOperator,
288    threshold: f64,
289}
290
291#[derive(Debug, Serialize, Deserialize)]
292#[serde(rename_all = "snake_case")]
293enum ComparisonOperator {
294    LessThan,
295    GreaterThan,
296    Equal,
297}
298
299#[derive(Debug, Serialize, Deserialize)]
300struct SensorCheckConfig {
301    topic: String,
302    field: String,
303    operator: ComparisonOperator,
304    threshold: f64,
305}
306
307impl SensorCheckNode {
308    /// Create a new SensorCheckNode from JSON configuration
309    pub fn from_json(config: Value) -> anyhow::Result<Self> {
310        let cfg: SensorCheckConfig = serde_json::from_value(config)?;
311        Ok(Self {
312            topic: cfg.topic,
313            field: cfg.field,
314            operator: cfg.operator,
315            threshold: cfg.threshold,
316        })
317    }
318}
319
320#[async_trait]
321impl BehaviorNode for SensorCheckNode {
322    async fn tick(&mut self, _ctx: &Context) -> anyhow::Result<NodeStatus> {
323        // TODO: Implement sensor checking once we have a get_latest_from_path method
324        // For now, return Success to allow testing of behavior tree composition
325        tracing::debug!(
326            "SensorCheckNode: checking {} {} {}",
327            self.field,
328            match self.operator {
329                ComparisonOperator::LessThan => "<",
330                ComparisonOperator::GreaterThan => ">",
331                ComparisonOperator::Equal => "==",
332            },
333            self.threshold
334        );
335        Ok(NodeStatus::Success)
336    }
337
338    fn name(&self) -> &str {
339        "SensorCheckNode"
340    }
341}
342
343/// TimerNode - Waits for a specified duration
344///
345/// Returns Running until the duration elapses, then returns Success.
346///
347/// # Configuration
348///
349/// ```json
350/// {
351///   "type": "timer",
352///   "config": {
353///     "duration_secs": 5.0
354///   }
355/// }
356/// ```
357#[derive(Debug)]
358pub struct TimerNode {
359    duration: Duration,
360    start_time: Option<Instant>,
361}
362
363#[derive(Debug, Serialize, Deserialize)]
364struct TimerConfig {
365    duration_secs: f64,
366}
367
368impl TimerNode {
369    /// Create a new TimerNode from JSON configuration
370    pub fn from_json(config: Value) -> anyhow::Result<Self> {
371        let cfg: TimerConfig = serde_json::from_value(config)?;
372        Ok(Self {
373            duration: Duration::from_secs_f64(cfg.duration_secs),
374            start_time: None,
375        })
376    }
377}
378
379#[async_trait]
380impl BehaviorNode for TimerNode {
381    async fn tick(&mut self, _ctx: &Context) -> anyhow::Result<NodeStatus> {
382        let now = Instant::now();
383
384        // Initialize start time on first tick
385        if self.start_time.is_none() {
386            self.start_time = Some(now);
387        }
388
389        // Check if duration has elapsed
390        if now.duration_since(self.start_time.unwrap()) >= self.duration {
391            Ok(NodeStatus::Success)
392        } else {
393            Ok(NodeStatus::Running)
394        }
395    }
396
397    async fn reset(&mut self) -> anyhow::Result<()> {
398        self.start_time = None;
399        Ok(())
400    }
401
402    fn name(&self) -> &str {
403        "TimerNode"
404    }
405}
406
407/// IdleNode - Returns Success immediately
408///
409/// This node does nothing and returns Success on every tick.
410/// Useful for testing and as a placeholder in behavior trees.
411///
412/// # Configuration
413///
414/// ```json
415/// {
416///   "type": "idle",
417///   "config": {}
418/// }
419/// ```
420#[derive(Debug)]
421pub struct IdleNode;
422
423impl IdleNode {
424    /// Create a new IdleNode from JSON configuration
425    pub fn from_json(_config: Value) -> anyhow::Result<Self> {
426        Ok(Self)
427    }
428}
429
430#[async_trait]
431impl BehaviorNode for IdleNode {
432    async fn tick(&mut self, _ctx: &Context) -> anyhow::Result<NodeStatus> {
433        Ok(NodeStatus::Success)
434    }
435
436    fn name(&self) -> &str {
437        "IdleNode"
438    }
439}
440
441/// PauseNode - Pauses execution for a specified duration
442///
443/// Similar to TimerNode but semantically represents a deliberate pause in behavior.
444/// Returns Running until the duration elapses, then returns Success.
445///
446/// # Configuration
447///
448/// ```json
449/// {
450///   "type": "pause",
451///   "config": {
452///     "duration_secs": 2.0
453///   }
454/// }
455/// ```
456#[derive(Debug)]
457pub struct PauseNode {
458    duration: Duration,
459    start_time: Option<Instant>,
460}
461
462#[derive(Debug, Serialize, Deserialize)]
463struct PauseConfig {
464    duration_secs: f64,
465}
466
467impl PauseNode {
468    /// Create a new PauseNode from JSON configuration
469    pub fn from_json(config: Value) -> anyhow::Result<Self> {
470        let cfg: PauseConfig = serde_json::from_value(config)?;
471        Ok(Self {
472            duration: Duration::from_secs_f64(cfg.duration_secs),
473            start_time: None,
474        })
475    }
476}
477
478#[async_trait]
479impl BehaviorNode for PauseNode {
480    async fn tick(&mut self, _ctx: &Context) -> anyhow::Result<NodeStatus> {
481        let now = Instant::now();
482
483        // Initialize start time on first tick
484        if self.start_time.is_none() {
485            self.start_time = Some(now);
486            tracing::debug!("PauseNode: starting pause for {:?}", self.duration);
487        }
488
489        // Check if duration has elapsed
490        if now.duration_since(self.start_time.unwrap()) >= self.duration {
491            tracing::debug!("PauseNode: pause complete");
492            Ok(NodeStatus::Success)
493        } else {
494            Ok(NodeStatus::Running)
495        }
496    }
497
498    async fn reset(&mut self) -> anyhow::Result<()> {
499        self.start_time = None;
500        Ok(())
501    }
502
503    fn name(&self) -> &str {
504        "PauseNode"
505    }
506}
507
508/// StopNode - Publishes zero velocity to stop the robot
509///
510/// This node publishes a zero velocity command to stop the robot immediately.
511/// Returns Success after publishing the stop command.
512///
513/// # Configuration
514///
515/// ```json
516/// {
517///   "type": "stop",
518///   "config": {
519///     "topic": "/motor/cmd_vel"
520///   }
521/// }
522/// ```
523#[derive(Debug)]
524pub struct StopNode {
525    topic: String,
526}
527
528#[derive(Debug, Serialize, Deserialize)]
529struct StopConfig {
530    topic: String,
531}
532
533impl StopNode {
534    /// Create a new StopNode from JSON configuration
535    pub fn from_json(config: Value) -> anyhow::Result<Self> {
536        let cfg: StopConfig = serde_json::from_value(config)?;
537        Ok(Self { topic: cfg.topic })
538    }
539}
540
541#[async_trait]
542impl BehaviorNode for StopNode {
543    async fn tick(&mut self, ctx: &Context) -> anyhow::Result<NodeStatus> {
544        // Publish zero velocity
545        let twist = Twist {
546            linear: 0.0,
547            angular: 0.0,
548        };
549
550        // Leak topic path for 'static lifetime
551        let topic_static: &'static str = Box::leak(self.topic.clone().into_boxed_str());
552        let topic = Topic::<Twist>::new(topic_static);
553
554        ctx.publish_to(topic, &twist).await?;
555        tracing::debug!("StopNode: published stop command");
556        Ok(NodeStatus::Success)
557    }
558
559    fn name(&self) -> &str {
560        "StopNode"
561    }
562}
563
564/// PlanPathNode - Plans a path to a target position (STUB)
565///
566/// This is a stub implementation for path planning.
567/// In a full implementation, this would calculate a collision-free path.
568///
569/// # Configuration
570///
571/// ```json
572/// {
573///   "type": "plan_path",
574///   "config": {
575///     "target_position": [5.0, 3.0, 0.0],
576///     "path_planning_algorithm": "a_star",
577///     "max_speed": 1.0
578///   }
579/// }
580/// ```
581#[derive(Debug)]
582pub struct PlanPathNode {
583    #[allow(dead_code)]
584    target_position: Vec<f64>,
585    #[allow(dead_code)]
586    algorithm: String,
587}
588
589#[derive(Debug, Serialize, Deserialize)]
590struct PlanPathConfig {
591    target_position: Vec<f64>,
592    #[serde(default = "default_algorithm")]
593    path_planning_algorithm: String,
594}
595
596fn default_algorithm() -> String {
597    "a_star".to_string()
598}
599
600impl PlanPathNode {
601    /// Create a new PlanPathNode from JSON configuration
602    pub fn from_json(config: Value) -> anyhow::Result<Self> {
603        let cfg: PlanPathConfig = serde_json::from_value(config)?;
604        Ok(Self {
605            target_position: cfg.target_position,
606            algorithm: cfg.path_planning_algorithm,
607        })
608    }
609}
610
611#[async_trait]
612impl BehaviorNode for PlanPathNode {
613    async fn tick(&mut self, _ctx: &Context) -> anyhow::Result<NodeStatus> {
614        // TODO: Implement actual path planning
615        // For now, return Success to allow behavior tree testing
616        tracing::debug!("PlanPathNode: planning path to {:?} (stub)", self.target_position);
617        Ok(NodeStatus::Success)
618    }
619
620    fn name(&self) -> &str {
621        "PlanPathNode"
622    }
623}
624
625/// FollowPathNode - Follows a planned path (STUB)
626///
627/// This is a stub implementation for path following.
628/// In a full implementation, this would execute the planned path.
629///
630/// # Configuration
631///
632/// ```json
633/// {
634///   "type": "follow_path",
635///   "config": {
636///     "max_speed": 1.0,
637///     "turn_rate": 0.5,
638///     "topic": "/motor/cmd_vel"
639///   }
640/// }
641/// ```
642#[derive(Debug)]
643pub struct FollowPathNode {
644    #[allow(dead_code)] // TODO: Use this when implementing path following
645    topic: String,
646    #[allow(dead_code)]
647    max_speed: f64,
648    #[allow(dead_code)]
649    turn_rate: f64,
650}
651
652#[derive(Debug, Serialize, Deserialize)]
653struct FollowPathConfig {
654    #[serde(default = "default_motor_topic")]
655    topic: String,
656    #[serde(default = "default_max_speed")]
657    max_speed: f64,
658    #[serde(default = "default_turn_rate")]
659    turn_rate: f64,
660}
661
662fn default_motor_topic() -> String {
663    "/motor/cmd_vel".to_string()
664}
665
666fn default_max_speed() -> f64 {
667    1.0
668}
669
670fn default_turn_rate() -> f64 {
671    0.5
672}
673
674impl FollowPathNode {
675    /// Create a new FollowPathNode from JSON configuration
676    pub fn from_json(config: Value) -> anyhow::Result<Self> {
677        let cfg: FollowPathConfig = serde_json::from_value(config)?;
678        Ok(Self {
679            topic: cfg.topic,
680            max_speed: cfg.max_speed,
681            turn_rate: cfg.turn_rate,
682        })
683    }
684}
685
686#[async_trait]
687impl BehaviorNode for FollowPathNode {
688    async fn tick(&mut self, _ctx: &Context) -> anyhow::Result<NodeStatus> {
689        // TODO: Implement actual path following
690        // For now, return Success to allow behavior tree testing
691        tracing::debug!("FollowPathNode: following path (stub)");
692        Ok(NodeStatus::Success)
693    }
694
695    fn name(&self) -> &str {
696        "FollowPathNode"
697    }
698}
699
700/// CheckArrivalNode - Checks if the robot has arrived at the target (STUB)
701///
702/// This is a stub implementation for arrival checking.
703/// In a full implementation, this would check the robot's position against the target.
704///
705/// # Configuration
706///
707/// ```json
708/// {
709///   "type": "check_arrival",
710///   "config": {
711///     "position_tolerance": 0.1,
712///     "angular_tolerance": 0.05
713///   }
714/// }
715/// ```
716#[derive(Debug)]
717pub struct CheckArrivalNode {
718    #[allow(dead_code)]
719    position_tolerance: f64,
720    #[allow(dead_code)]
721    angular_tolerance: f64,
722}
723
724#[derive(Debug, Serialize, Deserialize)]
725struct CheckArrivalConfig {
726    position_tolerance: f64,
727    angular_tolerance: f64,
728}
729
730impl CheckArrivalNode {
731    /// Create a new CheckArrivalNode from JSON configuration
732    pub fn from_json(config: Value) -> anyhow::Result<Self> {
733        let cfg: CheckArrivalConfig = serde_json::from_value(config)?;
734        Ok(Self {
735            position_tolerance: cfg.position_tolerance,
736            angular_tolerance: cfg.angular_tolerance,
737        })
738    }
739}
740
741#[async_trait]
742impl BehaviorNode for CheckArrivalNode {
743    async fn tick(&mut self, _ctx: &Context) -> anyhow::Result<NodeStatus> {
744        // TODO: Implement actual arrival checking
745        // For now, return Success to allow behavior tree testing
746        tracing::debug!("CheckArrivalNode: checking arrival (stub)");
747        Ok(NodeStatus::Success)
748    }
749
750    fn name(&self) -> &str {
751        "CheckArrivalNode"
752    }
753}
mecha10_behavior_runtime/actions.rs

mecha10_behavior_runtime/
actions.rs
