|
| 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) |
0 commit comments