From ec09af4170c2df3bcc4ad53830179335a178f859 Mon Sep 17 00:00:00 2001
From: Alexander Entinger <consulting@lxrobotics.com>
Date: Wed, 17 Jan 2024 09:50:49 +0100
Subject: [PATCH] Publish IMU message with orientation as Quaternion.

---
 CMakeLists.txt |  3 ++-
 launch/imu.py  |  1 +
 main.cpp       | 46 ++++++++++++++++++++++++++++------------------
 package.xml    |  1 +
 4 files changed, 32 insertions(+), 19 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 732ad8e..3ea4772 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -6,6 +6,7 @@ set(PIKA_SPARK_BNO085_TARGET ${PROJECT_NAME}_node)
 ##########################################################################
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
 ##########################################################################
 add_subdirectory(sh2)
 ##########################################################################
@@ -25,7 +26,7 @@ target_link_libraries(${PIKA_SPARK_BNO085_TARGET} pika-spark-sh2)
 ##########################################################################
 target_compile_features(${PIKA_SPARK_BNO085_TARGET} PUBLIC cxx_std_17)
 ##########################################################################
-ament_target_dependencies(${PIKA_SPARK_BNO085_TARGET} rclcpp)
+ament_target_dependencies(${PIKA_SPARK_BNO085_TARGET} rclcpp sensor_msgs)
 ##########################################################################
 install(TARGETS ${PIKA_SPARK_BNO085_TARGET} DESTINATION lib/${PROJECT_NAME})
 install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
diff --git a/launch/imu.py b/launch/imu.py
index 39df745..5f350c8 100644
--- a/launch/imu.py
+++ b/launch/imu.py
@@ -14,6 +14,7 @@ def generate_launch_description():
       output='screen',
       emulate_tty=True,
       parameters=[
+          {'imu_topic': 'imu'},
       ]
     )
   ])
diff --git a/main.cpp b/main.cpp
index 3e514d3..7e0efb7 100644
--- a/main.cpp
+++ b/main.cpp
@@ -20,6 +20,8 @@
 #include <rclcpp/qos.hpp>
 #include <rclcpp/rclcpp.hpp>
 
+#include <sensor_msgs/msg/imu.hpp>
+
 #include "spi.h"
 #include "gpio-sysfs.h"
 
@@ -44,6 +46,19 @@ int main(int argc, char ** argv) try
 
   auto const node = rclcpp::Node::make_shared("pika_spark_bno085_driver_node");
 
+  rclcpp::QoS imu_qos_profile(rclcpp::KeepLast(1), rmw_qos_profile_sensor_data);
+
+  node->declare_parameter("imu_topic", "joy");
+  auto const imu_topic = node->get_parameter("imu_topic").as_string();
+  auto const imu_topic_deadline = std::chrono::milliseconds(100);
+  auto const imu_topic_liveliness_lease_duration = std::chrono::milliseconds(1000);
+
+  imu_qos_profile.deadline(imu_topic_deadline);
+  imu_qos_profile.liveliness(RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC);
+  imu_qos_profile.liveliness_lease_duration(imu_topic_liveliness_lease_duration);
+
+  auto const imu_pub = node->create_publisher<sensor_msgs::msg::Imu>(imu_topic, imu_qos_profile);
+
   auto const gpio_nboot = std::make_shared<SysGPIO>(nBOOT_PIN);
   gpio_nboot->gpio_set_dir(true);
   gpio_nboot->gpio_set_value(1); /* Note: setting it to '0' activates bootloader mode. */
@@ -60,25 +75,20 @@ int main(int argc, char ** argv) try
   gpio_nirq->gpio_set_dir(false);
   gpio_nirq->gpio_set_edge("falling");
 
-  auto arvrStabilizedRV_callback_last = std::chrono::steady_clock::now();
-  auto const arvrStabilizedRV_callback = [&arvrStabilizedRV_callback_last](sh2_RotationVectorWAcc_t const & data)
+  auto const arvrStabilizedRV_callback = [node, imu_pub](sh2_RotationVectorWAcc_t const & data)
   {
-    auto const now = std::chrono::steady_clock::now();
-    auto const diff_time = (now - arvrStabilizedRV_callback_last);
-    auto const diff_time_us = std::chrono::duration_cast<std::chrono::microseconds>(diff_time).count();
-    arvrStabilizedRV_callback_last = now;
-
-    char msg[128] = {0};
-    snprintf(msg,
-             sizeof(msg),
-             "[%4ld] [i, j, k, real, accuracy] = [%0.3f, %0.3f, %0.3f, %0.3f, %0.3f]",
-             diff_time_us,
-             data.i,
-             data.j,
-             data.k,
-             data.real,
-             data.accuracy);
-    std::cout << msg << std::endl;
+    RCLCPP_INFO(node->get_logger(),
+                "[i, j, k, real, accuracy] = [%0.3f, %0.3f, %0.3f, %0.3f, %0.3f]",
+                data.i, data.j, data.k, data.real, data.accuracy);
+
+    sensor_msgs::msg::Imu imu_msg;
+    imu_msg.header.stamp = node->now();
+    imu_msg.orientation.x = data.i;
+    imu_msg.orientation.y = data.j;
+    imu_msg.orientation.z = data.k;
+    imu_msg.orientation.w = data.real;
+
+    imu_pub->publish(imu_msg);
   };
 
   auto spi = std::make_shared<SPI>("/dev/spidev0.0", SPI_MODE_3, 8, 3*1000*1000UL);
diff --git a/package.xml b/package.xml
index 1f36ce4..789686c 100644
--- a/package.xml
+++ b/package.xml
@@ -13,6 +13,7 @@
 
   <depend>l4xz</depend>
   <depend>rclcpp</depend>
+  <depend>sensor_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>