Convert quaternion to actual yaw angle.

CMakeLists.txt

@@ -13,6 +13,8 @@ find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(std_srvs REQUIRED)
 find_package(sensor_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
 find_package(robotem_rovne REQUIRED)
 find_package(mp-units REQUIRED)
 #######################################################################################
@@ -47,7 +49,7 @@ target_link_libraries(${ROBOTEM_ROVNE_GUI_TARGET}
         ${GTKMM_LIBRARIES}
 )
 #######################################################################################
-ament_target_dependencies(${ROBOTEM_ROVNE_GUI_TARGET} rclcpp std_msgs std_srvs sensor_msgs)
+ament_target_dependencies(${ROBOTEM_ROVNE_GUI_TARGET} rclcpp std_msgs std_srvs sensor_msgs tf2 tf2_geometry_msgs)
 #######################################################################################
 install(TARGETS ${ROBOTEM_ROVNE_GUI_TARGET} DESTINATION lib/${PROJECT_NAME})
 install(DIRECTORY glade DESTINATION share/${PROJECT_NAME})

package.xml

@@ -13,6 +13,7 @@
   <depend>std_msgs</depend>
   <depend>std_srvs</depend>
   <depend>sensor_msgs</depend>
+  <depend>tf2</depend>
   <depend>robotem_rovne</depend>
 
   <test_depend>ament_lint_auto</test_depend>

src/Node.cpp

@@ -10,6 +10,9 @@
 
 #include <robotem_rovne_gui/Node.h>
 
+#include <tf2/utils.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
 /**************************************************************************************
  * NAMESPACE
  **************************************************************************************/
@@ -183,8 +186,11 @@ void Node::init_imu_sub()
     _imu_qos_profile,
     [this](sensor_msgs::msg::Imu::SharedPtr const msg)
     {
+      double const heading_actual = tf2::getYaw(msg->orientation);
+
       RCLCPP_INFO(get_logger(),
-                  "IMU Pose (x,y,z,w): %0.2f %0.2f %0.2f %0.2f",
+                  "IMU Pose (theta) | (x,y,z,w): %0.2f | %0.2f %0.2f %0.2f %0.2f",
+                  heading_actual,
                   msg->orientation.x,
                   msg->orientation.y,
                   msg->orientation.z,
@@ -195,7 +201,7 @@ void Node::init_imu_sub()
       assert(label_heading_actual);
 
       char heading_actual_buf[32] = {0};
-      snprintf(heading_actual_buf, sizeof(heading_actual_buf), "%0.2f", msg->orientation.z);
+      snprintf(heading_actual_buf, sizeof(heading_actual_buf), "%0.2f", heading_actual);
       label_heading_actual->set_label(std::string(heading_actual_buf));
     },
     _imu_sub_options);