diff --git a/real_robot/coordination_formation_control_pkg/CMakeLists.txt b/real_robot/coordination_formation_control_pkg/CMakeLists.txt
index 4e8db4fc05525792b2194f7519481c6fd1a393be..fdca1de8d36c39c0122a4d29ae7aef550ac48450 100644
--- a/real_robot/coordination_formation_control_pkg/CMakeLists.txt
+++ b/real_robot/coordination_formation_control_pkg/CMakeLists.txt
@@ -58,7 +58,7 @@ add_executable(mission_controller src/mission_controller.cpp src/robot_state.cpp
add_dependencies(mission_controller ${PROJECT_NAME}_gencfg)
target_link_libraries(mission_controller ${catkin_LIBRARIES} ${Eigen_LIBRARIES})
-add_executable(drone_node src/drone_node.cpp src/robot_state.cpp)
+add_executable(drone_node src/drone_node.cpp src/robot_state.cpp src/swamControllerAlg.cpp)
target_link_libraries(drone_node ${catkin_LIBRARIES} ${Eigen_LIBRARIES})
diff --git a/real_robot/coordination_formation_control_pkg/config/crazyfliesConfig.yaml b/real_robot/coordination_formation_control_pkg/config/crazyfliesConfig.yaml
index 8d7912f8688fa833d2f7f81712501c026589cbef..b0351bc64287ef0936897141ae92e27f3703ff35 100644
--- a/real_robot/coordination_formation_control_pkg/config/crazyfliesConfig.yaml
+++ b/real_robot/coordination_formation_control_pkg/config/crazyfliesConfig.yaml
@@ -11,7 +11,7 @@ crazyflies:
channel: 80
initialPosition: [0.0, 0.0, 0.0]
type: default
-# - id: 4
-# 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
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml b/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml
index f4f82b558ec349de8baf94d33bbea7d7b8445545..e915a4080219bc4abccaae95a6605ea5bac3506e 100644
--- a/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml
+++ b/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml
@@ -1,2 +1,2 @@
-n_drones: 3
+n_drones: 4
hz_freq: 50 # controller frquency
diff --git a/real_robot/coordination_formation_control_pkg/include/robot_state.h b/real_robot/coordination_formation_control_pkg/include/robot_state.h
index 3c446f238172b1ef6e9ee75d0bf5f0bed527f923..4f9550bdef79435e642a205e2b07e1d1bfc62f14 100644
--- a/real_robot/coordination_formation_control_pkg/include/robot_state.h
+++ b/real_robot/coordination_formation_control_pkg/include/robot_state.h
@@ -7,6 +7,7 @@
#include "geometry_msgs/Pose.h"
#include "crazyswarm/GenericLogData.h"
#include "robot_state.h"
+#include "tf/transform_datatypes.h"
// #include <message_filters/subscriber.h>
// #include <message_filters/time_synchronizer.h>
diff --git a/real_robot/coordination_formation_control_pkg/include/shared_definitions.h b/real_robot/coordination_formation_control_pkg/include/shared_definitions.h
index e6bd0e87bd943ab17730de13b477884da6828818..8934bb8d23adbe664f551d7eeaa621456b2f6254 100644
--- a/real_robot/coordination_formation_control_pkg/include/shared_definitions.h
+++ b/real_robot/coordination_formation_control_pkg/include/shared_definitions.h
@@ -17,7 +17,7 @@ typedef enum robot_type
/* robot state-ugv and agv*/
typedef struct robot_state
{
- Vector3d q; /*position*/
+ Vector4d q; /*position 3 position 1 orientation yaw*/
Vector3d p; /* velocity*/
}robot_state_t;
@@ -27,6 +27,9 @@ typedef struct robot_ind
int id; /*d*/
std::string robot_name; /* robot name*/
robot_type_t t; /*robot_type*/
+ double safe_ditance; /* important to prevent mistakenly putting a very small number*/
+ double robot_size; /* also for safety reasons*/
+ bool leader;
}robot_ind_t;
/* obstacle definition*/
@@ -37,6 +40,62 @@ typedef struct obstacle
float radius; /*radius of the obstacle*/
}obstacle_t;
+/* goal waypoint*/
+typedef struct waypoint
+{
+ Vector4d q; /*position 3 position 1 orientation yaw*/
+ Vector3d p; /* velocity*/
+ MatrixXf cluster;
+
+}waypoint_t;
+
+/* formation configuraiton*/
+
+
+
+
+/* formation gain*/
+typedef struct form_gains
+{
+ double c1_alpha; /*formation gains*/
+ double c2_alpha;
+ double c1_beta; /* consesus gains*/
+ double c2_beta;
+ double c1_gamma; /* navigation gains*/
+ double c2_gamma;
+ double c1_theta; /* orientation gains*/
+ double c1_delta; /* integral gains*/
+ double c2_delta;
+ double c3_delta;
+
+}form_gains_t;
+
+/* formation parameters*/
+typedef struct form_param
+{
+ double d; /* distance between agent*/
+ double formation_t; /* type must be minimum 2, 3: tuangle formation, 4 suare, 5 pentagon*/
+ double k;
+ double ratio; /* ratio between dist inter and obstacle*/
+ double eps;
+ double a;
+ double b;
+ double h_alpha;
+ double h_beta;
+ double d_obs;
+ double nav_type; /* convergence approach -1, parallel approach 2*/
+ double integrator; /* single/ double integrator*/
+ double dt; /* sample time*/
+ double int_max; /* maximum integral*/
+}form_param_t;
+
+/* formation configuraiton*/
+typedef struct formation_config
+{
+ form_gains_t gain;
+ form_param_t param;
+}formation_config_t;
+
diff --git a/real_robot/coordination_formation_control_pkg/include/swamControllerAlg.h b/real_robot/coordination_formation_control_pkg/include/swamControllerAlg.h
new file mode 100644
index 0000000000000000000000000000000000000000..2526e7b664412afff33d3fada39cf7d3ba2f37e3
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/include/swamControllerAlg.h
@@ -0,0 +1,51 @@
+#ifndef SWAM_CONTROLLER_ALG_UAV_H
+#define SWAM_CONTROLLER_ALG_UAV_H
+
+#include "shared_definitions.h"
+
+typedef Eigen::Matrix<double, 2,5> input_vector_t;
+/* input vector conatains the following
+ * totall input x y
+ * ofr debugging I print the remaining input
+ * input_formation
+ * input obstacle
+ * input navigation
+ * input orientation
+ */
+
+ typedef Eigen::Matrix <double, 2, 1>row_vector_2d_t;
+
+class swamControllerAlg
+{
+private:
+ formation_config_t form_param; /*parameters of the formation*/
+ row_vector_2d_t integral_error;
+ row_vector_2d_t previous_error;
+ robot_ind_t robot_id;
+
+public:
+ /* constructor*/
+ swamControllerAlg(robot_ind_t indentification, formation_config_t formation_config);
+ /* destructor*/
+ ~swamControllerAlg();
+ /* includes the main controller*/
+ input_vector_t controller(Eigen::MatrixXf *q, Eigen::MatrixXf *p, waypoint_t ref, obstacle_t* obs);
+
+protected:
+ /* configuration formation information like interagent distances*/
+ void configure_formation();
+
+
+
+};
+
+
+
+
+
+
+
+
+
+
+#endif
diff --git a/real_robot/coordination_formation_control_pkg/launch/bringup_crazyflie_servers.launch b/real_robot/coordination_formation_control_pkg/launch/bringup_crazyflie_servers.launch
index 689252ca17ac342774ef5a72e6e2a80d1c39ddfd..0c023f14e47def5e6a92c81607a363bd4038292c 100644
--- a/real_robot/coordination_formation_control_pkg/launch/bringup_crazyflie_servers.launch
+++ b/real_robot/coordination_formation_control_pkg/launch/bringup_crazyflie_servers.launch
@@ -11,7 +11,7 @@
# Logging configuration (Use enable_logging to actually enable logging)
genericLogTopics: ["position", "velocity"]
genericLogTopicFrequencies: [10,10]
- genericLogTopic_position_Variables: ["stateEstimateZ.x", "stateEstimateZ.y","stateEstimateZ.z"]
+ genericLogTopic_position_Variables: ["stateEstimateZ.x", "stateEstimateZ.y","stateEstimateZ.z", "stateEstimate.yaw"]
genericLogTopic_velocity_Variables: ["stateEstimateZ.vx", "stateEstimateZ.vy","stateEstimateZ.vz"]
# firmware parameters for all drones (use crazyflieTypes.yaml to set per type, or
# allCrazyflies.yaml to set per drone)
diff --git a/real_robot/coordination_formation_control_pkg/launch/mission_controller.launch b/real_robot/coordination_formation_control_pkg/launch/mission_controller.launch
index 77d5b342a9ef97226487a1899f7a60e2e4d28e76..141806d2a21feccb25b2254adbc07aa5692c0f83 100644
--- a/real_robot/coordination_formation_control_pkg/launch/mission_controller.launch
+++ b/real_robot/coordination_formation_control_pkg/launch/mission_controller.launch
@@ -15,16 +15,16 @@
</node>
</group>
- <!-- <group ns="uav_3">
+ <group ns="uav_3">
<node name="drone_node" pkg="coordination_formation_control_pkg" type="drone_node" output="screen" args="3" launch-prefix="xterm -e">
</node>
- </group> -->
-<!--
+ </group>
+
<group ns="uav_4">
<node name="drone_node" pkg="coordination_formation_control_pkg" type="drone_node" output="screen" args="4" launch-prefix="xterm -e">
</node>
- </group> -->
+ </group>
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 c2d1b1fa983534e0902398d0f40e8e4f4c6c13fc..879445da203899d6155f4f710ae2f32924a696a5 100644
--- a/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
@@ -3,6 +3,7 @@
#include "ros/ros.h"
#include "robot_state.h"
#include "shared_definitions.h"
+#include "swamControllerAlg.h"
#include "coordination_formation_control_pkg/coordination.h"
#include "crazyswarm/Hover.h"
#include "crazyswarm/Land.h"
@@ -61,6 +62,9 @@ int main(int argc, char **argv) {
/* TODO publisher to publish results and rosbag it*/
+ /* intialize controller*/
+
+
/*create a list of drones to subscribe to*/
std::vector<robotState *> uavs;
robot_ind_t uav_def;
diff --git a/real_robot/coordination_formation_control_pkg/src/robot_state.cpp b/real_robot/coordination_formation_control_pkg/src/robot_state.cpp
index 05caa22543fd995e13cb2a4c39fd8caa191111fc..4f33c59846c5eb1b48447a68a16bfad95cd0479b 100644
--- a/real_robot/coordination_formation_control_pkg/src/robot_state.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/robot_state.cpp
@@ -66,6 +66,14 @@ void robotState::stateCallbackUgvPose(const geometry_msgs::PoseConstPtr &pose_ms
this->state.q(0) = pose_msg->position.x;
this->state.q(1) = pose_msg->position.y;
this->state.q(2) = pose_msg->position.z;
+ double roll, pitch,yaw;
+ tf::Quaternion quat(pose_msg->orientation.x,
+ pose_msg->orientation.y,
+ pose_msg->orientation.z,
+ pose_msg->orientation.w);
+
+ tf::Matrix3x3(quat).getRPY(roll, pitch,yaw);
+ this->state.q(3) = yaw*180.0/M_PI; // degres to mantain consistency
}
void robotState::stateCallbackUavVel(const crazyswarm::GenericLogDataConstPtr &vel_msg)
{
@@ -78,4 +86,6 @@ void robotState::stateCallbackUavPos(const crazyswarm::GenericLogDataConstPtr &p
this->state.q(0) = pose_msg->values[0];
this->state.q(1) = pose_msg->values[1];
this->state.q(2) = pose_msg->values[2];
+ this->state.q(3) = pose_msg->values[3]; // degres
+
}
diff --git a/real_robot/coordination_formation_control_pkg/src/swamControllerAlg.cpp b/real_robot/coordination_formation_control_pkg/src/swamControllerAlg.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fe76f2797f63bf7130a46f05d944e0cd4c8ed95a
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/src/swamControllerAlg.cpp
@@ -0,0 +1 @@
+#include "swamControllerAlg.h"