From 21b8170e57e973dde4ba42fdecaff3be6a26896b Mon Sep 17 00:00:00 2001 From: Marlon Date: Sun, 4 Aug 2024 19:21:17 +0200 Subject: [PATCH] Added new trajectory gen script and added variant 1 trajectories --- config/trajectory/trajectory_files/v1_tx1.txt | 1236 +++++++++++++++++ config/trajectory/trajectory_files/v1_tx2.txt | 1236 +++++++++++++++++ launch/sim_three_sided_marlon.launch | 2 +- scripts/position3.csv | 2 +- scripts/trajectory_generation.py | 127 +- scripts/two_drones/session_marlon.yml | 39 +- 6 files changed, 2567 insertions(+), 75 deletions(-) create mode 100644 config/trajectory/trajectory_files/v1_tx1.txt create mode 100644 config/trajectory/trajectory_files/v1_tx2.txt mode change 100755 => 100644 scripts/trajectory_generation.py diff --git a/config/trajectory/trajectory_files/v1_tx1.txt b/config/trajectory/trajectory_files/v1_tx1.txt new file mode 100644 index 0000000..1b0f4af --- /dev/null +++ b/config/trajectory/trajectory_files/v1_tx1.txt @@ -0,0 +1,1236 @@ +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60,15,4,0.785 +-60.0,15.0,3.8,0.785 +-60.0,15.0,3.6,0.785 +-60.0,15.0,3.4,0.785 +-60.0,15.0,3.2,0.785 +-60.0,15.0,3.0,0.785 +-60.0,15.0,2.8,0.785 +-60.0,15.0,2.5999999999999996,0.785 +-60.0,15.0,2.4,0.785 +-60.0,15.0,2.2,0.785 +-60.0,15.0,2.0,0.785 +-60,15,2,0.785 +-60,15,2,0.785 +-60,15,2,0.785 +-60,15,2,0.785 +-60,15,2,0.785 +-60,15,2,0.785 +-60,15,2,0.785 +-60,15,2,0.785 +-60,15,2,0.785 +-60,15,2,0.785 +-60,15,2,0.785 +-60,15,2,0.785 +-60,15,2,0.785 +-60,15,2,0.785 +-60,15,2,0.785 +-60,15,2,0.785 +-60,15,2,0.785 +-60,15,2,0.785 +-60,15,2,0.785 +-60,15,2,0.785 +-60,15,2,0.785 +-60,15,2,0.785 +-60,15,2,0.785 +-60,15,2,0.785 +-60,15,2,0.785 diff --git a/config/trajectory/trajectory_files/v1_tx2.txt b/config/trajectory/trajectory_files/v1_tx2.txt new file mode 100644 index 0000000..051b5fa --- /dev/null +++ b/config/trajectory/trajectory_files/v1_tx2.txt @@ -0,0 +1,1236 @@ +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50,20,4,0.785 +-50.0,20.0,3.8,0.785 +-50.0,20.0,3.6,0.785 +-50.0,20.0,3.4,0.785 +-50.0,20.0,3.2,0.785 +-50.0,20.0,3.0,0.785 +-50.0,20.0,2.8,0.785 +-50.0,20.0,2.5999999999999996,0.785 +-50.0,20.0,2.4,0.785 +-50.0,20.0,2.2,0.785 +-50.0,20.0,2.0,0.785 +-50,20,2,0.785 +-50,20,2,0.785 +-50,20,2,0.785 +-50,20,2,0.785 +-50,20,2,0.785 +-50,20,2,0.785 +-50,20,2,0.785 +-50,20,2,0.785 +-50,20,2,0.785 +-50,20,2,0.785 +-50,20,2,0.785 +-50,20,2,0.785 +-50,20,2,0.785 +-50,20,2,0.785 +-50,20,2,0.785 +-50,20,2,0.785 +-50,20,2,0.785 +-50,20,2,0.785 +-50,20,2,0.785 +-50,20,2,0.785 +-50,20,2,0.785 +-50,20,2,0.785 +-50,20,2,0.785 +-50,20,2,0.785 +-50,20,2,0.785 diff --git a/launch/sim_three_sided_marlon.launch b/launch/sim_three_sided_marlon.launch index 6e20f91..f6cb8e2 100644 --- a/launch/sim_three_sided_marlon.launch +++ b/launch/sim_three_sided_marlon.launch @@ -31,7 +31,7 @@ - + diff --git a/scripts/position3.csv b/scripts/position3.csv index 972c000..10355ed 100644 --- a/scripts/position3.csv +++ b/scripts/position3.csv @@ -1 +1 @@ -3,7.0,1.0,0.5,0.7 +3,5.0,10.0,0.5,0.7 diff --git a/scripts/trajectory_generation.py b/scripts/trajectory_generation.py old mode 100755 new mode 100644 index 0e2f630..cf24ec6 --- a/scripts/trajectory_generation.py +++ b/scripts/trajectory_generation.py @@ -1,55 +1,80 @@ -#!/usr/bin/env python -import rospy -from mrs_msgs.srv import PathSrv -from std_msgs.msg import Header -from mrs_msgs.msg import Reference, Path -from geometry_msgs.msg import Point - -def call_trajectory_service(points): - rospy.init_node('trajectory_generation_client') - rospy.wait_for_service('/uav1/trajectory_generation/path') +import numpy as np +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D +import os + + + +def generate_trajectory(waypoints, hover_times, T=0.2): + trajectory = [] + + # Add the first waypoint + x0, y0, z0, h0 = waypoints[0] + trajectory.append((x0, y0, z0, h0)) - try: - trajectory_service = rospy.ServiceProxy('/uav1/trajectory_generation/path', PathSrv) - - request = Path() - request.header = Header(seq=0, stamp=rospy.Time.now(), frame_id='') - request.input_id = 0 - request.use_heading = False - request.fly_now = False - request.stop_at_waypoints = False - request.loop = False - request.max_execution_time = 0.0 - request.max_deviation_from_path = 0.0 - request.dont_prepend_current_state = False - request.override_constraints = False - request.override_max_velocity_horizontal = 0.0 - request.override_max_acceleration_horizontal = 0.0 - request.override_max_jerk_horizontal = 0.0 - request.override_max_velocity_vertical = 0.0 - request.override_max_acceleration_vertical = 0.0 - request.override_max_jerk_vertical = 0.0 - request.relax_heading = False - request.points = points + for i in range(len(waypoints) - 1): + x0, y0, z0, h0 = waypoints[i] + x1, y1, z1, h1 = waypoints[i+1] + hover_time = hover_times[i] - response = trajectory_service(request) + # Calculate number of samples for the hover period + num_hover_samples = int(hover_time / T) + trajectory.extend([(x0, y0, z0, h0)] * num_hover_samples) - return response - except rospy.ServiceException as e: - print("Service call failed: %s" % e) - -if __name__ == "__main__": - - points = [ - Reference(position=Point(x=5.0, y=5.0, z=0.0), heading=0.0), - Reference(position=Point(x=4.0, y=5.0, z=0.0), heading=0.0), - Reference(position=Point(x=3.0, y=5.0, z=0.0), heading=0.0), - Reference(position=Point(x=2.0, y=5.0, z=0.0), heading=0.0), - Reference(position=Point(x=1.0, y=5.0, z=0.0), heading=0.0), - Reference(position=Point(x=0.0, y=5.0, z=0.0), heading=0.0) - #Reference(position=Point(x=-5.0, y=5.0, z=0.0), heading=0.0), - #Reference(position=Point(x=0.0, y=5.0, z=0.0), heading=0.0) - ] + # Interpolate waypoints + distance = np.sqrt((x1 - x0)**2 + (y1 - y0)**2 + (z1 - z0)**2) + num_travel_samples = int(np.ceil(distance / T)) + if num_travel_samples > 0: + x = np.linspace(x0, x1, num_travel_samples + 1) + y = np.linspace(y0, y1, num_travel_samples + 1) + z = np.linspace(z0, z1, num_travel_samples + 1) + heading = np.linspace(h0, h1, num_travel_samples + 1) + trajectory.extend([(xi, yi, zi, hi) for xi, yi, zi, hi in zip(x[1:], y[1:], z[1:], heading[1:])]) - response = call_trajectory_service(points) - rospy.loginfo(response) + # Add hover time at the last waypoint + x_last, y_last, z_last, h_last = waypoints[-1] + num_final_hover_samples = int(hover_times[-1] / T) + trajectory.extend([(x_last, y_last, z_last, h_last)] * num_final_hover_samples) + + return trajectory + + +def plot_trajectory(trajectory): + fig = plt.figure() + ax = fig.add_subplot(111, projection='3d') + x, y, z, _ = zip(*trajectory) + + ax.scatter(x[0], y[0], z[0], color='red', s=100) + + ax.plot(x, y, z, marker='o', linestyle='-') + ax.set_xlabel('X Position') + ax.set_ylabel('Y Position') + ax.set_zlabel('Z Position') + plt.title('3D Trajectory Visualization') + plt.show() + + +def save_trajectory(trajectory, filename): + with open(filename, 'w') as file: + for point in trajectory: + line = ','.join(map(str, point)) + '\n' + file.write(line) + + +if __name__ == '__main__': + + waypoints = [ + (-60,15,4,0.785), + (-60,15,2,0.785) + ] + + + #0 hover time for each waypoint + # time = 240 + hover_times = [240,5] + # hover_times = [time] * len(waypoints) + trajectory = generate_trajectory(waypoints, hover_times, T=0.2) + plot_trajectory(trajectory) + file_name = 'v1_tx1.txt' + file_location = os.path.expanduser('~/catkin_ws/src/uvdar_core/config/trajectory/trajectory_files/' + file_name) + save_trajectory(trajectory, file_location) diff --git a/scripts/two_drones/session_marlon.yml b/scripts/two_drones/session_marlon.yml index b89f3da..64f0976 100644 --- a/scripts/two_drones/session_marlon.yml +++ b/scripts/two_drones/session_marlon.yml @@ -25,17 +25,19 @@ windows: panes: - export UAV_NAME=uav1; waitForHw; roslaunch mrs_uav_status status.launch - export UAV_NAME=uav2; waitForHw; roslaunch mrs_uav_status status.launch + # - export UAV_NAME=uav3; waitForHw; roslaunch mrs_uav_status status.launch - spawn: layout: tiled panes: - waitForGazebo; rosservice call /mrs_drone_spawner/spawn "1 --$UAV_TYPE --pos-file $(rospack find uvdar_core)/scripts/position1.csv --enable-rangefinder --enable-ground-truth --enable-dual-uv-cameras calib_file:=$(rospack find uvdar_core)/config/ocamcalib/calib_results_bf_uv_fe.txt --enable-back-uv-camera calib_file:=$(rospack find uvdar_core)/config/ocamcalib/calib_results_bf_uv_fe.txt" - - waitForGazebo; sleep 12; rosservice call /mrs_drone_spawner/spawn "2 --$UAV_TYPE --pos-file $(rospack find uvdar_core)/scripts/position2.csv --enable-rangefinder --enable-ground-truth --enable-uv-leds signal_id:=[0,1,2,3]" + - waitForGazebo; sleep 12; rosservice call /mrs_drone_spawner/spawn "2 --$UAV_TYPE --pos-file $(rospack find uvdar_core)/scripts/position2.csv --enable-rangefinder --enable-ground-truth --enable-uv-leds signal_id:=[1,1,1,1]" + # - waitForGazebo; sleep 24; rosservice call /mrs_drone_spawner/spawn "3 --$UAV_TYPE --pos-file $(rospack find uvdar_core)/scripts/position3.csv --enable-rangefinder --enable-ground-truth --enable-uv-leds signal_id:=[2,2,2,2]" - hw_api: layout: tiled panes: - export UAV_NAME=uav1; waitForTime; roslaunch mrs_uav_px4_api api.launch - export UAV_NAME=uav2; waitForTime; roslaunch mrs_uav_px4_api api.launch - - export UAV_NAME=uav3; waitForTime; roslaunch mrs_uav_px4_api api.launch + # - export UAV_NAME=uav3; waitForTime; roslaunch mrs_uav_px4_api api.launch - core: layout: tiled panes: @@ -49,6 +51,12 @@ windows: custom_config:=./config/custom_config.yaml world_config:=./config/world_config.yaml network_config:=./config/network_config.yaml + # - export UAV_NAME=uav3; waitForHw; roslaunch mrs_uav_core core.launch + # platform_config:=`rospack find mrs_uav_gazebo_simulation`/config/mrs_uav_system/$UAV_TYPE.yaml + # custom_config:=./config/custom_config.yaml + # world_config:=./config/world_config.yaml + # network_config:=./config/network_config.yaml + - takeoff: layout: tiled panes: @@ -56,6 +64,9 @@ windows: - 'export UAV_NAME=uav1; waitForControl; rosservice call /$UAV_NAME/hw_api/arming 1; sleep 2; rosservice call /$UAV_NAME/hw_api/offboard' - export UAV_NAME=uav2; waitForHw; roslaunch mrs_uav_autostart automatic_start.launch - 'export UAV_NAME=uav2; waitForControl; rosservice call /$UAV_NAME/hw_api/arming 1; sleep 2; rosservice call /$UAV_NAME/hw_api/offboard' + # - export UAV_NAME=uav3; waitForHw; roslaunch mrs_uav_autostart automatic_start.launch + # - 'export UAV_NAME=uav3; waitForControl; rosservice call /$UAV_NAME/hw_api/arming 1; sleep 2; rosservice call /$UAV_NAME/hw_api/offboard' + - uv_observer: layout: even-vertical panes: @@ -63,29 +74,13 @@ windows: - slow_down: layout: even-vertical panes: - - waitForGazebo; waitForRos; sleep 20; gz physics -u 80 - - uav1_waypoint_flier: - layout: tiled - panes: - - export UAV_NAME=uav1; waitForControl; roslaunch example_waypoint_flier example_waypoint_flier.launch - - 'history -s rosservice call /$UAV_NAME/example_waypoint_flier/fly_to_first_waypoint' - - 'history -s rosservice call /$UAV_NAME/example_waypoint_flier/start_waypoints_following' - - 'history -s rosservice call /$UAV_NAME/example_waypoint_flier/stop_waypoints_following' - - uav2_waypoint_flier: - layout: tiled - panes: - - export UAV_NAME=uav2; waitForControl; roslaunch example_waypoint_flier example_waypoint_flier_2.launch - - 'export UAV_NAME=uav2; history -s rosservice call /$UAV_NAME/example_waypoint_flier/fly_to_first_waypoint' - - 'export UAV_NAME=uav2; history -s rosservice call /$UAV_NAME/example_waypoint_flier/start_waypoints_following' - - 'export UAV_NAME=uav2; history -s rosservice call /$UAV_NAME/example_waypoint_flier/stop_waypoints_following' - + - waitForGazebo; waitForRos; sleep 20; gz physics -u 125 - trajectories: layout: tiled panes: - - export UAV_NAME=uav1; history -s rosservice call /'"$UAV_NAME"'/control_manager/start_trajectory_tracking; history -s rosservice call /'"$UAV_NAME"'/control_manager/goto_trajectory_start; history -s roslaunch uvdar_core load_trajectory.launch file:="trajectory_rx_8m.txt" - - export UAV_NAME=uav2; history -s rosservice call /'"$UAV_NAME"'/control_manager/start_trajectory_tracking; history -s rosservice call /'"$UAV_NAME"'/control_manager/goto_trajectory_start; history -s roslaunch uvdar_core load_trajectory.launch file:="trajectory_tx_2m.txt" - - #- export UAV_NAME=uav3; history -s rosservice call /'"$UAV_NAME"'/control_manager/start_trajectory_tracking; history -s rosservice call /'"$UAV_NAME"'/control_manager/goto_trajectory_start; history -s roslaunch uvdar_core load_trajectory.launch file:="two_tx/tx2_fly_by.txt" loop:=true + - export UAV_NAME=uav1; history -s rosservice call /'"$UAV_NAME"'/control_manager/start_trajectory_tracking; history -s rosservice call /'"$UAV_NAME"'/control_manager/goto_trajectory_start; history -s roslaunch uvdar_core load_trajectory.launch file:="trajectory_rx.txt" + - export UAV_NAME=uav2; history -s rosservice call /'"$UAV_NAME"'/control_manager/start_trajectory_tracking; history -s rosservice call /'"$UAV_NAME"'/control_manager/goto_trajectory_start; history -s roslaunch uvdar_core load_trajectory.launch file:="v1_tx1.txt" + # - export UAV_NAME=uav3; history -s rosservice call /'"$UAV_NAME"'/control_manager/start_trajectory_tracking; history -s rosservice call /'"$UAV_NAME"'/control_manager/goto_trajectory_start; history -s roslaunch uvdar_core load_trajectory.launch file:="v1_tx2.txt" - rviz: layout: even-vertical panes: