-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
Sebastian
committed
Jun 3, 2024
1 parent
d220676
commit 4969732
Showing
7 changed files
with
240 additions
and
12 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 | ||
| ) | ||
| ] | ||
| ) | ||
| ) | ||
| ) | ||
| ) | ||
|
|
||
|
|
||
|
|
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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<Segment>, | ||
| } | ||
|
|
||
| #[derive(Serialize)] | ||
| struct Segmentations { | ||
| segmentations: Vec<NamedSegmentation>, | ||
| } | ||
|
|
||
| pub fn generate_toml( | ||
| location: String, | ||
| best_segmentation: ClonedSegmentation, | ||
| alternative_segmentation: Option<Vec<ClonedSegmentation>>, | ||
| ) -> std::io::Result<()> { | ||
| let mut segmentations = Vec::<NamedSegmentation>::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::<Segment>::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, | ||
| }, | ||
| ) | ||
| } |