diff --git a/real_robot/coordination_formation_control_pkg/cfg/missionController.cfg b/real_robot/coordination_formation_control_pkg/cfg/missionController.cfg
index dc6d89e0cd7d22c2feb5b9054fdea96cf45e65e7..84fa02acace50fa9278778d4533bfad95b4b1d9b 100644
--- a/real_robot/coordination_formation_control_pkg/cfg/missionController.cfg
+++ b/real_robot/coordination_formation_control_pkg/cfg/missionController.cfg
@@ -11,6 +11,7 @@ demo.add("start_formation", bool_t, 0,
demo.add("stop", bool_t, 0, "stop formation ", False)
demo.add("mantain_position", bool_t, 0, "mantain goal centroid", False)
demo.add("update_waypoint", bool_t, 0, "update waypoint", False)
+demo.add("start_orientation", bool_t, 0, "start orientation", False)
demo.add("x", double_t, 0, "desired goal x", 0.0, -3,3)
demo.add("y", double_t, 0, "desired goal y", 0.0, -3,3)
demo.add("vx", double_t, 0, "desired goal vx", 0.0,-3,3)
diff --git a/real_robot/coordination_formation_control_pkg/config/crazyfliesConfig.yaml b/real_robot/coordination_formation_control_pkg/config/crazyfliesConfig.yaml
index 8d7912f8688fa833d2f7f81712501c026589cbef..0ff98620433972c2275f0a76be7d2a6443e0f68d 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 8cc33e1a34623d6405440b5b52263db6b5f14aa9..371bc92b6285e93fe5f41ffcc1be4ca11079af50 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.25
+uav_c1_gamma: 0.05
uav_c2_gamma: 0.2
uav_c1_theta: 0.2
uav_c1_delta: 0.07
@@ -18,7 +18,7 @@ uav_c1_delta: 0.07
# 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_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
diff --git a/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml b/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml
index a35d54c2fde0d9a6976a179a6595305af271f699..f2fcd01e22259f3d88635487fa82493ed0cc675e 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: false
+include_obstacle: true
include_orientation: false
-n_obs: 0
-log_name: "/home/spot/steven_master_ws/src/coordination_formation_control_pkg/results/experiment_5_free_space_maintain_3_uav/result"
+n_obs: 1
+log_name: "/home/spot/steven_master_ws/src/coordination_formation_control_pkg/results/experiment_14_formation_flocking_obstacle_3_uav/result"
enable_log: true
# waypoint
diff --git a/real_robot/coordination_formation_control_pkg/include/integration.h b/real_robot/coordination_formation_control_pkg/include/integration.h
index f89f9a744bf3e45b9e9fc3e0198d0fbe051f7a1a..bea542c9f0eb87c471389799eea5a5234df35abb 100644
--- a/real_robot/coordination_formation_control_pkg/include/integration.h
+++ b/real_robot/coordination_formation_control_pkg/include/integration.h
@@ -37,7 +37,7 @@ public:
this->accel(1) = ay;
this->current_vel(0) = vx;
this->current_vel(1) = vy;
- this->vel = this->current_vel + this->accel*dt;
+ this->vel = this->vel + this->accel*dt;
return this->vel;
}
diff --git a/real_robot/coordination_formation_control_pkg/include/mission_controller.h b/real_robot/coordination_formation_control_pkg/include/mission_controller.h
index 8b3dedeac3d900c2f601cad3eb5d62fe22e873e1..7abb9445a466ea5e97821fe8fc15bf9314cea066 100644
--- a/real_robot/coordination_formation_control_pkg/include/mission_controller.h
+++ b/real_robot/coordination_formation_control_pkg/include/mission_controller.h
@@ -7,6 +7,8 @@
#include <coordination_formation_control_pkg/missionControllerConfig.h>
#include <coordination_formation_control_pkg/coordination.h>
#include <coordination_formation_control_pkg/waypoint.h>
+#include "crazyswarm/GenericLogData.h"
+
#include "opencv2/highgui.hpp"
#include "opencv2/core.hpp"
#include "opencv2/imgproc.hpp"
@@ -46,6 +48,8 @@ class missionController
bool start_publishing_waypoint;
bool cluster_initialized;
ros::NodeHandle nh;
+ Eigen::MatrixXf obstacle_pose;
+
/*dynamic reconfigure*/
dynamic_reconfigure::Server<coordination_formation_control_pkg::missionControllerConfig> server;
@@ -54,9 +58,14 @@ class missionController
/* service responsible for the initialization, shut_down of all the robot*/
ros::Publisher coordination_publisher;
ros::Publisher waypoint_publisher;
+ ros::Subscriber obs_state_publisher;
+
+ float eps;
+ float n_obs;
coordination_formation_control_pkg::coordination current_configuration;
coordination_formation_control_pkg::waypoint current_waypoint;
+ void stateCallbackObsPos(const crazyswarm::GenericLogDataConstPtr &pose_msg);
diff --git a/real_robot/coordination_formation_control_pkg/msg/coordination.msg b/real_robot/coordination_formation_control_pkg/msg/coordination.msg
index 3a6a44b4f3876a3fabb3bf61531904c0a22a9ec3..f02a5ba0f743d1f3be5a28ace2df1e1cf2447e26 100644
--- a/real_robot/coordination_formation_control_pkg/msg/coordination.msg
+++ b/real_robot/coordination_formation_control_pkg/msg/coordination.msg
@@ -1,3 +1,4 @@
bool stop_demo
bool init_formation
bool initialize_robot
+bool start_orientation
diff --git a/real_robot/coordination_formation_control_pkg/msg/logData.msg b/real_robot/coordination_formation_control_pkg/msg/logData.msg
index 9816f0758f69bcdece5b74d3229f16f1438ea873..907a38cb199cce232bc0e3a2ea2d5c0e99e416b6 100644
--- a/real_robot/coordination_formation_control_pkg/msg/logData.msg
+++ b/real_robot/coordination_formation_control_pkg/msg/logData.msg
@@ -11,3 +11,6 @@ geometry_msgs/Vector3 input_navigation
geometry_msgs/Vector3 input_obstacle
geometry_msgs/Vector3 input_integration
geometry_msgs/Vector3 input_to_system
+geometry_msgs/Vector3 input_before_to_system
+float32 yaw
+float32[] current_obstacle
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_10_free_space_orientaion_maintain_2_uav/notes.MD b/real_robot/coordination_formation_control_pkg/results/experiment_10_free_space_orientaion_maintain_2_uav/notes.MD
new file mode 100644
index 0000000000000000000000000000000000000000..b2ae02f5e0d3c23f23fb3f7956dcdd16422ec1ef
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_10_free_space_orientaion_maintain_2_uav/notes.MD
@@ -0,0 +1,58 @@
+# Goal
+- Maintain the current centroid
+- X: 0
+- Y: 0
+- Vx: 0
+- Vy: 0
+- Orientation 0
+
+Experiment premises
+same as exp8 but increase the integral control# comment on the results obtained during test
+
+#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_10_free_space_orientaion_maintain_2_uav/result_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_10_free_space_orientaion_maintain_2_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_10_free_space_orientaion_maintain_2_uav/result_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_10_free_space_orientaion_maintain_2_uav/result_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_10_free_space_orientaion_maintain_2_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_10_free_space_orientaion_maintain_2_uav/result_crazyflie_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_11_formation_flocking_3_uav/notes.MD b/real_robot/coordination_formation_control_pkg/results/experiment_11_formation_flocking_3_uav/notes.MD
new file mode 100644
index 0000000000000000000000000000000000000000..eca4b053c896fe3c0458aec8a0b6e0ad9ba726d9
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_11_formation_flocking_3_uav/notes.MD
@@ -0,0 +1,56 @@
+# Goal
+- Maintain the current centroid
+- X: 1.43
+- Y: 0
+- Vx: 0.009
+- Vy: 0.0
+- Orientation 0
+
+Experiment premises
+We want to perform flocking and formation with 3 uav
+#comments
+Wors well but next experiment should wait for the formation to form first, we slow down the velocity alot, ending not so good, remove
+
+# 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_11_formation_flocking_3_uav/result_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_11_formation_flocking_3_uav/result_crazyflie_1_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..440d069e82d9c612eb0d2d300c3ac08325463168
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_11_formation_flocking_3_uav/result_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_11_formation_flocking_3_uav/result_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_11_formation_flocking_3_uav/result_crazyflie_2_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..b4932079a7b023ea48facedd66429a9b923ab1ab
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_11_formation_flocking_3_uav/result_crazyflie_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_11_formation_flocking_3_uav/result_crazyflie_3_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_11_formation_flocking_3_uav/result_crazyflie_3_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..083302420df6868fa0caa8ceb7ca8dd49ca1862d
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_11_formation_flocking_3_uav/result_crazyflie_3_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_12_formation_flocking_3_uav/notes.MD b/real_robot/coordination_formation_control_pkg/results/experiment_12_formation_flocking_3_uav/notes.MD
new file mode 100644
index 0000000000000000000000000000000000000000..957474d720785d76161dfa60cf2df852cac7df79
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_12_formation_flocking_3_uav/notes.MD
@@ -0,0 +1,55 @@
+# Goal
+- Maintain the current centroid
+- X: 1.4
+- Y: 0
+- Vx: 0.005
+- Vy: 0.0
+- Orientation 0
+
+Experiment premises
+We want to perform flocking and formation with 3 uav
+#comments
+Really nice results
+# 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_12_formation_flocking_3_uav/result_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_12_formation_flocking_3_uav/result_crazyflie_1_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..c0c37da18c567d74e59fa5982ce04ac00c0a56e0
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_12_formation_flocking_3_uav/result_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_12_formation_flocking_3_uav/result_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_12_formation_flocking_3_uav/result_crazyflie_2_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..2c5b9bb8022447f08aa9806d29dafb3477eb4597
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_12_formation_flocking_3_uav/result_crazyflie_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_12_formation_flocking_3_uav/result_crazyflie_3_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_12_formation_flocking_3_uav/result_crazyflie_3_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..6f3bd9b68ffa859ea7f56c0715a88f6af037815c
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_12_formation_flocking_3_uav/result_crazyflie_3_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_13_formation_flocking_3_uav/notes.MD b/real_robot/coordination_formation_control_pkg/results/experiment_13_formation_flocking_3_uav/notes.MD
new file mode 100644
index 0000000000000000000000000000000000000000..3ed5d74954ff1c3ebc89432c33c797dcfb21513e
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_13_formation_flocking_3_uav/notes.MD
@@ -0,0 +1,55 @@
+# Goal
+- Maintain the current centroid
+- X: 1.1
+- Y: 0
+- Vx: 0.001
+- Vy: 0.0
+- Orientation 0
+
+Experiment premises
+We want to perform flocking and formation with 3 uav
+#comments
+Really nice results
+# 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_13_formation_flocking_3_uav/result_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_13_formation_flocking_3_uav/result_crazyflie_1_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..c7417c7df63f5cb745351d200748740a550b84c8
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_13_formation_flocking_3_uav/result_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_13_formation_flocking_3_uav/result_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_13_formation_flocking_3_uav/result_crazyflie_2_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..1de0dfddad3431381da24b076abb6f1a2249e484
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_13_formation_flocking_3_uav/result_crazyflie_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_13_formation_flocking_3_uav/result_crazyflie_3_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_13_formation_flocking_3_uav/result_crazyflie_3_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..d69db3a4c419adbdf5045d515bbdc4e65d9237f4
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_13_formation_flocking_3_uav/result_crazyflie_3_logdata.bag differ
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
new file mode 100644
index 0000000000000000000000000000000000000000..7f112cf98e566c26d907d3db98cba190ece708ac
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_14_formation_flocking_obstacle_3_uav/notes.MD
@@ -0,0 +1,54 @@
+# Goal
+- Maintain the current centroid
+- X: 1.1
+- 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
+Really nice results
+# 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.3
+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_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
new file mode 100644
index 0000000000000000000000000000000000000000..ca595d66ac42f6aed57701fefa3da5d42ed49d7e
Binary files /dev/null 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
new file mode 100644
index 0000000000000000000000000000000000000000..ca595d66ac42f6aed57701fefa3da5d42ed49d7e
Binary files /dev/null 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
new file mode 100644
index 0000000000000000000000000000000000000000..ca595d66ac42f6aed57701fefa3da5d42ed49d7e
Binary files /dev/null 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_6_free_space_orientaion_maintain_3_uav/notes.MD b/real_robot/coordination_formation_control_pkg/results/experiment_6_free_space_orientaion_maintain_3_uav/notes.MD
index 390b8d945494f5aa2f25062392f265b67a3f87d4..0f7dd95a72f2014f220f7fcbebdd2a55d7a6cff3 100644
--- a/real_robot/coordination_formation_control_pkg/results/experiment_6_free_space_orientaion_maintain_3_uav/notes.MD
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_6_free_space_orientaion_maintain_3_uav/notes.MD
@@ -7,19 +7,18 @@
- Orientation 0
Experiment premises
-Formation control of 3 crazyflies center at 0,0 position and 0,0 velocity without orientation. The same as experiment 2 but instead of integrating to get the velocity
-reference with previous velocity reference, we integrate with current velocity measurement. To see if the result improves
+Formation control of 3 crazyflies center at 0,0 position and 0,0 velocity with orientation.The same paramters as
# comment on the results obtained during test
-Uses Paramters as expriment 4, but with orienation included
+Uses Paramters as expriment 4, we are using the current velocity to integrate the acceleration as in experiment 4. but with orientation included. Result is bad because of the positioning system in the crazyflie1 drone. It kepps oscilating alot.
#Paramters
# Mission configuration
n_drones: 3
n_ugvs: 0
-hz_freq: 50 # controller frquency
+hz_freq: 50 # controller frequency
include_obstacle: false
-include_orientation: false
+include_orientation: true
n_obs: 0
## Formation configuration
### Uav
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_free_space_orientaion_maintain_3_uav/result_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_6_free_space_orientaion_maintain_3_uav/result_crazyflie_1_logdata.bag
index 7bef26fe3d477e3c1dd98d4f178a5bb6fbb9e1d4..58ab200d2203b2c95048545d1002b81ff22fd7ea 100644
Binary files a/real_robot/coordination_formation_control_pkg/results/experiment_6_free_space_orientaion_maintain_3_uav/result_crazyflie_1_logdata.bag and b/real_robot/coordination_formation_control_pkg/results/experiment_6_free_space_orientaion_maintain_3_uav/result_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_free_space_orientaion_maintain_3_uav/result_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_6_free_space_orientaion_maintain_3_uav/result_crazyflie_2_logdata.bag
index 55df334a2d548060f1c6860e88aecfc9d7862e6a..fe5d914e71832f8be30dbc4acf7a74c60104164d 100644
Binary files a/real_robot/coordination_formation_control_pkg/results/experiment_6_free_space_orientaion_maintain_3_uav/result_crazyflie_2_logdata.bag and b/real_robot/coordination_formation_control_pkg/results/experiment_6_free_space_orientaion_maintain_3_uav/result_crazyflie_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_free_space_orientaion_maintain_3_uav/result_crazyflie_3_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_6_free_space_orientaion_maintain_3_uav/result_crazyflie_3_logdata.bag
index cf3e27f374b8020bbc034e764d7e5f958a007e11..10f13ae4d9096fb583c1adebc2b1fd08f3f4b182 100644
Binary files a/real_robot/coordination_formation_control_pkg/results/experiment_6_free_space_orientaion_maintain_3_uav/result_crazyflie_3_logdata.bag and b/real_robot/coordination_formation_control_pkg/results/experiment_6_free_space_orientaion_maintain_3_uav/result_crazyflie_3_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_7_free_space_maintain_2_uav/notes.MD b/real_robot/coordination_formation_control_pkg/results/experiment_7_free_space_maintain_2_uav/notes.MD
new file mode 100644
index 0000000000000000000000000000000000000000..126e3e3d4a2b34d0f2bc4841b779ed0fb5dafd3c
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_7_free_space_maintain_2_uav/notes.MD
@@ -0,0 +1,56 @@
+# Goal
+- Maintain the current centroid
+- X: 0
+- Y: 0
+- Vx: 0
+- Vy: 0
+- Orientation 0
+
+Experiment premises
+This does not include any integral action , it is with two drone, does not include orientaion, works very well
+# comment on the results obtained during test
+Uses Paramters as expriment 4, we are using the current velocity to integrate the acceleration as in experiment 4. but with orientation included. Result is bad because of the positioning system in the crazyflie1 drone. It kepps oscilating alot.
+Works fin, the issue before was I was integrating with the previous velocity
+#Paramters
+# 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.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.009
+
+# 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_7_free_space_maintain_2_uav/result_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_7_free_space_maintain_2_uav/result_crazyflie_1_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..3d77f4bfe371a39b30b59307d264e10093328d5c
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_7_free_space_maintain_2_uav/result_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_7_free_space_maintain_2_uav/result_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_7_free_space_maintain_2_uav/result_crazyflie_2_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..8ed23de95d7c2daad4a58f3b0c0188a86c5a964b
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_7_free_space_maintain_2_uav/result_crazyflie_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_7_free_space_orientaion_maintain_3_uav/notes.MD b/real_robot/coordination_formation_control_pkg/results/experiment_7_free_space_orientaion_maintain_3_uav/notes.MD
new file mode 100644
index 0000000000000000000000000000000000000000..55dc2362121b1ed2509e968b6be097a9247b67b5
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_7_free_space_orientaion_maintain_3_uav/notes.MD
@@ -0,0 +1,49 @@
+# Goal
+- Maintain the current centroid
+- X: 0
+- Y: 0
+- Vx: 0
+- Vy: 0
+- Orientation 0
+
+Experiment premises
+Formation control of 3 crazyflies center at 0,0 position and 0,0 velocity with orientation.The same paramters as experiment_6 but adding the orinetation also to integral action
+
+# comment on the results obtained during test
+Uses Paramters as expriment 4, we are using the current velocity to integrate the acceleration as in experiment 4. but with orientation included. Result is bad because of the positioning system in the crazyflie1 drone. It kepps oscilating alot.
+
+#Paramters
+# 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_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.1
+uav_c1_delta: 0.07
+
+
+
+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.1 #/* sample time*/
+uav_int_max: 0.01 #/* maximum integral*/
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_7_free_space_orientaion_maintain_3_uav/result_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_7_free_space_orientaion_maintain_3_uav/result_crazyflie_1_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..958142cf07508ff629a346c72816cead9019aef2
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_7_free_space_orientaion_maintain_3_uav/result_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_7_free_space_orientaion_maintain_3_uav/result_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_7_free_space_orientaion_maintain_3_uav/result_crazyflie_2_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..8f04f395499c1917209a8468d7c8536e04805851
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_7_free_space_orientaion_maintain_3_uav/result_crazyflie_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_7_free_space_orientaion_maintain_3_uav/result_crazyflie_3_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_7_free_space_orientaion_maintain_3_uav/result_crazyflie_3_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..ec2ea45229dc64356018df67f3c8788f4bf8a74c
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_7_free_space_orientaion_maintain_3_uav/result_crazyflie_3_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_8_free_space_orientaion_maintain_2_uav/notes.MD b/real_robot/coordination_formation_control_pkg/results/experiment_8_free_space_orientaion_maintain_2_uav/notes.MD
new file mode 100644
index 0000000000000000000000000000000000000000..31c4e9717f5763de156395894930d479ec45edda
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_8_free_space_orientaion_maintain_2_uav/notes.MD
@@ -0,0 +1,56 @@
+# Goal
+- Maintain the current centroid
+- X: 0
+- Y: 0
+- Vx: 0
+- Vy: 0
+- Orientation 0
+
+Experiment premises
+This does not include any integral action , it is with two drone, sames as before but with integral action. this includes orineation and it seem to works
+# comment on the results obtained during test
+Uses Paramters as expriment 4, we are using the current velocity to integrate the acceleration as in experiment 4. but with orientation included. Result is bad because of the positioning system in the crazyflie1 drone. It keeps oscilating alot.
+Works fin, the issue before was I was integrating with the previous velocity
+#Paramters
+# 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.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.009
+
+# 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_8_free_space_orientaion_maintain_2_uav/result_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_8_free_space_orientaion_maintain_2_uav/result_crazyflie_1_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..86b68a9e2dd6a0f052e85fd569d6044a99526166
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_8_free_space_orientaion_maintain_2_uav/result_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_8_free_space_orientaion_maintain_2_uav/result_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_8_free_space_orientaion_maintain_2_uav/result_crazyflie_2_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..cde28002a2d4a1673839adcc2ba729505adb4a4b
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_8_free_space_orientaion_maintain_2_uav/result_crazyflie_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_9_free_space_orientaion_maintain_2_uav/notes.MD b/real_robot/coordination_formation_control_pkg/results/experiment_9_free_space_orientaion_maintain_2_uav/notes.MD
new file mode 100644
index 0000000000000000000000000000000000000000..4a9b0cfdaf7713550f0d80167a687b5c0340168e
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_9_free_space_orientaion_maintain_2_uav/notes.MD
@@ -0,0 +1,58 @@
+# Goal
+- Maintain the current centroid
+- X: 0
+- Y: 0
+- Vx: 0
+- Vy: 0
+- Orientation 0
+
+Experiment premises
+same as exp8 but increase the integral control# comment on the results obtained during test
+
+#comments
+
+it works quite well, it was able to rotate correclty
+#Paramters
+# 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_9_free_space_orientaion_maintain_2_uav/result_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_9_free_space_orientaion_maintain_2_uav/result_crazyflie_1_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..9073b76df716258e5d189aac2e8cc778aefad266
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_9_free_space_orientaion_maintain_2_uav/result_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_9_free_space_orientaion_maintain_2_uav/result_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_9_free_space_orientaion_maintain_2_uav/result_crazyflie_2_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..204c6f6ab9c1e27b7f7e04651a3e00b94a3d2a1a
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_9_free_space_orientaion_maintain_2_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 667af43f17dd63e6b0bdd68a04d1196e12d746f1..60dc7fea341597c2aa691b711dc87a28506cde61 100644
--- a/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
@@ -89,6 +89,7 @@ int main(int argc, char **argv) {
logdata->current_pose.resize(n_drones);
logdata->current_twist.resize(n_drones);
logdata->current_acc.resize(n_drones);
+ logdata->current_obstacle.resize(n_obs*3);
/* current command*/
crazyswarm::Hover current_command;
@@ -235,7 +236,8 @@ int main(int argc, char **argv) {
/* main loop*/
/* TODO improve state machine*/
- float vxx = 0, vyy = 0;
+ float vxx = 0, vyy = 0, yaw = 0;
+ Eigen::Vector2d vel_command(0,0);
while(ros::ok())
{
/*get current state of all the robot*/
@@ -245,6 +247,7 @@ int main(int argc, char **argv) {
current_state = uavs[i-1]->get_state();
uav_state_q(0, i-1) =current_state.q(0);
uav_state_q(1, i-1) =current_state.q(1);
+ yaw = current_state.q(3);
uav_state_p(0, i-1) = current_state.p(0);
uav_state_p(1, i-1) = current_state.p(1);
uav_state_a(0, i-1) = current_state.a(0);
@@ -281,7 +284,14 @@ int main(int argc, char **argv) {
/* use orienation: update orientation*/
if (use_orientation)
{
- current_waypoint.orientation = next_waypoint->orientation;
+ if(coord_config->start_orientation)
+ {
+ current_waypoint.orientation = next_waypoint->orientation;
+ }
+ else
+ {
+ current_waypoint.orientation = NOORIENTATION;
+ }
}
else
{
@@ -299,6 +309,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;
}
}
else
@@ -310,7 +321,7 @@ int main(int argc, char **argv) {
input = swam_controller.controller(uav_state_q, uav_state_p,current_waypoint,current_obs);
// std::cout << input << std::endl;
/* integrate acceleration*/
- Eigen::Vector2d vel_command = acc_int.integrate(input(0,0), input(1,0),uav_state_p(0, robotid-1),uav_state_p(1, robotid-1) );
+ vel_command = acc_int.integrate(input(0,0), input(1,0),uav_state_p(0, robotid-1),uav_state_p(1, robotid-1) );
Eigen::Vector2d vel_command_;
vel_command_ << input(0,0), input(1,0);
/* reached a certain waypoint and we want to stop sending command*/
@@ -319,7 +330,7 @@ int main(int argc, char **argv) {
current_command.vx = 0;
current_command.vy = 0;
}
- else if((abs(uav_state_q(0, robotid-1)) > 1.7) || (abs(uav_state_q(1, robotid-1)) > 1.0))
+ else if((abs(uav_state_q(0, robotid-1)) > 2) || (abs(uav_state_q(1, robotid-1)) > 1.0))
{
current_command.vx = 0;
current_command.vy = 0;
@@ -331,11 +342,11 @@ int main(int argc, char **argv) {
/* get latest yaw*/
robot_state_t current_state_;
current_state_ = uavs[robotid-1]->get_state();
- float yaw = current_state_.q(3)*M_PI/180.0; // TODO confirm that indeed it is in degrees
+ float yaw_ = current_state_.q(3)*M_PI/180.0; // TODO confirm that indeed it is in degrees
// float vxl = vel_command_(0)*std::cos(yaw) + vel_command_(1)*std::sin(yaw) ;
// float vyl = - vel_command_(0)*std::sin(yaw) + vel_command_(1)*std::cos(yaw) ;
- float vxl = vel_command(0)*std::cos(yaw) + vel_command(1)*std::sin(yaw) ;
- float vyl = - vel_command(0)*std::sin(yaw) + vel_command(1)*std::cos(yaw) ;
+ float vxl = vel_command(0)*std::cos(yaw_) + vel_command(1)*std::sin(yaw_) ;
+ float vyl = - vel_command(0)*std::sin(yaw_) + vel_command(1)*std::cos(yaw_) ;
// current_command.vx = vxl;
// current_command.vy = vyl;
@@ -343,7 +354,7 @@ int main(int argc, char **argv) {
in << vxl, vyl;
row_vector_2d_t range;
// range << -0.15, 0.15;
- range << -3, 3;
+ range << -1, 1;
row_vector_2d_t out = saturate(in, range);
current_command.vx = out(0);
@@ -435,8 +446,26 @@ int main(int argc, char **argv) {
logdata->input_orientation.y =input(1,4);
logdata->input_integration.x = input(0,5);
logdata->input_integration.y = input(1,5);
- logdata->input_to_system.x = current_command.vx;
- logdata->input_to_system.y = current_command.vy;
+ logdata->input_to_system.x =vel_command(0);
+ logdata->input_to_system.y =vel_command(1);
+ logdata->input_before_to_system.x = current_command.vx;
+ logdata->input_before_to_system.y = current_command.vy;
+ logdata->yaw = yaw;
+ // TODO add to omni wheel node
+ if(n_obs != 0)
+ {
+ 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];
+ indx = indx+3;
+ }
+ }
+
+ // std::cout << yaw << std::endl;
// uav_state_p(0, robotid-1),uav_state_p(1, robotid-1)
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 989f71e64430a3f1b215b60b84874bba84b340a7..4042212cc961d85edcdd161d1ac8071dbe6d2d69 100644
--- a/real_robot/coordination_formation_control_pkg/src/mission_controller.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/mission_controller.cpp
@@ -17,10 +17,25 @@ nh(nh)
this->current_configuration.stop_demo = false;
this->current_configuration.init_formation = false;
this->current_configuration.initialize_robot = false;
+ this->current_configuration.start_orientation = false;
this->waypoint_publisher = this->nh.advertise<coordination_formation_control_pkg::waypoint>("goal_waypoint",1);
+
+
this->current_waypoint.vel[0] = 0;
this->current_waypoint.vel[1] = 0;
this->start_publishing_waypoint = false;
+ this->eps = 0.1;
+
+ this->nh.getParam("/n_obs", this->n_obs);
+ std::cout << this->n_obs << std::endl;
+ if (this->n_obs>0)
+ {
+ this->obstacle_pose.resize(3,this->n_obs);
+
+ this->obs_state_publisher = nh.subscribe("/cf4/position", 1,&missionController::stateCallbackObsPos, this);
+ this->current_waypoint.obstacle.resize(this->n_obs*3);
+
+ }
/*dynamic_reconfigure*/
@@ -151,6 +166,20 @@ for (int i = 0; i ++; i < n_ugvs)
}
+ /* get position of current obstacle*/
+ if(this->n_obs>0)
+ {
+ for(int i = 0; i < this->n_obs; i++)
+ {
+ 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;
+ }
+ }
+
+
+
if(this->shut_down)
{
@@ -309,7 +338,7 @@ for (int i = 0; i ++; i < n_ugvs)
{
// std::cout << dist << std::endl;
/* send goals*/
- if (dist < 0.1) // waypoint reached TODO, this is hard coded, not a good idea
+ if (dist < this->eps) // waypoint reached TODO, this is hard coded, not a good idea
{
if(this->mantain_position == false)
{
@@ -396,6 +425,9 @@ void missionController::updateCofig(coordination_formation_control_pkg::missionC
}
+ this->current_configuration.start_orientation =config.start_orientation;
+
+
}
Eigen::MatrixXf missionController::compute_cluster(Eigen::MatrixXf uav_q, Eigen::MatrixXf ugv_q,int num_ugvs, int num_uavs,Mat &previous_labels,Eigen::MatrixXf & previous_centroids)
@@ -502,6 +534,14 @@ this->cluster_initialized = true;
}
+// calback for obstacle pose
+void missionController::stateCallbackObsPos(const crazyswarm::GenericLogDataConstPtr &pose_msg)
+{
+ // not the best implementation
+ this->obstacle_pose(0,0) = pose_msg->values[0]/1000.0;
+ this->obstacle_pose(1,0) = pose_msg->values[1]/1000.0;
+}
+
int main(int argc, char **argv)
{
ros::init(argc, argv, "mission_controller_node");
diff --git a/real_robot/coordination_formation_control_pkg/src/swamControllerAlg.cpp b/real_robot/coordination_formation_control_pkg/src/swamControllerAlg.cpp
index 911acd90345ff2b69a189a944c8dbfccbab7c786..3df94e842e3e822e6dc190300839dedcf1b75250 100644
--- a/real_robot/coordination_formation_control_pkg/src/swamControllerAlg.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/swamControllerAlg.cpp
@@ -338,8 +338,8 @@ input_vector_t swamControllerAlg::controller(Eigen::MatrixXf q, Eigen::MatrixXf
}
else
{
- q_centroid << centroid_q(0), centroid_q(1),0;
- q_leader << q(0,0),q(1,0),0;
+ q_centroid << q(0,0),q(1,0),0;//centroid_q(0), centroid_q(1),0;
+ q_leader << q(0,1),q(1,1),0;
}
float dlc = (q_leader-q_centroid).norm(); // distance from centroid to leader drone
row_vector_3d_t lcv = q_leader-q_centroid; // vector from centroid to leader
@@ -370,8 +370,13 @@ input_vector_t swamControllerAlg::controller(Eigen::MatrixXf q, Eigen::MatrixXf
}
row_vector_2d_t final_rot_d(rot_d(0), rot_d(1));
+ // if(this->robot_id.id_m == 0)
+ // {
+ u_orientation = this->form_param.gain.c1_theta*abs(alpha)*final_rot_d;
+ // std::cout << alpha << std::endl;
+ //
+ // }
- u_orientation = this->form_param.gain.c1_theta*abs(alpha)*final_rot_d;
}
// ROS_INFO("Swam Controller: orientation completed");
@@ -379,7 +384,7 @@ input_vector_t swamControllerAlg::controller(Eigen::MatrixXf q, Eigen::MatrixXf
/* integral term*/
row_vector_2d_t u_integral;
u_integral << 0.0,0.0;
- this->integral_error = this->integral_error + error_grad_term*this->form_param.param.dt;
+ this->integral_error = this->integral_error + (error_grad_term)*this->form_param.param.dt;
u_integral = this->form_param.gain.c1_delta *this->integral_error;
u_integral = this->saturate(u_integral, range_sat);