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
8/// PlanPathNode - Plans a path to a target position (STUB)
9///
10/// This is a stub implementation for path planning.
11/// In a full implementation, this would calculate a collision-free path.
12///
13/// # Configuration
14///
15/// ```json
16/// {
17///   "type": "plan_path",
18///   "config": {
19///     "target_position": [5.0, 3.0, 0.0],
20///     "path_planning_algorithm": "a_star",
21///     "max_speed": 1.0
22///   }
23/// }
24/// ```
25#[derive(Debug)]
26pub struct PlanPathNode {
27    #[allow(dead_code)]
28    target_position: Vec<f64>,
29    #[allow(dead_code)]
30    algorithm: String,
31}
32
33#[derive(Debug, Serialize, Deserialize)]
34struct PlanPathConfig {
35    target_position: Vec<f64>,
36    #[serde(default = "default_algorithm")]
37    path_planning_algorithm: String,
38}
39
40fn default_algorithm() -> String {
41    "a_star".to_string()
42}
43
44impl PlanPathNode {
45    /// Create a new PlanPathNode from JSON configuration
46    pub fn from_json(config: Value) -> anyhow::Result<Self> {
47        let cfg: PlanPathConfig = serde_json::from_value(config)?;
48        Ok(Self {
49            target_position: cfg.target_position,
50            algorithm: cfg.path_planning_algorithm,
51        })
52    }
53}
54
55#[async_trait]
56impl BehaviorNode for PlanPathNode {
57    async fn tick(&mut self, _ctx: &Context) -> anyhow::Result<NodeStatus> {
58        // TODO: Implement actual path planning
59        // For now, return Success to allow behavior tree testing
60        tracing::debug!("PlanPathNode: planning path to {:?} (stub)", self.target_position);
61        Ok(NodeStatus::Success)
62    }
63
64    fn name(&self) -> &str {
65        "PlanPathNode"
66    }
67}
68
69/// FollowPathNode - Follows a planned path (STUB)
70///
71/// This is a stub implementation for path following.
72/// In a full implementation, this would execute the planned path.
73///
74/// # Configuration
75///
76/// ```json
77/// {
78///   "type": "follow_path",
79///   "config": {
80///     "max_speed": 1.0,
81///     "turn_rate": 0.5,
82///     "topic": "/motor/cmd_vel"
83///   }
84/// }
85/// ```
86#[derive(Debug)]
87pub struct FollowPathNode {
88    #[allow(dead_code)] // TODO: Use this when implementing path following
89    topic: String,
90    #[allow(dead_code)]
91    max_speed: f64,
92    #[allow(dead_code)]
93    turn_rate: f64,
94}
95
96#[derive(Debug, Serialize, Deserialize)]
97struct FollowPathConfig {
98    #[serde(default = "default_motor_topic")]
99    topic: String,
100    #[serde(default = "default_max_speed")]
101    max_speed: f64,
102    #[serde(default = "default_turn_rate")]
103    turn_rate: f64,
104}
105
106fn default_motor_topic() -> String {
107    "/motor/cmd_vel".to_string()
108}
109
110fn default_max_speed() -> f64 {
111    1.0
112}
113
114fn default_turn_rate() -> f64 {
115    0.5
116}
117
118impl FollowPathNode {
119    /// Create a new FollowPathNode from JSON configuration
120    pub fn from_json(config: Value) -> anyhow::Result<Self> {
121        let cfg: FollowPathConfig = serde_json::from_value(config)?;
122        Ok(Self {
123            topic: cfg.topic,
124            max_speed: cfg.max_speed,
125            turn_rate: cfg.turn_rate,
126        })
127    }
128}
129
130#[async_trait]
131impl BehaviorNode for FollowPathNode {
132    async fn tick(&mut self, _ctx: &Context) -> anyhow::Result<NodeStatus> {
133        // TODO: Implement actual path following
134        // For now, return Success to allow behavior tree testing
135        tracing::debug!("FollowPathNode: following path (stub)");
136        Ok(NodeStatus::Success)
137    }
138
139    fn name(&self) -> &str {
140        "FollowPathNode"
141    }
142}
143
144/// CheckArrivalNode - Checks if the robot has arrived at the target (STUB)
145///
146/// This is a stub implementation for arrival checking.
147/// In a full implementation, this would check the robot's position against the target.
148///
149/// # Configuration
150///
151/// ```json
152/// {
153///   "type": "check_arrival",
154///   "config": {
155///     "position_tolerance": 0.1,
156///     "angular_tolerance": 0.05
157///   }
158/// }
159/// ```
160#[derive(Debug)]
161pub struct CheckArrivalNode {
162    #[allow(dead_code)]
163    position_tolerance: f64,
164    #[allow(dead_code)]
165    angular_tolerance: f64,
166}
167
168#[derive(Debug, Serialize, Deserialize)]
169struct CheckArrivalConfig {
170    position_tolerance: f64,
171    angular_tolerance: f64,
172}
173
174impl CheckArrivalNode {
175    /// Create a new CheckArrivalNode from JSON configuration
176    pub fn from_json(config: Value) -> anyhow::Result<Self> {
177        let cfg: CheckArrivalConfig = serde_json::from_value(config)?;
178        Ok(Self {
179            position_tolerance: cfg.position_tolerance,
180            angular_tolerance: cfg.angular_tolerance,
181        })
182    }
183}
184
185#[async_trait]
186impl BehaviorNode for CheckArrivalNode {
187    async fn tick(&mut self, _ctx: &Context) -> anyhow::Result<NodeStatus> {
188        // TODO: Implement actual arrival checking
189        // For now, return Success to allow behavior tree testing
190        tracing::debug!("CheckArrivalNode: checking arrival (stub)");
191        Ok(NodeStatus::Success)
192    }
193
194    fn name(&self) -> &str {
195        "CheckArrivalNode"
196    }
197}
mecha10_behavior_runtime/actions/navigation.rs

mecha10_behavior_runtime/actions/
navigation.rs
