feat(steering_offset_estimator): add steering offset estimation packa…

…ge (#930)

* feat(steering_offset_estimator): add steering offset estimation package

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* chore: update license

Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com>

* chore: update maintainer

Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com>

* chore: update license

Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com>

* chore: update cmake

Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com>

* chore: update depends

Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com>

* chore: update variables

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* fix: remaining

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* chore: remove unused

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* doc(steer_offset_estimator): add document

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* doc: update

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* fix: typo

* ci(pre-commit): autofix

* doc: update algo

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* doc : update

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

vehicle/steer_offset_estimator/CMakeLists.txt

@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 3.5)
+project(steer_offset_estimator)
+
+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 -Werror)
+endif()
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_library(steer_offset_estimator_node SHARED
+  src/steer_offset_estimator_node.cpp
+)
+
+rclcpp_components_register_node(steer_offset_estimator_node
+  PLUGIN "steer_offset_estimator::SteerOffsetEstimatorNode"
+  EXECUTABLE steer_offset_estimator
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+    launch
+)

vehicle/steer_offset_estimator/Readme.md

@@ -0,0 +1,72 @@
+# steer_offset_estimator
+
+## Purpose
+
+The role of this node is to automatically calibrate `steer_offset` used in the `vehicle_interface` node.
+
+The base steer offset value is 0 by default, which is standard, is updated iteratively with the loaded driving data. This module is supposed to be used in below straight driving situation.
+![image](./image/steer_offset.png)
+
+## Inner-workings / Algorithms
+
+Estimates sequential steering offsets from kinematic model and state observations.
+![image2](./image/kinematic_constraints.png)
+Calculate yaw rate error and then calculate steering error recursively by least squared method, for more details see `updateSteeringOffset()` function.
+
+## Inputs / Outputs
+
+### Input
+
+| Name            | Type                                              | Description   |
+| --------------- | ------------------------------------------------- | ------------- |
+| `~/input/twist` | `geometry_msgs::msg::TwistStamped`                | vehicle twist |
+| `~/input/steer` | `autoware_auto_vehicle_msgs::msg::SteeringReport` | steering      |
+
+### Output
+
+| Name                                  | Type                                    | Description                   |
+| ------------------------------------- | --------------------------------------- | ----------------------------- |
+| `~/output/steering_offset`            | `tier4_debug_msgs::msg::Float32Stamped` | steering offset               |
+| `~/output/steering_offset_covariance` | `tier4_debug_msgs::msg::Float32Stamped` | covariance of steering offset |
+
+## Launch Calibrator
+
+After launching Autoware, run the `steer_offset_estimator` by the following command and then perform autonomous driving. Note: You can collect data with manual driving if it is possible to use the same vehicle interface as during autonomous driving (e.g. using a joystick).
+
+```sh
+ros2 launch steer_offset_estimator steer_offset_estimator.launch.xml
+```
+
+Or if you want to use rosbag files, run the following commands.
+
+```sh
+ros2 param set /use_sim_time true
+ros2 bag play <rosbag_file> --clock
+```
+
+## Parameters
+
+| Params                  | Description                           |
+| ----------------------- | ------------------------------------- |
+| `steer_update_hz`       | update hz                             |
+| `initial_covariance`    | steer offset is larger than tolerance |
+| `forgetting_factor`     | weight of using previous value        |
+| `valid_min_velocity`    | velocity below this value is not used |
+| `valid_max_steer`       | steer above this value is not used    |
+| `warn_steer_offset_deg` | warn if offset is above this value    |
+
+## Diagnostics
+
+The `steer_offset_estimator` publishes diagnostics message depending on the calibration status.
+Diagnostic type `WARN` indicates that the current steer_offset is estimated to be inaccurate. In this situation, it is strongly recommended to perform a re-calibration of the steer_offset.
+
+| Status                  | Diagnostics Type | Diagnostics message                     |
+| ----------------------- | ---------------- | --------------------------------------- |
+| No calibration required | `OK`             | "Preparation"                           |
+| Calibration Required    | `WARN`           | "Steer offset is larger than tolerance" |
+
+This diagnostics status can be also checked on the following ROS topic.
+
+```sh
+ros2 topic echo /vehicle/status/steering_offset
+```

vehicle/steer_offset_estimator/include/steer_offset_estimator/steer_offset_estimator_node.hpp

