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; +}