Merge pull request 'IMU calibration routine (Issue #278)' (#282) from sl-controls/issue-278-imu-cal into main

jetson/ros2_ws/src/saltybot_imu_calibration/config/imu_calibration_config.yaml

@@ -0,0 +1,4 @@
+imu_calibration:
+  ros__parameters:
+    calibration_samples: 100
+    auto_calibrate: false

jetson/ros2_ws/src/saltybot_imu_calibration/launch/imu_calibration.launch.py

@@ -0,0 +1,30 @@
+"""Launch file for IMU calibration node."""
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.substitutions import LaunchConfiguration
+from launch.actions import DeclareLaunchArgument
+import os
+from ament_index_python.packages import get_package_share_directory
+
+
+def generate_launch_description():
+    pkg_dir = get_package_share_directory("saltybot_imu_calibration")
+    config_file = os.path.join(pkg_dir, "config", "imu_calibration_config.yaml")
+
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument(
+                "config_file",
+                default_value=config_file,
+                description="Path to configuration YAML file",
+            ),
+            Node(
+                package="saltybot_imu_calibration",
+                executable="imu_calibration_node",
+                name="imu_calibration",
+                output="screen",
+                parameters=[LaunchConfiguration("config_file")],
+            ),
+        ]
+    )

jetson/ros2_ws/src/saltybot_imu_calibration/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>saltybot_imu_calibration</name>
+  <version>0.1.0</version>
+  <description>IMU gyro + accel calibration node for SaltyBot</description>
+  <maintainer email="sl-controls@saltylab.local">SaltyLab Controls</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_python</buildtool_depend>
+  <depend>rclpy</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+
+  <test_depend>pytest</test_depend>
+  <test_depend>sensor_msgs</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

jetson/ros2_ws/src/saltybot_imu_calibration/saltybot_imu_calibration/imu_calibration_node.py

@@ -0,0 +1,126 @@
+#!/usr/bin/env python3
+"""IMU calibration node for SaltyBot."""
+
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Imu
+from std_srvs.srv import Trigger
+from std_msgs.msg import Header
+import numpy as np
+from collections import deque
+
+
+class IMUCalibrationNode(Node):
+    """ROS2 node for IMU gyro + accel calibration."""
+
+    def __init__(self):
+        super().__init__("imu_calibration")
+        self.declare_parameter("calibration_samples", 100)
+        self.declare_parameter("auto_calibrate", False)
+
+        self.calibration_samples = self.get_parameter("calibration_samples").value
+        self.auto_calibrate = self.get_parameter("auto_calibrate").value
+
+        self.gyro_bias = np.array([0.0, 0.0, 0.0])
+        self.accel_bias = np.array([0.0, 0.0, 0.0])
+        self.is_calibrated = False
+
+        self.gyro_samples = deque(maxlen=self.calibration_samples)
+        self.accel_samples = deque(maxlen=self.calibration_samples)
+        self.calibrating = False
+
+        self.sub_imu = self.create_subscription(Imu, "/imu", self._on_imu_raw, 10)
+        self.pub_calibrated = self.create_publisher(Imu, "/imu/calibrated", 10)
+        self.srv_calibrate = self.create_service(
+            Trigger, "/saltybot/calibrate_imu", self._on_calibrate_service
+        )
+
+        self.get_logger().info(
+            f"IMU calibration node initialized. Samples: {self.calibration_samples}. Auto: {self.auto_calibrate}"
+        )
+
+        if self.auto_calibrate:
+            self.calibrating = True
+            self.get_logger().info("Starting auto-calibration...")
+
+    def _on_imu_raw(self, msg: Imu) -> None:
+        if self.calibrating:
+            gyro = np.array([msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z])
+            accel = np.array([msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z])
+            self.gyro_samples.append(gyro)
+            self.accel_samples.append(accel)
+
+            if len(self.gyro_samples) == self.calibration_samples:
+                self._compute_calibration()
+        else:
+            self._publish_calibrated(msg)
+
+    def _compute_calibration(self) -> None:
+        if len(self.gyro_samples) == 0 or len(self.accel_samples) == 0:
+            return
+
+        gyro_data = np.array(list(self.gyro_samples))
+        accel_data = np.array(list(self.accel_samples))
+
+        self.gyro_bias = np.mean(gyro_data, axis=0)
+        self.accel_bias = np.mean(accel_data, axis=0)
+
+        self.is_calibrated = True
+        self.calibrating = False
+
+        self.get_logger().info(
+            f"Calibration complete. Gyro: {self.gyro_bias}. Accel: {self.accel_bias}"
+        )
+
+        self.gyro_samples.clear()
+        self.accel_samples.clear()
+
+    def _on_calibrate_service(self, request, response) -> Trigger.Response:
+        if self.calibrating:
+            response.success = False
+            response.message = "Calibration already in progress"
+            return response
+
+        self.get_logger().info("Calibration service called")
+        self.calibrating = True
+        self.gyro_samples.clear()
+        self.accel_samples.clear()
+
+        response.success = True
+        response.message = f"Calibration started, collecting {self.calibration_samples} samples"
+        return response
+
+    def _publish_calibrated(self, msg: Imu) -> None:
+        calibrated = Imu()
+        calibrated.header = Header(frame_id=msg.header.frame_id, stamp=msg.header.stamp)
+
+        calibrated.angular_velocity.x = msg.angular_velocity.x - self.gyro_bias[0]
+        calibrated.angular_velocity.y = msg.angular_velocity.y - self.gyro_bias[1]
+        calibrated.angular_velocity.z = msg.angular_velocity.z - self.gyro_bias[2]
+
+        calibrated.linear_acceleration.x = msg.linear_acceleration.x - self.accel_bias[0]
+        calibrated.linear_acceleration.y = msg.linear_acceleration.y - self.accel_bias[1]
+        calibrated.linear_acceleration.z = msg.linear_acceleration.z - self.accel_bias[2]
+
+        calibrated.angular_velocity_covariance = msg.angular_velocity_covariance
+        calibrated.linear_acceleration_covariance = msg.linear_acceleration_covariance
+        calibrated.orientation_covariance = msg.orientation_covariance
+        calibrated.orientation = msg.orientation
+
+        self.pub_calibrated.publish(calibrated)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = IMUCalibrationNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

