From 28869aada1529b01edd96bd8e23b8393061a9a7a Mon Sep 17 00:00:00 2001
From: Stevedan Ogochukwu Omodolor <stevedan.o.omodolor@gmail.com>
Date: Thu, 19 May 2022 14:42:54 +0200
Subject: [PATCH] testing controller
---
.../src/test_controller.cpp | 62 +++++++++++++++++++
1 file changed, 62 insertions(+)
create mode 100644 real_robot/coordination_formation_control_pkg/src/test_controller.cpp
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 0000000..7badb88
--- /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;
+}
--
GitLab