diff --git a/real_robot/coordination_formation_control_pkg/src/test_controller.cpp b/real_robot/coordination_formation_control_pkg/src/test_controller.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7badb8830482358cd980e18151aa003147ba202d
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/src/test_controller.cpp
@@ -0,0 +1,62 @@
+#include "ros/ros.h"
+#include "shared_definitions.h"
+#include "swamControllerAlg.h"
+/* this node is to test and ensure that the controller result a valid result
+ * the result is compared with the result obtained from the simulation*/
+
+
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "test_swam_controller");
+  ros::NodeHandle nh("~");
+  input_vector_t input;
+  std::cout << "Starting test" << std::endl;
+
+
+  /* remember to rosparam load the configuration file in the conifg path*/
+  formation_config_t formation_cfg;
+  nh.getParam("/uav_c1_alpha",formation_cfg.gain.c1_alpha);
+  nh.getParam("/uav_c2_alpha",formation_cfg.gain.c2_alpha);
+  nh.getParam("/uav_c1_beta",formation_cfg.gain.c1_beta);
+  nh.getParam("/uav_c2_beta",formation_cfg.gain.c2_beta);
+  nh.getParam("/uav_c1_gamma",formation_cfg.gain.c1_gamma);
+  nh.getParam("/uav_c2_gamma",formation_cfg.gain.c2_gamma);
+  nh.getParam("/uav_c1_theta",formation_cfg.gain.c1_theta);
+  nh.getParam("/uav_c1_delta",formation_cfg.gain.c1_delta);
+  nh.getParam("/dt",formation_cfg.param.dt);
+  nh.getParam("/uav_d", formation_cfg.param.d);
+  nh.getParam("/uav_formation_t", formation_cfg.param.formation_t);
+  nh.getParam("/uav_k", formation_cfg.param.k);
+  nh.getParam("/uav_ratio", formation_cfg.param.ratio);
+  nh.getParam("/uav_eps", formation_cfg.param.eps);
+  nh.getParam("/uav_a", formation_cfg.param.a);
+  nh.getParam("/uav_b", formation_cfg.param.b);
+  nh.getParam("/uav_h_alpha", formation_cfg.param.h_alpha);
+  nh.getParam("/uav_h_beta", formation_cfg.param.h_beta);
+  nh.getParam("/uav_d_obs", formation_cfg.param.d_obs);
+  nh.getParam("/uav_nav_type", formation_cfg.param.nav_type);
+  nh.getParam("/uav_integrator", formation_cfg.param.integrator);
+  nh.getParam("/uav_int_max", formation_cfg.param.int_max);
+
+
+  robot_ind_t robot_ind;
+
+  robot_ind.id = 1;
+  robot_ind.id_m = 0;
+
+  robot_ind.robot_name = "crazyflie_" + std::to_string(robot_ind.id);
+  robot_ind.t = CRAZYFLIE;
+  robot_ind.safe_ditance = 0.15; // to prevent collision
+  robot_ind.robot_size = 0.15; // drone radius
+  robot_ind.leader = true;
+
+  swamControllerAlg swam_controller(robot_ind, formation_cfg);
+
+
+  // input = swam_controller.controller(uav_state_q, uav_state_p,current_waypoint,current_obs);
+  std::cout << input << std::endl;
+
+
+  return 0;
+}