From 35ebb06142aa4ee567d8c5f021e1372083eb7253 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Sat, 25 May 2024 18:55:15 -0600 Subject: [PATCH] AP_DDS: Use common quaternion initialization function Signed-off-by: Ryan Friedman --- libraries/AP_DDS/AP_DDS_Client.cpp | 30 +++++++++++++----------------- 1 file changed, 13 insertions(+), 17 deletions(-) diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index 86be5ab2d0809..b95ddf052d311 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -77,6 +77,14 @@ const AP_Param::GroupInfo AP_DDS_Client::var_info[] { AP_GROUPEND }; +static void initialize(geometry_msgs_msg_Quaternion& q) +{ + q.x = 0.0; + q.y = 0.0; + q.z = 0.0; + q.w = 1.0; +} + AP_DDS_Client::~AP_DDS_Client() { // close transport @@ -226,11 +234,8 @@ void AP_DDS_Client::populate_static_transforms(tf2_msgs_msg_TFMessage& msg) msg.transforms[i].transform.translation.y = -1 * offset[1]; msg.transforms[i].transform.translation.z = -1 * offset[2]; - // Ensure rotation is normalised - msg.transforms[i].transform.rotation.x = 0.0; - msg.transforms[i].transform.rotation.y = 0.0; - msg.transforms[i].transform.rotation.z = 0.0; - msg.transforms[i].transform.rotation.w = 1.0; + // Ensure rotation is initialized. + initialize(msg.transforms[i].transform.rotation); msg.transforms_size++; } @@ -344,10 +349,7 @@ void AP_DDS_Client::update_topic(geometry_msgs_msg_PoseStamped& msg) msg.pose.orientation.y = orientation[2]; msg.pose.orientation.z = orientation[3]; } else { - msg.pose.orientation.x = 0.0; - msg.pose.orientation.y = 0.0; - msg.pose.orientation.z = 0.0; - msg.pose.orientation.w = 1.0; + initialize(msg.pose.orientation); } } @@ -428,10 +430,7 @@ void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPoseStamped& msg) msg.pose.orientation.y = orientation[2]; msg.pose.orientation.z = orientation[3]; } else { - msg.pose.orientation.x = 0.0; - msg.pose.orientation.y = 0.0; - msg.pose.orientation.z = 0.0; - msg.pose.orientation.w = 1.0; + initialize(msg.pose.orientation); } } @@ -452,10 +451,7 @@ void AP_DDS_Client::update_topic(sensor_msgs_msg_Imu& msg) msg.orientation.z = orientation[2]; msg.orientation.w = orientation[3]; } else { - msg.orientation.x = 0.0; - msg.orientation.y = 0.0; - msg.orientation.z = 0.0; - msg.orientation.w = 1.0; + initialize(msg.orientation); } msg.orientation_covariance[0] = -1;