From bdb9f438953171a24bb78f8ad391c4bf96a923c5 Mon Sep 17 00:00:00 2001 From: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> Date: Thu, 24 Oct 2024 17:42:29 +0900 Subject: [PATCH] AP_DDS: Fix NaN handling --- libraries/AP_DDS/AP_DDS_ExternalControl.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/libraries/AP_DDS/AP_DDS_ExternalControl.cpp b/libraries/AP_DDS/AP_DDS_ExternalControl.cpp index e82404adfb2db3..4f71522769153d 100644 --- a/libraries/AP_DDS/AP_DDS_ExternalControl.cpp +++ b/libraries/AP_DDS/AP_DDS_ExternalControl.cpp @@ -57,6 +57,8 @@ bool AP_DDS_External_Control::handle_velocity_control(geometry_msgs_msg_TwistSta if (external_control == nullptr) { return false; } + + // TODO handle stale data from timestamp gracefully. if (strcmp(cmd_vel.header.frame_id, BASE_LINK_FRAME_ID) == 0) { // Convert commands from body frame (x-forward, y-left, z-up) to NED. @@ -66,7 +68,8 @@ bool AP_DDS_External_Control::handle_velocity_control(geometry_msgs_msg_TwistSta float(cmd_vel.twist.linear.y), float(-cmd_vel.twist.linear.z) }; const float yaw_rate = -cmd_vel.twist.angular.z; - if (linear_velocity_base_link == Vector3f(NAN, NAN, NAN)) { + + if (linear_velocity_base_link.is_all_nan()) { return external_control->set_yaw_rate(yaw_rate); }