mecha10_behavior_runtime/actions/
navigation.rs

1//! Stub navigation action nodes for path planning, following, and arrival checks.
2
3use crate::{BehaviorNode, Context, NodeStatus, TreeStatusSnapshot};
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    last_status: Option<NodeStatus>,
22}
23
24#[derive(Debug, Serialize, Deserialize)]
25struct PlanPathConfig {
26    target_position: Vec<f64>,
27    #[serde(default = "default_algorithm")]
28    path_planning_algorithm: String,
29}
30
31fn default_algorithm() -> String {
32    "a_star".to_string()
33}
34
35impl PlanPathNode {
36    /// Create a new PlanPathNode from JSON configuration
37    pub fn from_json(config: Value) -> anyhow::Result<Self> {
38        let cfg: PlanPathConfig = serde_json::from_value(config)?;
39        Ok(Self {
40            target_position: cfg.target_position,
41            algorithm: cfg.path_planning_algorithm,
42            last_status: None,
43        })
44    }
45}
46
47#[async_trait]
48impl BehaviorNode for PlanPathNode {
49    async fn tick(&mut self, _ctx: &Context) -> anyhow::Result<NodeStatus> {
50        tracing::debug!("PlanPathNode: planning path to {:?} (stub)", self.target_position);
51        self.last_status = Some(NodeStatus::Success);
52        Ok(NodeStatus::Success)
53    }
54
55    fn name(&self) -> &str {
56        "PlanPathNode"
57    }
58
59    fn status_snapshot(&self) -> TreeStatusSnapshot {
60        TreeStatusSnapshot {
61            name: self.name().to_string(),
62            status: self.last_status,
63            active_capability: self.active_capability().map(String::from),
64            children: Vec::new(),
65        }
66    }
67}
68
69#[derive(Debug)]
70pub struct FollowPathNode {
71    #[allow(dead_code)]
72    topic: String,
73    #[allow(dead_code)]
74    max_speed: f64,
75    #[allow(dead_code)]
76    turn_rate: f64,
77    last_status: Option<NodeStatus>,
78}
79
80#[derive(Debug, Serialize, Deserialize)]
81struct FollowPathConfig {
82    #[serde(default = "default_motor_topic")]
83    topic: String,
84    #[serde(default = "default_max_speed")]
85    max_speed: f64,
86    #[serde(default = "default_turn_rate")]
87    turn_rate: f64,
88}
89
90fn default_motor_topic() -> String {
91    "/motor/cmd_vel".to_string()
92}
93
94fn default_max_speed() -> f64 {
95    1.0
96}
97
98fn default_turn_rate() -> f64 {
99    0.5
100}
101
102impl FollowPathNode {
103    /// Create a new FollowPathNode from JSON configuration
104    pub fn from_json(config: Value) -> anyhow::Result<Self> {
105        let cfg: FollowPathConfig = serde_json::from_value(config)?;
106        Ok(Self {
107            topic: cfg.topic,
108            max_speed: cfg.max_speed,
109            turn_rate: cfg.turn_rate,
110            last_status: None,
111        })
112    }
113}
114
115#[async_trait]
116impl BehaviorNode for FollowPathNode {
117    async fn tick(&mut self, _ctx: &Context) -> anyhow::Result<NodeStatus> {
118        self.last_status = Some(NodeStatus::Success);
119        stub_tick_success!("FollowPathNode: following path (stub)")
120    }
121
122    fn name(&self) -> &str {
123        "FollowPathNode"
124    }
125
126    fn status_snapshot(&self) -> TreeStatusSnapshot {
127        TreeStatusSnapshot {
128            name: self.name().to_string(),
129            status: self.last_status,
130            active_capability: self.active_capability().map(String::from),
131            children: Vec::new(),
132        }
133    }
134}
135
136#[derive(Debug)]
137pub struct CheckArrivalNode {
138    #[allow(dead_code)]
139    position_tolerance: f64,
140    #[allow(dead_code)]
141    angular_tolerance: f64,
142    last_status: Option<NodeStatus>,
143}
144
145#[derive(Debug, Serialize, Deserialize)]
146struct CheckArrivalConfig {
147    position_tolerance: f64,
148    angular_tolerance: f64,
149}
150
151impl CheckArrivalNode {
152    /// Create a new CheckArrivalNode from JSON configuration
153    pub fn from_json(config: Value) -> anyhow::Result<Self> {
154        let cfg: CheckArrivalConfig = serde_json::from_value(config)?;
155        Ok(Self {
156            position_tolerance: cfg.position_tolerance,
157            angular_tolerance: cfg.angular_tolerance,
158            last_status: None,
159        })
160    }
161}
162
163#[async_trait]
164impl BehaviorNode for CheckArrivalNode {
165    async fn tick(&mut self, _ctx: &Context) -> anyhow::Result<NodeStatus> {
166        self.last_status = Some(NodeStatus::Success);
167        stub_tick_success!("CheckArrivalNode: checking arrival (stub)")
168    }
169
170    fn name(&self) -> &str {
171        "CheckArrivalNode"
172    }
173
174    fn status_snapshot(&self) -> TreeStatusSnapshot {
175        TreeStatusSnapshot {
176            name: self.name().to_string(),
177            status: self.last_status,
178            active_capability: self.active_capability().map(String::from),
179            children: Vec::new(),
180        }
181    }
182}
mecha10_behavior_runtime/actions/navigation.rs

mecha10_behavior_runtime/actions/
navigation.rs
