mecha10_behavior_runtime/actions/
navigation.rs

1//! Stub navigation action nodes for path planning, following, and arrival checks.
2
3use crate::{BehaviorNode, Context, NodeStatus};
4use async_trait::async_trait;
5use serde::{Deserialize, Serialize};
6use serde_json::Value;
7
8macro_rules! stub_tick_success {
9    ($msg:literal) => {{
10        tracing::debug!($msg);
11        Ok(NodeStatus::Success)
12    }};
13}
14
15#[derive(Debug)]
16pub struct PlanPathNode {
17    #[allow(dead_code)]
18    target_position: Vec<f64>,
19    #[allow(dead_code)]
20    algorithm: String,
21}
22
23#[derive(Debug, Serialize, Deserialize)]
24struct PlanPathConfig {
25    target_position: Vec<f64>,
26    #[serde(default = "default_algorithm")]
27    path_planning_algorithm: String,
28}
29
30fn default_algorithm() -> String {
31    "a_star".to_string()
32}
33
34impl PlanPathNode {
35    /// Create a new PlanPathNode from JSON configuration
36    pub fn from_json(config: Value) -> anyhow::Result<Self> {
37        let cfg: PlanPathConfig = serde_json::from_value(config)?;
38        Ok(Self {
39            target_position: cfg.target_position,
40            algorithm: cfg.path_planning_algorithm,
41        })
42    }
43}
44
45#[async_trait]
46impl BehaviorNode for PlanPathNode {
47    async fn tick(&mut self, _ctx: &Context) -> anyhow::Result<NodeStatus> {
48        tracing::debug!("PlanPathNode: planning path to {:?} (stub)", self.target_position);
49        Ok(NodeStatus::Success)
50    }
51
52    fn name(&self) -> &str {
53        "PlanPathNode"
54    }
55}
56
57#[derive(Debug)]
58pub struct FollowPathNode {
59    #[allow(dead_code)]
60    topic: String,
61    #[allow(dead_code)]
62    max_speed: f64,
63    #[allow(dead_code)]
64    turn_rate: f64,
65}
66
67#[derive(Debug, Serialize, Deserialize)]
68struct FollowPathConfig {
69    #[serde(default = "default_motor_topic")]
70    topic: String,
71    #[serde(default = "default_max_speed")]
72    max_speed: f64,
73    #[serde(default = "default_turn_rate")]
74    turn_rate: f64,
75}
76
77fn default_motor_topic() -> String {
78    "/motor/cmd_vel".to_string()
79}
80
81fn default_max_speed() -> f64 {
82    1.0
83}
84
85fn default_turn_rate() -> f64 {
86    0.5
87}
88
89impl FollowPathNode {
90    /// Create a new FollowPathNode from JSON configuration
91    pub fn from_json(config: Value) -> anyhow::Result<Self> {
92        let cfg: FollowPathConfig = serde_json::from_value(config)?;
93        Ok(Self {
94            topic: cfg.topic,
95            max_speed: cfg.max_speed,
96            turn_rate: cfg.turn_rate,
97        })
98    }
99}
100
101#[async_trait]
102impl BehaviorNode for FollowPathNode {
103    async fn tick(&mut self, _ctx: &Context) -> anyhow::Result<NodeStatus> {
104        stub_tick_success!("FollowPathNode: following path (stub)")
105    }
106
107    fn name(&self) -> &str {
108        "FollowPathNode"
109    }
110}
111
112#[derive(Debug)]
113pub struct CheckArrivalNode {
114    #[allow(dead_code)]
115    position_tolerance: f64,
116    #[allow(dead_code)]
117    angular_tolerance: f64,
118}
119
120#[derive(Debug, Serialize, Deserialize)]
121struct CheckArrivalConfig {
122    position_tolerance: f64,
123    angular_tolerance: f64,
124}
125
126impl CheckArrivalNode {
127    /// Create a new CheckArrivalNode from JSON configuration
128    pub fn from_json(config: Value) -> anyhow::Result<Self> {
129        let cfg: CheckArrivalConfig = serde_json::from_value(config)?;
130        Ok(Self {
131            position_tolerance: cfg.position_tolerance,
132            angular_tolerance: cfg.angular_tolerance,
133        })
134    }
135}
136
137#[async_trait]
138impl BehaviorNode for CheckArrivalNode {
139    async fn tick(&mut self, _ctx: &Context) -> anyhow::Result<NodeStatus> {
140        stub_tick_success!("CheckArrivalNode: checking arrival (stub)")
141    }
142
143    fn name(&self) -> &str {
144        "CheckArrivalNode"
145    }
146}
mecha10_behavior_runtime/actions/navigation.rs

mecha10_behavior_runtime/actions/
navigation.rs
