Skip to content

Commit 61d0716

Browse files
mitsudome-rmitsudome-r
authored
feat(autoware_twist2accel): port autoware_twist2accel from Autoware Universe (#302)
Signed-off-by: Ryohsuke Mitsudome <ryohsuke.mitsudome@tier4.jp>
1 parent 0de9080 commit 61d0716

File tree

8 files changed

+324
-0
lines changed

8 files changed

+324
-0
lines changed

‎localization/autoware_twist2accel/CMakeLists.txt

+21
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
1+
cmake_minimum_required(VERSION 3.14)
2+
project(autoware_twist2accel)
3+
4+
find_package(autoware_cmake REQUIRED)
5+
autoware_package()
6+
7+
ament_auto_add_library(${PROJECT_NAME} SHARED
8+
src/twist2accel.cpp
9+
)
10+
11+
rclcpp_components_register_node(${PROJECT_NAME}
12+
PLUGIN "autoware::twist2accel::Twist2Accel"
13+
EXECUTABLE ${PROJECT_NAME}_node
14+
EXECUTOR SingleThreadedExecutor
15+
)
16+
17+
ament_auto_package(
18+
INSTALL_TO_SHARE
19+
launch
20+
config
21+
)

‎localization/autoware_twist2accel/README.md

+28
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,28 @@
1+
# autoware_twist2accel
2+
3+
## Purpose
4+
5+
This package is responsible for estimating acceleration using the output of `ekf_localizer`. It uses lowpass filter to mitigate the noise.
6+
7+
## Inputs / Outputs
8+
9+
### Input
10+
11+
| Name | Type | Description |
12+
| ------------- | ------------------------------------------------ | --------------------- |
13+
| `input/odom` | `nav_msgs::msg::Odometry` | localization odometry |
14+
| `input/twist` | `geometry_msgs::msg::TwistWithCovarianceStamped` | twist |
15+
16+
### Output
17+
18+
| Name | Type | Description |
19+
| -------------- | ------------------------------------------------ | ---------------------- |
20+
| `output/accel` | `geometry_msgs::msg::AccelWithCovarianceStamped` | estimated acceleration |
21+
22+
## Parameters
23+
24+
{{ json_to_markdown("localization/autoware_twist2accel/schema/twist2accel.schema.json") }}
25+
26+
## Future work
27+
28+
Future work includes integrating acceleration into the EKF state.

‎localization/autoware_twist2accel/config/twist2accel.param.yaml

+4
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
/**:
2+
ros__parameters:
3+
use_odom: true
4+
accel_lowpass_gain: 0.9

‎localization/autoware_twist2accel/launch/twist2accel.launch.xml

+13
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,13 @@
1+
<launch>
2+
<arg name="param_file" default="$(find-pkg-share autoware_twist2accel)/config/twist2accel.param.yaml"/>
3+
4+
<arg name="in_odom" default="in_odom"/>
5+
<arg name="in_twist" default="in_twist"/>
6+
<arg name="out_accel" default="out_accel"/>
7+
<node pkg="autoware_twist2accel" exec="autoware_twist2accel_node" output="both">
8+
<remap from="input/odom" to="$(var in_odom)"/>
9+
<remap from="input/twist" to="$(var in_twist)"/>
10+
<remap from="output/accel" to="$(var out_accel)"/>
11+
<param from="$(var param_file)"/>
12+
</node>
13+
</launch>

‎localization/autoware_twist2accel/package.xml

+34
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>autoware_twist2accel</name>
5+
<version>0.0.0</version>
6+
<description>The acceleration estimation package</description>
7+
<maintainer email="yamato.ando@tier4.jp">Yamato Ando</maintainer>
8+
<maintainer email="masahiro.sakamoto@tier4.jp">Masahiro Sakamoto</maintainer>
9+
<maintainer email="kento.yabuuchi.2@tier4.jp">Kento Yabuuchi</maintainer>
10+
<maintainer email="anh.nguyen.2@tier4.jp">NGUYEN Viet Anh</maintainer>
11+
<maintainer email="taiki.yamada@tier4.jp">Taiki Yamada</maintainer>
12+
<maintainer email="shintaro.sakoda@tier4.jp">Shintaro Sakoda</maintainer>
13+
<maintainer email="ryu.yamamoto@tier4.jp">Ryu Yamamoto</maintainer>
14+
<license>Apache License 2.0</license>
15+
<author>Koji Minoda</author>
16+
17+
<buildtool_depend>ament_cmake_auto</buildtool_depend>
18+
<buildtool_depend>autoware_cmake</buildtool_depend>
19+
20+
<depend>autoware_signal_processing</depend>
21+
<depend>geometry_msgs</depend>
22+
<depend>nav_msgs</depend>
23+
<depend>rclcpp</depend>
24+
<depend>rclcpp_components</depend>
25+
<depend>tf2</depend>
26+
27+
<test_depend>ament_cmake_ros</test_depend>
28+
<test_depend>ament_lint_auto</test_depend>
29+
<test_depend>autoware_lint_common</test_depend>
30+
31+
<export>
32+
<build_type>ament_cmake</build_type>
33+
</export>
34+
</package>

‎localization/autoware_twist2accel/schema/twist2accel.schema.json

+36
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,36 @@
1+
{
2+
"$schema": "http://json-schema.org/draft-07/schema#",
3+
"title": "Parameters for twist2accel Nodes",
4+
"type": "object",
5+
"definitions": {
6+
"twist2accel": {
7+
"type": "object",
8+
"properties": {
9+
"use_odom": {
10+
"type": "boolean",
11+
"default": true,
12+
"description": "use odometry if true, else use twist input."
13+
},
14+
"accel_lowpass_gain": {
15+
"type": "number",
16+
"default": 0.9,
17+
"minimum": 0.0,
18+
"description": "lowpass gain for lowpass filter in estimating acceleration."
19+
}
20+
},
21+
"required": ["use_odom", "accel_lowpass_gain"]
22+
}
23+
},
24+
"properties": {
25+
"/**": {
26+
"type": "object",
27+
"properties": {
28+
"ros__parameters": {
29+
"$ref": "#/definitions/twist2accel"
30+
}
31+
},
32+
"required": ["ros__parameters"]
33+
}
34+
},
35+
"required": ["/**"]
36+
}

‎localization/autoware_twist2accel/src/twist2accel.cpp

+113
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,113 @@
1+
// Copyright 2022 TIER IV
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include "twist2accel.hpp"
16+
17+
#include <rclcpp/logging.hpp>
18+
19+
#include <algorithm>
20+
#include <functional>
21+
#include <memory>
22+
#include <string>
23+
#include <utility>
24+
25+
using autoware::signal_processing::LowpassFilter1d;
26+
27+
namespace autoware::twist2accel
28+
{
29+
using std::placeholders::_1;
30+
31+
Twist2Accel::Twist2Accel(const rclcpp::NodeOptions & node_options)
32+
: rclcpp::Node("twist2accel", node_options)
33+
{
34+
sub_odom_ = create_subscription<nav_msgs::msg::Odometry>(
35+
"input/odom", 1, std::bind(&Twist2Accel::callback_odometry, this, _1));
36+
sub_twist_ = create_subscription<geometry_msgs::msg::TwistWithCovarianceStamped>(
37+
"input/twist", 1, std::bind(&Twist2Accel::callback_twist_with_covariance, this, _1));
38+
39+
pub_accel_ = create_publisher<geometry_msgs::msg::AccelWithCovarianceStamped>("output/accel", 1);
40+
41+
prev_twist_ptr_ = nullptr;
42+
accel_lowpass_gain_ = declare_parameter<double>("accel_lowpass_gain");
43+
use_odom_ = declare_parameter<bool>("use_odom");
44+
45+
lpf_alx_ptr_ = std::make_shared<LowpassFilter1d>(accel_lowpass_gain_);
46+
lpf_aly_ptr_ = std::make_shared<LowpassFilter1d>(accel_lowpass_gain_);
47+
lpf_alz_ptr_ = std::make_shared<LowpassFilter1d>(accel_lowpass_gain_);
48+
lpf_aax_ptr_ = std::make_shared<LowpassFilter1d>(accel_lowpass_gain_);
49+
lpf_aay_ptr_ = std::make_shared<LowpassFilter1d>(accel_lowpass_gain_);
50+
lpf_aaz_ptr_ = std::make_shared<LowpassFilter1d>(accel_lowpass_gain_);
51+
}
52+
53+
void Twist2Accel::callback_odometry(const nav_msgs::msg::Odometry::SharedPtr msg)
54+
{
55+
if (!use_odom_) return;
56+
57+
geometry_msgs::msg::TwistStamped twist;
58+
twist.header = msg->header;
59+
twist.twist = msg->twist.twist;
60+
estimate_accel(std::make_shared<geometry_msgs::msg::TwistStamped>(twist));
61+
}
62+
63+
void Twist2Accel::callback_twist_with_covariance(
64+
const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg)
65+
{
66+
if (use_odom_) return;
67+
68+
geometry_msgs::msg::TwistStamped twist;
69+
twist.header = msg->header;
70+
twist.twist = msg->twist.twist;
71+
estimate_accel(std::make_shared<geometry_msgs::msg::TwistStamped>(twist));
72+
}
73+
74+
void Twist2Accel::estimate_accel(const geometry_msgs::msg::TwistStamped::SharedPtr msg)
75+
{
76+
geometry_msgs::msg::AccelWithCovarianceStamped accel_msg;
77+
accel_msg.header = msg->header;
78+
79+
if (prev_twist_ptr_ != nullptr) {
80+
const double dt = std::max(
81+
(rclcpp::Time(msg->header.stamp) - rclcpp::Time(prev_twist_ptr_->header.stamp)).seconds(),
82+
1.0e-3);
83+
84+
double alx = (msg->twist.linear.x - prev_twist_ptr_->twist.linear.x) / dt;
85+
double aly = (msg->twist.linear.y - prev_twist_ptr_->twist.linear.y) / dt;
86+
double alz = (msg->twist.linear.z - prev_twist_ptr_->twist.linear.z) / dt;
87+
double aax = (msg->twist.angular.x - prev_twist_ptr_->twist.angular.x) / dt;
88+
double aay = (msg->twist.angular.y - prev_twist_ptr_->twist.angular.y) / dt;
89+
double aaz = (msg->twist.angular.z - prev_twist_ptr_->twist.angular.z) / dt;
90+
91+
accel_msg.accel.accel.linear.x = lpf_alx_ptr_->filter(alx);
92+
accel_msg.accel.accel.linear.y = lpf_aly_ptr_->filter(aly);
93+
accel_msg.accel.accel.linear.z = lpf_alz_ptr_->filter(alz);
94+
accel_msg.accel.accel.angular.x = lpf_aax_ptr_->filter(aax);
95+
accel_msg.accel.accel.angular.y = lpf_aay_ptr_->filter(aay);
96+
accel_msg.accel.accel.angular.z = lpf_aaz_ptr_->filter(aaz);
97+
98+
// Ideally speaking, these covariance should be properly estimated.
99+
accel_msg.accel.covariance[0 * 6 + 0] = 1.0;
100+
accel_msg.accel.covariance[1 * 6 + 1] = 1.0;
101+
accel_msg.accel.covariance[2 * 6 + 2] = 1.0;
102+
accel_msg.accel.covariance[3 * 6 + 3] = 0.05;
103+
accel_msg.accel.covariance[4 * 6 + 4] = 0.05;
104+
accel_msg.accel.covariance[5 * 6 + 5] = 0.05;
105+
}
106+
107+
pub_accel_->publish(accel_msg);
108+
prev_twist_ptr_ = msg;
109+
}
110+
} // namespace autoware::twist2accel
111+
112+
#include <rclcpp_components/register_node_macro.hpp>
113+
RCLCPP_COMPONENTS_REGISTER_NODE(autoware::twist2accel::Twist2Accel)

‎localization/autoware_twist2accel/src/twist2accel.hpp

+75
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,75 @@
1+
// Copyright 2022 TIER IV
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef TWIST2ACCEL_HPP_
16+
#define TWIST2ACCEL_HPP_
17+
18+
#include "autoware/signal_processing/lowpass_filter_1d.hpp"
19+
20+
#include <rclcpp/rclcpp.hpp>
21+
22+
#include <geometry_msgs/msg/accel_with_covariance_stamped.hpp>
23+
#include <geometry_msgs/msg/twist_stamped.hpp>
24+
#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
25+
#include <nav_msgs/msg/odometry.hpp>
26+
27+
#include <tf2/LinearMath/Quaternion.h>
28+
#include <tf2/utils.h>
29+
30+
#include <chrono>
31+
#include <fstream>
32+
#include <iostream>
33+
#include <memory>
34+
#include <mutex>
35+
#include <queue>
36+
#include <string>
37+
#include <vector>
38+
39+
using autoware::signal_processing::LowpassFilter1d;
40+
41+
namespace autoware::twist2accel
42+
{
43+
class Twist2Accel : public rclcpp::Node
44+
{
45+
public:
46+
explicit Twist2Accel(const rclcpp::NodeOptions & node_options);
47+
48+
private:
49+
rclcpp::Publisher<geometry_msgs::msg::AccelWithCovarianceStamped>::SharedPtr
50+
pub_accel_; //!< @brief stop flag publisher
51+
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr
52+
sub_odom_; //!< @brief measurement odometry subscriber
53+
rclcpp::Subscription<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr
54+
sub_twist_; //!< @brief measurement odometry subscriber
55+
56+
geometry_msgs::msg::TwistStamped::SharedPtr prev_twist_ptr_;
57+
double accel_lowpass_gain_;
58+
bool use_odom_;
59+
std::shared_ptr<LowpassFilter1d> lpf_aax_ptr_;
60+
std::shared_ptr<LowpassFilter1d> lpf_aay_ptr_;
61+
std::shared_ptr<LowpassFilter1d> lpf_aaz_ptr_;
62+
std::shared_ptr<LowpassFilter1d> lpf_alx_ptr_;
63+
std::shared_ptr<LowpassFilter1d> lpf_aly_ptr_;
64+
std::shared_ptr<LowpassFilter1d> lpf_alz_ptr_;
65+
66+
/**
67+
* @brief set odometry measurement
68+
*/
69+
void callback_twist_with_covariance(
70+
const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg);
71+
void callback_odometry(const nav_msgs::msg::Odometry::SharedPtr msg);
72+
void estimate_accel(const geometry_msgs::msg::TwistStamped::SharedPtr msg);
73+
};
74+
} // namespace autoware::twist2accel
75+
#endif // TWIST2ACCEL_HPP_

0 commit comments

Comments
 (0)