not working version test_controller

real_robot/coordination_formation_control_pkg/CMakeLists.txt

@@ -71,6 +71,10 @@ target_link_libraries(omniwheel_node ${catkin_LIBRARIES} ${Eigen_LIBRARIES}   ${
 add_executable(testbag src/testbag.cpp )
 target_link_libraries(testbag ${catkin_LIBRARIES} ${Eigen_LIBRARIES}   ${OpenCV_LIBS})
 
+
+add_executable(test_controller src/test_controller.cpp )
+target_link_libraries(test_controller ${catkin_LIBRARIES} ${Eigen_LIBRARIES}   ${OpenCV_LIBS})
+
 install(DIRECTORY
 launch
 DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}

real_robot/coordination_formation_control_pkg/src/drone_node.cpp

@@ -285,17 +285,17 @@ int main(int argc, char **argv) {
 
 
       }
-      /* testing velocity command*/
-      robot_state_t current_state_;
-      float vx = 0.05;
-      float vy = 0;
-      current_state_ = uavs[robotid-1]->get_state();
-      float yaw = current_state_.q(3)*M_PI/180.0;  // TODO confirm that indeed it is in degrees
-      float vxl =  vx*std::cos(yaw) +  0*std::sin(yaw) ;
-      float vyl = -vx*std::sin(yaw)  +  0*std::cos(yaw) ;
-
-      current_command.vx = vxl;
-      current_command.vy = vyl;
+      // /* testing velocity command*/
+      // robot_state_t current_state_;
+      // float vx = 0.05;
+      // float vy = 0;
+      // current_state_ = uavs[robotid-1]->get_state();
+      // float yaw = current_state_.q(3)*M_PI/180.0;  // TODO confirm that indeed it is in degrees
+      // float vxl =  vx*std::cos(yaw) +  0*std::sin(yaw) ;
+      // float vyl = -vx*std::sin(yaw)  +  0*std::cos(yaw) ;
+      //
+      // current_command.vx = vxl;
+      // current_command.vy = vyl;
       // std::cout << "vx: normal: " << vx << "vy normal: " << vy << std::endl;
       // std::cout << "***********************************" << std::endl;
       // std::cout << "vy: transformed: " << vxl << "vy transformed: " << vyl << std::endl;

real_robot/coordination_formation_control_pkg/src/test_controller.cpp

@@ -53,7 +53,15 @@ int main(int argc, char **argv)
   current_waypoint ref;
   ref.q << 0,0;
   ref.p << 5,1;
+  ref.orienation = 0;
+  obstacle_t obs[3];
 
+  obs[0].q << 1 -1.15;
+  obs[0].radius = 0.5
+  obs[1].q << 0 0;
+  obs[1].radius = 0;
+  obs[2].q << 0 0;
+  obs[2].radius = 0
 
 
   robot_ind.id = 1;
@@ -68,7 +76,7 @@ int main(int argc, char **argv)
   swamControllerAlg swam_controller(robot_ind, formation_cfg);
 
 
-  // input = swam_controller.controller(uav_state_q, uav_state_p,current_waypoint,current_obs);
+  input = swam_controller.controller(uav_state_q, uav_state_p,ref,current_obs);
   std::cout << input << std::endl;