@@ -0,0 +1,69 @@
+// Copyright 2022 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 STEER_OFFSET_ESTIMATOR__STEER_OFFSET_ESTIMATOR_NODE_HPP_
+#define STEER_OFFSET_ESTIMATOR__STEER_OFFSET_ESTIMATOR_NODE_HPP_
+
+#include "diagnostic_updater/diagnostic_updater.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
+#include "tier4_debug_msgs/msg/float32_stamped.hpp"
+
+#include <memory>
+
+namespace steer_offset_estimator
+{
+using geometry_msgs::msg::TwistStamped;
+using tier4_debug_msgs::msg::Float32Stamped;
+using Steering = autoware_auto_vehicle_msgs::msg::SteeringReport;
+using diagnostic_updater::DiagnosticStatusWrapper;
+using diagnostic_updater::Updater;
+
+class SteerOffsetEstimatorNode : public rclcpp::Node
+{
+private:
+  rclcpp::Publisher<Float32Stamped>::SharedPtr pub_steer_offset_;
+  rclcpp::Publisher<Float32Stamped>::SharedPtr pub_steer_offset_cov_;
+  rclcpp::Subscription<TwistStamped>::SharedPtr sub_twist_;
+  rclcpp::Subscription<Steering>::SharedPtr sub_steer_;
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  TwistStamped::ConstSharedPtr twist_ptr_;
+  Steering::ConstSharedPtr steer_ptr_;
+  double wheel_base_;
+  double update_hz_;
+  double estimated_steer_offset_{0.0};
+  double covariance_;
+  double forgetting_factor_;
+  double valid_min_velocity_;
+  double valid_max_steer_;
+  double warn_steer_offset_abs_error_;
+
+  // Diagnostic Updater
+  std::shared_ptr<Updater> updater_ptr_;
+
+  void onTwist(const TwistStamped::ConstSharedPtr msg);
+  void onSteer(const Steering::ConstSharedPtr msg);
+  void onTimer();
+  bool updateSteeringOffset();
+  void monitorSteerOffset(DiagnosticStatusWrapper & stat);
+
+public:
+  explicit SteerOffsetEstimatorNode(const rclcpp::NodeOptions & node_options);
+};
+}  // namespace steer_offset_estimator
+
+#endif  // STEER_OFFSET_ESTIMATOR__STEER_OFFSET_ESTIMATOR_NODE_HPP_

vehicle/steer_offset_estimator/launch/steer_offset_estimtor.launch.xml

@@ -0,0 +1,31 @@
+<launch>
+  <arg name="steer_update_hz" default="10.0"/>
+  <arg name="initial_covariance" default="1000.0"/>
+  <arg name="forgetting_factor" default="0.999"/>
+  <arg name="valid_min_velocity" default="5.0"/>
+  <arg name="valid_max_steer" default="0.05"/>
+  <arg name="warn_steer_offset_deg" default="2.5" description="if absolute estimated offset is larger than 2.5[deg] => warning"/>
+
+  <!-- get wheel base from vehicle info -->
+  <include file="$(find-pkg-share global_parameter_loader)/launch/global_params.launch.py"/>
+
+  <!-- ndt twist publisher -->
+  <include file="$(find-pkg-share pose2twist)/launch/pose2twist.launch.xml">
+    <arg name="input_pose_topic" value="/localization/pose_estimator/pose"/>
+    <arg name="output_twist_topic" value="/estimate_twist"/>
+  </include>
+
+  <!-- steer offset estimator -->
+  <node pkg="steer_offset_estimator" exec="steer_offset_estimator" name="steer_offset_estimator" output="screen">
+    <param name="steer_update_hz" value="$(var steer_update_hz)"/>
+    <param name="initial_covariance" value="$(var initial_covariance)"/>
+    <param name="forgetting_factor" value="$(var forgetting_factor)"/>
+    <param name="valid_min_velocity" value="$(var valid_min_velocity)"/>
+    <param name="valid_max_steer" value="$(var valid_max_steer)"/>
+    <param name="warn_steer_offset_deg" value="$(var warn_steer_offset_deg)"/>
+    <remap from="~/input/twist" to="$(var output_twist_topic)"/>
+    <remap from="~/input/steer" to="/vehicle/status/steering_status"/>
+    <remap from="~/output/steering_offset" to="/vehicle/status/steering_offset"/>
+    <remap from="~/output/steering_offset_covariance" to="/vehicle/status/steering_offset_covariance"/>
+  </node>
+</launch>

vehicle/steer_offset_estimator/package.xml

@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>steer_offset_estimator</name>
+  <version>0.1.0</version>
+  <description>The steer_offset_estimator</description>
+  <maintainer email="taiki.tanaka@tier4.jp">Taiki Tanaka</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>std_msgs</depend>
+  <depend>tier4_debug_msgs</depend>
+  <depend>vehicle_info_util</depend>
+  <exec_depend>global_parameter_loader</exec_depend>
+  <exec_depend>pose2twist</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

vehicle/steer_offset_estimator/src/steer_offset_estimator_node.cpp

