Add IMU gyro calibration

leo_fw/CMakeLists.txt

@@ -8,13 +8,19 @@ find_package(rclcpp_components REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(leo_msgs REQUIRED)
+find_package(yaml-cpp REQUIRED)
+
+include_directories(
+  ${YAML_CPP_INCLUDE_DIRS}
+)
 
 add_library(leo_fw SHARED
   src/firmware_message_converter.cpp
 )
 target_link_libraries(leo_fw
   rclcpp::rclcpp
   rclcpp_components::component
+  ${YAML_CPP_LIBRARIES}
 )
 ament_target_dependencies(leo_fw
   sensor_msgs
@@ -39,6 +45,7 @@ install(
     scripts/flash
     scripts/update
     scripts/test_hw
+    scripts/calibrate_imu
   DESTINATION lib/${PROJECT_NAME}
 )
 

leo_fw/package.xml

@@ -36,6 +36,7 @@
   <build_depend>sensor_msgs</build_depend>
   <build_depend>nav_msgs</build_depend>
   <build_depend>leo_msgs</build_depend>
+  <build_depend>yaml-cpp</build_depend>
 
   <exec_depend>rclcpp</exec_depend>
   <exec_depend>rclcpp_components</exec_depend>

leo_fw/scripts/calibrate_imu

@@ -0,0 +1,94 @@
+#!/usr/bin/env python3
+
+import argparse
+from calendar import day_abbr
+
+import numpy as np
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSReliabilityPolicy
+from leo_msgs.srv import SetImuCalibration
+from leo_msgs.msg import Imu
+
+
+class Calibrator(Node):
+    def __init__(self, time=2.0):
+        super().__init__("minimal_client_async")
+
+        self.service_client = self.create_client(
+            SetImuCalibration, "set_imu_calibration"
+        )
+
+        while not self.service_client.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info("service not available, waiting again...")
+
+        self.req = SetImuCalibration.Request()
+
+        self.data = []
+        self.end_time = self.get_clock().now() + rclpy.duration.Duration(seconds=time)
+
+        qos = QoSProfile(
+            depth=1,
+            durability=QoSDurabilityPolicy.VOLATILE,
+            reliability=QoSReliabilityPolicy.BEST_EFFORT,
+        )
+        self.imu_sub = self.create_subscription(
+            Imu, "firmware/imu", self.imu_sub_callback, qos
+        )
+
+        self.running = True
+
+    def imu_sub_callback(self, data: Imu):
+        if self.get_clock().now() >= self.end_time:
+            self.running = False
+            self.destroy_subscription(self.imu_sub)
+
+        self.data.append([data.gyro_x, data.gyro_y, data.gyro_z])
+
+    def send_bias(self):
+        self.get_logger().info(f"Calculating bias from {len(self.data)} samples.")
+
+        matrix = np.matrix(self.data)
+        bias = matrix.mean(0) * -1.0
+        bias = bias.tolist()[0]
+
+        self.get_logger().info(f"Calculated bias: {bias}")
+
+        self.req.gyro_bias_x, self.req.gyro_bias_y, self.req.gyro_bias_z = bias
+
+        self.future = self.service_client.call_async(self.req)
+        rclpy.spin_until_future_complete(self, self.future)
+
+        if not self.future.result().success:
+            self.get_logger().error(f"Failed to set new imu calibration.")
+
+
+def add_arguments(parser: argparse.ArgumentParser):
+    parser.add_argument(
+        "duration",
+        default=10.0,
+        type=float,
+        nargs="?",
+        help="The duration for which the IMU data is collected.",
+    )
+
+
+if __name__ == "__main__":
+    rclpy.init()
+
+    parser = argparse.ArgumentParser(
+        description="Calculate imu bias and send it to firmware_message_converter."
+    )
+    add_arguments(parser)
+    args = parser.parse_args()
+
+    calib = Calibrator(args.duration)
+
+    while calib.running and rclpy.ok():
+        rclpy.spin_once(calib)
+
+    calib.send_bias()
+    calib.get_logger().info("Finishing node.")
+    calib.destroy_node()
+    rclpy.shutdown()

leo_fw/src/firmware_message_converter.cpp

@@ -20,8 +20,10 @@
 
 #include <cmath>
 #include <chrono>
+#include <fstream>
 
 #include "rclcpp/rclcpp.hpp"
+#include "yaml-cpp/yaml.h"
 
 #include "nav_msgs/msg/odometry.hpp"
 #include "sensor_msgs/msg/imu.hpp"
@@ -31,6 +33,9 @@
 #include "leo_msgs/msg/wheel_odom.hpp"
 #include "leo_msgs/msg/wheel_states.hpp"
 
+#include "leo_msgs/srv/set_imu_calibration.hpp"
+
+
 using namespace std::chrono_literals;
 using std::placeholders::_1;
 
@@ -43,6 +48,15 @@ class FirmwareMessageConverter : public rclcpp::Node
   FirmwareMessageConverter(rclcpp::NodeOptions options)
   : Node("firmware_message_converter", options.use_intra_process_comms(true))
   {
+    calib_file_path = get_calib_path();
+    load_yaml_bias();
+    set_imu_calibration_service = create_service<leo_msgs::srv::SetImuCalibration>(
+      "set_imu_calibration",
+      std::bind(
+        &FirmwareMessageConverter::set_imu_calibration_callback, this,
+        std::placeholders::_1, std::placeholders::_2));
+
+
     robot_frame_id_ = declare_parameter("robot_frame_id", robot_frame_id_);
     odom_frame_id_ = declare_parameter("odom_frame_id", odom_frame_id_);
     imu_frame_id_ = declare_parameter("imu_frame_id", imu_frame_id_);
@@ -101,9 +115,9 @@ class FirmwareMessageConverter : public rclcpp::Node
     sensor_msgs::msg::Imu imu;
     imu.header.frame_id = tf_frame_prefix_ + imu_frame_id_;
     imu.header.stamp = msg->stamp;
-    imu.angular_velocity.x = msg->gyro_x;
-    imu.angular_velocity.y = msg->gyro_y;
-    imu.angular_velocity.z = msg->gyro_z;
+    imu.angular_velocity.x = msg->gyro_x + imu_calibration_bias[0];
+    imu.angular_velocity.y = msg->gyro_y + imu_calibration_bias[1];
+    imu.angular_velocity.z = msg->gyro_z + imu_calibration_bias[2];
     imu.linear_acceleration.x = msg->accel_x;
     imu.linear_acceleration.y = msg->accel_y;
     imu.linear_acceleration.z = msg->accel_z;
@@ -118,6 +132,69 @@ class FirmwareMessageConverter : public rclcpp::Node
     imu_pub_->publish(imu);
   }
 
+  void set_imu_calibration_callback(
+    const std::shared_ptr<leo_msgs::srv::SetImuCalibration::Request> request,
+    std::shared_ptr<leo_msgs::srv::SetImuCalibration::Response> response)
+  {
+    RCLCPP_INFO(
+      get_logger(), "SetImuCalibration request for: [ %f, %f, %f]", request->gyro_bias_x,
+      request->gyro_bias_y, request->gyro_bias_z);
+
+    YAML::Node node = YAML::LoadFile(calib_file_path);
+    node["gyro_bias_x"] = imu_calibration_bias[0] = request->gyro_bias_x;
+    node["gyro_bias_y"] = imu_calibration_bias[1] = request->gyro_bias_y;
+    node["gyro_bias_z"] = imu_calibration_bias[2] = request->gyro_bias_z;
+    std::ofstream fout(calib_file_path);
+    fout << node;
+
+    response->success = true;
+  }
+
+  void load_yaml_bias()
+  {
+    YAML::Node node;
+    try {
+      node = YAML::LoadFile(calib_file_path);
+
+      if (node["gyro_bias_x"]) {
+        imu_calibration_bias[0] = node["gyro_bias_x"].as<float>();
+      }
+
+      if (node["gyro_bias_y"]) {
+        imu_calibration_bias[1] = node["gyro_bias_y"].as<float>();
+      }
+
+      if (node["gyro_bias_z"]) {
+        imu_calibration_bias[2] = node["gyro_bias_z"].as<float>();
+      }
+
+    } catch (YAML::BadFile & e) {
+      RCLCPP_WARN(get_logger(), "Calibration file doesn't exist.");
+      RCLCPP_WARN(get_logger(), "Creating calibration file with default gyrometer bias.");
+
+      node["gyro_bias_x"] = imu_calibration_bias[0];
+      node["gyro_bias_y"] = imu_calibration_bias[1];
+      node["gyro_bias_z"] = imu_calibration_bias[2];
+
+      std::ofstream fout(calib_file_path);
+      fout << node;
+    }
+  }
+
+  std::string get_calib_path()
+  {
+    std::string ros_home;
+    char * ros_home_env;
+    if (ros_home_env = std::getenv("ROS_HOME")) {
+      ros_home = ros_home_env;
+    } else if (ros_home_env = std::getenv("HOME")) {
+      ros_home = ros_home_env;
+      ros_home += "/.ros";
+    }
+
+    return ros_home + "/imu_calibration.yaml";
+  }
+
   void timer_callback()
   {
     size_t wheel_states_publishers = count_publishers(wheel_states_topic_);
@@ -194,6 +271,8 @@ class FirmwareMessageConverter : public rclcpp::Node
   std::vector<double> imu_linear_acceleration_covariance_diagonal_ = {0.001, 0.001,
     0.001};
   std::string tf_frame_prefix_ = "";
+  std::vector<float> imu_calibration_bias = {0.0, 0.0, 0.0};
+  std::string calib_file_path = "";
 
   // Topic names
   std::string wheel_states_topic_;
@@ -212,6 +291,9 @@ class FirmwareMessageConverter : public rclcpp::Node
   rclcpp::Subscription<leo_msgs::msg::WheelStates>::SharedPtr wheel_states_sub_;
   rclcpp::Subscription<leo_msgs::msg::WheelOdom>::SharedPtr wheel_odom_sub_;
   rclcpp::Subscription<leo_msgs::msg::Imu>::SharedPtr imu_sub_;
+
+  // Service
+  rclcpp::Service<leo_msgs::srv::SetImuCalibration>::SharedPtr set_imu_calibration_service;
 };
 
 }  // namespace leo_fw