Merge pull request #13 from DLR-FT/xmloutput

Xmloutput

Cargo.toml

@@ -19,3 +19,5 @@ pest_derive = "2.7.9"
 csv = "1.2.2"
 clap = "2.33.3"
 num-format = "0.4.4"
+toml = "0.8.13"
+serde = { version = "1.0", features = ["derive"] }

README.md

@@ -65,6 +65,8 @@ We can also replay the flight by, e.g.,  ``python visualize_ship_landing.py live
 
 For more information call ``python visualize_ship_landing.py --help``.
 
+> Using ``--toml <FILE>`` as command-line-argument generates a .toml-file for the computed segmentations.
+
 ## Folder Structure
 - [figs](figs) are resources used for this readme document
 - [res](res) contains the logfiles used in the HSCC paper
@@ -89,6 +91,7 @@ For more information call ``python visualize_ship_landing.py --help``.
     - [functions.rs](src/functions.rs) is used to represent atomic functions that are used by the TBT parser
     - [parser.rs](src/parser.rs) is a TBT parser that reads a .tbt-file and produces a ```Tree```
     - [tbt.pest](src/tbt.pest) represents the grammar used by the TBT parser
+    - [toml_out.rs](src/toml_out.rs) is used to produce a .toml-file
     - [csv_reader.rs](src/csv_reader.rs) represent auxiliary functions such as reading a csv-file
     - [table.rs](src/table.rs) represents the main data structure for the dynamic programming
     - [user_defined/](src/user_defined/) is an example implementation for the *UserProvidedFunctions* required by [lib.rs](src/lib.rs)

specification/stl_formula_45Deg.tbt

@@ -0,0 +1,97 @@
+// SPDX-FileCopyrightText: 2023 German Aerospace Center (DLR)
+// SPDX-License-Identifier: Apache-2.0
+
+Deg45_HeightAboveShip = 20.0;
+Deg45_DistanceToShip = 30.0;
+Deg45_AngleToShip = 135.0;
+Deg45_AboveTouchdown = 20.0;
+
+Lateral_HeightAboveShip = 20.0;
+Lateral_DistanceToShip = 20.0;
+Lateral_AngleToShip = 90.0;
+Lateral_AboveTouchdown = 20.0;
+
+Oblique_HeightAboveShip = 20.0;
+Oblique_DistanceToShip = 30.0;
+Oblique_AngleToShip = 135.0;
+Oblique_AngleOblique = 45.0;
+Oblique_AboveTouchdown = 20.0;
+
+Straight_HeightAboveShip = 20.0;
+Straight_DistanceToShip = 20.0;
+Straight_AngleToShip = 180.0;
+Straight_AboveTouchdown = 20.0;
+
+In_Position_Pos_Slack = 2.5;
+In_Position_HeadingAligned_Slack = 1.0;
+In_Position_HeadingOblique_Slack = 1.0;
+In_Position_Velocity_Slack = 2.0;
+Move_to_Touchdown_Slack = 2.5;
+Descend_Slack = 1.0;
+
+
+
+
+<>  
+(
+    in_pos(uas_x, uas_y, uas_z, ship_x, ship_y, ship_z, ship_heading)
+    [
+        In_Position_Pos_Slack - 
+        !sqrt(
+            (ship_x + Deg45_DistanceToShip * !cos(!degToRad(Deg45_AngleToShip) + ship_heading) - uas_x)^2 +
+            (ship_y + Deg45_DistanceToShip * !sin(!degToRad(Deg45_AngleToShip) + ship_heading) - uas_y)^2 + 
+            (ship_z + Deg45_HeightAboveShip  - uas_z)^2
+        )
+    ]
+    &
+    O (
+        [0..5]  combined_inpos_ha_va(uas_x, uas_y, uas_z, uas_u, uas_v, uas_w, uas_heading, ship_x, ship_y, ship_z, ship_u, ship_v, ship_w, ship_heading)
+                [
+                    !min(
+                        In_Position_Pos_Slack - 
+                        !sqrt(
+                            (ship_x + Deg45_DistanceToShip * !cos(!degToRad(Deg45_AngleToShip) + ship_heading) - uas_x)^2 +
+                            (ship_y + Deg45_DistanceToShip * !sin(!degToRad(Deg45_AngleToShip) + ship_heading) - uas_y)^2 + 
+                            (ship_z + Deg45_HeightAboveShip  - uas_z)^2
+                        )
+                        , 
+                        !min(
+                            In_Position_HeadingAligned_Slack - !abs(!radToDeg(uas_heading - ship_heading))
+                            , 
+                            In_Position_Velocity_Slack - !abs(!sqrt( uas_u^2 + uas_v^2 + uas_w^2) - !sqrt( ship_u^2 + ship_v^2 + ship_w^2))
+                        )
+                    )
+                ]
+        & 
+        OOOOO (
+             <>  combined_movetp_ha(uas_x, uas_y, uas_z, uas_heading, ship_x, ship_y, ship_z, ship_heading)
+                [
+                    !min(
+                        Move_to_Touchdown_Slack - 
+                        !sqrt(
+                            (uas_x - ship_x)^2 +
+                            (uas_y - ship_y)^2 +
+                            (uas_z - (ship_z + Deg45_AboveTouchdown))^2
+                        )
+                    ,
+                        In_Position_HeadingAligned_Slack - !abs(!radToDeg(uas_heading - ship_heading))
+                    )
+                ]
+                &
+                O (
+                    <> descend(uas_x, uas_y, uas_z, ship_x, ship_y, ship_z)
+        [
+            Descend_Slack - 
+            !sqrt(
+                (uas_x - ship_x)^2 +
+                (uas_y - ship_y)^2 +
+                (uas_z - ship_z )^2
+            )
+        ]
+                )
+        )
+    )
+)
+
+
+

