From 3f2ffd6dc7e3b91d01ccba02dbfa2110d59052f5 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 12:43:02 +0200 Subject: [PATCH] add hover after 20 s --- src/px4_connection/src/px4_controller.cpp | 22 ++++++++++++++++++---- 1 file changed, 18 insertions(+), 4 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index ebfd82f5..3908dcfa 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -58,11 +58,25 @@ private: void send_trajectory_setpoint() { + auto msg = px4_msgs::msg::TrajectorySetpoint(); - msg.velocity[0] = 0; - msg.velocity[1] = 0; - msg.velocity[2] = -15; - msg.yawspeed = 0; + if (start_time_ - this->get_clock().now().seconds() < 20) + { + msg.velocity[0] = 0; + msg.velocity[1] = 0; + msg.velocity[2] = -15; + msg.yawspeed = 0; + } else { + if (!has_swithed) + { + RCLCPP_INFO(this->get_logger(), "switching to 0 vel"); + has_swithed = true; + } + msg.velocity[0] = 0; + msg.velocity[1] = 0; + msg.velocity[2] = 0; + msg.yawspeed = 0; + } // if (setpoint_count < 30) // {