diff --git a/real_robot/coordination_formation_control_pkg/CMakeLists.txt b/real_robot/coordination_formation_control_pkg/CMakeLists.txt
index fdca1de8d36c39c0122a4d29ae7aef550ac48450..cc3cc07f5767df2b81c6d10d61a9bab25541b905 100644
--- a/real_robot/coordination_formation_control_pkg/CMakeLists.txt
+++ b/real_robot/coordination_formation_control_pkg/CMakeLists.txt
@@ -61,6 +61,9 @@ target_link_libraries(mission_controller ${catkin_LIBRARIES} ${Eigen_LIBRARIES})
add_executable(drone_node src/drone_node.cpp src/robot_state.cpp src/swamControllerAlg.cpp)
target_link_libraries(drone_node ${catkin_LIBRARIES} ${Eigen_LIBRARIES})
+add_executable(omniwheel_node src/omniwheel_node.cpp src/robot_state.cpp src/swamControllerAlg.cpp)
+target_link_libraries(omniwheel_node ${catkin_LIBRARIES} ${Eigen_LIBRARIES})
+
install(DIRECTORY
launch
diff --git a/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml b/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..c77a88d45204dc4cd6e0dd4dcd73c9876d822015 100644
--- a/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml
+++ b/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml
@@ -0,0 +1,53 @@
+# Uav
+uav_c1_alpha: 0.2
+uav_c2_alpha: 1 # this is recomputed
+uav_c1_beta: 0.3
+uav_c2_beta: 1 # this is recomputed
+uav_c1_gamma: 0.1
+uav_c2_gamma: 0.2
+uav_c1_theta: 0.2
+uav_c1_delta: 0.09
+
+
+uav_d: 0.5
+uav_formation_t: 4 #/* 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.02 #/* sample time*/
+uav_int_max: 0.1 #/* maximum integral*/
+
+# ugv
+
+# Uav
+ugv_c1_alpha: 0.2
+ugv_c2_alpha: 1 # this is recomputed
+ugv_c1_beta: 0.3
+ugv_c2_beta: 1 # this is recomputed
+ugv_c1_gamma: 0.1
+ugv_c2_gamma: 0.2
+ugv_c1_theta: 0.2
+ugv_c1_delta: 0.09
+
+
+ugv_d: 0.5
+ugv_formation_t: 4 #/* type must be minimum 2, 3: tuangle formation, 4 suare, 5 pentagon*/
+ugv_k: 7
+ugv_ratio: 0.8 # /* ratio between dist inter and obstacle*/
+ugv_eps: 0.1
+ugv_a: 5
+ugv_b: 5
+ugv_h_alpha: 0.2
+ugv_h_beta: 0.9
+ugv_d_obs: 0
+ugv_nav_type: 1 #/* convergence approach -1, parallel approach 2*/
+ugv_integrator: 2 #/* single/ double integrator*/
+ugv_dt: 0.02 #/* sample time*/
+ugv_int_max: 0.1 #/* maximum integral*/
diff --git a/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml b/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml
index e915a4080219bc4abccaae95a6605ea5bac3506e..177b3db309e21be29d6fab7a7ddd2f12e4103ec9 100644
--- a/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml
+++ b/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml
@@ -1,2 +1,6 @@
n_drones: 4
+n_ugvs: 2
hz_freq: 50 # controller frquency
+include_obstacle: false
+include_orientation: false
+n_obs: 0
diff --git a/real_robot/coordination_formation_control_pkg/include/integration.h b/real_robot/coordination_formation_control_pkg/include/integration.h
new file mode 100644
index 0000000000000000000000000000000000000000..e48652110f237d7d33ecb36967f600fe3527b1f6
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/include/integration.h
@@ -0,0 +1,43 @@
+#ifndef RUNGE_KUTTE_INTEGRATION_H
+#define RUNGE_KUTTE_INTEGRATION_H
+#include "shared_definitions.h"
+
+class integration
+{
+private:
+
+Eigen::Vector2d accel;
+Eigen::Vector2d vel;
+float dt;
+
+
+public:
+ integration(float ax_initial, float ay_initial,float dt)
+ {
+ this->accel(0) = ax_initial;
+ this->accel(1) = ay_initial;
+ this->vel(0) = 0;
+ this->vel(1) = 0;
+ this->dt = dt;
+
+ }
+ ~integration()
+ {
+
+ }
+
+ Eigen::Vector2d integrate(float ax, float ay)
+ {
+ /* returns the vx and vy*/
+ this->accel(0) = ax;
+ this->accel(1) = ay;
+ this->vel = this->vel + this->accel*dt;
+ return this->vel;
+ }
+
+
+};
+
+
+
+#endif
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 8934bb8d23adbe664f551d7eeaa621456b2f6254..bb0c98914b691fdc195a4fd3f0ab177d0770e640 100644
--- a/real_robot/coordination_formation_control_pkg/include/shared_definitions.h
+++ b/real_robot/coordination_formation_control_pkg/include/shared_definitions.h
@@ -6,6 +6,7 @@
// #define MAX_CHAR 30
using namespace Eigen;
+#define NOORIENTATION -370 // just a random number out of 0 to 360
typedef enum robot_type
{
@@ -27,8 +28,8 @@ 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*/
+ float safe_ditance; /* important to prevent mistakenly putting a very small number*/
+ float robot_size; /* also for safety reasons*/
bool leader;
}robot_ind_t;
@@ -43,9 +44,10 @@ typedef struct obstacle
/* goal waypoint*/
typedef struct waypoint
{
- Vector4d q; /*position 3 position 1 orientation yaw*/
- Vector3d p; /* velocity*/
+ Vector3d q; /*position 2 position 1 orientation yaw*/
+ Vector2d p; /* velocity*/
MatrixXf cluster;
+ float orientation;
}waypoint_t;
@@ -57,36 +59,35 @@ typedef struct waypoint
/* 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;
+ float c1_alpha; /*formation gains*/
+ float c2_alpha;
+ float c1_beta; /* consesus gains*/
+ float c2_beta;
+ float c1_gamma; /* navigation gains*/
+ float c2_gamma;
+ float c1_theta; /* orientation gains*/
+ float c1_delta; /* integral gains*/
+
}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*/
+ float d; /* distance between agent*/
+ float formation_t; /* type must be minimum 2, 3: tuangle formation, 4 suare, 5 pentagon*/
+ float k;
+ float ratio; /* ratio between dist inter and obstacle*/
+ float eps;
+ float a;
+ float b;
+ float h_alpha;
+ float h_beta;
+ float d_obs;
+ float nav_type; /* convergence approach -1, parallel approach 2*/
+ float integrator; /* single/ double integrator*/
+ float dt; /* sample time*/
+ float int_max; /* maximum integral*/
}form_param_t;
/* formation configuraiton*/
@@ -98,6 +99,17 @@ typedef struct formation_config
+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;
diff --git a/real_robot/coordination_formation_control_pkg/include/swamControllerAlg.h b/real_robot/coordination_formation_control_pkg/include/swamControllerAlg.h
index 2526e7b664412afff33d3fada39cf7d3ba2f37e3..c51b84511575b71650f25188030f55eb3ec0f81f 100644
--- a/real_robot/coordination_formation_control_pkg/include/swamControllerAlg.h
+++ b/real_robot/coordination_formation_control_pkg/include/swamControllerAlg.h
@@ -3,18 +3,6 @@
#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:
@@ -29,7 +17,7 @@ public:
/* destructor*/
~swamControllerAlg();
/* includes the main controller*/
- input_vector_t controller(Eigen::MatrixXf *q, Eigen::MatrixXf *p, waypoint_t ref, obstacle_t* obs);
+ input_vector_t controller(Eigen::MatrixXf q, Eigen::MatrixXf p, waypoint_t ref, obstacle_t* obs);
protected:
/* configuration formation information like interagent distances*/
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 141806d2a21feccb25b2254adbc07aa5692c0f83..bc882f0afe3ec1673506e2bffbc756424ccd555a 100644
--- a/real_robot/coordination_formation_control_pkg/launch/mission_controller.launch
+++ b/real_robot/coordination_formation_control_pkg/launch/mission_controller.launch
@@ -2,7 +2,7 @@
<launch>
<rosparam command="load" file="$(find coordination_formation_control_pkg)/config/missionConfig.yaml" />
-
+ <rosparam command="load" file="$(find coordination_formation_control_pkg)/config/formationConfig.yaml" />
<!-- Initialize all the drones first -->
<group ns="uav_1">
diff --git a/real_robot/coordination_formation_control_pkg/msg/waypoint.msg b/real_robot/coordination_formation_control_pkg/msg/waypoint.msg
index 586c455800566d62666cf5b58eb075df7b9f9363..5e1c0fd93cb09e33b0e6dbb05df08874d85d7586 100644
--- a/real_robot/coordination_formation_control_pkg/msg/waypoint.msg
+++ b/real_robot/coordination_formation_control_pkg/msg/waypoint.msg
@@ -1,3 +1,7 @@
-float32[3] pos # x, y and orientation
+float32[2] pos # x, y and orientation
float32[2] vel #vx vy
-float32[] cluster # x1 y1 x2 y2 and so on
+float32[] cluster_pos # x1 y1 x2 y2 and so on
+float32[2] cluster_vel # x1 y1 x2 y2 and so on
+float32[] obstacle # x1 y1 r1 x2 y2 r2
+float32 orientation # deg
+bool stop # when we reach waypoint stop the robot movement
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 879445da203899d6155f4f710ae2f32924a696a5..2192280c94103790d9bd9df917981b2a7974095c 100644
--- a/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
@@ -5,29 +5,39 @@
#include "shared_definitions.h"
#include "swamControllerAlg.h"
#include "coordination_formation_control_pkg/coordination.h"
+#include "coordination_formation_control_pkg/waypoint.h"
+
#include "crazyswarm/Hover.h"
#include "crazyswarm/Land.h"
#include "std_msgs/Empty.h"
#include <unistd.h>
+#include "integration.h"
/* global variables*/
coordination_formation_control_pkg::coordination *coord_config;
+coordination_formation_control_pkg::waypoint *next_waypoint;
+
+bool waypoint_received = false;
-void configurationCallback(const coordination_formation_control_pkg::coordination::ConstPtr& msg );
-void configurationCallback(const coordination_formation_control_pkg::coordination msg);
+void configurationCallback(const coordination_formation_control_pkg::coordination::ConstPtr& msg );
+void waypointCallback(const coordination_formation_control_pkg::waypoint::ConstPtr& msg);
int main(int argc, char **argv) {
ros::init(argc, argv, "~");
int robotid = std::atoi(argv[1]);
ros::NodeHandle nh;
- std::cout << "Here" << std::endl;
ROS_INFO("Drone %d turned ON", robotid);
int n_drones;
- int hz_freq;
+ int hz_freq, n_obs;
+ bool use_obstacle,use_orientation;
nh.getParam("/n_drones", n_drones);
nh.getParam("/hz_freq", hz_freq);
+ nh.getParam("/include_obstacle", use_obstacle);
+ nh.getParam("/include_orientation", use_orientation);
+ nh.getParam("/n_obs", n_obs);
+
float dt = 1/(float)hz_freq;
/* intialization*/
@@ -35,6 +45,8 @@ int main(int argc, char **argv) {
coord_config->stop_demo = false;
coord_config->init_formation = false;
coord_config->initialize_robot = false;
+ /* current waypoint*/
+ next_waypoint = new coordination_formation_control_pkg::waypoint();
/* current command*/
crazyswarm::Hover current_command;
current_command.vx = 0;
@@ -42,12 +54,71 @@ int main(int argc, char **argv) {
current_command.yawrate = 0;
current_command.zDistance = 0;
+ /* current state*/
+ /* current implementaiton is in 2D*/
+ Eigen::MatrixXf uav_state_q(2,n_drones);
+ Eigen::MatrixXf uav_state_p(2,n_drones);
+ obstacle_t *current_obs = new obstacle_t[n_obs]();
+ waypoint_t current_waypoint;
+ integration acc_int(0.0,0.0, dt);
+
+
+ /* robot indentification*/
+ robot_ind_t robot_ind;
+
+ robot_ind.id = robotid;
+ robot_ind.robot_name = "crazyflie_" + std::to_string(robotid);
+ robot_ind.t = CRAZYFLIE;
+ robot_ind.safe_ditance = 0.15; // to prevent collision
+ robot_ind.robot_size = 0.15; // drone radius
+ /* make drone 1 the leader, this is for orientaiton*/
+ if (robotid == 1)
+ {
+ robot_ind.leader = true;
+ }
+ else
+ {
+ robot_ind.leader = false;
+ }
+
+ /* initialization of the formation configuration */
+ 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);
+
+ formation_cfg.param.dt = 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);
+
+ /* Initialize Swam controller*/
+ swamControllerAlg swam_controller(robot_ind, formation_cfg);
+
+
/*subcribers*/
ros::Subscriber configuration_sub = nh.subscribe("/coordination_config", 1,configurationCallback);
std::stringstream ss_t;
ss_t << "/cf" << std::to_string(robotid) << "/land";
+ ros::Subscriber waypoint_sub = nh.subscribe("/goal_waypoint", 1,waypointCallback);
ros::ServiceClient land_drone_client = nh.serviceClient <crazyswarm::Land>(ss_t.str());
@@ -62,7 +133,6 @@ int main(int argc, char **argv) {
/* TODO publisher to publish results and rosbag it*/
- /* intialize controller*/
/*create a list of drones to subscribe to*/
@@ -79,7 +149,7 @@ int main(int argc, char **argv) {
uavs.push_back(uav);
if (sucess_initialization == false)
{
- ROS_ERROR("Initialization: %d produced an error in mission controller", uav_def.id);
+ ROS_ERROR("Initialization: Agv %d produced an error in mission controller", uav_def.id);
EXIT_FAILURE;
}
}
@@ -107,14 +177,74 @@ int main(int argc, char **argv) {
else if(coord_config->initialize_robot && coord_config->init_formation && coord_config->stop_demo == false)
{
/* start formation controll*/
+ if (waypoint_received)
+ {
- /*get current state of all the robot*/
-
+ /*get current state of all the robot*/
+ for(int i = 1; i <=n_drones; i++)
+ {
+ robot_state_t current_state;
+ 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);
+ uav_state_p(0, i-1) = current_state.p(0);
+ uav_state_p(1, i-1) = current_state.p(1);
+
+ }
+ /* update reference point*/
+ current_waypoint.q(0) = next_waypoint->pos[0] ; /*position 2 position 1 orientation yaw*/
+ current_waypoint.q(1) = next_waypoint->pos[1] ; /*position 2 position 1 orientation yaw*/
+ current_waypoint.p(0) = next_waypoint->vel[0] ; /*position 2 position 1 orientation yaw*/
+ current_waypoint.p(1) = next_waypoint->pos[1] ; /*position 2 position 1 orientation yaw*/
+
+ /* use orienation: update orientation*/
+ if (use_orientation)
+ {
+ current_waypoint.orientation = next_waypoint->orientation;
+ }
+ else
+ {
+ current_waypoint.orientation = NOORIENTATION;
+
+ }
+ /* use obstacle update obstacle*/
+ if(n_obs != 0)
+ {
+ int indx = 0;
+ for(int n = 0; n < n_obs; n++)
+ {
+
+ current_obs[n].q(0) = next_waypoint->obstacle[indx];
+ current_obs[n].q(1) = next_waypoint->obstacle[indx+1];
+ current_obs[n].radius = next_waypoint->obstacle[indx+2];
+ indx = indx+3;
+ }
+ }
+ else
+ {
+ current_obs = NULL;
+ }
+
+ /* call controller*/
+ input_vector_t input = swam_controller.controller(uav_state_q, uav_state_p,current_waypoint,current_obs);
+ /* integrate acceleration*/
+ Eigen::Vector2d vel_command = acc_int.integrate(input(0,0), input(1,0));
+ /* reached a certain waypoint and we want to stop sending command*/
+ if(next_waypoint->stop)
+ {
+ current_command.vx = 0;
+ current_command.vy = 0;
+ }
+ else
+ {
+ current_command.vx = vel_command(0);
+ current_command.vy = vel_command(1);
+ }
- /* call controller*/
- /* modify command to robot*/
+ }
+ vel_cmd_publisher.publish(current_command);
}
if(coord_config->stop_demo)
@@ -170,3 +300,9 @@ void configurationCallback(const coordination_formation_control_pkg::coordinatio
// std::cout <<"Here in callback" << std::endl;
*coord_config = *msg;
}
+
+void waypointCallback(const coordination_formation_control_pkg::waypoint::ConstPtr& msg)
+{
+ waypoint_received = false;
+ *next_waypoint = *msg;
+}
diff --git a/real_robot/coordination_formation_control_pkg/src/omniwheel_node.cpp b/real_robot/coordination_formation_control_pkg/src/omniwheel_node.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9c707da498226cbe5fece08290dcad0b20a49d37
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/src/omniwheel_node.cpp
@@ -0,0 +1,242 @@
+#include "ros/ros.h"
+#include "geometry_msgs/Twist.h"
+#include "coordination_formation_control_pkg/coordination.h"
+#include "coordination_formation_control_pkg/waypoint.h"
+#include "robot_state.h"
+#include "shared_definitions.h"
+#include "swamControllerAlg.h"
+
+/* global variables*/
+coordination_formation_control_pkg::coordination *coord_config;
+coordination_formation_control_pkg::waypoint *next_waypoint;
+
+bool waypoint_received = false;
+
+void configurationCallback(const coordination_formation_control_pkg::coordination::ConstPtr& msg );
+void waypointCallback(const coordination_formation_control_pkg::waypoint::ConstPtr& msg);
+
+
+
+int main(int argc, char **argv) {
+ ros::init(argc, argv, "~");
+ int robotid = std::atoi(argv[1]);
+ ros::NodeHandle nh;
+ ROS_INFO("Ugv %d turned ON", robotid);
+ int hz_freq, n_ugvs, n_obs;
+ bool use_obstacle;
+ nh.getParam("/n_ugvs", n_ugvs);
+ nh.getParam("/hz_freq", hz_freq);
+ nh.getParam("/include_obstacle", use_obstacle);
+ nh.getParam("/n_obs", n_obs);
+
+ float dt = 1/(float)hz_freq;
+
+ /* intialization*/
+ coord_config = new coordination_formation_control_pkg::coordination();
+ coord_config->stop_demo = false;
+ coord_config->init_formation = false;
+ coord_config->initialize_robot = false;
+ /* current waypoint*/
+ next_waypoint = new coordination_formation_control_pkg::waypoint();
+
+ /*current command*/
+ geometry_msgs::Twist current_command;
+ current_command.linear.x = 0;
+ current_command.linear.y = 0;
+ current_command.angular.z = 0;
+
+ /*current state include cluster and ugv pose*/
+ Eigen::MatrixXf current_state_q(2,2);
+ Eigen::MatrixXf current_state_p(2,2);
+ /* obstacle = number of obstacle + the other ugvs*/
+ n_obs = (n_obs) + (n_ugvs-1);
+ obstacle_t *current_obs = new obstacle_t[n_obs]();
+ waypoint_t current_waypoint;
+
+ /* robot indentification*/
+ robot_ind_t robot_ind;
+
+ robot_ind.id = robotid;
+ robot_ind.robot_name = "omniwheel" + std::to_string(robotid);
+ robot_ind.t = OMNIWHEELS;
+ robot_ind.safe_ditance = 0.15; // to prevent collision
+ robot_ind.robot_size = 0.32; // drone radius
+ robot_ind.leader = false;
+ /* initialization of the formation configuration */
+ formation_config_t formation_cfg;
+ nh.getParam("/ugv_c1_alpha",formation_cfg.gain.c1_alpha);
+ nh.getParam("/ugv_c2_alpha",formation_cfg.gain.c2_alpha);
+ nh.getParam("/ugv_c1_beta",formation_cfg.gain.c1_beta);
+ nh.getParam("/ugv_c2_beta",formation_cfg.gain.c2_beta);
+ nh.getParam("/ugv_c1_gamma",formation_cfg.gain.c1_gamma);
+ nh.getParam("/ugv_c2_gamma",formation_cfg.gain.c2_gamma);
+ nh.getParam("/ugv_c1_theta",formation_cfg.gain.c1_theta);
+ nh.getParam("/ugv_c1_delta",formation_cfg.gain.c1_delta);
+
+ formation_cfg.param.dt = dt;
+
+ nh.getParam("/ugv_d", formation_cfg.param.d);
+ nh.getParam("/ugv_formation_t", formation_cfg.param.formation_t);
+ nh.getParam("/ugv_k", formation_cfg.param.k);
+ nh.getParam("/ugv_ratio", formation_cfg.param.ratio);
+ nh.getParam("/ugv_eps", formation_cfg.param.eps);
+ nh.getParam("/ugv_a", formation_cfg.param.a);
+ nh.getParam("/ugv_b", formation_cfg.param.b);
+ nh.getParam("/ugv_h_alpha", formation_cfg.param.h_alpha);
+ nh.getParam("/ugv_h_beta", formation_cfg.param.h_beta);
+ nh.getParam("/ugv_d_obs", formation_cfg.param.d_obs);
+ nh.getParam("/ugv_nav_type", formation_cfg.param.nav_type);
+ nh.getParam("/ugv_integrator", formation_cfg.param.integrator);
+ nh.getParam("/ugv_int_max", formation_cfg.param.int_max);
+
+ /* Initialize Swam controller*/
+ swamControllerAlg swam_controller(robot_ind, formation_cfg);
+ /* Subscribers*/
+ ros::Subscriber configuration_sub = nh.subscribe("/coordination_config", 1,configurationCallback);
+ ros::Subscriber waypoint_sub = nh.subscribe("/goal_waypoint", 1,waypointCallback);
+
+ /* publishers*/
+ std::stringstream ss;
+ ss << "/omniwheel" << std::to_string(robotid) << "/vel_cmd";
+ ros::Publisher vel_cmd_publisher = nh.advertise<geometry_msgs::Twist>(ss.str(),1);
+
+ std::vector<robotState *> ugvs;
+ robot_ind_t ugv_def;
+ bool sucess_initialization;
+ for (int i = 1; i <= n_ugvs; i++)
+ {
+ ugv_def.robot_name = "omniwheel";
+ ugv_def.t = OMNIWHEELS;
+ ugv_def.id = i;
+ sucess_initialization = false;
+ auto * ugv = new robotState(ugv_def, nh, sucess_initialization);
+ ugvs.push_back(ugv);
+ if (sucess_initialization == false)
+ {
+ ROS_ERROR("Initialization: ugv %d produced an error in mission controller", ugv_def.id);
+ EXIT_FAILURE;
+ }
+ }
+
+
+
+ ros::Rate loop_rate(hz_freq);
+ /* main loop*/
+ /* TODO improve state machine*/
+ while(ros::ok())
+ {
+ if(coord_config->initialize_robot && coord_config->init_formation ==false && coord_config->stop_demo == false)
+ {
+ // send the robot to hover at zero velocity in plane
+ // std::cout << "Here" << std::endl;
+ current_command.linear.x = 0;
+ current_command.linear.y = 0;
+ current_command.angular.z = 0;
+ vel_cmd_publisher.publish(current_command);
+ }
+ else if(coord_config->initialize_robot && coord_config->init_formation && coord_config->stop_demo == false)
+ {
+ /* start formation controll*/
+ if (waypoint_received)
+ {
+
+ /*get current state of all the robot*/
+ for(int i = 1; i <=n_ugvs; i++)
+ {
+ robot_state_t current_state;
+ current_state = ugvs[i-1]->get_state();
+ current_state_q(0, i-1) =current_state.q(0);
+ current_state_q(1, i-1) =current_state.q(1);
+ current_state_p(0, i-1) = current_state.p(0);
+ current_state_p(1, i-1) = current_state.p(1);
+
+ }
+ /* update reference point*/
+ current_waypoint.q(0) = next_waypoint->pos[0] ; /*position 2 position 1 orientation yaw*/
+ current_waypoint.q(1) = next_waypoint->pos[1] ; /*position 2 position 1 orientation yaw*/
+ current_waypoint.p(0) = next_waypoint->vel[0] ; /*position 2 position 1 orientation yaw*/
+ current_waypoint.p(1) = next_waypoint->pos[1] ; /*position 2 position 1 orientation yaw*/
+
+ // /* use orienation: update orientation*/
+ // if (use_orientation)
+ // {
+ // current_waypoint.orientation = next_waypoint->orientation;
+ // }
+ // else
+ // {
+ // current_waypoint.orientation = NOORIENTATION;
+ //
+ // }
+ // /* use obstacle update obstacle*/
+ // if(n_obs != 0)
+ // {
+ // int indx = 0;
+ // for(int n = 0; n < n_obs; n++)
+ // {
+ //
+ // current_obs[n].q(0) = next_waypoint->obstacle[indx];
+ // current_obs[n].q(1) = next_waypoint->obstacle[indx+1];
+ // current_obs[n].radius = next_waypoint->obstacle[indx+2];
+ // indx = indx+3;
+ // }
+ // }
+ // else
+ // {
+ // current_obs = NULL;
+ // }
+ //
+ // /* call controller*/
+ // input_vector_t input = swam_controller.controller(uav_state_q, uav_state_p,current_waypoint,current_obs);
+ // /* integrate acceleration*/
+ // Eigen::Vector2d vel_command = acc_int.integrate(input(0,0), input(1,0));
+ // /* reached a certain waypoint and we want to stop sending command*/
+ // if(next_waypoint->stop)
+ // {
+ // current_command.vx = 0;
+ // current_command.vy = 0;
+ // }
+ // else
+ // {
+ // current_command.vx = vel_command(0);
+ // current_command.vy = vel_command(1);
+ // }
+ //
+ //
+ //
+ }
+ vel_cmd_publisher.publish(current_command);
+
+ }
+ if(coord_config->stop_demo)
+ {
+ current_command.linear.x = 0;
+ current_command.linear.y = 0;
+ current_command.angular.z = 0;
+ vel_cmd_publisher.publish(current_command);
+ sleep(3);
+ break;
+
+
+ }
+
+ ros::spinOnce();
+ loop_rate.sleep();
+ }
+
+
+ return 0;
+}
+
+
+/* callbacks*/
+void configurationCallback(const coordination_formation_control_pkg::coordination::ConstPtr& msg )
+{
+ // std::cout <<"Here in callback" << std::endl;
+ *coord_config = *msg;
+}
+
+void waypointCallback(const coordination_formation_control_pkg::waypoint::ConstPtr& msg)
+{
+ waypoint_received = false;
+ *next_waypoint = *msg;
+}
diff --git a/real_robot/coordination_formation_control_pkg/src/swamControllerAlg.cpp b/real_robot/coordination_formation_control_pkg/src/swamControllerAlg.cpp
index fe76f2797f63bf7130a46f05d944e0cd4c8ed95a..3c82f9d4e28e9e079d8fe191b79c4cd04a001751 100644
--- a/real_robot/coordination_formation_control_pkg/src/swamControllerAlg.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/swamControllerAlg.cpp
@@ -1 +1,22 @@
#include "swamControllerAlg.h"
+
+
+
+
+swamControllerAlg::swamControllerAlg(robot_ind_t indentification, formation_config_t formation_config)
+{
+
+}
+
+
+swamControllerAlg::~swamControllerAlg()
+{
+
+}
+
+
+/* main controller implementation*/
+input_vector_t swamControllerAlg::controller(Eigen::MatrixXf q, Eigen::MatrixXf p, waypoint_t ref, obstacle_t* obs)
+{
+
+}
diff --git a/simulation/controller/SwamControllerAlg.m b/simulation/controller/SwamControllerAlg.m
index 52dae764124fa6bb4a9b8d21ddc32234a9d98a0b..167694d7606d46da9ef52d918de705fee8e252c8 100644
--- a/simulation/controller/SwamControllerAlg.m
+++ b/simulation/controller/SwamControllerAlg.m
@@ -336,7 +336,7 @@ q_obs_vector = [0;0];
thisSwamControllerAlg.integral_error = thisSwamControllerAlg.integral_error + error_grad_term*thisSwamControllerAlg.formation_param.dt;
% saturate error to prevent anti-winup
% thisSwamControllerAlg.integral_error = sat_func( thisSwamControllerAlg.integral_error, thisSwamControllerAlg.formation_param.int_max);
- u_integral = thisSwamControllerAlg.gain_param.c1_delta * thisSwamControllerAlg.integral_error + deriv_term*thisSwamControllerAlg.gain_param.c2_delta;
+ u_integral = thisSwamControllerAlg.gain_param.c1_delta * thisSwamControllerAlg.integral_error; % + deriv_term*thisSwamControllerAlg.gain_param.c2_delta;
% u_derivative = ((error_grad_term-previous_error)/ thisSwamControllerAlg.formation_param.dt)*thisSwamControllerAlg.gain_param.c3_delta;
% thisSwamControllerAlg.previous_error = error_grad_term;
% ce = error_grad_term;