@@ -0,0 +1,137 @@
+// Copyright 2022 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 "steer_offset_estimator/steer_offset_estimator_node.hpp"
+
+#include "vehicle_info_util/vehicle_info_util.hpp"
+
+#include <memory>
+#include <utility>
+
+namespace steer_offset_estimator
+{
+SteerOffsetEstimatorNode::SteerOffsetEstimatorNode(const rclcpp::NodeOptions & node_options)
+: Node("steer_offset_estimator", node_options)
+{
+  using std::placeholders::_1;
+  // get parameter
+  const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo();
+  wheel_base_ = vehicle_info.wheel_base_m;
+  covariance_ = this->declare_parameter("initial_covariance", 1000.0);
+  forgetting_factor_ = this->declare_parameter("forgetting_factor", 0.999);
+  update_hz_ = this->declare_parameter<double>("steer_update_hz", 10.0);
+  valid_min_velocity_ = this->declare_parameter<double>("valid_min_velocity", 1.0);
+  valid_max_steer_ = this->declare_parameter<double>("valid_max_steer", 0.1);
+  warn_steer_offset_abs_error_ =
+    this->declare_parameter<double>("warn_steer_offset_deg", 2.5) * M_PI / 180.0;
+
+  // publisher
+  pub_steer_offset_ = this->create_publisher<Float32Stamped>("~/output/steering_offset", 1);
+  pub_steer_offset_cov_ =
+    this->create_publisher<Float32Stamped>("~/output/steering_offset_covariance", 1);
+
+  // subscriber
+  sub_twist_ = this->create_subscription<TwistStamped>(
+    "~/input/twist", 1, std::bind(&SteerOffsetEstimatorNode::onTwist, this, _1));
+  sub_steer_ = this->create_subscription<Steering>(
+    "~/input/steer", 1, std::bind(&SteerOffsetEstimatorNode::onSteer, this, _1));
+
+  /* Diagnostic Updater */
+  updater_ptr_ = std::make_shared<Updater>(this, 1.0 / update_hz_);
+  updater_ptr_->setHardwareID("steer_offset_estimator");
+  updater_ptr_->add("steer_offset_estimator", this, &SteerOffsetEstimatorNode::monitorSteerOffset);
+
+  // timer
+  {
+    auto on_timer = std::bind(&SteerOffsetEstimatorNode::onTimer, this);
+    const auto period = rclcpp::Rate(update_hz_).period();
+    timer_ = std::make_shared<rclcpp::GenericTimer<decltype(on_timer)>>(
+      this->get_clock(), period, std::move(on_timer),
+      this->get_node_base_interface()->get_context());
+    this->get_node_timers_interface()->add_timer(timer_, nullptr);
+  }
+}
+
+// function for diagnostics
+void SteerOffsetEstimatorNode::monitorSteerOffset(DiagnosticStatusWrapper & stat)
+{
+  using DiagStatus = diagnostic_msgs::msg::DiagnosticStatus;
+  const double eps = 1e-3;
+  if (covariance_ > eps) {
+    stat.summary(DiagStatus::OK, "Preparation");
+    return;
+  }
+  if (std::abs(estimated_steer_offset_) > warn_steer_offset_abs_error_) {
+    stat.summary(DiagStatus::WARN, "Steer offset is larger than tolerance");
+    return;
+  }
+  stat.summary(DiagStatus::OK, "Calibration OK");
+}
+
+void SteerOffsetEstimatorNode::onTwist(const TwistStamped::ConstSharedPtr msg) { twist_ptr_ = msg; }
+
+void SteerOffsetEstimatorNode::onSteer(const Steering::ConstSharedPtr msg) { steer_ptr_ = msg; }
+
+bool SteerOffsetEstimatorNode::updateSteeringOffset()
+{
+  // RLS; recursive least-squares algorithm
+
+  if (!twist_ptr_ || !steer_ptr_) {
+    // null input
+    return false;
+  }
+
+  const double vel = twist_ptr_->twist.linear.x;
+  const double wz = twist_ptr_->twist.angular.z;
+  const double steer = steer_ptr_->steering_tire_angle;
+
+  if (std::abs(vel) < valid_min_velocity_ || std::abs(steer) > valid_max_steer_) {
+    // invalid velocity/steer value for estimating steering offset
+    return false;
+  }
+
+  // use following approximation: tan(a+b) = tan(a) + tan(b) = a + b
+
+  const double phi = vel / wheel_base_;
+  covariance_ = (covariance_ - (covariance_ * phi * phi * covariance_) /
+                                 (forgetting_factor_ + phi * covariance_ * phi)) /
+                forgetting_factor_;
+
+  const double coef = (covariance_ * phi) / (forgetting_factor_ + phi * covariance_ * phi);
+  const double measured_wz_offset = wz - phi * steer;
+  const double error_wz_offset = measured_wz_offset - phi * estimated_steer_offset_;
+
+  estimated_steer_offset_ = estimated_steer_offset_ + coef * error_wz_offset;
+
+  return true;
+}
+
+void SteerOffsetEstimatorNode::onTimer()
+{
+  if (updateSteeringOffset()) {
+    auto msg = std::make_unique<Float32Stamped>();
+    msg->stamp = this->now();
+    msg->data = estimated_steer_offset_;
+    pub_steer_offset_->publish(std::move(msg));
+
+    auto cov_msg = std::make_unique<Float32Stamped>();
+    cov_msg->stamp = this->now();
+    cov_msg->data = covariance_;
+    pub_steer_offset_cov_->publish(std::move(cov_msg));
+  }
+}
+}  // namespace steer_offset_estimator
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(steer_offset_estimator::SteerOffsetEstimatorNode)