feat(twist2accel)!: add new package for estimating acceleration in lo…

…calization module (#1089)

* first commit

Signed-off-by: kminoda <[email protected]>

* update launch arg names

Signed-off-by: kminoda <[email protected]>

* use lowpassfilter in signalprocessing

Signed-off-by: kminoda <[email protected]>

* fixed

Signed-off-by: kminoda <[email protected]>

* add acceleration estimation

Signed-off-by: kminoda <[email protected]>

* ci(pre-commit): autofix

* fix readme and lisence

Signed-off-by: kminoda <[email protected]>

* ci(pre-commit): autofix

* fix readme

Signed-off-by: kminoda <[email protected]>

* ci(pre-commit): autofix

* added covariance values

Signed-off-by: kminoda <[email protected]>

* removed unnecessary variable

Signed-off-by: kminoda <[email protected]>

* rename acceleration_estimator -> twist2accel

Signed-off-by: kminoda <[email protected]>

* ci(pre-commit): autofix

* added future work

Signed-off-by: kminoda <[email protected]>

* ci(pre-commit): autofix

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

launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml

@@ -27,4 +27,10 @@
     <arg name="input_twist_with_covariance_name" value="/localization/pose_twist_fusion_filter/twist_with_covariance"/>
     <arg name="output_odom_name" value="/localization/kinematic_state"/>
   </include>
+  <include file="$(find-pkg-share twist2accel)/launch/twist2accel.launch.xml">
+    <arg name="use_odom" value="true"/>
+    <arg name="in_odom" value="/localization/kinematic_state"/>
+    <arg name="in_twist" value="/localization/twist_estimator/twist_with_covariance"/>
+    <arg name="out_accel" value="/localization/acceleration"/>
+  </include>
 </launch>

localization/twist2accel/CMakeLists.txt

@@ -0,0 +1,16 @@
+cmake_minimum_required(VERSION 3.14)
+project(twist2accel)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_executable(twist2accel
+  src/twist2accel_node.cpp
+  src/twist2accel.cpp
+)
+ament_target_dependencies(twist2accel)
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  launch
+)

localization/twist2accel/README.md

@@ -0,0 +1,31 @@
+# twist2accel
+
+## Purpose
+
+This package is responsible for estimating acceleration using the output of `ekf_localizer`. It uses lowpass filter to mitigate the noise.
+
+## Inputs / Outputs
+
+### Input
+
+| Name          | Type                                             | Description           |
+| ------------- | ------------------------------------------------ | --------------------- |
+| `input/odom`  | `nav_msgs::msg::Odometry`                        | localization odometry |
+| `input/twist` | `geometry_msgs::msg::TwistWithCovarianceStamped` | twist                 |
+
+### Output
+
+| Name           | Type                                             | Description            |
+| -------------- | ------------------------------------------------ | ---------------------- |
+| `output/accel` | `geometry_msgs::msg::AccelWithCovarianceStamped` | estimated acceleration |
+
+## Parameters
+
+| Name                 | Type   | Description                                                               |
+| -------------------- | ------ | ------------------------------------------------------------------------- |
+| `use_odom`           | bool   | use odometry if true, else use twist input (default: true)                |
+| `accel_lowpass_gain` | double | lowpass gain for lowpass filter in estimating acceleration (default: 0.5) |
+
+## Future work
+
+Future work includes integrating acceleration into the EKF state.

localization/twist2accel/include/twist2accel/twist2accel.hpp

