Add IMU calibration tool and publish corrected IMU data

[708yamaguchi]

Summary

Added a GUI tool for calibrating the IMU accelerometer and implemented a feature to publish corrected IMU data using the calculated parameters.

Changes

• Add Calibration Tool (imu_calibrator.py):

  • Implemented a Matplotlib-based GUI tool. Users can collect acceleration data by rotating the robot, and the tool calculates bias and scale parameters by fitting the data to a sphere using the least squares method.
  • The results are saved to a specified path in YAML format.

• Add Calculation Logic (rcb4/imu_utils.py):

  • Added fit_sphere_to_imu_data function to calculate bias and scale factors.

• Update ROS Bridge (rcb4_ros_bridge.py):

  • Added functionality to load the calibration YAML file.
  • Modified the node to publish a new topic /imu_corrected with the calibration parameters applied, in addition to the existing /imu topic.

• Update Launch File (kxr_controller.launch):

  • Added imu_config_path argument to specify the calibration file path.

Usage

1. Run Calibration

Run the following command to start the tool. Follow the GUI instructions to collect data and calculate the parameters. The result will be saved to the specified YAML file.

rosrun kxr_controller imu_calibrator.py -o ~/calibration_result.yaml

2. Launch with Calibration Data
   Launch the controller with the generated YAML file. By setting publish_imu:=true, the corrected data will be published to /imu_corrected.

roslaunch kxr_controller kxr_controller.launch publish_imu:=true imu_config_path:=~/calibration_result.yaml

[github-actions]

🎬️ Watch Hardware CI Test Video