diff --git a/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml b/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml
index 84b30b6fa371573d3ecac84d67a995ae082a5314..4b9d1960917b4db42fd52ef903b677bdb654a76e 100644
--- a/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml
+++ b/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml
@@ -3,7 +3,7 @@ uav_c1_alpha: 0.20
uav_c2_alpha: 0.5 # this is recomputed
uav_c1_beta: 0.3
uav_c2_beta: 0.1 # this is recomputed
-uav_c1_gamma: 0.05
+uav_c1_gamma: 0.25
uav_c2_gamma: 0.2
uav_c1_theta: 0.1
uav_c1_delta: 0.07
@@ -17,7 +17,7 @@ uav_c1_delta: 0.07
# uav_c1_theta: 0.3
# uav_c1_delta: 0.01
-uav_d: 0.8
+uav_d: 1.2
uav_formation_t: 3 #/* type must be minimum 2, 3: tuangle formation, 4 suare, 5 pentagon*/
uav_k: 7
uav_ratio: 0.8 # /* ratio between dist inter and obstacle*/
@@ -37,15 +37,15 @@ uav_int_max: 0.1 #/* maximum integral*/
# Uav
ugv_c1_alpha: 1
ugv_c2_alpha: 1 # this is recomputed
-ugv_c1_beta: 0.35
+ugv_c1_beta: 0.3
ugv_c2_beta: 1 # this is recomputed
ugv_c1_gamma: 0.25
ugv_c2_gamma: 0.2
-ugv_c1_theta: 0.1
+ugv_c1_theta: 0.0
ugv_c1_delta: 0.07
-ugv_d: 0.3
+ugv_d: 0.9
ugv_formation_t: 2 #/* type must be minimum 2, 3: tuangle formation, 4 suare, 5 pentagon*/
ugv_k: 7
ugv_ratio: 0.8 # /* ratio between dist inter and obstacle*/
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_crazyflie_1_logdata.bag
index fbedf3f0005851b2060deb57fa7094645592d76b..6030eca86c6869ae8dc06f80f495e76620b1cf1a 100644
Binary files a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_crazyflie_1_logdata.bag and b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_crazyflie_2_logdata.bag
index ee77381d7aeb8dbff3f9a5c7f2cf4fca872a8a8e..56169d8c4a8ee968d33aa159f8ff34e1804ec518 100644
Binary files a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_crazyflie_2_logdata.bag and b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_crazyflie_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_crazyflie_3_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_crazyflie_3_logdata.bag
index 36a5c940ae4b846b5a1866d9178d545057b75ec3..fe3fcd66e9e0f4bbc58ecdbd6ce78afd8f042df9 100644
Binary files a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_crazyflie_3_logdata.bag and b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_crazyflie_3_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_omniwheel_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_omniwheel_1_logdata.bag
index 296740ee93ec7843cbb38291ac85e0399e8017a6..ef819be40cc737c51dce346f428163c97e64aee8 100644
Binary files a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_omniwheel_1_logdata.bag and b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_omniwheel_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_omniwheel_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_omniwheel_2_logdata.bag
index 5b1556170df7ab1f92b20bf6e9efa5e46b22733d..3b8fc91228522e13f40ee1c9bc053325a67cb981 100644
Binary files a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_omniwheel_2_logdata.bag and b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_omniwheel_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_/parameters b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_/parameters
new file mode 100644
index 0000000000000000000000000000000000000000..81d36e99e8ad4154e93f1ca0c9aa5f749276e3e7
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_/parameters
@@ -0,0 +1,63 @@
+flock at position(1.72, 0) position(0.01,0)
+# Uav
+# Uav
+uav_c1_alpha: 0.20
+uav_c2_alpha: 0.5 # this is recomputed
+uav_c1_beta: 0.3
+uav_c2_beta: 0.1 # this is recomputed
+uav_c1_gamma: 0.05
+uav_c2_gamma: 0.2
+uav_c1_theta: 0.1
+uav_c1_delta: 0.07
+
+# uav_c1_alpha: 0.035
+# uav_c2_alpha: 0.04 # this is recomputed
+# uav_c1_beta: 0.04
+# uav_c2_beta: 0.02 # this is recomputed
+# uav_c1_gamma: 0.02
+# uav_c2_gamma: 0.03
+# uav_c1_theta: 0.3
+# uav_c1_delta: 0.01
+
+uav_d: 0.8
+uav_formation_t: 3 #/* type must be minimum 2, 3: tuangle formation, 4 suare, 5 pentagon*/
+uav_k: 7
+uav_ratio: 0.8 # /* ratio between dist inter and obstacle*/
+uav_eps: 0.1
+uav_a: 5
+uav_b: 5
+uav_h_alpha: 0.2
+uav_h_beta: 0.9
+uav_d_obs: 0
+uav_nav_type: 1 #/* convergence approach -1, parallel approach 2*/
+uav_integrator: 2 #/* single/ double integrator*/
+uav_dt: 0.01 #/* sample time*/
+uav_int_max: 0.1 #/* maximum integral*/
+
+# ugv
+
+# Uav
+ugv_c1_alpha: 1
+ugv_c2_alpha: 1 # this is recomputed
+ugv_c1_beta: 0.35
+ugv_c2_beta: 1 # this is recomputed
+ugv_c1_gamma: 0.25
+ugv_c2_gamma: 0.2
+ugv_c1_theta: 0.1
+ugv_c1_delta: 0.1
+
+
+ugv_d: 1
+ugv_formation_t: 2 #/* type must be minimum 2, 3: tuangle formation, 4 suare, 5 pentagon*/
+ugv_k: 7
+ugv_ratio: 0.8 # /* ratio between dist inter and obstacle*/
+ugv_eps: 0.1
+ugv_a: 5
+ugv_b: 5
+ugv_h_alpha: 0.2
+ugv_h_beta: 0.9
+ugv_d_obs: 0
+ugv_nav_type: 1 #/* convergence approach -1, parallel approach 2*/
+ugv_integrator: 1 #/* single/ double integrator*/
+ugv_dt: 0.02 #/* sample time*/
+ugv_int_max: 0.1 #/* maximum integral*/
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_/result_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_/result_crazyflie_1_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..31cc5eaf2c9ff01929f11217f48f4b95196ac8a7
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_/result_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_/result_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_/result_crazyflie_2_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..ab7cf1d8118ba98b9c1dd75e3712a670790fced2
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_/result_crazyflie_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_/result_crazyflie_3_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_/result_crazyflie_3_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..466cb6aa3c129fca9dd97b389642a93f52e76469
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_/result_crazyflie_3_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_/result_omniwheel_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_/result_omniwheel_1_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..19c308cc5249ca6bdbd7cf0724812601ab3329f8
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_/result_omniwheel_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_/result_omniwheel_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_/result_omniwheel_2_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..4b6c6711942dd79097af2f96c483259250bfd20b
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_/result_omniwheel_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_2/parameters b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_2/parameters
new file mode 100644
index 0000000000000000000000000000000000000000..81d36e99e8ad4154e93f1ca0c9aa5f749276e3e7
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_2/parameters
@@ -0,0 +1,63 @@
+flock at position(1.72, 0) position(0.01,0)
+# Uav
+# Uav
+uav_c1_alpha: 0.20
+uav_c2_alpha: 0.5 # this is recomputed
+uav_c1_beta: 0.3
+uav_c2_beta: 0.1 # this is recomputed
+uav_c1_gamma: 0.05
+uav_c2_gamma: 0.2
+uav_c1_theta: 0.1
+uav_c1_delta: 0.07
+
+# uav_c1_alpha: 0.035
+# uav_c2_alpha: 0.04 # this is recomputed
+# uav_c1_beta: 0.04
+# uav_c2_beta: 0.02 # this is recomputed
+# uav_c1_gamma: 0.02
+# uav_c2_gamma: 0.03
+# uav_c1_theta: 0.3
+# uav_c1_delta: 0.01
+
+uav_d: 0.8
+uav_formation_t: 3 #/* type must be minimum 2, 3: tuangle formation, 4 suare, 5 pentagon*/
+uav_k: 7
+uav_ratio: 0.8 # /* ratio between dist inter and obstacle*/
+uav_eps: 0.1
+uav_a: 5
+uav_b: 5
+uav_h_alpha: 0.2
+uav_h_beta: 0.9
+uav_d_obs: 0
+uav_nav_type: 1 #/* convergence approach -1, parallel approach 2*/
+uav_integrator: 2 #/* single/ double integrator*/
+uav_dt: 0.01 #/* sample time*/
+uav_int_max: 0.1 #/* maximum integral*/
+
+# ugv
+
+# Uav
+ugv_c1_alpha: 1
+ugv_c2_alpha: 1 # this is recomputed
+ugv_c1_beta: 0.35
+ugv_c2_beta: 1 # this is recomputed
+ugv_c1_gamma: 0.25
+ugv_c2_gamma: 0.2
+ugv_c1_theta: 0.1
+ugv_c1_delta: 0.1
+
+
+ugv_d: 1
+ugv_formation_t: 2 #/* type must be minimum 2, 3: tuangle formation, 4 suare, 5 pentagon*/
+ugv_k: 7
+ugv_ratio: 0.8 # /* ratio between dist inter and obstacle*/
+ugv_eps: 0.1
+ugv_a: 5
+ugv_b: 5
+ugv_h_alpha: 0.2
+ugv_h_beta: 0.9
+ugv_d_obs: 0
+ugv_nav_type: 1 #/* convergence approach -1, parallel approach 2*/
+ugv_integrator: 1 #/* single/ double integrator*/
+ugv_dt: 0.02 #/* sample time*/
+ugv_int_max: 0.1 #/* maximum integral*/
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_2/result_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_2/result_crazyflie_1_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..26595003cd84b6431309abe3e9f1778e2c83cd75
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_2/result_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_2/result_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_2/result_crazyflie_2_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..6e89607a196f9b5bd2104f9e21e2d5bf71c6d692
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_2/result_crazyflie_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_2/result_crazyflie_3_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_2/result_crazyflie_3_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..4315d63ce6abf05dee19d2ca9c5a0abd0245b165
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_2/result_crazyflie_3_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_2/result_omniwheel_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_2/result_omniwheel_1_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..9c1fa1dcfea466af812b9965933d2255d9257693
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_2/result_omniwheel_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_2/result_omniwheel_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_2/result_omniwheel_2_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..6f87905994bf4431c29737bbfd91ce210b1f7051
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_2/result_omniwheel_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_3/parameters b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_3/parameters
new file mode 100644
index 0000000000000000000000000000000000000000..81d36e99e8ad4154e93f1ca0c9aa5f749276e3e7
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_3/parameters
@@ -0,0 +1,63 @@
+flock at position(1.72, 0) position(0.01,0)
+# Uav
+# Uav
+uav_c1_alpha: 0.20
+uav_c2_alpha: 0.5 # this is recomputed
+uav_c1_beta: 0.3
+uav_c2_beta: 0.1 # this is recomputed
+uav_c1_gamma: 0.05
+uav_c2_gamma: 0.2
+uav_c1_theta: 0.1
+uav_c1_delta: 0.07
+
+# uav_c1_alpha: 0.035
+# uav_c2_alpha: 0.04 # this is recomputed
+# uav_c1_beta: 0.04
+# uav_c2_beta: 0.02 # this is recomputed
+# uav_c1_gamma: 0.02
+# uav_c2_gamma: 0.03
+# uav_c1_theta: 0.3
+# uav_c1_delta: 0.01
+
+uav_d: 0.8
+uav_formation_t: 3 #/* type must be minimum 2, 3: tuangle formation, 4 suare, 5 pentagon*/
+uav_k: 7
+uav_ratio: 0.8 # /* ratio between dist inter and obstacle*/
+uav_eps: 0.1
+uav_a: 5
+uav_b: 5
+uav_h_alpha: 0.2
+uav_h_beta: 0.9
+uav_d_obs: 0
+uav_nav_type: 1 #/* convergence approach -1, parallel approach 2*/
+uav_integrator: 2 #/* single/ double integrator*/
+uav_dt: 0.01 #/* sample time*/
+uav_int_max: 0.1 #/* maximum integral*/
+
+# ugv
+
+# Uav
+ugv_c1_alpha: 1
+ugv_c2_alpha: 1 # this is recomputed
+ugv_c1_beta: 0.35
+ugv_c2_beta: 1 # this is recomputed
+ugv_c1_gamma: 0.25
+ugv_c2_gamma: 0.2
+ugv_c1_theta: 0.1
+ugv_c1_delta: 0.1
+
+
+ugv_d: 1
+ugv_formation_t: 2 #/* type must be minimum 2, 3: tuangle formation, 4 suare, 5 pentagon*/
+ugv_k: 7
+ugv_ratio: 0.8 # /* ratio between dist inter and obstacle*/
+ugv_eps: 0.1
+ugv_a: 5
+ugv_b: 5
+ugv_h_alpha: 0.2
+ugv_h_beta: 0.9
+ugv_d_obs: 0
+ugv_nav_type: 1 #/* convergence approach -1, parallel approach 2*/
+ugv_integrator: 1 #/* single/ double integrator*/
+ugv_dt: 0.02 #/* sample time*/
+ugv_int_max: 0.1 #/* maximum integral*/
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_3/result_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_3/result_crazyflie_1_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..6030eca86c6869ae8dc06f80f495e76620b1cf1a
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_3/result_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_3/result_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_3/result_crazyflie_2_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..56169d8c4a8ee968d33aa159f8ff34e1804ec518
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_3/result_crazyflie_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_3/result_crazyflie_3_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_3/result_crazyflie_3_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..fe3fcd66e9e0f4bbc58ecdbd6ce78afd8f042df9
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_3/result_crazyflie_3_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_3/result_omniwheel_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_3/result_omniwheel_1_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..ef819be40cc737c51dce346f428163c97e64aee8
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_3/result_omniwheel_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_3/result_omniwheel_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_3/result_omniwheel_2_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..3b8fc91228522e13f40ee1c9bc053325a67cb981
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv_3/result_omniwheel_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/scripts/plot_real_experiment_pgf_plot.py b/real_robot/coordination_formation_control_pkg/scripts/plot_real_experiment_pgf_plot.py
index 4bb156b1803e31ae132cccba0c7592a30fe62f7f..3a57ec1c431c2f51f638035dd7dca37331df0f7c 100644
--- a/real_robot/coordination_formation_control_pkg/scripts/plot_real_experiment_pgf_plot.py
+++ b/real_robot/coordination_formation_control_pkg/scripts/plot_real_experiment_pgf_plot.py
@@ -31,8 +31,8 @@ bag = rosbag.Bag(result_folder_name + file_name)
N_uavs = 3
N_ugvs = 2
n_obstacles = 0
-d_uav = 0.8;
-d_ugv = 1;
+d_uav =1.2;
+d_ugv = 0.9;
ratio_uav = 0.8;
ratio_ugv = 0.8;
d_uav2obs = d_uav*ratio_uav;
@@ -181,16 +181,16 @@ if N_ugvs > 0:
axis_traj.plot(ugv_cluster_x[-1,:], ugv_cluster_y[-1,:], ".",marker="+",markersize=12, markeredgecolor="black", markerfacecolor="black")
-
-#plot final configuration
-# axis_traj.plot(np.append(position_x[-1,:],position_x[-1,0]) , np.append(position_y[-1,:],position_y[-1,0]),"r")
-if N_ugvs > 0:
- for i in range(N_ugvs):
- axis_traj.plot(np.append(ugv_position_x[-1,i],ugv_cluster_x[-1,i]) , np.append(ugv_position_y[-1,i],ugv_cluster_y[-1,i]),"k")
- axis_traj.plot(np.append(ugv_position_x[-1,i],ugv_cluster_x[-1,i]) , np.append(ugv_position_y[-1,i],ugv_cluster_y[-1,i]),"k")
-#plot trajectories
+#
+# #plot final configuration
+axis_traj.plot(np.append(position_x[-1,:],position_x[-1,0]) , np.append(position_y[-1,:],position_y[-1,0]),"r")
+# if N_ugvs > 0:
+# for i in range(N_ugvs):
+# axis_traj.plot(np.append(ugv_position_x[-1,i],ugv_cluster_x[-1,i]) , np.append(ugv_position_y[-1,i],ugv_cluster_y[-1,i]),"k")
+# axis_traj.plot(np.append(ugv_position_x[-1,i],ugv_cluster_x[-1,i]) , np.append(ugv_position_y[-1,i],ugv_cluster_y[-1,i]),"k")
+# # #plot trajectories
color_array = ["c", "g","b", "y", "m", "C1", "C2"]
-# plot the uavs
+# # plot the uavs
for i in range(N_uavs):
c = color_array[i];
axis_traj.plot(position_x[:,i], position_y[:,i], c,label="UAV_" + str(i+1))
@@ -198,8 +198,8 @@ for i in range(N_uavs):
for t in range(N_ugvs):
c = color_array[t+N_uavs];
c1 = color_array[t+N_uavs+2];
- # axis_traj.plot(ugv_position_x[:,t], ugv_position_y[:,t],c+"--",label="UGV_" + str(t+1))
- axis_traj.plot(ugv_cluster_x[:,t], ugv_cluster_y[:,t],c1+"--",label="Cluster_" + str(t+1),marker="*",markersize=10)
+ axis_traj.plot(ugv_position_x[:,t], ugv_position_y[:,t],c,label="UGV_" + str(t+1))
+ axis_traj.plot(ugv_cluster_x[:,t], ugv_cluster_y[:,t],c1,label="Cluster_" + str(t+1))
# plot the obstacle
if n_obstacles >0:
diff --git a/real_robot/coordination_formation_control_pkg/src/mission_controller.cpp b/real_robot/coordination_formation_control_pkg/src/mission_controller.cpp
index 19e3a292448f8ab79d09b5e3523807a37aabaef9..70f7b7b1c262f3a9f20bfa1f76aa3f561925f9aa 100644
--- a/real_robot/coordination_formation_control_pkg/src/mission_controller.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/mission_controller.cpp
@@ -391,6 +391,10 @@ for (int i = 0; i ++; i < n_ugvs)
void missionController::updateCofig(coordination_formation_control_pkg::missionControllerConfig &config, uint32_t level)
{
ROS_INFO("mission controller: reconfigure callback");
+ current_waypoint.pos[0] = config.x;
+ current_waypoint.pos[1] = config.y;
+ current_waypoint.vel[0] = config.vx;
+ current_waypoint.vel[1] = config.vy;
// demo
if(config.initialize_all_robot)
{
@@ -415,11 +419,8 @@ void missionController::updateCofig(coordination_formation_control_pkg::missionC
if(config.update_waypoint)
{
config.update_waypoint = false;
- current_waypoint.pos[0] = config.x;
- current_waypoint.pos[1] = config.y;
current_waypoint.orientation = config.orientation;
- current_waypoint.vel[0] = config.vx;
- current_waypoint.vel[1] = config.vy;
+
this->mantain_position = config.mantain_position;
this->start_publishing_waypoint = true;
config.mantain_position = false;
@@ -447,25 +448,40 @@ Eigen::MatrixXf missionController::compute_cluster(Eigen::MatrixXf uav_q, Eigen:
{
std::vector<Point2f> centers;
- // for (int i = 0; i <num_ugvs; i++)
- // {
- // /* make intial ugv be the centroid */
- // Point2f center;
- // center.x = ugv_q(0,i);
- // center.y = ugv_q(1,i);
- // centers.push_back(center);
- // }
+ for (int i = 0; i <num_ugvs; i++)
+ {
+ /* make intial ugv be the centroid */
+ Point2f center;
+ center.x = ugv_q(0,i);
+ center.y = ugv_q(1,i);
+ centers.push_back(center);
+ }
/* points*/
std::vector<Point2f> points;
+ /* this is to prevent random junts test again later*/
- for(int t = 0; t < num_uavs; t++)
+ if(this->cluster_initialized)
+ {
+ for(int t = 0; t < num_uavs; t++)
+ {
+ Point2f point;
+ point.x = previous_centroids(0,t);
+ point.y = previous_centroids(1,t);
+ points.push_back(point);
+ }
+ }
+ else
{
- Point2f point;
- point.x = uav_q(0,t);
- point.y = uav_q(1,t);
- points.push_back(point);
+ for(int t = 0; t < num_uavs; t++)
+ {
+ Point2f point;
+ point.x = uav_q(0,t);
+ point.y = uav_q(1,t);
+ points.push_back(point);
+ }
}
+
// ROS_INFO("*****************************************Here");
Mat labels = previous_labels;
@@ -500,12 +516,18 @@ Eigen::MatrixXf missionController::compute_cluster(Eigen::MatrixXf uav_q, Eigen:
if(num_ugvs == 2)
{
- float man1 = (centroids.col(0) - uav_q.col(0)).norm();
- float man2 = (centroids.col(1) - uav_q.col(1)).norm();
- float cha1 = (centroids.col(1) - uav_q.col(0)).norm();
- float cha2 = (centroids.col(0) - uav_q.col(1)).norm();
-
- if( cha1 < man1 && cha2 <man2)
+ float man1 = (centroids.col(0) - ugv_q.col(0)).norm();
+ float man2 = (centroids.col(1) - ugv_q.col(1)).norm();
+ float cha1 = (centroids.col(1) - ugv_q.col(0)).norm();
+ float cha2 = (centroids.col(0) - ugv_q.col(1)).norm();
+ float total_man = man1 +man2;
+ float total_cha = cha1+cha2;
+ // std::cout << "man" << man1 << " " << man2 << std::endl;
+ // std::cout << "cha1" << cha2 << " " << cha2 << std::endl;
+ //
+ // std:: cout <<total_man << " " << total_cha << std::endl;
+
+ if( total_cha < total_man)
{
Eigen::MatrixXf dummy_centroid(2,num_ugvs);
@@ -516,6 +538,8 @@ Eigen::MatrixXf missionController::compute_cluster(Eigen::MatrixXf uav_q, Eigen:
+
+
}
else
{
@@ -525,8 +549,12 @@ Eigen::MatrixXf missionController::compute_cluster(Eigen::MatrixXf uav_q, Eigen:
centroids = centroids;
}
+
}
+ // check for intersections
+ // if this->intersectionExist(centroid, ugv_state_q,num_ugvs)
+
}
previous_centroids = centroids;
// std::cout << "final centroids" << centroids << std::endl;