@@ -0,0 +1,71 @@
+// Copyright 2022 TIER IV
+//
+// 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 TWIST2ACCEL__TWIST2ACCEL_HPP_
+#define TWIST2ACCEL__TWIST2ACCEL_HPP_
+
+#include "signal_processing/lowpass_filter_1d.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <geometry_msgs/msg/accel_with_covariance_stamped.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
+#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <tier4_debug_msgs/msg/bool_stamped.hpp>
+
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2/utils.h>
+
+#include <chrono>
+#include <fstream>
+#include <iostream>
+#include <memory>
+#include <mutex>
+#include <queue>
+#include <string>
+#include <vector>
+
+class Twist2Accel : public rclcpp::Node
+{
+public:
+  Twist2Accel(const std::string & node_name, const rclcpp::NodeOptions & options);
+
+private:
+  rclcpp::Publisher<geometry_msgs::msg::AccelWithCovarianceStamped>::SharedPtr
+    pub_accel_;  //!< @brief stop flag publisher
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr
+    sub_odom_;  //!< @brief measurement odometry subscriber
+  rclcpp::Subscription<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr
+    sub_twist_;  //!< @brief measurement odometry subscriber
+
+  geometry_msgs::msg::TwistStamped::SharedPtr prev_twist_ptr_;
+  double accel_lowpass_gain_;
+  bool use_odom_;
+  std::shared_ptr<LowpassFilter1d> lpf_aax_ptr_;
+  std::shared_ptr<LowpassFilter1d> lpf_aay_ptr_;
+  std::shared_ptr<LowpassFilter1d> lpf_aaz_ptr_;
+  std::shared_ptr<LowpassFilter1d> lpf_alx_ptr_;
+  std::shared_ptr<LowpassFilter1d> lpf_aly_ptr_;
+  std::shared_ptr<LowpassFilter1d> lpf_alz_ptr_;
+
+  /**
+   * @brief set odometry measurement
+   */
+  void callbackTwistWithCovariance(
+    const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg);
+  void callbackOdometry(const nav_msgs::msg::Odometry::SharedPtr msg);
+  void estimateAccel(const geometry_msgs::msg::TwistStamped::SharedPtr msg);
+};
+#endif  // TWIST2ACCEL__TWIST2ACCEL_HPP_

localization/twist2accel/launch/twist2accel.launch.xml

@@ -0,0 +1,14 @@
+<launch>
+  <arg name="use_odom" default="true"/>
+  <arg name="accel_lowpass_gain" default="0.5"/>
+  <arg name="in_odom" default="in_odom"/>
+  <arg name="in_twist" default="in_twist"/>
+  <arg name="out_accel" default="out_accel"/>
+  <node pkg="twist2accel" exec="twist2accel" name="twist2accel" output="screen">
+    <remap from="input/odom" to="$(var in_odom)"/>
+    <remap from="input/twist" to="$(var in_twist)"/>
+    <remap from="output/accel" to="$(var out_accel)"/>
+    <param name="accel_lowpass_gain" value="$(var accel_lowpass_gain)"/>
+    <param name="use_odom" value="$(var use_odom)"/>
+  </node>
+</launch>

localization/twist2accel/package.xml

@@ -0,0 +1,28 @@
+<?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>twist2accel</name>
+  <version>0.0.0</version>
+  <description>The acceleration estimation package</description>
+  <maintainer email="[email protected]">Koji Minoda</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <build_depend>autoware_cmake</build_depend>
+
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>signal_processing</depend>
+  <depend>tf2</depend>
+  <depend>tier4_debug_msgs</depend>
+
+  <test_depend>ament_cmake_ros</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

localization/twist2accel/src/twist2accel.cpp

