From 67272bd700c19e256d1d1451f94bba669feb6b66 Mon Sep 17 00:00:00 2001
From: Lucas Walter <wsacul@gmail.com>
Date: Mon, 16 Sep 2024 10:01:10 -0700
Subject: [PATCH] convert quaternion to roll-pitch-yaw in tf_echo

---
 tf_roslibrust/src/bin/echo.rs | 10 +++++++++-
 1 file changed, 9 insertions(+), 1 deletion(-)

diff --git a/tf_roslibrust/src/bin/echo.rs b/tf_roslibrust/src/bin/echo.rs
index ea814e63..75460655 100644
--- a/tf_roslibrust/src/bin/echo.rs
+++ b/tf_roslibrust/src/bin/echo.rs
@@ -80,9 +80,17 @@ async fn main() -> Result<(), anyhow::Error> {
                 println!("- Translation: [{:.3} {:.3} {:.3}]", xyz.x, xyz.y, xyz.z);
                 let quat = tf.transform.rotation;
                 println!(
-                    "- Rotation: [{:.3} {:.3} {:.3} {:.3}]",
+                    "- Rotation: in Quaternion [{:.6} {:.6} {:.6} {:.6}]",
                     quat.x, quat.y, quat.z, quat.w
                 );
+
+                // nalgebra documentation ought to say what components are in what place in this
+                // constructor
+                let unit_quat = nalgebra::UnitQuaternion::from_quaternion(
+                    nalgebra::Quaternion::new(quat.w, quat.x, quat.y, quat.z),
+                );
+                let (roll, pitch, yaw) = unit_quat.euler_angles();
+                println!("            in RPY (radian) [{roll:.6} {pitch:.6} {yaw:.6}]",);
             }
             Err(err) => {
                 println!("{t1:?} {err:?}");