diff --git a/real_robot/coordination_formation_control_pkg/msg/waypoint.msg b/real_robot/coordination_formation_control_pkg/msg/waypoint.msg
index 5e1c0fd93cb09e33b0e6dbb05df08874d85d7586..4165b5d8630d28720e561675414478521af41206 100644
--- a/real_robot/coordination_formation_control_pkg/msg/waypoint.msg
+++ b/real_robot/coordination_formation_control_pkg/msg/waypoint.msg
@@ -1,4 +1,4 @@
-float32[2] pos # x, y and orientation
+float32[2] pos # x, y and orientation m m deg
float32[2] vel #vx vy
float32[] cluster_pos # x1 y1 x2 y2 and so on
float32[2] cluster_vel # x1 y1 x2 y2 and so on
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 2192280c94103790d9bd9df917981b2a7974095c..e6b5f1c24c5ee389f3f8841c87a0a9ba2553b5ee 100644
--- a/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
@@ -12,6 +12,7 @@
#include "std_msgs/Empty.h"
#include <unistd.h>
#include "integration.h"
+#include <cmath>
/* global variables*/
coordination_formation_control_pkg::coordination *coord_config;
@@ -237,8 +238,16 @@ int main(int argc, char **argv) {
}
else
{
- current_command.vx = vel_command(0);
- current_command.vy = vel_command(1);
+ /* transform from global velocity to local velocity*/
+ /* get latest yaw*/
+ robot_state_t current_state_;
+ current_state_ = uavs[robotid-1]->get_state();
+ float yaw = current_state_.p(2)*M_PI/180.0; // TODO confirm that indeed it is in degrees
+ float vxl = vel_command(0)*std::cos(yaw) + vel_command(1)*std::sin(yaw) ;
+ float vyl = - vel_command(0)*std::sin(yaw) + vel_command(1)*std::cos(yaw) ;
+
+ current_command.vx = vxl;
+ current_command.vy = vyl;
}
diff --git a/real_robot/coordination_formation_control_pkg/src/omniwheel_node.cpp b/real_robot/coordination_formation_control_pkg/src/omniwheel_node.cpp
index 9c707da498226cbe5fece08290dcad0b20a49d37..d3ee72a128bbad530bf3e15607c887afb884b59c 100644
--- a/real_robot/coordination_formation_control_pkg/src/omniwheel_node.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/omniwheel_node.cpp
@@ -48,9 +48,13 @@ int main(int argc, char **argv) {
/*current state include cluster and ugv pose*/
Eigen::MatrixXf current_state_q(2,2);
Eigen::MatrixXf current_state_p(2,2);
+
+ /* vector of ugvs*/
+ Eigen::MatrixXf ugv_state_q(2,n_ugvs);
+ Eigen::MatrixXf ugv_state_p(2,n_ugvs);
/* obstacle = number of obstacle + the other ugvs*/
- n_obs = (n_obs) + (n_ugvs-1);
- obstacle_t *current_obs = new obstacle_t[n_obs]();
+ int n_obs_ugv = (n_obs) + (n_ugvs-1);
+ obstacle_t *current_obs = new obstacle_t[n_obs_ugv]();
waypoint_t current_waypoint;
/* robot indentification*/
@@ -140,69 +144,91 @@ int main(int argc, char **argv) {
if (waypoint_received)
{
- /*get current state of all the robot*/
+ /*get current state of all the ugv*/
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);
+ ugv_state_q(0, i-1) =current_state.q(0);
+ ugv_state_q(1, i-1) =current_state.q(1);
+ ugv_state_p(0, i-1) = current_state.p(0);
+ ugv_state_p(1, i-1) = current_state.p(1);
}
+
+ /* create a state vector of the cluster and the current agv*/
+ current_state_q(0,0) = next_waypoint->cluster_pos[robotid*2 -2];
+ current_state_q(1,0) = next_waypoint->cluster_pos[robotid*2 -1];
+ current_state_p(0,0) = next_waypoint->cluster_vel[0];
+ current_state_p(1,0) = next_waypoint->cluster_vel[1];
+ current_state_q.col(1) = ugv_state_q.col(robotid-1);
+ current_state_p.col(1) = ugv_state_p.col(robotid-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);
- // }
- //
- //
+ current_waypoint.q(0) =next_waypoint->cluster_pos[robotid*2 -2];
+ current_waypoint.q(1) = next_waypoint->cluster_pos[robotid*2 -1];
+ current_waypoint.p(0) = next_waypoint->cluster_vel[0];
+ current_waypoint.p(1) = next_waypoint->cluster_vel[1];
+
+ /* mantain a 180 degress*/
+ current_waypoint.orientation = 180;
+ if (n_obs_ugv > 0)
+ {
+ /* add 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;
+ }
+ }
+ /* add other ugv as obstacle*/
+ if((n_ugvs-1) !=0)
+ {
+ int ugv_ind = 0;
+ for(int n = 0; n < (n_ugvs); n++)
+ {
+ if ((n+1) != robotid) // exclude current ugv as it own obstacle
+ {
+ current_obs[n_obs+ugv_ind].q(0) =ugv_state_q(0, n) ;
+ current_obs[n_obs+ugv_ind].q(1) =ugv_state_q(1, n) ;
+ current_obs[n_obs+ugv_ind].radius = robot_ind.safe_ditance + robot_ind.robot_size;
+ ugv_ind = ugv_ind+1;
+ }
+
+ }
+ }
+
+ }
+ else
+ {
+ current_obs = NULL;
+ }
//
+ /* call controller*/
+ input_vector_t input = swam_controller.controller(current_state_q, current_state_p,current_waypoint,current_obs);
+ /* integrate acceleration*/
+ if(next_waypoint->stop)
+ {
+ current_command.linear.x = 0;
+ current_command.linear.y = 0;
+ current_command.angular.z = 0;
+ }
+ else
+ {
+ current_command.linear.x = input(0,0);
+ current_command.linear.y = input(1,0);
+ current_command.angular.z = 0;
+
+ }
+
+
+
}
vel_cmd_publisher.publish(current_command);