feat(autoware_twist2accel): port autoware_twist2accel from Autoware Universe

localization/autoware_twist2accel/CMakeLists.txt

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

localization/autoware_twist2accel/README.md

@@ -0,0 +1,28 @@
+# autoware_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
+
+{{ json_to_markdown("localization/autoware_twist2accel/schema/twist2accel.schema.json") }}
+
+## Future work
+
+Future work includes integrating acceleration into the EKF state.

localization/autoware_twist2accel/config/twist2accel.param.yaml

@@ -0,0 +1,4 @@
+/**:
+  ros__parameters:
+    use_odom: true
+    accel_lowpass_gain: 0.9

localization/autoware_twist2accel/launch/twist2accel.launch.xml

@@ -0,0 +1,13 @@
+<launch>
+  <arg name="param_file" default="$(find-pkg-share autoware_twist2accel)/config/twist2accel.param.yaml"/>
+
+  <arg name="in_odom" default="in_odom"/>
+  <arg name="in_twist" default="in_twist"/>
+  <arg name="out_accel" default="out_accel"/>
+  <node pkg="autoware_twist2accel" exec="autoware_twist2accel_node" output="both">
+    <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 from="$(var param_file)"/>
+  </node>
+</launch>

localization/autoware_twist2accel/package.xml

@@ -0,0 +1,34 @@
+<?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>autoware_twist2accel</name>
+  <version>0.0.0</version>
+  <description>The acceleration estimation package</description>
+  <maintainer email="yamato.ando@tier4.jp">Yamato Ando</maintainer>
+  <maintainer email="masahiro.sakamoto@tier4.jp">Masahiro Sakamoto</maintainer>
+  <maintainer email="kento.yabuuchi.2@tier4.jp">Kento Yabuuchi</maintainer>
+  <maintainer email="anh.nguyen.2@tier4.jp">NGUYEN Viet Anh</maintainer>
+  <maintainer email="taiki.yamada@tier4.jp">Taiki Yamada</maintainer>
+  <maintainer email="shintaro.sakoda@tier4.jp">Shintaro Sakoda</maintainer>
+  <maintainer email="ryu.yamamoto@tier4.jp">Ryu Yamamoto</maintainer>
+  <license>Apache License 2.0</license>
+  <author>Koji Minoda</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>autoware_signal_processing</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>tf2</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/autoware_twist2accel/schema/twist2accel.schema.json

@@ -0,0 +1,36 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "Parameters for twist2accel Nodes",
+  "type": "object",
+  "definitions": {
+    "twist2accel": {
+      "type": "object",
+      "properties": {
+        "use_odom": {
+          "type": "boolean",
+          "default": true,
+          "description": "use odometry if true, else use twist input."
+        },
+        "accel_lowpass_gain": {
+          "type": "number",
+          "default": 0.9,
+          "minimum": 0.0,
+          "description": "lowpass gain for lowpass filter in estimating acceleration."
+        }
+      },
+      "required": ["use_odom", "accel_lowpass_gain"]
+    }
+  },
+  "properties": {
+    "/**": {
+      "type": "object",
+      "properties": {
+        "ros__parameters": {
+          "$ref": "#/definitions/twist2accel"
+        }
+      },
+      "required": ["ros__parameters"]
+    }
+  },
+  "required": ["/**"]
+}

localization/autoware_twist2accel/src/twist2accel.cpp

@@ -0,0 +1,113 @@
+// 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.hpp"
+
+#include <rclcpp/logging.hpp>
+
+#include <algorithm>
+#include <functional>
+#include <memory>
+#include <string>
+#include <utility>
+
+using autoware::signal_processing::LowpassFilter1d;
+
+namespace autoware::twist2accel
+{
+using std::placeholders::_1;
+
+Twist2Accel::Twist2Accel(const rclcpp::NodeOptions & node_options)
+: rclcpp::Node("twist2accel", node_options)
+{
+  sub_odom_ = create_subscription<nav_msgs::msg::Odometry>(
+    "input/odom", 1, std::bind(&Twist2Accel::callback_odometry, this, _1));
+  sub_twist_ = create_subscription<geometry_msgs::msg::TwistWithCovarianceStamped>(
+    "input/twist", 1, std::bind(&Twist2Accel::callback_twist_with_covariance, this, _1));
+
+  pub_accel_ = create_publisher<geometry_msgs::msg::AccelWithCovarianceStamped>("output/accel", 1);
+
+  prev_twist_ptr_ = nullptr;
+  accel_lowpass_gain_ = declare_parameter<double>("accel_lowpass_gain");
+  use_odom_ = declare_parameter<bool>("use_odom");
+
+  lpf_alx_ptr_ = std::make_shared<LowpassFilter1d>(accel_lowpass_gain_);
+  lpf_aly_ptr_ = std::make_shared<LowpassFilter1d>(accel_lowpass_gain_);
+  lpf_alz_ptr_ = std::make_shared<LowpassFilter1d>(accel_lowpass_gain_);
+  lpf_aax_ptr_ = std::make_shared<LowpassFilter1d>(accel_lowpass_gain_);
+  lpf_aay_ptr_ = std::make_shared<LowpassFilter1d>(accel_lowpass_gain_);
+  lpf_aaz_ptr_ = std::make_shared<LowpassFilter1d>(accel_lowpass_gain_);
+}
+
+void Twist2Accel::callback_odometry(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;
+  estimate_accel(std::make_shared<geometry_msgs::msg::TwistStamped>(twist));
+}
+
+void Twist2Accel::callback_twist_with_covariance(
+  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;
+  estimate_accel(std::make_shared<geometry_msgs::msg::TwistStamped>(twist));
+}
+
+void Twist2Accel::estimate_accel(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;
+}
+}  // namespace autoware::twist2accel
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(autoware::twist2accel::Twist2Accel)

localization/autoware_twist2accel/src/twist2accel.hpp

@@ -0,0 +1,75 @@
+// 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_HPP_
+#define TWIST2ACCEL_HPP_
+
+#include "autoware/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 <tf2/LinearMath/Quaternion.h>
+#include <tf2/utils.h>
+
+#include <chrono>
+#include <fstream>
+#include <iostream>
+#include <memory>
+#include <mutex>
+#include <queue>
+#include <string>
+#include <vector>
+
+using autoware::signal_processing::LowpassFilter1d;
+
+namespace autoware::twist2accel
+{
+class Twist2Accel : public rclcpp::Node
+{
+public:
+  explicit Twist2Accel(const rclcpp::NodeOptions & node_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 callback_twist_with_covariance(
+    const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg);
+  void callback_odometry(const nav_msgs::msg::Odometry::SharedPtr msg);
+  void estimate_accel(const geometry_msgs::msg::TwistStamped::SharedPtr msg);
+};
+}  // namespace autoware::twist2accel
+#endif  // TWIST2ACCEL_HPP_