diff --git a/localization/autoware_twist2accel/CMakeLists.txt b/localization/autoware_twist2accel/CMakeLists.txt new file mode 100644 index 0000000000..c5e3aadfb7 --- /dev/null +++ b/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 +) diff --git a/localization/autoware_twist2accel/README.md b/localization/autoware_twist2accel/README.md new file mode 100644 index 0000000000..4bdf29947e --- /dev/null +++ b/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. diff --git a/localization/autoware_twist2accel/config/twist2accel.param.yaml b/localization/autoware_twist2accel/config/twist2accel.param.yaml new file mode 100644 index 0000000000..e58e029248 --- /dev/null +++ b/localization/autoware_twist2accel/config/twist2accel.param.yaml @@ -0,0 +1,4 @@ +/**: + 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 new file mode 100644 index 0000000000..6fe10a8c80 --- /dev/null +++ b/localization/autoware_twist2accel/launch/twist2accel.launch.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/localization/autoware_twist2accel/package.xml b/localization/autoware_twist2accel/package.xml new file mode 100644 index 0000000000..47f4468c5f --- /dev/null +++ b/localization/autoware_twist2accel/package.xml @@ -0,0 +1,34 @@ + + + + autoware_twist2accel + 0.0.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 new file mode 100644 index 0000000000..6b3c2bd094 --- /dev/null +++ b/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": ["/**"] +} diff --git a/localization/autoware_twist2accel/src/twist2accel.cpp b/localization/autoware_twist2accel/src/twist2accel.cpp new file mode 100644 index 0000000000..0f904132f6 --- /dev/null +++ b/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 + +#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 new file mode 100644 index 0000000000..48bc03a326 --- /dev/null +++ b/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 + +#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_