jetson/ros2_ws/src/saltybot_imu_calibration/setup.cfg

@@ -0,0 +1,5 @@
+[develop]
+script_dir=$base/lib/saltybot_imu_calibration
+
+[install]
+install_scripts=$base/lib/saltybot_imu_calibration

jetson/ros2_ws/src/saltybot_imu_calibration/setup.py

@@ -0,0 +1,24 @@
+from setuptools import setup, find_packages
+
+setup(
+    name='saltybot_imu_calibration',
+    version='0.1.0',
+    packages=find_packages(),
+    data_files=[
+        ('share/ament_index/resource_index/packages', ['resource/saltybot_imu_calibration']),
+        ('share/saltybot_imu_calibration', ['package.xml']),
+        ('share/saltybot_imu_calibration/config', ['config/imu_calibration_config.yaml']),
+        ('share/saltybot_imu_calibration/launch', ['launch/imu_calibration.launch.py']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    author='SaltyLab Controls',
+    author_email='sl-controls@saltylab.local',
+    description='IMU gyro + accel calibration node for SaltyBot',
+    license='MIT',
+    entry_points={
+        'console_scripts': [
+            'imu_calibration_node=saltybot_imu_calibration.imu_calibration_node:main',
+        ],
+    },
+)

jetson/ros2_ws/src/saltybot_imu_calibration/test/test_imu_calibration.py

@@ -0,0 +1,67 @@
+"""Tests for IMU calibration node."""
+
+import pytest
+import numpy as np
+from sensor_msgs.msg import Imu
+from geometry_msgs.msg import Quaternion
+import rclpy
+from rclpy.time import Time
+
+from saltybot_imu_calibration.imu_calibration_node import IMUCalibrationNode
+
+
+@pytest.fixture
+def rclpy_fixture():
+    rclpy.init()
+    yield
+    rclpy.shutdown()
+
+
+@pytest.fixture
+def node(rclpy_fixture):
+    node = IMUCalibrationNode()
+    yield node
+    node.destroy_node()
+
+
+class TestInit:
+    def test_node_initialization(self, node):
+        assert node.calibration_samples == 100
+        assert node.is_calibrated is False
+        assert node.calibrating is False
+
+
+class TestCalibration:
+    def test_calibration_samples(self, node):
+        node.calibration_samples = 3
+        node.gyro_samples.maxlen = 3
+        node.accel_samples.maxlen = 3
+        node.calibrating = True
+
+        for i in range(3):
+            node.gyro_samples.append(np.array([0.1, 0.2, 0.3]))
+            node.accel_samples.append(np.array([0.0, 0.0, 9.81]))
+
+        node._compute_calibration()
+
+        assert node.is_calibrated is True
+        assert len(node.gyro_samples) == 0
+
+
+class TestCorrection:
+    def test_imu_correction(self, node):
+        node.gyro_bias = np.array([0.1, 0.2, 0.3])
+        node.accel_bias = np.array([0.0, 0.0, 0.1])
+
+        msg = Imu()
+        msg.header.stamp = Time().to_msg()
+        msg.header.frame_id = "imu_link"
+        msg.angular_velocity.x = 0.11
+        msg.angular_velocity.y = 0.22
+        msg.angular_velocity.z = 0.33
+        msg.linear_acceleration.x = 0.0
+        msg.linear_acceleration.y = 0.0
+        msg.linear_acceleration.z = 9.91
+        msg.orientation = Quaternion(x=0, y=0, z=0, w=1)
+
+        node._publish_calibrated(msg)