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 74367c10c8209da1993feb5f0a17173d8cdc15c1..bb6b16a2754a97e98b0eca7eb2a31a3c0bb4faac 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;