mecha10_behavior_runtime/actions/
flight.rs

1//! Flight-stabilization action nodes for rotorcraft: `TakeoffNode` climbs to a target altitude,
2//! `LandNode` descends back to the ground, both using a PID altitude-hold loop
3//! ([`crate::control::PidController`]) against odometry feedback to compute per-rotor thrust.
4
5use crate::control::{PidController, PidGains};
6use crate::{BehaviorNode, Context, NodeStatus};
7use async_trait::async_trait;
8use mecha10_core::messages::Odometry;
9use mecha10_core::topics::Topic;
10use mecha10_core::Receiver;
11use serde::{Deserialize, Serialize};
12use serde_json::Value;
13use std::time::{Duration, Instant};
14
15/// Shared configuration fields for [`TakeoffNode`]/[`LandNode`] - everything except the
16/// altitude each one holds as its setpoint.
17#[derive(Debug, Serialize, Deserialize)]
18struct AltitudeHoldConfig {
19    /// Topic to publish per-rotor thrust commands to (a `Vec<f32>`, one entry per rotor, all
20    /// equal since this is a pure vertical-thrust hold with no attitude correction).
21    #[serde(default = "default_motor_cmd_topic")]
22    motor_cmd_topic: String,
23    /// Topic to read altitude feedback from (`Odometry.position[2]`).
24    #[serde(default = "default_odometry_topic")]
25    odometry_topic: String,
26    /// Number of rotors to publish a thrust value for - matches the 4-prop drone template.
27    #[serde(default = "default_rotor_count")]
28    rotor_count: usize,
29    /// Baseline thrust (fraction of `max_thrust`) assumed to roughly counteract gravity at
30    /// hover; the PID output is added on top of this rather than driving thrust from zero.
31    #[serde(default = "default_hover_thrust")]
32    hover_thrust: f64,
33    #[serde(default = "default_min_thrust")]
34    min_thrust: f64,
35    #[serde(default = "default_max_thrust")]
36    max_thrust: f64,
37    /// Altitude error (meters) within which the target is considered reached.
38    #[serde(default = "default_tolerance_m")]
39    tolerance_m: f64,
40    /// Give up and report `Failure` if the target altitude hasn't been reached within this many
41    /// seconds (e.g. the drone is stuck, or there's no odometry feed at all).
42    #[serde(default = "default_timeout_secs")]
43    timeout_secs: f64,
44    #[serde(default)]
45    pid: PidGains,
46}
47
48fn default_motor_cmd_topic() -> String {
49    "/actuator/motor_cmd".to_string()
50}
51
52fn default_odometry_topic() -> String {
53    "/sensor/odometry".to_string()
54}
55
56fn default_rotor_count() -> usize {
57    4
58}
59
60fn default_hover_thrust() -> f64 {
61    0.5
62}
63
64fn default_min_thrust() -> f64 {
65    0.0
66}
67
68fn default_max_thrust() -> f64 {
69    1.0
70}
71
72fn default_tolerance_m() -> f64 {
73    0.05
74}
75
76fn default_timeout_secs() -> f64 {
77    15.0
78}
79
80/// Outcome of one [`AltitudeHoldLoop::step`] call, separated from `tick()`'s `ctx` wiring
81/// (subscribe/publish) so the altitude/PID decision logic is unit-testable without a live
82/// `Context` - same split as `SensorCheckNode::evaluate` vs. `SensorCheckNode::tick`.
83#[derive(Debug, PartialEq)]
84enum AltitudeStep {
85    /// Not reached yet - publish this thrust fraction to every rotor and keep running.
86    Hold(f64),
87    /// Within tolerance of the target altitude.
88    Reached,
89    /// `timeout_secs` elapsed without reaching the target altitude.
90    TimedOut,
91}
92
93/// Shared altitude-hold control loop used by both [`TakeoffNode`] and [`LandNode`].
94struct AltitudeHoldLoop {
95    motor_cmd_topic: String,
96    odometry_topic: String,
97    rotor_count: usize,
98    hover_thrust: f64,
99    min_thrust: f64,
100    max_thrust: f64,
101    tolerance_m: f64,
102    timeout: Duration,
103    target_altitude_m: f64,
104    pid: PidController,
105    receiver: Option<Receiver<Odometry>>,
106    current_altitude_m: Option<f64>,
107    last_tick: Option<Instant>,
108    started_at: Option<Instant>,
109}
110
111impl std::fmt::Debug for AltitudeHoldLoop {
112    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
113        f.debug_struct("AltitudeHoldLoop")
114            .field("motor_cmd_topic", &self.motor_cmd_topic)
115            .field("odometry_topic", &self.odometry_topic)
116            .field("target_altitude_m", &self.target_altitude_m)
117            .field("current_altitude_m", &self.current_altitude_m)
118            .field("subscribed", &self.receiver.is_some())
119            .finish()
120    }
121}
122
123impl AltitudeHoldLoop {
124    fn new(config: AltitudeHoldConfig, target_altitude_m: f64) -> Self {
125        let pid = PidController::new(config.pid, (config.min_thrust, config.max_thrust))
126            .with_integral_limits((config.min_thrust, config.max_thrust));
127
128        Self {
129            motor_cmd_topic: config.motor_cmd_topic,
130            odometry_topic: config.odometry_topic,
131            rotor_count: config.rotor_count,
132            hover_thrust: config.hover_thrust,
133            min_thrust: config.min_thrust,
134            max_thrust: config.max_thrust,
135            tolerance_m: config.tolerance_m,
136            timeout: Duration::from_secs_f64(config.timeout_secs),
137            target_altitude_m,
138            pid,
139            receiver: None,
140            current_altitude_m: None,
141            last_tick: None,
142            started_at: None,
143        }
144    }
145
146    /// Drain the odometry receiver (keeping only the most recent reading) and advance the PID
147    /// loop by one step. Pure with respect to messaging infrastructure - operates on the
148    /// already-subscribed `Receiver` and wall-clock time only.
149    fn step(&mut self) -> AltitudeStep {
150        if let Some(receiver) = self.receiver.as_mut() {
151            while let Ok(odom) = receiver.try_recv() {
152                self.current_altitude_m = Some(odom.position[2] as f64);
153            }
154        }
155
156        let now = Instant::now();
157        let started_at = *self.started_at.get_or_insert(now);
158        if now.duration_since(started_at) >= self.timeout {
159            return AltitudeStep::TimedOut;
160        }
161
162        let Some(altitude) = self.current_altitude_m else {
163            return AltitudeStep::Hold(self.hover_thrust);
164        };
165
166        if (self.target_altitude_m - altitude).abs() <= self.tolerance_m {
167            return AltitudeStep::Reached;
168        }
169
170        let dt = self
171            .last_tick
172            .map(|previous| now.duration_since(previous).as_secs_f64())
173            .unwrap_or(0.0);
174        self.last_tick = Some(now);
175
176        let correction = self.pid.update(self.target_altitude_m, altitude, dt);
177        let thrust = (self.hover_thrust + correction).clamp(self.min_thrust, self.max_thrust);
178        AltitudeStep::Hold(thrust)
179    }
180
181    async fn publish_thrust(&self, ctx: &Context, thrust: f64) -> anyhow::Result<()> {
182        let thrusts = vec![thrust as f32; self.rotor_count];
183        let topic_static: &'static str = Box::leak(self.motor_cmd_topic.clone().into_boxed_str());
184        ctx.publish_to(Topic::<Vec<f32>>::new(topic_static), &thrusts).await?;
185        Ok(())
186    }
187
188    async fn ensure_subscribed(&mut self, ctx: &Context) -> anyhow::Result<()> {
189        if self.receiver.is_none() {
190            self.receiver = Some(ctx.subscribe_raw::<Odometry>(&self.odometry_topic).await?);
191        }
192        Ok(())
193    }
194
195    fn reset(&mut self) {
196        self.pid.reset();
197        self.current_altitude_m = None;
198        self.last_tick = None;
199        self.started_at = None;
200    }
201
202    #[cfg(test)]
203    fn with_receiver(config: AltitudeHoldConfig, target_altitude_m: f64, receiver: Receiver<Odometry>) -> Self {
204        let mut loop_ = Self::new(config, target_altitude_m);
205        loop_.receiver = Some(receiver);
206        loop_
207    }
208}
209
210/// Climbs from the ground to `target_altitude_m` using a PID altitude-hold loop, publishing
211/// per-rotor thrust to `motor_cmd_topic` until the target is reached (`Success`) or
212/// `timeout_secs` elapses without reaching it (`Failure`).
213pub struct TakeoffNode {
214    control: AltitudeHoldLoop,
215}
216
217#[derive(Debug, Serialize, Deserialize)]
218struct TakeoffConfig {
219    target_altitude_m: f64,
220    #[serde(flatten)]
221    shared: AltitudeHoldConfig,
222}
223
224impl std::fmt::Debug for TakeoffNode {
225    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
226        f.debug_struct("TakeoffNode").field("control", &self.control).finish()
227    }
228}
229
230impl TakeoffNode {
231    /// Create a new TakeoffNode from JSON configuration
232    pub fn from_json(config: Value) -> anyhow::Result<Self> {
233        let cfg: TakeoffConfig = serde_json::from_value(config)?;
234        Ok(Self {
235            control: AltitudeHoldLoop::new(cfg.shared, cfg.target_altitude_m),
236        })
237    }
238}
239
240#[async_trait]
241impl BehaviorNode for TakeoffNode {
242    async fn tick(&mut self, ctx: &Context) -> anyhow::Result<NodeStatus> {
243        self.control.ensure_subscribed(ctx).await?;
244
245        match self.control.step() {
246            AltitudeStep::Hold(thrust) => {
247                self.control.publish_thrust(ctx, thrust).await?;
248                Ok(NodeStatus::Running)
249            }
250            AltitudeStep::Reached => {
251                tracing::debug!("TakeoffNode: reached target altitude");
252                Ok(NodeStatus::Success)
253            }
254            AltitudeStep::TimedOut => {
255                tracing::warn!("TakeoffNode: timed out before reaching target altitude");
256                Ok(NodeStatus::Failure)
257            }
258        }
259    }
260
261    fn reset_self(&mut self) {
262        self.control.reset();
263    }
264
265    fn name(&self) -> &str {
266        "TakeoffNode"
267    }
268}
269
270/// Descends from the current altitude down to `ground_altitude_m` (default `0.0`) using the
271/// same PID altitude-hold loop as [`TakeoffNode`], publishing a zero-thrust command once
272/// touchdown is detected so the rotors don't keep holding hover thrust at ground level.
273pub struct LandNode {
274    control: AltitudeHoldLoop,
275}
276
277#[derive(Debug, Serialize, Deserialize)]
278struct LandConfig {
279    #[serde(default)]
280    ground_altitude_m: f64,
281    #[serde(flatten)]
282    shared: AltitudeHoldConfig,
283}
284
285impl std::fmt::Debug for LandNode {
286    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
287        f.debug_struct("LandNode").field("control", &self.control).finish()
288    }
289}
290
291impl LandNode {
292    /// Create a new LandNode from JSON configuration
293    pub fn from_json(config: Value) -> anyhow::Result<Self> {
294        let cfg: LandConfig = serde_json::from_value(config)?;
295        Ok(Self {
296            control: AltitudeHoldLoop::new(cfg.shared, cfg.ground_altitude_m),
297        })
298    }
299}
300
301#[async_trait]
302impl BehaviorNode for LandNode {
303    async fn tick(&mut self, ctx: &Context) -> anyhow::Result<NodeStatus> {
304        self.control.ensure_subscribed(ctx).await?;
305
306        match self.control.step() {
307            AltitudeStep::Hold(thrust) => {
308                self.control.publish_thrust(ctx, thrust).await?;
309                Ok(NodeStatus::Running)
310            }
311            AltitudeStep::Reached => {
312                tracing::debug!("LandNode: touched down, cutting rotor thrust");
313                self.control.publish_thrust(ctx, 0.0).await?;
314                Ok(NodeStatus::Success)
315            }
316            AltitudeStep::TimedOut => {
317                tracing::warn!("LandNode: timed out before reaching ground altitude");
318                Ok(NodeStatus::Failure)
319            }
320        }
321    }
322
323    fn reset_self(&mut self) {
324        self.control.reset();
325    }
326
327    fn name(&self) -> &str {
328        "LandNode"
329    }
330}
331
332#[cfg(test)]
333mod tests {
334    use super::*;
335    use mecha10_core::Receiver;
336    use serde_json::json;
337    use tokio::sync::mpsc;
338
339    fn default_config() -> AltitudeHoldConfig {
340        AltitudeHoldConfig {
341            motor_cmd_topic: default_motor_cmd_topic(),
342            odometry_topic: default_odometry_topic(),
343            rotor_count: 4,
344            hover_thrust: 0.5,
345            min_thrust: 0.0,
346            max_thrust: 1.0,
347            tolerance_m: 0.05,
348            timeout_secs: 15.0,
349            pid: PidGains {
350                kp: 0.5,
351                ki: 0.1,
352                kd: 0.05,
353            },
354        }
355    }
356
357    fn odometry_at(altitude: f32) -> Odometry {
358        Odometry {
359            timestamp: 0,
360            position: [0.0, 0.0, altitude],
361            orientation: [0.0, 0.0, 0.0, 1.0],
362            linear_velocity: [0.0, 0.0, 0.0],
363            angular_velocity: [0.0, 0.0, 0.0],
364            frame_id: None,
365            child_frame_id: None,
366        }
367    }
368
369    fn loop_with_channel(target_altitude_m: f64) -> (AltitudeHoldLoop, mpsc::UnboundedSender<Odometry>) {
370        let (tx, rx) = mpsc::unbounded_channel();
371        let control = AltitudeHoldLoop::with_receiver(default_config(), target_altitude_m, Receiver::from(rx));
372        (control, tx)
373    }
374
375    #[test]
376    fn no_reading_yet_holds_at_hover_thrust() {
377        let (mut control, _tx) = loop_with_channel(1.0);
378        assert_eq!(control.step(), AltitudeStep::Hold(0.5));
379    }
380
381    #[test]
382    fn below_target_climbs_with_thrust_above_hover() {
383        let (mut control, tx) = loop_with_channel(1.0);
384        tx.send(odometry_at(0.0)).unwrap();
385
386        match control.step() {
387            AltitudeStep::Hold(thrust) => assert!(thrust > 0.5, "expected climb thrust above hover, got {thrust}"),
388            other => panic!("expected Hold, got {other:?}"),
389        }
390    }
391
392    #[test]
393    fn within_tolerance_reports_reached() {
394        let (mut control, tx) = loop_with_channel(1.0);
395        tx.send(odometry_at(0.98)).unwrap();
396        assert_eq!(control.step(), AltitudeStep::Reached);
397    }
398
399    #[test]
400    fn exact_target_reports_reached() {
401        let (mut control, tx) = loop_with_channel(1.0);
402        tx.send(odometry_at(1.0)).unwrap();
403        assert_eq!(control.step(), AltitudeStep::Reached);
404    }
405
406    #[test]
407    fn overshoot_above_target_still_within_tolerance_is_reached() {
408        let (mut control, tx) = loop_with_channel(1.0);
409        tx.send(odometry_at(1.03)).unwrap();
410        assert_eq!(control.step(), AltitudeStep::Reached);
411    }
412
413    #[test]
414    fn thrust_is_clamped_to_configured_max() {
415        let mut config = default_config();
416        config.pid.kp = 100.0;
417        let (tx, rx) = mpsc::unbounded_channel();
418        let mut control = AltitudeHoldLoop::with_receiver(config, 100.0, Receiver::from(rx));
419        tx.send(odometry_at(0.0)).unwrap();
420
421        match control.step() {
422            AltitudeStep::Hold(thrust) => assert!(thrust <= 1.0),
423            other => panic!("expected Hold, got {other:?}"),
424        }
425    }
426
427    #[test]
428    fn later_reading_overrides_earlier_one() {
429        let (mut control, tx) = loop_with_channel(1.0);
430        tx.send(odometry_at(0.0)).unwrap();
431        tx.send(odometry_at(1.0)).unwrap();
432
433        assert_eq!(control.step(), AltitudeStep::Reached);
434    }
435
436    #[test]
437    fn timed_out_after_timeout_elapses_without_reaching_target() {
438        let mut config = default_config();
439        config.timeout_secs = 0.0;
440        let (tx, rx) = mpsc::unbounded_channel();
441        let mut control = AltitudeHoldLoop::with_receiver(config, 1.0, Receiver::from(rx));
442        tx.send(odometry_at(0.0)).unwrap();
443
444        assert_eq!(control.step(), AltitudeStep::TimedOut);
445    }
446
447    #[test]
448    fn reset_clears_altitude_and_pid_state() {
449        let (mut control, tx) = loop_with_channel(1.0);
450        tx.send(odometry_at(1.0)).unwrap();
451        assert_eq!(control.step(), AltitudeStep::Reached);
452
453        control.reset();
454        assert_eq!(control.current_altitude_m, None);
455    }
456
457    #[test]
458    fn takeoff_from_json_parses_defaults() {
459        let config = json!({ "target_altitude_m": 1.5 });
460        assert!(TakeoffNode::from_json(config).is_ok());
461    }
462
463    #[test]
464    fn takeoff_from_json_requires_target_altitude() {
465        let config = json!({});
466        assert!(TakeoffNode::from_json(config).is_err());
467    }
468
469    #[test]
470    fn land_from_json_defaults_ground_altitude_to_zero() {
471        let config = json!({});
472        let node = LandNode::from_json(config).unwrap();
473        assert_eq!(node.control.target_altitude_m, 0.0);
474    }
475
476    #[test]
477    fn land_from_json_accepts_custom_ground_altitude() {
478        let config = json!({ "ground_altitude_m": 0.2 });
479        let node = LandNode::from_json(config).unwrap();
480        assert_eq!(node.control.target_altitude_m, 0.2);
481    }
482}
mecha10_behavior_runtime/actions/flight.rs

mecha10_behavior_runtime/actions/
flight.rs
