From 20f976d8818f72f714e97fba9b7c7899552723ad Mon Sep 17 00:00:00 2001
From: Stevedan Ogochukwu Omodolor <stevedan.o.omodolor@gmail.com>
Date: Thu, 19 May 2022 15:03:14 +0200
Subject: [PATCH] not working version test_controller
---
.../CMakeLists.txt | 4 ++++
.../src/drone_node.cpp | 22 +++++++++----------
.../src/test_controller.cpp | 10 ++++++++-
3 files changed, 24 insertions(+), 12 deletions(-)
diff --git a/real_robot/coordination_formation_control_pkg/CMakeLists.txt b/real_robot/coordination_formation_control_pkg/CMakeLists.txt
index 679bdb8..3cb883a 100644
--- a/real_robot/coordination_formation_control_pkg/CMakeLists.txt
+++ b/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}
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 a4fd071..a426aaf 100644
--- a/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
+++ b/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;
diff --git a/real_robot/coordination_formation_control_pkg/src/test_controller.cpp b/real_robot/coordination_formation_control_pkg/src/test_controller.cpp
index 3944eb0..327bb8e 100644
--- a/real_robot/coordination_formation_control_pkg/src/test_controller.cpp
+++ b/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;
--
GitLab