From 4995bdd717774b2bf43598a1c67248dfe7e02480 Mon Sep 17 00:00:00 2001
From: Stevedan Ogochukwu Omodolor <stevedan.o.omodolor@gmail.com>
Date: Tue, 14 Jun 2022 08:09:31 +0200
Subject: [PATCH] activate control robot
---
.../coordination_formation_control_pkg/src/drone_node.cpp | 8 +-------
1 file changed, 1 insertion(+), 7 deletions(-)
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 74367c1..bb6b16a 100644
--- a/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
@@ -289,7 +289,7 @@ int main(int argc, char **argv) {
current_command.vy = 0;
current_command.yawrate = 0;
current_command.zDistance = 0.5;
- // vel_cmd_publisher.publish(current_command);
+ vel_cmd_publisher.publish(current_command);
}
@@ -370,17 +370,11 @@ int main(int argc, char **argv) {
/* get latest yaw*/
robot_state_t current_state_;
current_state_ = uavs[robotid-1]->get_state();
-<<<<<<< HEAD
- float yaw = current_state_.q(3)*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) ;
-=======
float yaw_ = current_state_.q(3)*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) ;
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_) ;
->>>>>>> second_integrator
// current_command.vx = vxl;
// current_command.vy = vyl;
--
GitLab