From f6873d553ef46f0f4095633f521b766fba741d1d Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 15 May 2023 11:28:05 +0200 Subject: [PATCH] add checking for NaN when handling velocity --- src/px4_connection/src/px4_controller.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 6054d8a6..a383c2cf 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -160,7 +160,12 @@ private: { for (int i = 0; i < 3; i++) { - velocity[i] += request->values[i]; + if (request->values[i] != NAN) + { + velocity[i] += request->values[i]; + } else { + velocity[i] = request->values[i]; + } } RCLCPP_INFO(this->get_logger(), "Got new velocity setpoint. %f %f %f", velocity[0], velocity[1], velocity[2]); }