diff --git a/Cargo.toml b/Cargo.toml index 93e1b0d..1d3fba5 100644 --- a/Cargo.toml +++ b/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"] } diff --git a/README.md b/README.md index 7b57de0..3d458e9 100644 --- a/README.md +++ b/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 `` 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) diff --git a/specification/stl_formula_45Deg.tbt b/specification/stl_formula_45Deg.tbt new file mode 100644 index 0000000..604bb47 --- /dev/null +++ b/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 + ) + ] + ) + ) + ) +) + + + diff --git a/src/behaviortree.rs b/src/behaviortree.rs index ce8f8d7..f591931 100644 --- a/src/behaviortree.rs +++ b/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 } diff --git a/src/command_line_parser.rs b/src/command_line_parser.rs index abf2105..911600a 100644 --- a/src/command_line_parser.rs +++ b/src/command_line_parser.rs @@ -4,6 +4,7 @@ pub struct CommandLineArguments { pub specification: String, pub logfile: String, + pub toml: Option, 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, 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, diff --git a/src/lib.rs b/src/lib.rs index 5cde2a0..0a5ad3a 100644 --- a/src/lib.rs +++ b/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, debug: bool, -) -> (f32, ClonedSegmentation, Option>) { +) -> (ClonedSegmentation, Option>) { // 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>, +) -> std::io::Result<()> { + generate_toml(location, best_segmentation, alternative_segmentation) +} diff --git a/src/main.rs b/src/main.rs index 13dae35..21757b6 100644 --- a/src/main.rs +++ b/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 *********************/ diff --git a/src/toml_out.rs b/src/toml_out.rs new file mode 100644 index 0000000..b914dd6 --- /dev/null +++ b/src/toml_out.rs @@ -0,0 +1,85 @@ +// SPDX-FileCopyrightText: 2023 German Aerospace Center (DLR) +// SPDX-License-Identifier: Apache-2.0 + +use serde::Serialize; +use std::fs::File; +use std::io::Write; + +use crate::behaviortree::ClonedSegmentation; + +#[derive(Serialize)] +struct Segment { + lower: usize, + upper: usize, + rho: f32, + details: SegmentDetail, +} + +#[derive(Serialize)] +struct SegmentDetail { + id: usize, + description: String, +} + +#[derive(Serialize)] +struct NamedSegmentation(String, TomlSegmentation); + +#[derive(Serialize)] +struct TomlSegmentation { + robustness: f32, + segments: Vec, +} + +#[derive(Serialize)] +struct Segmentations { + segmentations: Vec, +} + +pub fn generate_toml( + location: String, + best_segmentation: ClonedSegmentation, + alternative_segmentation: Option>, +) -> std::io::Result<()> { + let mut segmentations = Vec::::new(); + // Adding best segmentation + segmentations.push(read_segmentation(best_segmentation, "best".to_string())); + // Adding the alternative segmentations + if let Some(alternatives) = alternative_segmentation { + alternatives.into_iter().enumerate().for_each(|(i, seg)| { + segmentations.push(read_segmentation(seg, format!("alternative_{}", i + 1))) + }) + } + let all_segmentations = Segmentations { segmentations }; + let toml_string = + toml::to_string_pretty(&all_segmentations).expect("Failed to serialize to TOML"); + let mut file = File::create(location)?; + file.write_all(toml_string.as_bytes())?; + Ok(()) +} + +fn read_segmentation( + segmentation: (f32, Vec<(crate::behaviortree::TbtNode, usize, usize, f32)>), + name: String, +) -> NamedSegmentation { + let mut segments = Vec::::new(); + for (node, lower, upper, rho) in segmentation.1 { + let details = SegmentDetail { + id: node.get_index(), + description: node.pretty_print(false, 0), + }; + let segment = Segment { + lower, + upper, + rho, + details, + }; + segments.push(segment); + } + NamedSegmentation( + name, + TomlSegmentation { + robustness: segmentation.0, + segments, + }, + ) +}