src/behaviortree.rs

@@ -6,7 +6,7 @@ use std::{collections::HashMap, time::SystemTime};
 
 type SubtreeIdx = usize;
 pub type Segmentation<'a> = Vec<(&'a TbtNode, usize, usize, f32)>;
-pub type ClonedSegmentation = Vec<(TbtNode, usize, usize, f32)>;
+pub type ClonedSegmentation = (f32, Vec<(TbtNode, usize, usize, f32)>);
 static mut NODECOUNT: SubtreeIdx = 0;
 /// Gets formula count
 fn gnc() -> SubtreeIdx {
@@ -121,7 +121,7 @@ pub enum TbtNode {
 #[allow(dead_code)]
 impl TbtNode {
     /// Returns the index of a TBT node
-    fn get_index(&self) -> SubtreeIdx {
+    pub fn get_index(&self) -> SubtreeIdx {
         match self {
             TbtNode::Leaf(index, _, _)
             | TbtNode::Fallback(index, _)
@@ -1233,7 +1233,7 @@ impl TbtNode {
         rho_dif: f32,
         number: usize,
         print_leaf_segments_only: bool,
-    ) -> Vec<(usize, Segmentation)> {
+    ) -> Vec<(f32, (usize, Segmentation))> {
         println!("\n\nAlternatives:");
         let mut res_segmentation = Vec::new();
         let mut segmentations = vec![best_segmentation.to_vec()];
@@ -1253,7 +1253,7 @@ impl TbtNode {
                 print_segmentation(&segmentation.1, print_leaf_segments_only, false);
             println!("Segmentation with remaining tau difference of {} and robustness of {robustness_value} is:\n{segmentation_str}", segmentation.0);
             segmentations.push(segmentation.1.to_vec());
-            res_segmentation.push(segmentation);
+            res_segmentation.push((robustness_value, segmentation));
         }
         res_segmentation
     }

src/command_line_parser.rs

@@ -4,6 +4,7 @@
 pub struct CommandLineArguments {
     pub specification: String,
     pub logfile: String,
+    pub toml: Option<String>,
     pub lazy_evaluation: bool,
     pub sub_sampling: bool,
     pub debug_console: bool,
@@ -22,6 +23,7 @@ impl CommandLineArguments {
     fn new(
         specification: String,
         logfile: String,
+        toml: Option<String>,
         lazy_evaluation: bool,
         sub_sampling: bool,
         debug_console: bool,
@@ -31,6 +33,7 @@ impl CommandLineArguments {
         CommandLineArguments {
             specification,
             logfile,
+            toml,
             lazy_evaluation,
             sub_sampling,
             debug_console,
@@ -61,6 +64,12 @@ pub fn parse_command_line() -> CommandLineArguments {
         .takes_value(true)
         .value_name("FILE")
         .help("Get logfile location");
+    let toml = clap::Arg::with_name("toml")
+        .required(false)
+        .long("toml")
+        .takes_value(true)
+        .value_name("FILE")
+        .help("Create xml output file (e.g., tbt_seg_out.toml)");
     let lazy_evaluation = clap::Arg::with_name("lazy_evaluation")
         .required(false)
         .short("l")
@@ -112,6 +121,7 @@ pub fn parse_command_line() -> CommandLineArguments {
     let app = app
         .arg(specification)
         .arg(logfile)
+        .arg(toml)
         .arg(lazy_evaluation)
         .arg(sub_sampling)
         .arg(debugging)
@@ -130,6 +140,7 @@ pub fn parse_command_line() -> CommandLineArguments {
         .value_of("logfile")
         .expect("This can't be None, since it is required")
         .to_string();
+    let toml = matches.value_of("toml").map(|s| s.to_string());
     let lazy_evaluation = matches.is_present("lazy_evaluation");
     let sub_sampling = matches.is_present("sub_sampling");
     let debug_console = matches.is_present("debugging");
@@ -162,6 +173,7 @@ pub fn parse_command_line() -> CommandLineArguments {
     CommandLineArguments::new(
         specification,
         logfile,
+        toml,
         lazy_evaluation,
         sub_sampling,
         debug_console,

src/lib.rs

@@ -11,6 +11,7 @@ mod functions;
 mod parser;
 pub mod stl;
 mod table;
+mod toml_out;
 use behaviortree::print_segmentation;
 use behaviortree::tbt_node_reset_count;
 use behaviortree::ClonedSegmentation;
@@ -29,6 +30,7 @@ use std::time::SystemTime;
 use stl::stl_reset_count;
 use stl::Stl;
 use table::Table;
+use toml_out::generate_toml;
 
 /*
  * This trait must be implemented by the user.
@@ -151,7 +153,7 @@ pub fn evaluate(
     print_leaf_segments_only: bool,
     segmentation_setting: Option<SegmentationSetting>,
     debug: bool,
-) -> (f32, ClonedSegmentation, Option<Vec<ClonedSegmentation>>) {
+) -> (ClonedSegmentation, Option<Vec<ClonedSegmentation>>) {
     // MEMORY ALLOCATIONS
     let mut tree_table = Table::new(Tbt::get_number_nodes(), trace.0);
     println!(
@@ -216,7 +218,7 @@ pub fn evaluate(
         .into_iter()
         .map(|(node, lower, upper, robustness)| (node.clone(), lower, upper, robustness))
         .collect();
-    (robustness_res, segmentation, alternative_segmentations)
+    ((robustness_res, segmentation), alternative_segmentations)
 }
 
 /***********************
@@ -308,10 +310,23 @@ pub fn get_alternative_segmentation(
     );
     other_segmentations
         .into_iter()
-        .map(|(_, v)| {
-            v.into_iter()
-                .map(|(node, lower, upper, robustness)| (node.clone(), lower, upper, robustness))
-                .collect()
+        .map(|(rho, (_, v))| {
+            (
+                rho,
+                v.into_iter()
+                    .map(|(node, lower, upper, robustness)| {
+                        (node.clone(), lower, upper, robustness)
+                    })
+                    .collect(),
+            )
         })
         .collect()
 }
+
+pub fn generate_toml_output_file(
+    location: String,
+    best_segmentation: ClonedSegmentation,
+    alternative_segmentation: Option<Vec<ClonedSegmentation>>,
+) -> std::io::Result<()> {
+    generate_toml(location, best_segmentation, alternative_segmentation)
+}

src/main.rs

@@ -3,7 +3,8 @@
 
 use std::time::SystemTime;
 use tbt_segmentation::{
-    evaluate, get_best_number_skipped_entries, get_tbt_and_trace, parse_command_line,
+    evaluate, generate_toml_output_file, get_best_number_skipped_entries, get_tbt_and_trace,
+    parse_command_line,
 };
 
 fn main() {
@@ -37,7 +38,7 @@ fn main() {
     /*********************
      * Evaluation
      *********************/
-    evaluate(
+    let (best_segmentation, alternative_segmentations) = evaluate(
         tbt,
         trace,
         start,
@@ -49,6 +50,22 @@ fn main() {
         arguments.debug_console,
     );
 
+    /*********************
+     * Create XML file
+     *********************/
+
+    if let Some(toml_output_location) = arguments.toml {
+        println!("Generating toml file: {}", toml_output_location);
+        match generate_toml_output_file(
+            toml_output_location,
+            best_segmentation,
+            alternative_segmentations,
+        ) {
+            Ok(_) => println!("Successfully generated toml file."),
+            Err(_) => println!("Failed to generate toml file."),
+        };
+    }
+
     /*********************
      * Finish Execution
      *********************/