From e13b7d936924d1e28dbcbd2715a804ef31c023f3 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Sun, 4 Feb 2024 17:50:22 -0700 Subject: [PATCH] AP_DDS: Fix NaN handling Signed-off-by: Ryan Friedman --- 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 eb848f068a8dc2..075ff8b33703f7 100644 --- a/libraries/AP_DDS/AP_DDS_ExternalControl.cpp +++ b/libraries/AP_DDS/AP_DDS_ExternalControl.cpp @@ -55,6 +55,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. @@ -64,7 +66,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); }