From fa102d30d305e8a62fbaed5bc5a42e7e604742c8 Mon Sep 17 00:00:00 2001 From: mitsudome-r Date: Tue, 25 Mar 2025 14:51:34 +0900 Subject: [PATCH] feat(autoware_twist2accel)!: port autoware_twist2accel to Autoware Core Signed-off-by: mitsudome-r --- .../autoware_twist2accel/CHANGELOG.rst | 82 ------------- .../autoware_twist2accel/CMakeLists.txt | 21 ---- localization/autoware_twist2accel/README.md | 28 ----- .../config/twist2accel.param.yaml | 4 - .../launch/twist2accel.launch.xml | 13 -- localization/autoware_twist2accel/package.xml | 34 ------ .../schema/twist2accel.schema.json | 36 ------ .../autoware_twist2accel/src/twist2accel.cpp | 113 ------------------ .../autoware_twist2accel/src/twist2accel.hpp | 75 ------------ 9 files changed, 406 deletions(-) delete mode 100644 localization/autoware_twist2accel/CHANGELOG.rst delete mode 100644 localization/autoware_twist2accel/CMakeLists.txt delete mode 100644 localization/autoware_twist2accel/README.md delete mode 100644 localization/autoware_twist2accel/config/twist2accel.param.yaml delete mode 100644 localization/autoware_twist2accel/launch/twist2accel.launch.xml delete mode 100644 localization/autoware_twist2accel/package.xml delete mode 100644 localization/autoware_twist2accel/schema/twist2accel.schema.json delete mode 100644 localization/autoware_twist2accel/src/twist2accel.cpp delete mode 100644 localization/autoware_twist2accel/src/twist2accel.hpp diff --git a/localization/autoware_twist2accel/CHANGELOG.rst b/localization/autoware_twist2accel/CHANGELOG.rst deleted file mode 100644 index 65f4ce35a219e..0000000000000 --- a/localization/autoware_twist2accel/CHANGELOG.rst +++ /dev/null @@ -1,82 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_twist2accel -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.43.0 (2025-03-21) -------------------- -* Merge remote-tracking branch 'origin/main' into chore/bump-version-0.43 -* chore: rename from `autoware.universe` to `autoware_universe` (`#10306 `_) -* Contributors: Hayato Mizushima, Yutaka Kondo - -0.42.0 (2025-03-03) -------------------- - -0.41.2 (2025-02-19) -------------------- -* chore: bump version to 0.41.1 (`#10088 `_) -* Contributors: Ryohsuke Mitsudome - -0.41.1 (2025-02-10) -------------------- - -0.41.0 (2025-01-29) -------------------- -* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base -* chore(autoware_twist2accel): remove an unused dependency (`#9881 `_) - Removed an unused dependency -* feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files localization/autoware_twist2accel (`#9868 `_) - Co-authored-by: SakodaShintaro -* Contributors: Fumiya Watanabe, SakodaShintaro, Vishal Chauhan - -0.40.0 (2024-12-12) -------------------- -* Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" - This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5. -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* chore(package.xml): bump version to 0.39.0 (`#9587 `_) - * chore(package.xml): bump version to 0.39.0 - * fix: fix ticket links in CHANGELOG.rst - * fix: remove unnecessary diff - --------- - Co-authored-by: Yutaka Kondo -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* 0.39.0 -* update changelog -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware_universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware_universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Fumiya Watanabe, Ryohsuke Mitsudome, Yutaka Kondo - -0.39.0 (2024-11-25) -------------------- -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware_universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware_universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.38.0 (2024-11-08) -------------------- -* unify package.xml version to 0.37.0 -* refactor(signal_processing): prefix package and namespace with autoware (`#8541 `_) -* fix(autoware_twist2accel): fix funcArgNamesDifferent (`#8391 `_) - fix:funcArgNamesDifferent -* refactor(twist2accel)!: prefix package and namespace with autoware (`#8299 `_) - * add autoware\_ prefix - * add autoware\_ prefix - * add autoware\_ prefix - --------- - Co-authored-by: SakodaShintaro -* Contributors: Esteve Fernandez, Masaki Baba, Yutaka Kondo, kobayu858 - -0.26.0 (2024-04-03) -------------------- diff --git a/localization/autoware_twist2accel/CMakeLists.txt b/localization/autoware_twist2accel/CMakeLists.txt deleted file mode 100644 index c5e3aadfb7fe0..0000000000000 --- a/localization/autoware_twist2accel/CMakeLists.txt +++ /dev/null @@ -1,21 +0,0 @@ -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 -) diff --git a/localization/autoware_twist2accel/README.md b/localization/autoware_twist2accel/README.md deleted file mode 100644 index 4bdf29947e0e7..0000000000000 --- a/localization/autoware_twist2accel/README.md +++ /dev/null @@ -1,28 +0,0 @@ -# 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. diff --git a/localization/autoware_twist2accel/config/twist2accel.param.yaml b/localization/autoware_twist2accel/config/twist2accel.param.yaml deleted file mode 100644 index e58e029248253..0000000000000 --- a/localization/autoware_twist2accel/config/twist2accel.param.yaml +++ /dev/null @@ -1,4 +0,0 @@ -/**: - ros__parameters: - use_odom: true - accel_lowpass_gain: 0.9 diff --git a/localization/autoware_twist2accel/launch/twist2accel.launch.xml b/localization/autoware_twist2accel/launch/twist2accel.launch.xml deleted file mode 100644 index 6fe10a8c80c32..0000000000000 --- a/localization/autoware_twist2accel/launch/twist2accel.launch.xml +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - diff --git a/localization/autoware_twist2accel/package.xml b/localization/autoware_twist2accel/package.xml deleted file mode 100644 index 99bbd58b1bd83..0000000000000 --- a/localization/autoware_twist2accel/package.xml +++ /dev/null @@ -1,34 +0,0 @@ - - - - autoware_twist2accel - 0.43.0 - The acceleration estimation package - Yamato Ando - Masahiro Sakamoto - Kento Yabuuchi - NGUYEN Viet Anh - Taiki Yamada - Shintaro Sakoda - Ryu Yamamoto - Apache License 2.0 - Koji Minoda - - ament_cmake_auto - autoware_cmake - - autoware_signal_processing - geometry_msgs - nav_msgs - rclcpp - rclcpp_components - tf2 - - ament_cmake_ros - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/localization/autoware_twist2accel/schema/twist2accel.schema.json b/localization/autoware_twist2accel/schema/twist2accel.schema.json deleted file mode 100644 index 6b3c2bd094166..0000000000000 --- a/localization/autoware_twist2accel/schema/twist2accel.schema.json +++ /dev/null @@ -1,36 +0,0 @@ -{ - "$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": ["/**"] -} diff --git a/localization/autoware_twist2accel/src/twist2accel.cpp b/localization/autoware_twist2accel/src/twist2accel.cpp deleted file mode 100644 index 0f904132f6cdc..0000000000000 --- a/localization/autoware_twist2accel/src/twist2accel.cpp +++ /dev/null @@ -1,113 +0,0 @@ -// 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 - -#include -#include -#include -#include -#include - -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( - "input/odom", 1, std::bind(&Twist2Accel::callback_odometry, this, _1)); - sub_twist_ = create_subscription( - "input/twist", 1, std::bind(&Twist2Accel::callback_twist_with_covariance, this, _1)); - - pub_accel_ = create_publisher("output/accel", 1); - - prev_twist_ptr_ = nullptr; - accel_lowpass_gain_ = declare_parameter("accel_lowpass_gain"); - use_odom_ = declare_parameter("use_odom"); - - lpf_alx_ptr_ = std::make_shared(accel_lowpass_gain_); - lpf_aly_ptr_ = std::make_shared(accel_lowpass_gain_); - lpf_alz_ptr_ = std::make_shared(accel_lowpass_gain_); - lpf_aax_ptr_ = std::make_shared(accel_lowpass_gain_); - lpf_aay_ptr_ = std::make_shared(accel_lowpass_gain_); - lpf_aaz_ptr_ = std::make_shared(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(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(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(autoware::twist2accel::Twist2Accel) diff --git a/localization/autoware_twist2accel/src/twist2accel.hpp b/localization/autoware_twist2accel/src/twist2accel.hpp deleted file mode 100644 index 48bc03a326091..0000000000000 --- a/localization/autoware_twist2accel/src/twist2accel.hpp +++ /dev/null @@ -1,75 +0,0 @@ -// 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 - -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -using autoware::signal_processing::LowpassFilter1d; - -namespace autoware::twist2accel -{ -class Twist2Accel : public rclcpp::Node -{ -public: - explicit Twist2Accel(const rclcpp::NodeOptions & node_options); - -private: - rclcpp::Publisher::SharedPtr - pub_accel_; //!< @brief stop flag publisher - rclcpp::Subscription::SharedPtr - sub_odom_; //!< @brief measurement odometry subscriber - rclcpp::Subscription::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 lpf_aax_ptr_; - std::shared_ptr lpf_aay_ptr_; - std::shared_ptr lpf_aaz_ptr_; - std::shared_ptr lpf_alx_ptr_; - std::shared_ptr lpf_aly_ptr_; - std::shared_ptr 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_