agent_tk/definitions/
tst.rs

1//! Definitions of tsts, for de/serialization and manipulation.
2
3use crate::uuid;
4use enum_dispatch::enum_dispatch;
5use serde::{Deserialize, Serialize};
6use serde_with::skip_serializing_none;
7
8mod consts;
9
10pub use crate::definitions::misc_structures::{GeoPoint, Speed};
11
12//  _   _           _
13// | \ | | ___   __| | ___
14// |  \| |/ _ \ / _` |/ _ \
15// | |\  | (_) | (_| |  __/
16// |_| \_|\___/ \__,_|\___|
17
18/// Represent a TST node
19#[derive(Serialize, Deserialize, Debug, Clone)]
20#[serde(tag = "name")]
21#[enum_dispatch(NodeTrait)]
22pub enum Node
23{
24  /// Noop node
25  #[serde(rename = "noop")]
26  Noop(Noop),
27  /// Sequential node
28  #[serde(rename = "seq")]
29  Seq(Seq),
30  /// Concurrent node
31  #[serde(rename = "conc")]
32  Conc(Conc),
33  /// MoveTo node
34  #[serde(rename = "move-to")]
35  MoveTo(MoveTo),
36  /// SearchArea node
37  #[serde(rename = "search-area")]
38  SearchArea(SearchArea),
39}
40
41impl Node
42{
43  /// Convenient function to convert a node to a JSON string
44  pub fn to_json_string(&self) -> serde_json::Result<String>
45  {
46    serde_json::to_string(self)
47  }
48  /// Convenient function to convert a JSON string to a node
49  pub fn from_json_string(def: impl AsRef<str>) -> serde_json::Result<Node>
50  {
51    serde_json::from_str(def.as_ref())
52  }
53}
54
55impl Default for Node
56{
57  fn default() -> Self
58  {
59    Self::Noop(Noop::default())
60  }
61}
62
63//  ____                                _
64// |  _ \ __ _ _ __ __ _ _ __ ___   ___| |_ ___ _ __ ___
65// | |_) / _` | '__/ _` | '_ ` _ \ / _ \ __/ _ \ '__/ __|
66// |  __/ (_| | | | (_| | | | | | |  __/ ||  __/ |  \__ \
67// |_|   \__,_|_|  \__,_|_| |_| |_|\___|\__\___|_|  |___/
68
69/// Parameters common to all the nodes
70#[skip_serializing_none]
71#[derive(Serialize, Deserialize, Debug, Clone, Default)]
72pub struct CommonParameters
73{
74  /// Unique identifier for the node
75  pub node_uuid: uuid::Uuid,
76  /// Execution unit
77  pub execunit: Option<String>,
78}
79
80/// Parameters for the move to tst
81#[skip_serializing_none]
82#[derive(Serialize, Deserialize, Debug, Clone, Default)]
83pub struct MoveToParameters
84{
85  /// Destination
86  pub waypoint: GeoPoint,
87  /// Optional speed
88  pub speed: Option<Speed>,
89}
90
91/// Parmeters for the search area test
92#[skip_serializing_none]
93#[derive(Serialize, Deserialize, Debug, Clone, Default)]
94pub struct SearchAreaParameters
95{
96  /// Polygon to explore
97  pub area: Vec<GeoPoint>,
98  /// Optional speed
99  pub speed: Option<Speed>,
100}
101
102/// Use for nodes without parameters
103#[derive(Serialize, Deserialize, Debug, Clone, Default)]
104pub struct NoParameters {}
105
106//  _   _           _      ____        _ _     _
107// | \ | | ___   __| | ___| __ ) _   _(_) | __| | ___ _ __ ___
108// |  \| |/ _ \ / _` |/ _ \  _ \| | | | | |/ _` |/ _ \ '__/ __|
109// | |\  | (_) | (_| |  __/ |_) | |_| | | | (_| |  __/ |  \__ \
110// |_| \_|\___/ \__,_|\___|____/ \__,_|_|_|\__,_|\___|_|  |___/
111
112/// This class is used to build tsts
113#[allow(private_bounds)]
114#[derive(Debug, Clone)]
115pub struct NodeBuilder<T: CompositeNode, P: NodeBuilderTrait>
116{
117  t: std::rc::Rc<std::cell::RefCell<T>>,
118  p: Option<P>,
119}
120
121/// Root null builder
122#[derive(Clone)]
123pub struct NullNodeBuilder;
124
125//  _____          _ _
126// |_   _| __ __ _(_) |_ ___
127//   | || '__/ _` | | __/ __|
128//   | || | | (_| | | |_\__ \
129//   |_||_|  \__,_|_|\__|___/
130
131trait NodeBuilderTrait: Clone
132{
133  fn append_child(&mut self, node: impl Into<Node>);
134}
135
136/// Base trait for nodes
137#[enum_dispatch]
138pub trait NodeTrait: Clone + Into<Node> + Default
139{
140  /// Return a reference to the common params of a node
141  fn common_params_ref(&self) -> &CommonParameters;
142  /// Set the comment parameter for a node
143  fn set_common_params(&mut self, params: CommonParameters);
144}
145
146/// Composite node, aka, node with children such as Conc and Seq
147#[allow(private_bounds)]
148pub trait CompositeNode: NodeTrait + IntoIterator + CompositeNodeBuildable
149{
150  /// Start building a tree
151  fn build(common_params: Option<CommonParameters>) -> NodeBuilder<Self, NullNodeBuilder>;
152
153  /// Returns an iterator over the children of the composite node.
154  ///
155  /// The iterator yields all items from start to end.
156  fn iter(&self) -> core::slice::Iter<'_, Node>;
157}
158
159trait CompositeNodeBuildable
160{
161  /// Add a child node
162  fn append_child(&mut self, node: impl Into<Node>);
163}
164
165trait LeafNode: NodeTrait
166{
167  type ParametersType: Clone + Default;
168  fn set_params(&mut self, params: Self::ParametersType);
169}
170
171trait LeafNodeParameters
172{
173  type NodeType: Default + LeafNode + Into<Node>;
174}
175
176// Nodes
177
178macro_rules! define_composite_node {
179  ($node_type:ident, $node_enum_type:path) => {
180    /// Node for $node_type
181    #[derive(Serialize, Deserialize, Debug, Clone, Default)]
182    pub struct $node_type
183    {
184      /// Common parameters
185      pub common_params: CommonParameters,
186      /// Childrent of the composite node
187      pub children: Vec<Node>,
188    }
189    impl $node_type
190    {
191      #[allow(private_interfaces)]
192      /// Start building a node
193      pub fn build(
194        common_params: Option<CommonParameters>,
195      ) -> NodeBuilder<$node_type, NullNodeBuilder>
196      {
197        Self::build_(common_params)
198      }
199      #[allow(private_interfaces)]
200      fn build_(common_params: Option<CommonParameters>)
201      -> NodeBuilder<$node_type, NullNodeBuilder>
202      {
203        let mut t = $node_type::default();
204        if let Some(common_params) = common_params
205        {
206          t.common_params = common_params;
207        }
208        NodeBuilder::<$node_type, NullNodeBuilder> {
209          t: std::rc::Rc::new(std::cell::RefCell::new(t)),
210          p: None,
211        }
212      }
213    }
214    impl NodeTrait for $node_type
215    {
216      fn common_params_ref(&self) -> &CommonParameters
217      {
218        &self.common_params
219      }
220      fn set_common_params(&mut self, common_params: CommonParameters)
221      {
222        self.common_params = common_params;
223      }
224    }
225    impl CompositeNodeBuildable for $node_type
226    {
227      fn append_child(&mut self, node: impl Into<Node>)
228      {
229        self.children.push(node.into());
230      }
231    }
232    impl CompositeNode for $node_type
233    {
234      #[allow(private_interfaces)]
235      fn build(common_params: Option<CommonParameters>)
236      -> NodeBuilder<$node_type, NullNodeBuilder>
237      {
238        Self::build_(common_params)
239      }
240      fn iter(&self) -> core::slice::Iter<'_, Node>
241      {
242        self.children.iter()
243      }
244    }
245
246    impl IntoIterator for $node_type
247    {
248      type Item = Node;
249      type IntoIter = std::vec::IntoIter<Node>;
250      fn into_iter(self) -> Self::IntoIter
251      {
252        self.children.into_iter()
253      }
254    }
255  };
256}
257
258macro_rules! define_leaf_node {
259  ($node_type:ident, $node_parameters_type:ident, $node_enum_type:path) => {
260    #[derive(Serialize, Deserialize, Debug, Clone, Default)]
261    /// Node for $node_type
262    pub struct $node_type
263    {
264      /// Common parameters
265      pub common_params: CommonParameters,
266      /// Parameters specific to the node
267      pub params: $node_parameters_type,
268    }
269    impl NodeTrait for $node_type
270    {
271      fn common_params_ref(&self) -> &CommonParameters
272      {
273        &self.common_params
274      }
275      fn set_common_params(&mut self, common_params: CommonParameters)
276      {
277        self.common_params = common_params;
278      }
279    }
280    impl LeafNode for $node_type
281    {
282      type ParametersType = $node_parameters_type;
283      fn set_params(&mut self, params: Self::ParametersType)
284      {
285        self.params = params;
286      }
287    }
288    impl LeafNodeParameters for $node_parameters_type
289    {
290      type NodeType = $node_type;
291    }
292  };
293}
294
295define_composite_node!(Seq, Node::Seq);
296define_composite_node!(Conc, Node::Conc);
297
298define_leaf_node!(MoveTo, MoveToParameters, Node::MoveTo);
299define_leaf_node!(SearchArea, SearchAreaParameters, Node::SearchArea);
300define_leaf_node!(Noop, NoParameters, Node::Noop);
301
302// Implementation
303
304/// Trait used to compute the velocity
305pub trait SpeedCompute
306{
307  /// Compute the actual velocity based on the maximum velocity of the platform
308  fn compute_velocity(&self, max_velocity: f32) -> f32;
309}
310
311impl SpeedCompute for Option<Speed>
312{
313  fn compute_velocity(&self, max_velocity: f32) -> f32
314  {
315    match self
316    {
317      Some(v) => match v
318      {
319        Speed::Slow => consts::VELOCITY_SLOW_RATIO * max_velocity,
320        Speed::Standard => consts::VELOCITY_STANDARD_RATIO * max_velocity,
321        Speed::Fast => consts::VELOCITY_FAST_RATIO * max_velocity,
322        Speed::MaxSpeed(s) => s.min(max_velocity),
323      },
324      None => consts::VELOCITY_STANDARD_RATIO * max_velocity,
325    }
326  }
327}
328
329#[allow(missing_docs)]
330#[inline]
331pub fn default<T: Default>() -> T
332{
333  Default::default()
334}
335
336impl NodeBuilderTrait for NullNodeBuilder
337{
338  fn append_child(&mut self, _node: impl Into<Node>)
339  {
340    panic!("Cannot add child to null builder.");
341  }
342}
343
344impl<T: CompositeNode, P: NodeBuilderTrait> NodeBuilderTrait for NodeBuilder<T, P>
345{
346  fn append_child(&mut self, node: impl Into<Node>)
347  {
348    self.t.borrow_mut().append_child(node);
349  }
350}
351
352#[allow(private_bounds)]
353impl<T: CompositeNode, P: NodeBuilderTrait> NodeBuilder<T, P>
354{
355  /// Start a composite node, must be followed by end
356  pub fn start<U: CompositeNode>(
357    self,
358    cp: Option<CommonParameters>,
359  ) -> NodeBuilder<U, NodeBuilder<T, P>>
360  {
361    let mut t = U::default();
362    if let Some(cp) = cp
363    {
364      t.set_common_params(cp);
365    }
366    NodeBuilder::<U, NodeBuilder<T, P>> {
367      t: std::rc::Rc::new(std::cell::RefCell::new(t)),
368      p: Some(self),
369    }
370  }
371  /// end a composite node
372  pub fn end(self) -> P
373  {
374    let mut p = self.p.unwrap();
375    p.append_child(self.t.borrow().to_owned());
376    p
377  }
378  /// Add a leaf node
379  #[allow(private_interfaces)]
380  pub fn add<U>(self, p: U, cp: Option<CommonParameters>) -> Self
381  where
382    U: LeafNodeParameters + Into<<<U as LeafNodeParameters>::NodeType as LeafNode>::ParametersType>,
383  {
384    let mut t = U::NodeType::default();
385    t.set_params(p.into());
386    if let Some(cp) = cp
387    {
388      t.set_common_params(cp);
389    }
390    self.t.borrow_mut().append_child(t);
391    self
392  }
393  /// Add to the node
394  pub fn add_node(self, n: Node) -> Self
395  {
396    self.t.borrow_mut().append_child(n);
397    self
398  }
399  /// Convert to the type
400  fn to_t(&self) -> T
401  {
402    self.t.borrow().clone()
403  }
404}
405
406impl<T: CompositeNode, U: NodeBuilderTrait> From<NodeBuilder<T, U>> for Node
407where
408  T: Into<Node>,
409{
410  fn from(value: NodeBuilder<T, U>) -> Self
411  {
412    value.to_t().into()
413  }
414}
415
416// Tests
417
418#[cfg(test)]
419mod tests
420{
421  use crate::definitions::tst::CommonParameters;
422
423  use super::GeoPoint;
424
425  macro_rules! get_tst_node {
426    ($value:expr, $variant:path) => {
427      match $value
428      {
429        $variant(x) => x,
430        _ => panic!("Unexpected TST Node."),
431      }
432    };
433  }
434
435  macro_rules! check_speed {
436    ($value:expr, $variant:path) => {
437      match $value.params.speed.as_ref().unwrap()
438      {
439        $variant => (),
440        _ => panic!("Wrong speed"),
441      }
442    };
443  }
444
445  macro_rules! check_max_speed {
446    ($value:expr, $max_speed:expr) => {
447      match $value.params.speed.as_ref().unwrap()
448      {
449        super::Speed::MaxSpeed(v) => assert_eq!(*v, $max_speed),
450        _ => panic!("Wrong speed"),
451      }
452    };
453  }
454
455  fn mgp(longitude: f64, latitude: f64, altitude: f64) -> GeoPoint
456  {
457    GeoPoint {
458      longitude,
459      latitude,
460      altitude,
461    }
462  }
463  #[test]
464  fn parse_test_tst()
465  {
466    let message = include_str!("../../../../data/test_tst.json");
467    let node: super::Node = serde_json::from_str(message).unwrap();
468    let node_seq = get_tst_node!(node, super::Node::Seq);
469
470    // First move
471    let seq_move_to_0 = get_tst_node!(&node_seq.children[0], super::Node::MoveTo);
472    assert_eq!(
473      seq_move_to_0.params.waypoint,
474      mgp(15.570883999999998, 58.394741999999994, 100.8)
475    );
476    check_speed!(seq_move_to_0, super::Speed::Standard);
477
478    // Concurrent
479    let seq_conc = get_tst_node!(&node_seq.children[1], super::Node::Conc);
480    let conc_searc_area = get_tst_node!(&seq_conc.children[0], super::Node::SearchArea);
481    assert_eq!(
482      conc_searc_area.params.area[0],
483      mgp(15.570883999999998, 58.394741999999994, 100.8)
484    );
485    assert_eq!(
486      conc_searc_area.params.area[1],
487      mgp(15.570883999999998, 58.30474199999999, 100.8)
488    );
489    assert_eq!(
490      conc_searc_area.params.area[2],
491      mgp(15.500883999999998, 58.30474199999999, 100.8)
492    );
493    check_max_speed!(conc_searc_area, 10.0);
494
495    let conc_move_to = get_tst_node!(&seq_conc.children[1], super::Node::MoveTo);
496    assert_eq!(
497      conc_move_to.params.waypoint,
498      mgp(15.571883999999998, 58.39474299999999, 100.8)
499    );
500    assert!(conc_move_to.params.speed.is_none());
501
502    // Second move
503    let seq_move_to_2 = get_tst_node!(&node_seq.children[2], super::Node::MoveTo);
504    assert_eq!(
505      seq_move_to_2.params.waypoint,
506      mgp(16.572884, 59.39474169999999, 110.8)
507    );
508    check_speed!(seq_move_to_2, super::Speed::Fast);
509  }
510  #[test]
511  fn gen_tst()
512  {
513    let node = super::Node::Seq(super::Seq {
514      common_params: CommonParameters {
515        node_uuid: crate::uuid::Uuid::from_string("f3bab5e0-a861-4f6e-bf28-37c9e3470181").unwrap(),
516        ..Default::default()
517      },
518      ..Default::default()
519    });
520    assert_eq!(
521      serde_json::to_string(&node).unwrap(),
522      r#"{"name":"seq","common_params":{"node_uuid":"f3bab5e0-a861-4f6e-bf28-37c9e3470181"},"children":[]}"#
523    );
524  }
525  #[test]
526  fn test_builder()
527  {
528    let node: super::Node = super::Conc::build(Some(CommonParameters {
529      node_uuid: crate::uuid::Uuid::from_string("68eb7e83-cc44-4404-bdb1-2fd66ffcbf1c").unwrap(),
530      ..Default::default()
531    }))
532    .start::<super::Seq>(Some(CommonParameters {
533      node_uuid: crate::uuid::Uuid::from_string("a5665c27-e2c6-442f-8ab3-357aa4427945").unwrap(),
534      ..Default::default()
535    }))
536    .add(
537      super::MoveToParameters {
538        waypoint: GeoPoint {
539          longitude: 16.4,
540          latitude: 59.3,
541          altitude: 110.8,
542        },
543
544        ..super::default()
545      },
546      Some(CommonParameters {
547        node_uuid: crate::uuid::Uuid::from_string("0cd50294-611a-4da6-b398-73b0f2e0ee7c").unwrap(),
548        ..Default::default()
549      }),
550    )
551    .add(
552      super::MoveToParameters {
553        waypoint: GeoPoint {
554          longitude: 16.5,
555          latitude: 59.5,
556          altitude: 110.7,
557        },
558        ..super::default()
559      },
560      Some(CommonParameters {
561        node_uuid: crate::uuid::Uuid::from_string("47288783-cfbd-48ad-9bb9-734fae54d2be").unwrap(),
562        ..Default::default()
563      }),
564    )
565    .end()
566    .add(
567      super::MoveToParameters {
568        waypoint: GeoPoint {
569          longitude: 16.6,
570          latitude: 59.4,
571          altitude: 110.9,
572        },
573        ..super::default()
574      },
575      Some(CommonParameters {
576        node_uuid: crate::uuid::Uuid::from_string("88eae25f-473f-4185-bfcb-ca3ee0ed2247").unwrap(),
577        ..Default::default()
578      }),
579    )
580    .into();
581    assert_eq!(
582      serde_json::to_string(&node).unwrap(),
583      r#"{"name":"conc","common_params":{"node_uuid":"68eb7e83-cc44-4404-bdb1-2fd66ffcbf1c"},"children":[{"name":"seq","common_params":{"node_uuid":"a5665c27-e2c6-442f-8ab3-357aa4427945"},"children":[{"name":"move-to","common_params":{"node_uuid":"0cd50294-611a-4da6-b398-73b0f2e0ee7c"},"params":{"waypoint":{"longitude":16.4,"latitude":59.3,"altitude":110.8}}},{"name":"move-to","common_params":{"node_uuid":"47288783-cfbd-48ad-9bb9-734fae54d2be"},"params":{"waypoint":{"longitude":16.5,"latitude":59.5,"altitude":110.7}}}]},{"name":"move-to","common_params":{"node_uuid":"88eae25f-473f-4185-bfcb-ca3ee0ed2247"},"params":{"waypoint":{"longitude":16.6,"latitude":59.4,"altitude":110.9}}}]}"#
584    );
585  }
586}
agent_tk/definitions/tst.rs

agent_tk/definitions/
tst.rs
