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

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

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

@@ -1,30 +0,0 @@
-"""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

@@ -1,22 +0,0 @@
-<?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

@@ -1,126 +0,0 @@
-#!/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

@@ -1,5 +0,0 @@
-[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

@@ -1,24 +0,0 @@
-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

@@ -1,67 +0,0 @@
-"""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)