@@ -0,0 +1,111 @@
+// Copyright 2022 TIER IV
+//
+// 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 "twist2accel/twist2accel.hpp"
+
+#include <rclcpp/logging.hpp>
+
+#include <algorithm>
+#include <functional>
+#include <memory>
+#include <string>
+#include <utility>
+
+// clang-format off
+#define PRINT_MAT(X) std::cout << #X << ":\n" << X << std::endl << std::endl
+#define DEBUG_INFO(...) {if (show_debug_info_) {RCLCPP_INFO(__VA_ARGS__);}}
+#define DEBUG_PRINT_MAT(X) {if (show_debug_info_) {std::cout << #X << ": " << X << std::endl;}}
+
+// clang-format on
+using std::placeholders::_1;
+
+Twist2Accel::Twist2Accel(const std::string & node_name, const rclcpp::NodeOptions & node_options)
+: rclcpp::Node(node_name, node_options)
+{
+  sub_odom_ = create_subscription<nav_msgs::msg::Odometry>(
+    "input/odom", 1, std::bind(&Twist2Accel::callbackOdometry, this, _1));
+  sub_twist_ = create_subscription<geometry_msgs::msg::TwistWithCovarianceStamped>(
+    "input/twist", 1, std::bind(&Twist2Accel::callbackTwistWithCovariance, this, _1));
+
+  pub_accel_ = create_publisher<geometry_msgs::msg::AccelWithCovarianceStamped>("output/accel", 1);
+
+  prev_twist_ptr_ = nullptr;
+  accel_lowpass_gain_ = declare_parameter("accel_lowpass_gain", 0.5);
+  use_odom_ = declare_parameter("use_odom", true);
+
+  lpf_alx_ptr_ = std::make_shared<LowpassFilter1d>(0.0, accel_lowpass_gain_);
+  lpf_aly_ptr_ = std::make_shared<LowpassFilter1d>(0.0, accel_lowpass_gain_);
+  lpf_alz_ptr_ = std::make_shared<LowpassFilter1d>(0.0, accel_lowpass_gain_);
+  lpf_aax_ptr_ = std::make_shared<LowpassFilter1d>(0.0, accel_lowpass_gain_);
+  lpf_aay_ptr_ = std::make_shared<LowpassFilter1d>(0.0, accel_lowpass_gain_);
+  lpf_aaz_ptr_ = std::make_shared<LowpassFilter1d>(0.0, accel_lowpass_gain_);
+}
+
+void Twist2Accel::callbackOdometry(const nav_msgs::msg::Odometry::SharedPtr msg)
+{
+  if (!use_odom_) return;
+
+  geometry_msgs::msg::TwistStamped twist;
+  twist.header = msg->header;
+  twist.twist = msg->twist.twist;
+  estimateAccel(std::make_shared<geometry_msgs::msg::TwistStamped>(twist));
+}
+
+void Twist2Accel::callbackTwistWithCovariance(
+  const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg)
+{
+  if (use_odom_) return;
+
+  geometry_msgs::msg::TwistStamped twist;
+  twist.header = msg->header;
+  twist.twist = msg->twist.twist;
+  estimateAccel(std::make_shared<geometry_msgs::msg::TwistStamped>(twist));
+}
+
+void Twist2Accel::estimateAccel(const geometry_msgs::msg::TwistStamped::SharedPtr msg)
+{
+  geometry_msgs::msg::AccelWithCovarianceStamped accel_msg;
+  accel_msg.header = msg->header;
+
+  if (prev_twist_ptr_ != nullptr) {
+    const double dt = std::max(
+      (rclcpp::Time(msg->header.stamp) - rclcpp::Time(prev_twist_ptr_->header.stamp)).seconds(),
+      1.0e-3);
+
+    double alx = (msg->twist.linear.x - prev_twist_ptr_->twist.linear.x) / dt;
+    double aly = (msg->twist.linear.y - prev_twist_ptr_->twist.linear.y) / dt;
+    double alz = (msg->twist.linear.z - prev_twist_ptr_->twist.linear.z) / dt;
+    double aax = (msg->twist.angular.x - prev_twist_ptr_->twist.angular.x) / dt;
+    double aay = (msg->twist.angular.y - prev_twist_ptr_->twist.angular.y) / dt;
+    double aaz = (msg->twist.angular.z - prev_twist_ptr_->twist.angular.z) / dt;
+
+    accel_msg.accel.accel.linear.x = lpf_alx_ptr_->filter(alx);
+    accel_msg.accel.accel.linear.y = lpf_aly_ptr_->filter(aly);
+    accel_msg.accel.accel.linear.z = lpf_alz_ptr_->filter(alz);
+    accel_msg.accel.accel.angular.x = lpf_aax_ptr_->filter(aax);
+    accel_msg.accel.accel.angular.y = lpf_aay_ptr_->filter(aay);
+    accel_msg.accel.accel.angular.z = lpf_aaz_ptr_->filter(aaz);
+
+    // Ideally speaking, these covariance should be properly estimated.
+    accel_msg.accel.covariance[0 * 6 + 0] = 1.0;
+    accel_msg.accel.covariance[1 * 6 + 1] = 1.0;
+    accel_msg.accel.covariance[2 * 6 + 2] = 1.0;
+    accel_msg.accel.covariance[3 * 6 + 3] = 0.05;
+    accel_msg.accel.covariance[4 * 6 + 4] = 0.05;
+    accel_msg.accel.covariance[5 * 6 + 5] = 0.05;
+  }
+
+  pub_accel_->publish(accel_msg);
+  prev_twist_ptr_ = msg;
+}

localization/twist2accel/src/twist2accel_node.cpp

@@ -0,0 +1,28 @@
+// Copyright 2022 TIER IV
+//
+// 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 "twist2accel/twist2accel.hpp"
+
+#include <memory>
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::NodeOptions node_options;
+  auto node = std::make_shared<Twist2Accel>("twist2accel", node_options);
+
+  rclcpp::spin(node);
+
+  return 0;
+}