Impl imu

device/pi_i2c/pi_i2c/imu.py

@@ -6,6 +6,8 @@
 from rpi_bno055 import BNO055
 from sensor_msgs.msg import Imu as ImuMsg
 
+from .utils import encode_quat, encode_vec3
+
 
 class Imu(Node):
     def __init__(self, bus: smbus2.SMBus | None = None) -> None:
@@ -39,6 +41,9 @@ def _timer_callback(self) -> None:
             # TODO: covariance の計算
             msg = ImuMsg()
             msg.header.frame_id = "bno055"
+            msg.orientation = encode_quat(quaternion)
+            msg.angular_velocity = encode_vec3(angular_velocity)
+            msg.linear_acceleration = encode_vec3(linear_accel)
         except OSError:
             self.get_logger().error("Failed to read data from IMU")
 

device/pi_i2c/pi_i2c/utils.py

@@ -0,0 +1,19 @@
+from geometry_msgs.msg import Quaternion, Vector3
+
+
+def encode_vec3(ls: tuple[float, float, float]) -> Vector3:
+    v = Vector3()
+    v.x = ls[0]
+    v.y = ls[1]
+    v.z = ls[2]
+    return v
+
+
+# [w, x, y, z]
+def encode_quat(ls: tuple[float, float, float, float]) -> Quaternion:
+    q = Quaternion()
+    q.x = ls[1]
+    q.y = ls[2]
+    q.z = ls[3]
+    q.w = ls[0]
+    return q