mecha10_behavior_runtime/actions/
movement.rs

1//! Action nodes that publish velocity commands to move the robot.
2
3use super::elapsed_timer::{reset_timer_field, ElapsedTimer};
4use crate::{BehaviorNode, Context, NodeStatus};
5use async_trait::async_trait;
6use mecha10_core::actuator::Twist;
7use mecha10_core::topics::Topic;
8use serde::{Deserialize, Serialize};
9use serde_json::Value;
10use std::time::{Duration, Instant};
11
12#[derive(Debug)]
13pub struct WanderNode {
14    topic: String,
15    linear_speed: f64,
16    angular_speed: f64,
17    change_interval: Duration,
18    last_change: Option<Instant>,
19    current_angular: f64,
20}
21
22#[derive(Debug, Serialize, Deserialize)]
23struct WanderConfig {
24    topic: String,
25    #[serde(default = "default_linear_speed")]
26    linear_speed: f64,
27    #[serde(default = "default_angular_speed")]
28    angular_speed: f64,
29    #[serde(default = "default_change_interval")]
30    change_interval_secs: f64,
31}
32
33fn default_linear_speed() -> f64 {
34    0.3
35}
36
37fn default_angular_speed() -> f64 {
38    0.5
39}
40
41fn default_change_interval() -> f64 {
42    3.0
43}
44
45impl WanderNode {
46    /// Create a new WanderNode from JSON configuration
47    pub fn from_json(config: Value) -> anyhow::Result<Self> {
48        let cfg: WanderConfig = serde_json::from_value(config)?;
49        Ok(Self {
50            topic: cfg.topic,
51            linear_speed: cfg.linear_speed,
52            angular_speed: cfg.angular_speed,
53            change_interval: Duration::from_secs_f64(cfg.change_interval_secs),
54            last_change: None,
55            current_angular: 0.0,
56        })
57    }
58}
59
60#[async_trait]
61impl BehaviorNode for WanderNode {
62    async fn tick(&mut self, ctx: &Context) -> anyhow::Result<NodeStatus> {
63        let now = Instant::now();
64
65        if self.last_change.is_none() || now.duration_since(self.last_change.unwrap()) >= self.change_interval {
66            self.current_angular = (rand::random::<f64>() * 2.0 - 1.0) * self.angular_speed;
67            self.last_change = Some(now);
68        }
69
70        let twist = Twist {
71            linear: self.linear_speed as f32,
72            angular: self.current_angular as f32,
73        };
74
75        let topic_static: &'static str = Box::leak(self.topic.clone().into_boxed_str());
76        let topic = Topic::<Twist>::new(topic_static);
77
78        ctx.publish_to(topic, &twist).await?;
79        Ok(NodeStatus::Running)
80    }
81
82    fn name(&self) -> &str {
83        "WanderNode"
84    }
85}
86
87#[derive(Debug, Default)]
88pub struct MoveNode {
89    topic: String,
90    linear: f64,
91    angular: f64,
92    duration: Option<Duration>,
93    timer: ElapsedTimer,
94}
95
96#[derive(Debug, Serialize, Deserialize)]
97struct MoveConfig {
98    topic: String,
99    linear: f64,
100    angular: f64,
101    duration_secs: Option<f64>,
102}
103
104impl MoveNode {
105    /// Create a new MoveNode from JSON configuration
106    pub fn from_json(config: Value) -> anyhow::Result<Self> {
107        let cfg: MoveConfig = serde_json::from_value(config)?;
108        Ok(Self {
109            topic: cfg.topic,
110            linear: cfg.linear,
111            angular: cfg.angular,
112            duration: cfg.duration_secs.map(Duration::from_secs_f64),
113            timer: ElapsedTimer::default(),
114        })
115    }
116}
117
118#[async_trait]
119impl BehaviorNode for MoveNode {
120    async fn tick(&mut self, ctx: &Context) -> anyhow::Result<NodeStatus> {
121        if let Some(duration) = self.duration {
122            if self.timer.elapsed_since_start(duration) {
123                return Ok(NodeStatus::Success);
124            }
125        }
126
127        let twist = Twist {
128            linear: self.linear as f32,
129            angular: self.angular as f32,
130        };
131
132        let topic_static: &'static str = Box::leak(self.topic.clone().into_boxed_str());
133        let topic = Topic::<Twist>::new(topic_static);
134
135        ctx.publish_to(topic, &twist).await?;
136
137        Ok(NodeStatus::Running)
138    }
139
140    reset_timer_field!(timer);
141
142    fn name(&self) -> &str {
143        "MoveNode"
144    }
145}
146
147#[derive(Debug)]
148pub struct StopNode {
149    topic: String,
150}
151
152#[derive(Debug, Serialize, Deserialize)]
153struct StopConfig {
154    topic: String,
155}
156
157impl StopNode {
158    /// Create a new StopNode from JSON configuration
159    pub fn from_json(config: Value) -> anyhow::Result<Self> {
160        let cfg: StopConfig = serde_json::from_value(config)?;
161        Ok(Self { topic: cfg.topic })
162    }
163}
164
165#[async_trait]
166impl BehaviorNode for StopNode {
167    async fn tick(&mut self, ctx: &Context) -> anyhow::Result<NodeStatus> {
168        let twist = Twist {
169            linear: 0.0,
170            angular: 0.0,
171        };
172
173        let topic_static: &'static str = Box::leak(self.topic.clone().into_boxed_str());
174        let topic = Topic::<Twist>::new(topic_static);
175
176        ctx.publish_to(topic, &twist).await?;
177        tracing::debug!("StopNode: published stop command");
178        Ok(NodeStatus::Success)
179    }
180
181    fn name(&self) -> &str {
182        "StopNode"
183    }
184}
mecha10_behavior_runtime/actions/movement.rs

mecha10_behavior_runtime/actions/
movement.rs
