feat: imu monitor (autowarefoundation#333)

* feat: add imu_anomaly_monitor

* feat: use covariance from msg

* chore: rename

* feat: add diagnostics

* feat: make component

* chore: clang-format

* chore: update yaml

* doc: add readme

* chore: remove file

* fix: uncomment

* fix: typo

system/imu_monitor/CMakeLists.txt

@@ -0,0 +1,21 @@
+cmake_minimum_required(VERSION 3.14)
+project(imu_monitor)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_library(imu_monitor SHARED
+  src/imu_monitor.cpp
+)
+ament_target_dependencies(imu_monitor)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "imu_monitor::ImuMonitor"
+  EXECUTABLE ${PROJECT_NAME}_node
+)
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  launch
+  config
+)

system/imu_monitor/README.md

@@ -0,0 +1,27 @@
+# imu_monitor
+
+## Purpose
+
+This module compares the yaw rate obtained from the IMU with that obtained from the vehicle and outputs ERROR diagnostics if the difference is large.
+
+## Inputs / Outputs
+
+### Input
+
+| Name            | Type                                             | Description                                        |
+| --------------- | ------------------------------------------------ | -------------------------------------------------- |
+| `~/input/imu`   | `sensor_msgs::msg::Imu`                          | output from IMU                                    |
+| `~/input/twist` | `geometry_msgs::msg::TwistWithCovarianceStamped` | twist with covariance converted from VehicleReport |
+
+### Output
+
+| Name           | Type                                    | Description |
+| -------------- | --------------------------------------- | ----------- |
+| `/diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | Diagnostics |
+
+## Parameters
+
+| Name                      | Type   | Description                      |
+| ------------------------- | ------ | -------------------------------- |
+| `frame_id`                | string | frame id for base_link           |
+| `yaw_rate_diff_threshold` | double | threshold of yaw rate difference |

system/imu_monitor/config/imu_monitor.param.yaml

@@ -0,0 +1,4 @@
+/**:
+  ros__parameters:
+    yaw_rate_diff_threshold: 0.07  # [rad/s]
+    frame_id: base_link

system/imu_monitor/include/imu_monitor/imu_monitor.hpp

@@ -0,0 +1,118 @@
+// Copyright 2023 TierIV
+//
+// 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_MONITOR__IMU_MONITOR_HPP_
+#define IMU_MONITOR__IMU_MONITOR_HPP_
+
+#include <diagnostic_updater/diagnostic_updater.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/ros/transform_listener.hpp>
+
+#include <geometry_msgs/msg/twist_stamped.hpp>
+#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+#include <tf2/utils.h>
+
+#include <memory>
+#include <string>
+
+namespace imu_monitor
+{
+
+class KalmanFilter1d
+{
+public:
+  KalmanFilter1d()
+  {
+    initialized_ = false;
+    x_ = 0;
+    dev_ = 1e9;
+    proc_dev_x_c_ = 0.0;
+    return;
+  };
+  void init(const double init_obs, const double obs_dev, const rclcpp::Time time)
+  {
+    x_ = init_obs;
+    dev_ = obs_dev;
+    latest_time_ = time;
+    initialized_ = true;
+    return;
+  };
+  void update(const double obs, const double obs_dev, const rclcpp::Time time)
+  {
+    if (!initialized_) {
+      init(obs, obs_dev, time);
+      return;
+    }
+
+    // Prediction step (current stddev_)
+    double dt = (time - latest_time_).seconds();
+    double proc_dev_x_d = proc_dev_x_c_ * dt * dt;
+    dev_ = dev_ + proc_dev_x_d;
+
+    // Update step
+    double kalman_gain = dev_ / (dev_ + obs_dev);
+    x_ = x_ + kalman_gain * (obs - x_);
+    dev_ = (1 - kalman_gain) * dev_;
+
+    latest_time_ = time;
+    return;
+  };
+  void set_proc_dev(const double proc_dev) { proc_dev_x_c_ = proc_dev; }
+  double get_x() { return x_; }
+
+private:
+  bool initialized_;
+  double x_;
+  double dev_;
+  double proc_dev_x_c_;
+  rclcpp::Time latest_time_;
+};
+
+class ImuMonitor : public rclcpp::Node
+{
+public:
+  explicit ImuMonitor(const rclcpp::NodeOptions & node_options);
+  ~ImuMonitor() = default;
+
+private:
+  void on_twist(const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr msg);
+  void on_imu(const sensor_msgs::msg::Imu::ConstSharedPtr msg);
+  void check_yaw_rate(diagnostic_updater::DiagnosticStatusWrapper & stat);
+
+  rclcpp::Subscription<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr twist_sub_;
+
+  rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
+
+  std::shared_ptr<tier4_autoware_utils::TransformListener> transform_listener_;
+
+  std::string frame_id_;
+  double yaw_rate_diff_threshold_;
+
+  geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_ptr_;
+  sensor_msgs::msg::Imu::ConstSharedPtr imu_ptr_;
+
+  double imu_yaw_rate_;
+  double twist_yaw_rate_;
+
+  KalmanFilter1d imu_filter_;
+  KalmanFilter1d twist_filter_;
+
+  diagnostic_updater::Updater updater_;
+};
+}  // namespace imu_monitor
+
+#endif  // IMU_MONITOR__IMU_MONITOR_HPP_

system/imu_monitor/launch/imu_monitor.launch.xml

@@ -0,0 +1,9 @@
+<launch>
+  <arg name="config_file" default="$(find-pkg-share imu_monitor)/config/imu_monitor.param.yaml"/>
+
+  <node pkg="imu_monitor" exec="imu_monitor_node" name="imu_monitor" output="screen">
+    <param from="$(var config_file)"/>
+    <remap from="~/input/twist" to="/sensing/vehicle_velocity_converter/twist_with_covariance"/>
+    <remap from="~/input/imu" to="/sensing/imu/imu_data"/>
+  </node>
+</launch>

system/imu_monitor/package.xml

@@ -0,0 +1,30 @@
+<?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_monitor</name>
+  <version>0.0.0</version>
+  <description>The imu_monitor package</description>
+  <maintainer email="shinnosuke.hirakawa@tier4.jp">Shinnosuke Hirakawa</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <build_depend>autoware_cmake</build_depend>
+
+  <depend>autoware_auto_vehicle_msgs</depend>
+  <depend>diagnostic_updater</depend>
+  <depend>geometry_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>sensor_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
+  <depend>tier4_autoware_utils</depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

system/imu_monitor/src/imu_monitor.cpp

@@ -0,0 +1,115 @@
+// Copyright 2023 TierIV
+//
+// 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_monitor/imu_monitor.hpp"
+
+#include "tier4_autoware_utils/ros/msg_covariance.hpp"
+
+namespace imu_monitor
+{
+
+ImuMonitor::ImuMonitor(const rclcpp::NodeOptions & node_options)
+: Node("imu_monitor", node_options), updater_(this)
+{
+  yaw_rate_diff_threshold_ = declare_parameter("yaw_rate_diff_threshold", 0.07);
+  frame_id_ = declare_parameter("frame_id", "base_link");
+
+  twist_sub_ = create_subscription<geometry_msgs::msg::TwistWithCovarianceStamped>(
+    "~/input/twist", rclcpp::QoS{100},
+    std::bind(&ImuMonitor::on_twist, this, std::placeholders::_1));
+
+  imu_sub_ = create_subscription<sensor_msgs::msg::Imu>(
+    "~/input/imu", rclcpp::QoS{100}, std::bind(&ImuMonitor::on_imu, this, std::placeholders::_1));
+
+  transform_listener_ = std::make_shared<tier4_autoware_utils::TransformListener>(this);
+
+  imu_filter_.set_proc_dev(0.1);
+  twist_filter_.set_proc_dev(0.1);
+
+  // Diagnostics Updater
+  updater_.setHardwareID("imu_monitor");
+  updater_.add("yaw_rate_status", this, &ImuMonitor::check_yaw_rate);
+  updater_.setPeriod(0.1);
+}
+
+void ImuMonitor::on_twist(const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr msg)
+{
+  if (msg->header.frame_id != frame_id_) {
+    RCLCPP_WARN(get_logger(), "frame_id is not base_link.");
+    return;
+  }
+
+  twist_ptr_ = msg;
+
+  using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
+  twist_filter_.update(
+    twist_ptr_->twist.twist.angular.z, twist_ptr_->twist.covariance[COV_IDX::YAW_YAW],
+    twist_ptr_->header.stamp);
+  twist_yaw_rate_ = twist_filter_.get_x();
+}
+
+void ImuMonitor::on_imu(const sensor_msgs::msg::Imu::ConstSharedPtr msg)
+{
+  auto imu_frame_ = msg->header.frame_id;
+  geometry_msgs::msg::TransformStamped::ConstSharedPtr tf2_imu_link_to_base_link =
+    transform_listener_->getLatestTransform(imu_frame_, frame_id_);
+  if (!tf2_imu_link_to_base_link) {
+    RCLCPP_ERROR(
+      this->get_logger(), "Please publish TF %s to %s", frame_id_.c_str(), (imu_frame_).c_str());
+    return;
+  }
+  imu_ptr_ = msg;
+
+  geometry_msgs::msg::Vector3Stamped angular_velocity;
+  angular_velocity.vector = imu_ptr_->angular_velocity;
+
+  geometry_msgs::msg::Vector3Stamped transformed_angular_velocity;
+  tf2::doTransform(angular_velocity, transformed_angular_velocity, *tf2_imu_link_to_base_link);
+
+  imu_filter_.update(
+    transformed_angular_velocity.vector.z, imu_ptr_->angular_velocity_covariance[8],
+    imu_ptr_->header.stamp);
+  imu_yaw_rate_ = imu_filter_.get_x();
+}
+
+void ImuMonitor::check_yaw_rate(diagnostic_updater::DiagnosticStatusWrapper & stat)
+{
+  if (!twist_ptr_) {
+    RCLCPP_WARN_THROTTLE(
+      this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for twist info...");
+    return;
+  }
+
+  if (!imu_ptr_) {
+    RCLCPP_WARN_THROTTLE(
+      this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for imu info...");
+    return;
+  }
+
+  diagnostic_msgs::msg::DiagnosticStatus status;
+  if (std::abs(imu_yaw_rate_ - twist_yaw_rate_) > yaw_rate_diff_threshold_) {
+    status.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
+    status.message = "The yaw_rate deviation is large";
+  } else {
+    status.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+  }
+  stat.addf("yaw rate from imu", "%lf", imu_yaw_rate_);
+  stat.addf("yaw rate from twist", "%lf", twist_yaw_rate_);
+  stat.summary(status.level, status.message);
+}
+
+}  // namespace imu_monitor
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(imu_monitor::ImuMonitor)