AP_DDS: Use common quaternion initialization function

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

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;