diff --git a/real_robot/coordination_formation_control_pkg/config/crazyfliesConfig.yaml b/real_robot/coordination_formation_control_pkg/config/crazyfliesConfig.yaml
index 0ff98620433972c2275f0a76be7d2a6443e0f68d..8d7912f8688fa833d2f7f81712501c026589cbef 100644
--- a/real_robot/coordination_formation_control_pkg/config/crazyfliesConfig.yaml
+++ b/real_robot/coordination_formation_control_pkg/config/crazyfliesConfig.yaml
@@ -1,17 +1,17 @@
crazyflies:
-# - id: 1
-# channel: 90
-# initialPosition: [0.0, 0.0, 0.0]
-# type: default
-# - id: 2
-# channel: 90
-# initialPosition: [0.0, 0.0, 0.0]
-# type: default
-# - id: 3
-# channel: 80
-# initialPosition: [0.0, 0.0, 0.0]
-# type: default
-- id: 4
+- id: 1
+ channel: 90
+ initialPosition: [0.0, 0.0, 0.0]
+ type: default
+- id: 2
+ channel: 90
+ initialPosition: [0.0, 0.0, 0.0]
+ type: default
+- id: 3
channel: 80
initialPosition: [0.0, 0.0, 0.0]
type: default
+# - id: 4
+# channel: 80
+# initialPosition: [0.0, 0.0, 0.0]
+# type: default
diff --git a/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml b/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml
index 371bc92b6285e93fe5f41ffcc1be4ca11079af50..777f40a60887a3d006858f94d746505e35179c3f 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.3 # 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.2
uav_c1_delta: 0.07
diff --git a/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml b/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml
index f2fcd01e22259f3d88635487fa82493ed0cc675e..ac75ef76dbd115e39af8b5da40ceb5eb46629e40 100644
--- a/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml
+++ b/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml
@@ -1,10 +1,10 @@
n_drones: 3
n_ugvs: 0
hz_freq: 50 # controller frquency
-include_obstacle: true
-include_orientation: false
-n_obs: 1
-log_name: "/home/spot/steven_master_ws/src/coordination_formation_control_pkg/results/experiment_14_formation_flocking_obstacle_3_uav/result"
+include_obstacle: false
+include_orientation: true
+n_obs: 0
+log_name: "/home/spot/steven_master_ws/src/coordination_formation_control_pkg/results/experiment_18_free_space_orientaion_maintain_3_uav/result"
enable_log: true
# waypoint
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_14_formation_flocking_obstacle_3_uav/notes.MD b/real_robot/coordination_formation_control_pkg/results/experiment_14_formation_flocking_obstacle_3_uav/notes.MD
index 7f112cf98e566c26d907d3db98cba190ece708ac..d975ec32cc0cb109bf1aa502aa8db4943a913391 100644
--- a/real_robot/coordination_formation_control_pkg/results/experiment_14_formation_flocking_obstacle_3_uav/notes.MD
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_14_formation_flocking_obstacle_3_uav/notes.MD
@@ -1,6 +1,6 @@
# Goal
- Maintain the current centroid
-- X: 1.1
+- X: 1.4
- Y: 0
- Vx: 0.001
- Vy: 0.0
@@ -9,7 +9,7 @@
Experiment premises
We want to perform flocking and formation with 3 uav with obstacle repre
#comments
-Really nice results
+Result uccesfull
# Mission configuration
n_drones: 3
n_ugvs: 0
@@ -22,7 +22,7 @@ n_obs: 0
# Uav
uav_c1_alpha: 0.20
uav_c2_alpha: 0.3 # this is recomputed
-uav_c1_beta: 0.3
+uav_c1_beta: 0.9
uav_c2_beta: 0.1 # this is recomputed
uav_c1_gamma: 0.05
uav_c2_gamma: 0.2
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_14_formation_flocking_obstacle_3_uav/result_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_14_formation_flocking_obstacle_3_uav/result_crazyflie_1_logdata.bag
index ca595d66ac42f6aed57701fefa3da5d42ed49d7e..43a1427ceff6f01b2d05e9e8904c2aeebf8b7b4b 100644
Binary files a/real_robot/coordination_formation_control_pkg/results/experiment_14_formation_flocking_obstacle_3_uav/result_crazyflie_1_logdata.bag and b/real_robot/coordination_formation_control_pkg/results/experiment_14_formation_flocking_obstacle_3_uav/result_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_14_formation_flocking_obstacle_3_uav/result_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_14_formation_flocking_obstacle_3_uav/result_crazyflie_2_logdata.bag
index ca595d66ac42f6aed57701fefa3da5d42ed49d7e..28ef5a22f56d2512d0ac991204b680a9203b843d 100644
Binary files a/real_robot/coordination_formation_control_pkg/results/experiment_14_formation_flocking_obstacle_3_uav/result_crazyflie_2_logdata.bag and b/real_robot/coordination_formation_control_pkg/results/experiment_14_formation_flocking_obstacle_3_uav/result_crazyflie_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_14_formation_flocking_obstacle_3_uav/result_crazyflie_3_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_14_formation_flocking_obstacle_3_uav/result_crazyflie_3_logdata.bag
index ca595d66ac42f6aed57701fefa3da5d42ed49d7e..4bda230568fd36b39dbd3fd0a2b59dca12aab6db 100644
Binary files a/real_robot/coordination_formation_control_pkg/results/experiment_14_formation_flocking_obstacle_3_uav/result_crazyflie_3_logdata.bag and b/real_robot/coordination_formation_control_pkg/results/experiment_14_formation_flocking_obstacle_3_uav/result_crazyflie_3_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_15_formation_flocking_obstacle_3_uav/notes.MD b/real_robot/coordination_formation_control_pkg/results/experiment_15_formation_flocking_obstacle_3_uav/notes.MD
new file mode 100644
index 0000000000000000000000000000000000000000..d975ec32cc0cb109bf1aa502aa8db4943a913391
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_15_formation_flocking_obstacle_3_uav/notes.MD
@@ -0,0 +1,54 @@
+# Goal
+- Maintain the current centroid
+- X: 1.4
+- Y: 0
+- Vx: 0.001
+- Vy: 0.0
+- Orientation 0
+
+Experiment premises
+We want to perform flocking and formation with 3 uav with obstacle repre
+#comments
+Result uccesfull
+# Mission configuration
+n_drones: 3
+n_ugvs: 0
+hz_freq: 50 # controller frequency
+include_obstacle: false
+include_orientation: true
+n_obs: 0
+## Formation configuration
+### Uav
+# Uav
+uav_c1_alpha: 0.20
+uav_c2_alpha: 0.3 # this is recomputed
+uav_c1_beta: 0.9
+uav_c2_beta: 0.1 # this is recomputed
+uav_c1_gamma: 0.05
+uav_c2_gamma: 0.2
+uav_c1_theta: 0.2
+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*/
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_15_formation_flocking_obstacle_3_uav/result_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_15_formation_flocking_obstacle_3_uav/result_crazyflie_1_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..6418380fdec27206b851dc48330b0b011648552f
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_15_formation_flocking_obstacle_3_uav/result_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_15_formation_flocking_obstacle_3_uav/result_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_15_formation_flocking_obstacle_3_uav/result_crazyflie_2_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..baa21a52faba4d6cb1c4f74298b3c54c0432f4f8
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_15_formation_flocking_obstacle_3_uav/result_crazyflie_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_15_formation_flocking_obstacle_3_uav/result_crazyflie_3_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_15_formation_flocking_obstacle_3_uav/result_crazyflie_3_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..9d6f7c05309546810da1c3377687ad264d41ec03
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_15_formation_flocking_obstacle_3_uav/result_crazyflie_3_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_16_formation_flocking_obstacle_3_uav/notes.MD b/real_robot/coordination_formation_control_pkg/results/experiment_16_formation_flocking_obstacle_3_uav/notes.MD
new file mode 100644
index 0000000000000000000000000000000000000000..4dd38899c9b6385946524c26b56c6aa287675abb
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_16_formation_flocking_obstacle_3_uav/notes.MD
@@ -0,0 +1,54 @@
+# Goal
+- Maintain the current centroid
+- X: 1.4
+- Y: 0
+- Vx: 0.0009
+- Vy: 0.0
+- Orientation 0
+
+Experiment premises
+We want to perform flocking and formation with 3 uav with dynamic obstacle
+#comments
+Successfull ahve video but mistakenly deleted log, repeating test
+# Mission configuration
+n_drones: 3
+n_ugvs: 0
+hz_freq: 50 # controller frequency
+include_obstacle: false
+include_orientation: true
+n_obs: 0
+## Formation configuration
+### Uav
+# Uav
+uav_c1_alpha: 0.20
+uav_c2_alpha: 0.3 # this is recomputed
+uav_c1_beta: 0.9
+uav_c2_beta: 0.1 # this is recomputed
+uav_c1_gamma: 0.05
+uav_c2_gamma: 0.2
+uav_c1_theta: 0.2
+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*/
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_16_formation_flocking_obstacle_3_uav/result_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_16_formation_flocking_obstacle_3_uav/result_crazyflie_1_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..f9ebf85f1de4c59fe79871e7d6a9c34b565de389
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_16_formation_flocking_obstacle_3_uav/result_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_16_formation_flocking_obstacle_3_uav/result_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_16_formation_flocking_obstacle_3_uav/result_crazyflie_2_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..d82b0e138a7013a355e0605e71a8243ad06b80cd
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_16_formation_flocking_obstacle_3_uav/result_crazyflie_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_16_formation_flocking_obstacle_3_uav/result_crazyflie_3_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_16_formation_flocking_obstacle_3_uav/result_crazyflie_3_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..d9e086782f2a12b2cfd0afae428217a4d3ec74e9
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_16_formation_flocking_obstacle_3_uav/result_crazyflie_3_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_17_formation_flocking_obstacle_3_uav/notes.MD b/real_robot/coordination_formation_control_pkg/results/experiment_17_formation_flocking_obstacle_3_uav/notes.MD
new file mode 100644
index 0000000000000000000000000000000000000000..057e7afdaad38b2e752b07a7fc05307ef94a4832
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_17_formation_flocking_obstacle_3_uav/notes.MD
@@ -0,0 +1,54 @@
+# Goal
+- Maintain the current centroid
+- X: 1.4
+- Y: 0
+- Vx: 0.001
+- Vy: 0.0
+- Orientation 0
+
+Experiment premises
+We want to perform flocking and formation with 3 uav with dynamic obstacle with strong gains
+#comments
+Successfull
+# Mission configuration
+n_drones: 3
+n_ugvs: 0
+hz_freq: 50 # controller frequency
+include_obstacle: false
+include_orientation: true
+n_obs: 0
+## Formation configuration
+### Uav
+# Uav
+uav_c1_alpha: 0.20
+uav_c2_alpha: 0.3 # this is recomputed
+uav_c1_beta: 0.9
+uav_c2_beta: 0.1 # this is recomputed
+uav_c1_gamma: 0.25
+uav_c2_gamma: 0.2
+uav_c1_theta: 0.2
+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*/
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_17_formation_flocking_obstacle_3_uav/result_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_17_formation_flocking_obstacle_3_uav/result_crazyflie_1_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..f06361c777191daec450b18e2e2a6cf994600e14
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_17_formation_flocking_obstacle_3_uav/result_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_17_formation_flocking_obstacle_3_uav/result_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_17_formation_flocking_obstacle_3_uav/result_crazyflie_2_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..8c8dccbb70e3f39ff5d1cdf68b32e1e19a3f00fc
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_17_formation_flocking_obstacle_3_uav/result_crazyflie_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_17_formation_flocking_obstacle_3_uav/result_crazyflie_3_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_17_formation_flocking_obstacle_3_uav/result_crazyflie_3_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..4093690c57f78373052a77b91355fbb29f237547
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_17_formation_flocking_obstacle_3_uav/result_crazyflie_3_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_18_free_space_orientaion_maintain_3_uav/notes.MD b/real_robot/coordination_formation_control_pkg/results/experiment_18_free_space_orientaion_maintain_3_uav/notes.MD
new file mode 100644
index 0000000000000000000000000000000000000000..52e57b5d5e06c80d2f343deac513d0c3a720a8d0
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_18_free_space_orientaion_maintain_3_uav/notes.MD
@@ -0,0 +1,57 @@
+# Goal
+- Maintain the current centroid
+- X: 0
+- Y: 0
+- Vx: 0
+- Vy: 0
+- Orientation 0
+
+Experiment premises
+Same as 10 but with 3 drones
+#comments
+
+Worked like a charm
+
+# Mission configuration
+n_drones: 3
+n_ugvs: 0
+hz_freq: 50 # controller frequency
+include_obstacle: false
+include_orientation: true
+n_obs: 0
+## Formation configuration
+### Uav
+# Uav
+# Uav
+uav_c1_alpha: 0.20
+uav_c2_alpha: 0.3 # this is recomputed
+uav_c1_beta: 0.3
+uav_c2_beta: 0.1 # this is recomputed
+uav_c1_gamma: 0.25
+uav_c2_gamma: 0.2
+uav_c1_theta: 0.2
+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: 2 #/* 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*/
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_18_free_space_orientaion_maintain_3_uav/result_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_18_free_space_orientaion_maintain_3_uav/result_crazyflie_1_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..92d5b64b4497d959eb04137f4294f0f30a3314c6
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_18_free_space_orientaion_maintain_3_uav/result_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_18_free_space_orientaion_maintain_3_uav/result_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_18_free_space_orientaion_maintain_3_uav/result_crazyflie_2_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..0ce1a46ab55557e7dca311ab7395c30914a3288f
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_18_free_space_orientaion_maintain_3_uav/result_crazyflie_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/src/drone_node.cpp b/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
index 60dc7fea341597c2aa691b711dc87a28506cde61..228cf494156670ff9b73477f2f52c0cb73c0c135 100644
--- a/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
@@ -83,6 +83,7 @@ int main(int argc, char **argv) {
next_waypoint->vel[0] = 0;
next_waypoint->vel[1] = 0;
next_waypoint->orientation = 0.0;
+ next_waypoint->obstacle.resize(n_obs*3);
logdata = new coordination_formation_control_pkg::logData();
@@ -309,7 +310,7 @@ int main(int argc, char **argv) {
current_obs[n].q(1) = next_waypoint->obstacle[indx+1];
current_obs[n].radius = next_waypoint->obstacle[indx+2];
indx = indx+3;
- std::cout << current_obs[n].q(0) << " " << current_obs[n].q(1) << std::endl;
+ // std::cout << current_obs[n].q(0) << " " << current_obs[n].q(1) << std::endl;
}
}
else
@@ -457,7 +458,7 @@ int main(int argc, char **argv) {
int indx = 0;
for(int n = 0; n < n_obs; n++)
{
-
+ //
logdata->current_obstacle[indx]= next_waypoint->obstacle[indx];
logdata->current_obstacle[indx] = next_waypoint->obstacle[indx+1];
logdata->current_obstacle[indx] = next_waypoint->obstacle[indx+2];
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 4042212cc961d85edcdd161d1ac8071dbe6d2d69..83e464a0432740251154334a606b34a4e41d3fb7 100644
--- a/real_robot/coordination_formation_control_pkg/src/mission_controller.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/mission_controller.cpp
@@ -173,8 +173,8 @@ for (int i = 0; i ++; i < n_ugvs)
{
this->current_waypoint.obstacle[i*3] = this->obstacle_pose(0,i);
this->current_waypoint.obstacle[i*3+1] = this->obstacle_pose(1,i);
- this->current_waypoint.obstacle[i*3+2] = 0.03;
- // std::cout << this->current_waypoint.obstacle[i*3] << " " << this->current_waypoint.obstacle[i*3+1] << std::endl;
+ this->current_waypoint.obstacle[i*3+2] = 0.05;
+ // std::cout << t§his->current_waypoint.obstacle[i*3] << " " << this->current_waypoint.obstacle[i*3+1] << std::endl;
}
}