feat: add imu_corrector package

sensing/imu_corrector/CMakeLists.txt

@@ -0,0 +1,35 @@
+cmake_minimum_required(VERSION 3.5)
+project(imu_corrector)
+
+### Compile options
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+ament_auto_add_library(imu_corrector_node SHARED
+  src/imu_corrector_core.cpp
+  include/imu_corrector/imu_corrector_core.hpp
+)
+
+rclcpp_components_register_node(imu_corrector_node
+  PLUGIN "imu_corrector::ImuCorrector"
+  EXECUTABLE imu_corrector
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE
+  launch
+  config
+)

sensing/imu_corrector/README.md

@@ -0,0 +1,43 @@
+# imu_corrector
+
+## Purpose
+
+`imu_corrector_node` is a node that correct imu data.
+
+1. Correct yaw rate offset by reading the parameter.
+2. Correct yaw rate standard deviation by reading the parameter.
+
+Use the value estimated by [deviation_estimator](https://github.com/tier4/calibration_tools/tree/main/localization/deviation_estimation_tools) as the parameters for this node.
+
+## Inputs / Outputs
+
+### Input
+
+| Name     | Type                    | Description  |
+| -------- | ----------------------- | ------------ |
+| `~input` | `sensor_msgs::msg::Imu` | raw imu data |
+
+### Output
+
+| Name      | Type                    | Description        |
+| --------- | ----------------------- | ------------------ |
+| `~output` | `sensor_msgs::msg::Imu` | corrected imu data |
+
+## Parameters
+
+### Core Parameters
+
+| Name                         | Type   | Description                         |
+| ---------------------------- | ------ | ----------------------------------- |
+| `angular_velocity_offset_z`  | double | yaw rate offset [rad/s]             |
+| `angular_velocity_stddev_zz` | double | yaw rate standard deviation [rad/s] |
+
+## Assumptions / Known limits
+
+## (Optional) Error detection and handling
+
+## (Optional) Performance characterization
+
+## (Optional) References/External links
+
+## (Optional) Future extensions / Unimplemented parts

sensing/imu_corrector/config/imu_corrector.param.yaml

@@ -0,0 +1,4 @@
+/**:
+  ros__parameters:
+    angular_velocity_offset_z: 0.0    # [rad/s]
+    angular_velocity_stddev_zz: 0.03  # [rad/s]

sensing/imu_corrector/include/imu_corrector/imu_corrector_core.hpp

@@ -0,0 +1,41 @@
+// Copyright 2020 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#ifndef IMU_CORRECTOR__IMU_CORRECTOR_CORE_HPP_
+#define IMU_CORRECTOR__IMU_CORRECTOR_CORE_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <sensor_msgs/msg/imu.hpp>
+
+namespace imu_corrector
+{
+class ImuCorrector : public rclcpp::Node
+{
+public:
+  explicit ImuCorrector(const rclcpp::NodeOptions & node_options);
+
+private:
+  void callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr);
+
+  rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
+
+  rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;
+
+  double angular_velocity_offset_z_;
+
+  double angular_velocity_stddev_zz_;
+};
+}  // namespace imu_corrector
+
+#endif  // IMU_CORRECTOR__IMU_CORRECTOR_CORE_HPP_

sensing/imu_corrector/launch/imu_corrector.launch.xml

@@ -0,0 +1,15 @@
+<?xml version="1.0" encoding="UTF-8"?>
+
+<launch>
+
+  <arg name="input_topic" default="imu_raw" />
+  <arg name="output_topic" default="imu_data" />
+  <arg name="param_file" default="$(find-pkg-share imu_corrector)/config/imu_corrector.param.yaml" />
+
+  <node pkg="imu_corrector" exec="imu_corrector" name="imu_corrector" output="screen">
+    <remap from="input" to="$(var input_topic)" />
+    <remap from="output" to="$(var output_topic)" />
+    <param from="$(var param_file)" />
+  </node>
+
+</launch>

sensing/imu_corrector/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>imu_corrector</name>
+  <version>1.0.0</version>
+  <description>The ROS2 imu_corrector package</description>
+  <maintainer email="yamato.ando@gmail.com">Yamato Ando</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>sensor_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

sensing/imu_corrector/src/imu_corrector_core.cpp

@@ -0,0 +1,48 @@
+// Copyright 2020 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "imu_corrector/imu_corrector_core.hpp"
+
+namespace imu_corrector
+{
+ImuCorrector::ImuCorrector(const rclcpp::NodeOptions & node_options)
+: Node("imu_corrector", node_options)
+{
+  angular_velocity_offset_z_ = declare_parameter<double>("angular_velocity_offset_z", 0.0);
+
+  angular_velocity_stddev_zz_ = declare_parameter<double>("angular_velocity_stddev_zz", 0.03);
+
+  imu_sub_ = create_subscription<sensor_msgs::msg::Imu>(
+    "input", rclcpp::QoS{1}, std::bind(&ImuCorrector::callbackImu, this, std::placeholders::_1));
+
+  imu_pub_ = create_publisher<sensor_msgs::msg::Imu>("output", rclcpp::QoS{10});
+}
+
+void ImuCorrector::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr)
+{
+  sensor_msgs::msg::Imu imu_msg;
+  imu_msg = *imu_msg_ptr;
+
+  imu_msg.angular_velocity.z += angular_velocity_offset_z_;
+
+  imu_msg.angular_velocity_covariance[8] =
+    angular_velocity_stddev_zz_ * angular_velocity_stddev_zz_;
+
+  imu_pub_->publish(imu_msg);
+}
+
+}  // namespace imu_corrector
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(imu_corrector::ImuCorrector)