diff --git a/common/tier4_calibration_msgs/CMakeLists.txt b/common/tier4_calibration_msgs/CMakeLists.txt new file mode 100644 index 00000000..4dbb5908 --- /dev/null +++ b/common/tier4_calibration_msgs/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.5) +project(tier4_calibration_msgs) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/BoolStamped.msg" + "msg/Float32Stamped.msg" + "msg/EstimationResult.msg" + "msg/TimeDelay.msg" + DEPENDENCIES + std_msgs +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package() diff --git a/common/tier4_calibration_msgs/msg/BoolStamped.msg b/common/tier4_calibration_msgs/msg/BoolStamped.msg new file mode 100644 index 00000000..b8b7e7a6 --- /dev/null +++ b/common/tier4_calibration_msgs/msg/BoolStamped.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +bool data diff --git a/common/tier4_calibration_msgs/msg/EstimationResult.msg b/common/tier4_calibration_msgs/msg/EstimationResult.msg new file mode 100755 index 00000000..58896d7c --- /dev/null +++ b/common/tier4_calibration_msgs/msg/EstimationResult.msg @@ -0,0 +1,7 @@ +float64[] result +float64[] result_mean +float64[] result_stddev +float64[] absolute_error +float64[] mean_absolute_error +float64[] stddev_absolute_error +float64[] covariance diff --git a/common/tier4_calibration_msgs/msg/Float32Stamped.msg b/common/tier4_calibration_msgs/msg/Float32Stamped.msg new file mode 100644 index 00000000..20724197 --- /dev/null +++ b/common/tier4_calibration_msgs/msg/Float32Stamped.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +float32 data diff --git a/common/tier4_calibration_msgs/msg/TimeDelay.msg b/common/tier4_calibration_msgs/msg/TimeDelay.msg new file mode 100755 index 00000000..db6361af --- /dev/null +++ b/common/tier4_calibration_msgs/msg/TimeDelay.msg @@ -0,0 +1,8 @@ +float64 time_delay +float64 correlation_peak +float64 mean +float64 stddev +bool is_valid_data +float64[] time_delay_by_cross_correlation +float64[] first_order_model_coefficients +float64[] second_order_model_coefficients diff --git a/common/tier4_calibration_msgs/package.xml b/common/tier4_calibration_msgs/package.xml new file mode 100644 index 00000000..6315f9de --- /dev/null +++ b/common/tier4_calibration_msgs/package.xml @@ -0,0 +1,28 @@ + + + + tier4_calibration_msgs + 0.1.0 + The tier4_calibration_msgs package + Tomoya Kimura + Taiki Tanaka + Apache License 2.0 + + ament_cmake_auto + rosidl_default_generators + + geometry_msgs + sensor_msgs + std_msgs + + rosidl_default_runtime + + ament_lint_auto + autoware_lint_common + + rosidl_interface_packages + + + ament_cmake + + diff --git a/control/stop_accel_evaluator/CMakeLists.txt b/control/stop_accel_evaluator/CMakeLists.txt new file mode 100644 index 00000000..60c1ed40 --- /dev/null +++ b/control/stop_accel_evaluator/CMakeLists.txt @@ -0,0 +1,20 @@ +cmake_minimum_required(VERSION 3.5) +project(stop_accel_evaluator) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(stop_accel_evaluator_node SHARED + src/stop_accel_evaluator_node.cpp +) + +rclcpp_components_register_node(stop_accel_evaluator_node + PLUGIN "stop_accel_evaluator::StopAccelEvaluatorNode" + EXECUTABLE stop_accel_evaluator +) + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/control/stop_accel_evaluator/COLCON_IGNORE b/control/stop_accel_evaluator/COLCON_IGNORE new file mode 100644 index 00000000..e69de29b diff --git a/control/stop_accel_evaluator/README.md b/control/stop_accel_evaluator/README.md new file mode 100644 index 00000000..3a16e3e7 --- /dev/null +++ b/control/stop_accel_evaluator/README.md @@ -0,0 +1,12 @@ +# Stop Accel Evaluator + +The role of this node is to evaluate how smooth it is when a vehicle stops by calculating vehicle acceleration just before stopping. + +## How to use + +```sh +ros2 launch stop_accel_evaluator stop_accel_evaluator.launch.xml +``` + +Then you can see `stop_accel_evaluator/stop_accel` topic. +This topic is published only when a vehicle stops. diff --git a/control/stop_accel_evaluator/config/stop_accel_evaluator.param.yaml b/control/stop_accel_evaluator/config/stop_accel_evaluator.param.yaml new file mode 100644 index 00000000..96c26cfc --- /dev/null +++ b/control/stop_accel_evaluator/config/stop_accel_evaluator.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + imu_deque_size: 30 + not_running_acc: 0.005 + not_running_vel: 0.1 + stop_valid_imu_accel_num: 4 + lpf_pitch_gain: 0.95 + lpf_acc_gain: 0.2 diff --git a/control/stop_accel_evaluator/include/stop_accel_evaluator/stop_accel_evaluator_node.hpp b/control/stop_accel_evaluator/include/stop_accel_evaluator/stop_accel_evaluator_node.hpp new file mode 100644 index 00000000..3c7c10ef --- /dev/null +++ b/control/stop_accel_evaluator/include/stop_accel_evaluator/stop_accel_evaluator_node.hpp @@ -0,0 +1,80 @@ +// Copyright 2021 Tier IV, Inc. +// +// 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 STOP_ACCEL_EVALUATOR__STOP_ACCEL_EVALUATOR_NODE_HPP_ +#define STOP_ACCEL_EVALUATOR__STOP_ACCEL_EVALUATOR_NODE_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "signal_processing/lowpass_filter_1d.hpp" +#include "tier4_autoware_utils/ros/self_pose_listener.hpp" + +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "sensor_msgs/msg/imu.hpp" +#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" +#include "tier4_debug_msgs/msg/float32_stamped.hpp" + +#include +#include + +namespace stop_accel_evaluator +{ +using geometry_msgs::msg::TwistStamped; +using nav_msgs::msg::Odometry; +using sensor_msgs::msg::Imu; +using tier4_autoware_utils::SelfPoseListener; +using tier4_debug_msgs::msg::Float32MultiArrayStamped; +using tier4_debug_msgs::msg::Float32Stamped; + +class StopAccelEvaluatorNode : public rclcpp::Node +{ +public: + explicit StopAccelEvaluatorNode(const rclcpp::NodeOptions & node_options); + +private: + std::unique_ptr current_vel_ptr_{nullptr}; + std::unique_ptr prev_vel_ptr_{nullptr}; + + double stop_accel_{0.0}; + double stop_accel_with_gravity_{0.0}; + + rclcpp::Subscription::SharedPtr sub_twist_; + rclcpp::Subscription::SharedPtr sub_imu_; + rclcpp::Subscription::SharedPtr sub_debug_values_; + + rclcpp::Publisher::SharedPtr pub_stop_accel_; + rclcpp::Publisher::SharedPtr pub_stop_accel_with_gravity_; + + std::deque imu_deque_; + + size_t imu_deque_size_; + double not_running_acc_; + double not_running_vel_; + int stop_valid_imu_accel_num_; + + double current_acc_{0.0}; + std::shared_ptr lpf_acc_{nullptr}; + std::shared_ptr lpf_pitch_{nullptr}; + + SelfPoseListener self_pose_listener_{this}; + + void onTwist(const Odometry::ConstSharedPtr msg); + void onImu(const Imu::ConstSharedPtr msg); + + void calculateStopAccel(); + void publishStopAccel() const; +}; +} // namespace stop_accel_evaluator + +#endif // STOP_ACCEL_EVALUATOR__STOP_ACCEL_EVALUATOR_NODE_HPP_ diff --git a/control/stop_accel_evaluator/launch/stop_accel_evaluator.launch.xml b/control/stop_accel_evaluator/launch/stop_accel_evaluator.launch.xml new file mode 100644 index 00000000..105567ce --- /dev/null +++ b/control/stop_accel_evaluator/launch/stop_accel_evaluator.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/control/stop_accel_evaluator/package.xml b/control/stop_accel_evaluator/package.xml new file mode 100644 index 00000000..7eeae0e4 --- /dev/null +++ b/control/stop_accel_evaluator/package.xml @@ -0,0 +1,29 @@ + + + stop_accel_evaluator + 0.1.0 + The stop_accel_evaluator + Takayuki Murooka + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + + geometry_msgs + nav_msgs + rclcpp + rclcpp_components + sensor_msgs + signal_processing + std_msgs + tier4_autoware_utils + tier4_debug_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/control/stop_accel_evaluator/src/stop_accel_evaluator_node.cpp b/control/stop_accel_evaluator/src/stop_accel_evaluator_node.cpp new file mode 100644 index 00000000..33922aed --- /dev/null +++ b/control/stop_accel_evaluator/src/stop_accel_evaluator_node.cpp @@ -0,0 +1,153 @@ +// Copyright 2021 Tier IV, Inc. +// +// 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 "stop_accel_evaluator/stop_accel_evaluator_node.hpp" + +#include "tier4_autoware_utils/math/constants.hpp" + +#include +#include +#include + +namespace +{ +double getPitchByQuaternion(const geometry_msgs::msg::Quaternion & quaternion) +{ + const Eigen::Quaterniond q(quaternion.w, quaternion.x, quaternion.y, quaternion.z); + const Eigen::Vector3d v = q.toRotationMatrix() * Eigen::Vector3d::UnitX(); + const double den = std::max(std::hypot(v.x(), v.y()), 1e-8); + const double pitch = -1.0 * std::atan2(v.z(), den); + return pitch; +} +} // namespace + +namespace stop_accel_evaluator +{ +StopAccelEvaluatorNode::StopAccelEvaluatorNode(const rclcpp::NodeOptions & node_options) +: Node("stop_accel_evaluator", node_options) +{ + using std::placeholders::_1; + + imu_deque_size_ = static_cast(this->declare_parameter("imu_deque_size", 30)); + not_running_acc_ = this->declare_parameter("not_running_acc", 0.005); + not_running_vel_ = this->declare_parameter("not_running_vel", 0.1); + stop_valid_imu_accel_num_ = this->declare_parameter("stop_valid_imu_accel_num", 4); + + { // lowpass filter + lpf_pitch_ = std::make_shared(this->declare_parameter("lpf_pitch_gain", 0.95)); + lpf_acc_ = std::make_shared(this->declare_parameter("lpf_acc_gain", 0.2)); + } + + sub_twist_ = this->create_subscription( + "~/twist", 1, std::bind(&StopAccelEvaluatorNode::onTwist, this, _1)); + sub_imu_ = + this->create_subscription("~/imu", 1, std::bind(&StopAccelEvaluatorNode::onImu, this, _1)); + + pub_stop_accel_ = this->create_publisher("~/stop_accel", 1); + pub_stop_accel_with_gravity_ = + this->create_publisher("~/stop_accel_with_gravity", 1); + + self_pose_listener_.waitForFirstPose(); +} + +void StopAccelEvaluatorNode::onTwist(const Odometry::ConstSharedPtr msg) +{ + if (current_vel_ptr_) { + prev_vel_ptr_ = std::move(current_vel_ptr_); + } + current_vel_ptr_ = std::unique_ptr(); + current_vel_ptr_->header = msg->header; + current_vel_ptr_->twist = msg->twist.twist; +} + +void StopAccelEvaluatorNode::onImu(const Imu::ConstSharedPtr msg) +{ + if (!current_vel_ptr_ || !prev_vel_ptr_) { + return; + } + + // save x acceleration of imu + imu_deque_.push_back(-msg->linear_acceleration.x); + while (imu_deque_.size() > imu_deque_size_) { + imu_deque_.pop_front(); + } + + calculateStopAccel(); +} + +void StopAccelEvaluatorNode::calculateStopAccel() +{ + // calculate accel + const double prev_acc = current_acc_; + const double dv = current_vel_ptr_->twist.linear.x - prev_vel_ptr_->twist.linear.x; + const double dt = std::max( + (rclcpp::Time(current_vel_ptr_->header.stamp) - rclcpp::Time(prev_vel_ptr_->header.stamp)) + .seconds(), + 1e-03); + const double accel = dv / dt; + current_acc_ = lpf_acc_->filter(accel); + + // calculate when the car has just stopped + const bool just_stop_flag = (std::abs(current_acc_) < not_running_acc_) && + (std::abs(prev_acc) > not_running_acc_) && + (std::abs(prev_vel_ptr_->twist.linear.x) < not_running_vel_); + if (just_stop_flag) { + int accel_num = 0; + double accel_sum = 0; + bool minus_accel_flag = false; + for (size_t t = 0; t < imu_deque_.size(); ++t) { + const double acc = imu_deque_.at(imu_deque_.size() - 1 - t); + if (acc < 0) { + minus_accel_flag = true; + } + + if (minus_accel_flag) { + accel_sum += acc; + accel_num += 1; + + // calculate stop accel with/without gravity + if (accel_num == stop_valid_imu_accel_num_) { + const auto current_pose_ptr = self_pose_listener_.getCurrentPose(); + + const double pitch = + lpf_pitch_->filter(getPitchByQuaternion(current_pose_ptr->pose.orientation)); + + stop_accel_ = accel_sum / accel_num + tier4_autoware_utils::gravity * + std::sin(pitch); // consider removing gravity + stop_accel_with_gravity_ = accel_sum / accel_num; // not consider removing gravity + + publishStopAccel(); + return; + } + } + } + } +} + +void StopAccelEvaluatorNode::publishStopAccel() const +{ + auto msg_with_gravity = std::make_unique(); + msg_with_gravity->stamp = this->now(); + msg_with_gravity->data = stop_accel_with_gravity_; + pub_stop_accel_with_gravity_->publish(std::move(msg_with_gravity)); + + auto msg = std::make_unique(); + msg->stamp = this->now(); + msg->data = stop_accel_; + pub_stop_accel_->publish(std::move(msg)); +} +} // namespace stop_accel_evaluator + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(stop_accel_evaluator::StopAccelEvaluatorNode) diff --git a/control/vehicle_cmd_analyzer/CMakeLists.txt b/control/vehicle_cmd_analyzer/CMakeLists.txt new file mode 100644 index 00000000..50ca1f88 --- /dev/null +++ b/control/vehicle_cmd_analyzer/CMakeLists.txt @@ -0,0 +1,20 @@ +cmake_minimum_required(VERSION 3.5) +project(vehicle_cmd_analyzer) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(vehicle_cmd_analyzer_node SHARED + src/vehicle_cmd_analyzer.cpp +) + +rclcpp_components_register_node(vehicle_cmd_analyzer_node + PLUGIN "VehicleCmdAnalyzer" + EXECUTABLE vehicle_cmd_analyzer +) + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/control/vehicle_cmd_analyzer/README.md b/control/vehicle_cmd_analyzer/README.md new file mode 100644 index 00000000..1d4862b6 --- /dev/null +++ b/control/vehicle_cmd_analyzer/README.md @@ -0,0 +1,39 @@ +# Vehicle Command Analyzer description + +## Overview + +This is a visualization tool for vehicle commands. +You need [plotjuggler](https://github.com/facontidavide/PlotJuggler) to plot. + +The following time series data will be plotted on the left side. + +- Velocity +- Acceleration (acceleration and derivative of velocity) +- Jerk (derivative of acceleration and second derivative of velocity) +- Lateral acceleration + +The following data will be plotted on the right side. + +- XY plot of jerk-acceleration + +![Plotted data](./media/plotted.png) + +## How to use + +1. Launch the node. + + ```Shell + ros2 launch vehicle_cmd_analyzer vehicle_cmd_analyzer.launch.xml vehicle_model:=lexus + ``` + +2. Launch plotjuggler. + + ```Shell + ros2 run plotjuggler plotjuggler + ``` + +3. Load layout.xml from File->Layout. + ![Load layout.xml](./media/load_layout.png) +4. Press ok in the confirmation dialog. +5. Select`/vehicle_cmd_analyzer/debug_values`. + ![Select topic](./media/select_topic.png) diff --git a/control/vehicle_cmd_analyzer/config/vehicle_cmd_analyzer.palam.yaml b/control/vehicle_cmd_analyzer/config/vehicle_cmd_analyzer.palam.yaml new file mode 100644 index 00000000..e69de29b diff --git a/control/vehicle_cmd_analyzer/include/vehicle_cmd_analyzer/debug_values.hpp b/control/vehicle_cmd_analyzer/include/vehicle_cmd_analyzer/debug_values.hpp new file mode 100644 index 00000000..c6fc2d39 --- /dev/null +++ b/control/vehicle_cmd_analyzer/include/vehicle_cmd_analyzer/debug_values.hpp @@ -0,0 +1,65 @@ +// Copyright 2021 Tier IV, Inc. +// +// 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 VEHICLE_CMD_ANALYZER__DEBUG_VALUES_HPP_ +#define VEHICLE_CMD_ANALYZER__DEBUG_VALUES_HPP_ + +#include + +/// Debug Values used for debugging or controller tuning +class DebugValues +{ +public: + /// Types of debug values + enum class TYPE { + DT = 0, + CURRENT_TARGET_VEL = 1, + CURRENT_TARGET_D_VEL = 2, + CURRENT_TARGET_DD_VEL = 3, + CURRENT_TARGET_ACC = 4, + CURRENT_TARGET_D_ACC = 5, + CURRENT_TARGET_LATERAL_ACC = 6, + SIZE // this is the number of enum elements + }; + + /** + * @brief get the index corresponding to the given value TYPE + * @param [in] type the TYPE enum for which to get the index + * @return index of the type + */ + int getValuesIdx(const TYPE & type) const { return static_cast(type); } + /** + * @brief get all the debug values as an std::array + * @return array of all debug values + */ + std::array(TYPE::SIZE)> getValues() const { return values_; } + /** + * @brief set the given type to the given value + * @param [in] type TYPE of the value + * @param [in] value value to set + */ + void setValues(const TYPE & type, const double val) { values_.at(static_cast(type)) = val; } + /** + * @brief set the given type to the given value + * @param [in] type index of the type + * @param [in] value value to set + */ + void setValues(const int type, const double val) { values_.at(type) = val; } + +private: + static constexpr int num_debug_values_ = static_cast(TYPE::SIZE); + std::array(TYPE::SIZE)> values_; +}; + +#endif // VEHICLE_CMD_ANALYZER__DEBUG_VALUES_HPP_ diff --git a/control/vehicle_cmd_analyzer/include/vehicle_cmd_analyzer/vehicle_cmd_analyzer.hpp b/control/vehicle_cmd_analyzer/include/vehicle_cmd_analyzer/vehicle_cmd_analyzer.hpp new file mode 100644 index 00000000..4deb2732 --- /dev/null +++ b/control/vehicle_cmd_analyzer/include/vehicle_cmd_analyzer/vehicle_cmd_analyzer.hpp @@ -0,0 +1,70 @@ +// Copyright 2021 Tier IV, Inc. +// +// 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 VEHICLE_CMD_ANALYZER__VEHICLE_CMD_ANALYZER_HPP_ +#define VEHICLE_CMD_ANALYZER__VEHICLE_CMD_ANALYZER_HPP_ + +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" +#include "vehicle_cmd_analyzer/debug_values.hpp" + +#include + +#include "autoware_control_msgs/msg/control.hpp" +#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" + +#include +#include +#include +#include +#include + +class VehicleCmdAnalyzer : public rclcpp::Node +{ +private: + rclcpp::Subscription::SharedPtr sub_vehicle_cmd_; + rclcpp::Publisher::SharedPtr pub_debug_; + rclcpp::TimerBase::SharedPtr timer_control_; + + std::shared_ptr vehicle_cmd_ptr_{nullptr}; + + // timer callback + double control_rate_; + double wheelbase_; + + // for calculating dt + std::shared_ptr prev_control_time_{nullptr}; + + double prev_target_vel_; + std::array prev_target_d_vel_ = {}; + double prev_target_acc_; + + // debug values + DebugValues debug_values_; + + void callbackVehicleCommand(const autoware_control_msgs::msg::Control::SharedPtr msg); + + void callbackTimerControl(); + + void publishDebugData(); + + double getDt(); + std::pair differentiateVelocity(const double dt); + double differentiateAcceleration(const double dt); + double calcLateralAcceleration() const; + +public: + explicit VehicleCmdAnalyzer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); +}; + +#endif // VEHICLE_CMD_ANALYZER__VEHICLE_CMD_ANALYZER_HPP_ diff --git a/control/vehicle_cmd_analyzer/launch/vehicle_cmd_analyzer.launch.xml b/control/vehicle_cmd_analyzer/launch/vehicle_cmd_analyzer.launch.xml new file mode 100644 index 00000000..7326cc16 --- /dev/null +++ b/control/vehicle_cmd_analyzer/launch/vehicle_cmd_analyzer.launch.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/control/vehicle_cmd_analyzer/layout.xml b/control/vehicle_cmd_analyzer/layout.xml new file mode 100644 index 00000000..a505fd9b --- /dev/null +++ b/control/vehicle_cmd_analyzer/layout.xml @@ -0,0 +1,87 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/control/vehicle_cmd_analyzer/media/load_layout.png b/control/vehicle_cmd_analyzer/media/load_layout.png new file mode 100644 index 00000000..fda21583 Binary files /dev/null and b/control/vehicle_cmd_analyzer/media/load_layout.png differ diff --git a/control/vehicle_cmd_analyzer/media/plotted.png b/control/vehicle_cmd_analyzer/media/plotted.png new file mode 100644 index 00000000..7979f15d Binary files /dev/null and b/control/vehicle_cmd_analyzer/media/plotted.png differ diff --git a/control/vehicle_cmd_analyzer/media/ros2topic.png b/control/vehicle_cmd_analyzer/media/ros2topic.png new file mode 100644 index 00000000..017b08ab Binary files /dev/null and b/control/vehicle_cmd_analyzer/media/ros2topic.png differ diff --git a/control/vehicle_cmd_analyzer/media/select_topic.png b/control/vehicle_cmd_analyzer/media/select_topic.png new file mode 100644 index 00000000..6f865e3b Binary files /dev/null and b/control/vehicle_cmd_analyzer/media/select_topic.png differ diff --git a/control/vehicle_cmd_analyzer/package.xml b/control/vehicle_cmd_analyzer/package.xml new file mode 100644 index 00000000..32b4585f --- /dev/null +++ b/control/vehicle_cmd_analyzer/package.xml @@ -0,0 +1,26 @@ + + + + vehicle_cmd_analyzer + 0.0.0 + TODO: Package description + taichihirano + TODO: License declaration + + ament_cmake + + autoware_cmake + + autoware_control_msgs + autoware_vehicle_info_utils + rclcpp + rclcpp_components + tier4_debug_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/control/vehicle_cmd_analyzer/src/vehicle_cmd_analyzer.cpp b/control/vehicle_cmd_analyzer/src/vehicle_cmd_analyzer.cpp new file mode 100644 index 00000000..e4d4a467 --- /dev/null +++ b/control/vehicle_cmd_analyzer/src/vehicle_cmd_analyzer.cpp @@ -0,0 +1,146 @@ +// Copyright 2021 Tier IV, Inc. +// +// 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 "vehicle_cmd_analyzer/vehicle_cmd_analyzer.hpp" + +#include + +#include +#include +#include +#include +#include + +VehicleCmdAnalyzer::VehicleCmdAnalyzer(const rclcpp::NodeOptions & options) +: Node("vehicle_cmd_analyzer", options) +{ + control_rate_ = declare_parameter("control_rate", 30.0); + + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); + wheelbase_ = vehicle_info.wheel_base_m; + + sub_vehicle_cmd_ = this->create_subscription( + "/control/command/control_cmd", rclcpp::QoS(10), + std::bind(&VehicleCmdAnalyzer::callbackVehicleCommand, this, std::placeholders::_1)); + pub_debug_ = create_publisher( + "~/debug_values", rclcpp::QoS{1}); + + // Timer + { + auto timer_callback = std::bind(&VehicleCmdAnalyzer::callbackTimerControl, this); + auto period = std::chrono::duration_cast( + std::chrono::duration(1.0 / control_rate_)); + timer_control_ = std::make_shared>( + this->get_clock(), period, std::move(timer_callback), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(timer_control_, nullptr); + } +} + +void VehicleCmdAnalyzer::callbackVehicleCommand( + const autoware_control_msgs::msg::Control::SharedPtr msg) +{ + vehicle_cmd_ptr_ = std::make_shared(*msg); +} + +void VehicleCmdAnalyzer::callbackTimerControl() +{ + // wait for initial pointers + if (!vehicle_cmd_ptr_) { + return; + } + + // publish debug data + publishDebugData(); +} + +void VehicleCmdAnalyzer::publishDebugData() +{ + const double dt = getDt(); + const double a_lat = calcLateralAcceleration(); + const auto [d_vel, dd_vel] = differentiateVelocity(dt); + + // set debug values + debug_values_.setValues(DebugValues::TYPE::DT, dt); + debug_values_.setValues( + DebugValues::TYPE::CURRENT_TARGET_VEL, vehicle_cmd_ptr_->longitudinal.velocity); + debug_values_.setValues(DebugValues::TYPE::CURRENT_TARGET_D_VEL, d_vel); + debug_values_.setValues(DebugValues::TYPE::CURRENT_TARGET_DD_VEL, dd_vel); + debug_values_.setValues( + DebugValues::TYPE::CURRENT_TARGET_ACC, vehicle_cmd_ptr_->longitudinal.acceleration); + debug_values_.setValues(DebugValues::TYPE::CURRENT_TARGET_D_ACC, differentiateAcceleration(dt)); + debug_values_.setValues(DebugValues::TYPE::CURRENT_TARGET_LATERAL_ACC, a_lat); + + // publish debug values + tier4_debug_msgs::msg::Float32MultiArrayStamped debug_msg{}; + debug_msg.stamp = this->now(); + for (const auto & v : debug_values_.getValues()) { + debug_msg.data.push_back(v); + } + pub_debug_->publish(debug_msg); +} + +double VehicleCmdAnalyzer::getDt() +{ + double dt; + if (!prev_control_time_) { + dt = 1.0 / control_rate_; + prev_control_time_ = std::make_shared(this->now()); + } else { + dt = (this->now() - *prev_control_time_).seconds(); + *prev_control_time_ = this->now(); + } + const double max_dt = 1.0 / control_rate_ * 2.0; + const double min_dt = 1.0 / control_rate_ * 0.5; + return std::max(std::min(dt, max_dt), min_dt); +} + +std::pair VehicleCmdAnalyzer::differentiateVelocity(const double dt) +{ + if (!prev_target_vel_) { + prev_target_vel_ = vehicle_cmd_ptr_->longitudinal.velocity; + prev_target_d_vel_.at(2) = 0.0; + return {0.0, 0.0}; + } + const double d_vel = (vehicle_cmd_ptr_->longitudinal.velocity - prev_target_vel_) / dt; + const double dd_vel = (d_vel - prev_target_d_vel_.at(0)) / 2 / dt; + prev_target_vel_ = vehicle_cmd_ptr_->longitudinal.velocity; + for (int i = 0; i < 2; i++) { + prev_target_d_vel_.at(i) = prev_target_d_vel_.at(i + 1); + } + prev_target_d_vel_.at(2) = d_vel; + return {d_vel, dd_vel}; +} + +double VehicleCmdAnalyzer::differentiateAcceleration(const double dt) +{ + if (!prev_target_acc_) { + prev_target_acc_ = vehicle_cmd_ptr_->longitudinal.acceleration; + return 0.0; + } + const double d_acc = (vehicle_cmd_ptr_->longitudinal.acceleration - prev_target_acc_) / dt; + prev_target_acc_ = vehicle_cmd_ptr_->longitudinal.acceleration; + return d_acc; +} + +double VehicleCmdAnalyzer::calcLateralAcceleration() const +{ + const double delta = vehicle_cmd_ptr_->lateral.steering_tire_angle; + const double vel = vehicle_cmd_ptr_->longitudinal.velocity; + const double a_lat = vel * vel * std::sin(delta) / wheelbase_; + return a_lat; +} + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(VehicleCmdAnalyzer) diff --git a/localization/deviation_estimation_tools/ReadMe.md b/localization/deviation_estimation_tools/ReadMe.md new file mode 100644 index 00000000..df26f2e2 --- /dev/null +++ b/localization/deviation_estimation_tools/ReadMe.md @@ -0,0 +1,311 @@ +# deviation_estimation_tools + +## 1. Quick start + +This repository consists of three main tools implemented on ROS2. + +1. Deviation Estimator +2. Deviation Evaluator +3. Deviation Evaluation Visualizer + +### A. Estimation step + +Here you estimate the following parameters using `deviation_estimator`. + +- the standard deviation of velocity and yaw rate +- the bias of velocity and yaw rate + +Launch the node with the following command. Make sure you set the correct parameters (see Sec. 2). + +```sh +ros2 launch deviation_estimator deviation_estimator.launch.xml +``` + +Then, you need to run either ROS bag or `autoware_launch` to provide `pose` and `twist` to `deviation_estimator`. + +If you are using rosbag, it should contain the following topics: + +- Raw IMU data (default: `/sensing/imu/tamagawa/imu_raw`) +- Raw velocity data (default: `/vehicle/status/velocity_status`) +- `/localization/pose_estimator/pose_with_covariance` +- `/clock` +- `/tf_static` (that contains transform from `base_link` to `imu_link`) + +NOTE that the pose and twist must be estimated with default parameters (see `known issues` section for detail). + +Play the rosbag in a different terminal: + +```sh +ros2 bag play YOUR_BAG # You can also play in a faster rate, e.g. -r 5 +``` + +You can check the results in the following three output files: + +1. IMU parameters (default: `$HOME/imu_corrector.param.yaml`) +2. Velocity parameters (default: `$HOME/vehicle_velocity_converter.param.yaml`) +3. Logs (default: `$HOME/output.txt`) + +
sample input (rosbag) +

+ +```sh +Files: localized_sensors_0.db3 +Bag size: 9.6 MiB +Storage id: sqlite3 +Duration: 76.539s +Start: Jul 8 2022 11:21:41.220 (1657246901.220) +End: Jul 8 2022 11:22:57.759 (1657246977.759) +Messages: 32855 +Topic information: Topic: /localization/pose_estimator/pose_with_covariance | Type: geometry_msgs/msg/PoseWithCovarianceStamped | Count: 2162 | Serialization Format: cdr + Topic: /clock | Type: rosgraph_msgs/msg/Clock | Count: 57309 | Serialization Format: cdr + Topic: /tf_static | Type: tf2_msgs/msg/TFMessage | Count: 2 | Serialization Format: cdr + Topic: /sensing/imu/tamagawa/imu_raw | Type: sensor_msgs/msg/Imu | Count: 8076 | Serialization Format: cdr + Topic: /vehicle/status/velocity_status | Type: autoware_vehicle_msgs/msg/VelocityReport | Count: 8275 | Serialization Format: cdr + +``` + +

+
+ +
sample output (output.txt) +

+ +```sh +# Validation results +# value: [min, max] +[OK] coef_vx: [0.99538, 0.99593] +[OK] stddev_vx: [0.17192, 0.19161] +[OK] angular_velocity_offset_x: [-0.00742, -0.00727] +[OK] angular_velocity_offset_y: [-0.00119, -0.00115] +[OK] angular_velocity_offset_z: [0.00635, 0.00641] +[OK] angular_velocity_stddev_xx: [0.04151, 0.04258] +[OK] angular_velocity_stddev_yy: [0.04151, 0.04258] +[OK] angular_velocity_stddev_zz: [0.04151, 0.04258] +``` + +

+
+ +
sample output (imu_corrector.param.yaml) + +```sh +# Estimated by deviation_estimator +/**: + ros__parameters: + angular_velocity_stddev_xx: 0.01798 + angular_velocity_stddev_yy: 0.01798 + angular_velocity_stddev_zz: 0.01798 + angular_velocity_offset_x: -0.00952 + angular_velocity_offset_y: -0.00095 + angular_velocity_offset_z: 0.00607 +``` + +

+

+ +
sample output (vehicle_velocity_converter.param.yaml) + +```sh +# Estimated by deviation_estimator +/**: + ros__parameters: + speed_scale_factor: 0.99507 + velocity_stddev_xx: 0.16708 + velocity_stddev_xx: 0.1 # Default value + frame_id: base_link # Default value +``` + +

+

+ +
unit tool + +If you build normally, a binary will be generated under `install/deviation_estimator/lib/`. + +```sh +colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-up-to deviation_estimator +source ~/autoware/install/setup.bash +~/autoware/install/deviation_estimator/lib/deviation_estimator/deviation_estimator_unit_tool +``` + +

+

+ +### B. Evaluation step + +Here, you can evaluate the estimated standard deviation and bias using a package `deviation_evaluator`. +Execute the following command: + +```sh +ros2 launch deviation_evaluator deviation_evaluator.launch.xml map_path:=MAP_PATH rviz:=true in_imu:=YOUR_IMU_TOPIC_NAME in_wheel_odometry:=YOUR_VELOCITY_TOPIC_NAME +ros2 bag play YOUR_BAG +``` + +### C. Visualization step + +After the evaluation, run the following command to generate the final results in `$HOME/deviation_evaluator_sample`. + +```sh +pip3 install -r requirements.txt +ros2 launch deviation_evaluator deviation_evaluation_visualizer.launch.xml +``` + +Done! + +## 2. Description of Deviation Estimator + +### Overview + +The **Deviation Estimator** estimates the standard deviation and bias for velocity and yaw bias, by comparing the velocity and gyro observations with ground truth poses (e.g. from LiDAR-based localization). + +Here are some assumptions made for input data: + +- The data should have accurate localization results. It doesn't need to be strictly precise, but data that is obviously incorrect should be avoided. In case of NDT in Autoware, it should not have any TP or NVTL warnings from the ndt_scan_matcher. +- The data should cover a reasonable duration of driving. A few minutes of data is sufficient. It is desirable to have a distance of approximately 500 meters. (For example, around 2 minutes at a speed of 15 km/h). +- The data should include sections of driving in a straight line. This is necessary for estimating velocity-related parameters. Having at least one minute of straight line driving is enough. +- The data should cover the expected speed range during operation. +- [Optional] Ideally, the data should be recorded as recently as possible. Especially in cases where IMU or tire replacement has occurred, data recorded before those changes may not provide accurate estimations. +- [Optional] The data should cover various driving behaviors expected during operation, such as right turns, left turns, acceleration, and deceleration. + +### Launch + +The `deviation_estimator` can be launched with the following command. + +```sh +ros2 launch deviation_estimator deviation_estimator.launch.xml +ros2 bag play YOUR_BAG # You can also play in a faster rate, e.g. -r 5 +``` + +The parameters and input topic names can be seen in the `deviation_estimator.launch.xml` file. +`YOUR_BAG` should include all the required inputs written below. + +### Inputs / Outputs + +#### Input + +| Name | Type | Description | +| ------------------------ | ----------------------------------------------- | -------------------- | +| `in_pose_with_covariance | `geometry_msgs::msg::PoseWithCovarianceStamped` | Input pose | +| `in_imu` | `sensor_msgs::msg::Imu` | Input IMU data | +| `in_wheel_odometry` | `autoware_vehicle_msgs::msg::VelocityReport` | Input wheel odometry | + +#### Output + +| Name | Type | Description | +| ------------------------------------ | --------------------------- | ------------------------------------------------ | +| `/estimated_stddev_vx` | `std_msgs::msg::Float64` | estimated standard deviation of vx | +| `/estimated_stddev_angular_velocity` | `geometry_msgs/msg/Vector3` | estimated standard deviation of angular velocity | +| `/estimated_coef_vx` | `std_msgs::msg::Float64` | coef of vx | +| `/estimated_bias_angular_velocity` | `geometry_msgs/msg/Vector3` | bias of angular velocity | + +### Parameters for deviation estimator + +| Name | Type | Description | Default value | +| ---------------------------------------------- | ------ | ------------------------------------------------------------------- | ------------- | +| show_debug_info | bool | Flag to display debug info | true | +| t_design | double | Maximum expected duration of dead-reckoning [s] | 10.0 | +| x_design | double | Maximum expected trajectory length of dead-reckoning [m] | 30.0 | +| time_window | double | Estimation period [s] | 4.0 | +| results_dir | string | Text path where the estimated results will be stored | "$(env HOME)" | +| gyro_estimation.only_use_straight | bool | Flag to use only straight sections for gyro estimation | true | +| gyro_estimation.only_use_moving | bool | Flag to use only moving sections for gyro estimation | true | +| gyro_estimation.only_use_constant_velocity | bool | Flag to use only constant velocity sections for gyro estimation | true | +| velocity_estimation.only_use_straight | bool | Flag to use only straight sections for velocity estimation | true | +| velocity_estimation.only_use_moving | bool | Flag to use only moving sections for velocity estimation | true | +| velocity_estimation.only_use_constant_velocity | bool | Flag to use only constant velocity sections for velocity estimation | true | + +### Functions + +#### Bias estimation + +By assuming that the pose information is a ground truth, the node estimates the bias of velocity and yaw rate. + +#### Standard deviation estimation + +The node also estimates the standard deviation of velocity and yaw rate. This can be used as a parameter in `ekf_localizer`. +Note that the final estimation takes into account the bias. + +## 3. Description of Deviation Evaluator + +You can use `deviation_evaluator` for evaluating the estimated standard deviation parameters. +This can be run with the following command: + +```sh +ros2 launch deviation_evaluator deviation_evaluator.launch.xml map_path:=MAP_PATH rviz:=true in_imu:=YOUR_IMU_TOPIC_NAME in_wheel_odometry:=YOUR_VELOCITY_TOPIC_NAME +ros2 bag play YOUR_BAG +``` + +All the ros2bag and config files will be stored in `$HOME/deviation_evaluator_sample` (you can change this with `save_dir` parameter in the launch file). + +### Features + +#### A. Visualization of confidence ellipse + +`deviation_evaluator` supports rviz visualization. To use this feature, set `rviz:=true` and `map_path:=/path/to/map_folder`. + +

+ +

+ +#### B. Check the compatibility with a threshold in `localization_error_monitor` + +The `deviation_evaluator` also checks the compatibility of the estimated parameters and the threshold in `localization_error_monitor`. + +Concretely, it checks if the two following statement holds: + +1. `localization_error_monitor` would NOT diagnose the system as `WARN` nor `ERROR` as long as the NDT is available. +2. `localization_error_monitor` detects the anomaly with a recall over 0.99. + +Given the result of this validation, the users can verify that the estimated parameters in `deviation_estimator` can be safely applied to Autoware. + +Here, note that the `localization_error_monitor` treat the system as an anomaly if either of error along long-axis of confidence ellipse or error along lateral direction is over threshold. Please refer to the package in autoware.universe for detail. + +### Architecture of `deviation_evaluator` + +The architecture of `deviation_evaluator` is shown below. It launches two `ekf_localizer`, one for ground truth estimation and one for (partially) dead reckoning estimation. Outputs of both `ekf_localizer` will be recorded and analyzed with `deviation_evaluation_visualizer`. + +

+ +

+ +### Inputs / Outputs + +#### Input + +| Name | Type | Description | +| ----------------------------- | ----------------------------------------------- | -------------------------- | +| `in_ndt_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Input pose | +| `in_ekf_dr_odom` | `nav_msgs::msg::Odometry` | dead-reckoning EKF outputs | +| `in_ekf_gt_odom` | `nav_msgs::msg::Odometry` | ground-truth EKF outputs | + +#### Output + +| Name | Type | Description | +| ---------------------------------- | ----------------------------------------------- | ------------------------------------------------ | +| `out_pose_with_covariance_dr` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Output pose (for dead reckoning `ekf_localizer`) | +| `out_pose_with_covariance_gt` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Output pose (for ground truth `ekf_localizer`) | +| `out_initial_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Output initial pose (for both `ekf_localizer`) | + +### Parameters for deviation evaluator + +| Name | Type | Description | Default value | +| -------- | ---------- | -------------------------------------------------------------------------- | ----------------------------------------- | +| rviz | bool | Show rviz if true | false | +| map_path | string | Path to the directory where map data (OpenStreetMap or .osm data) is saved | "" | +| save_dir | string | Output directory where figures, parameter files, and scores are saved | "$(env HOME)/deviation_evaluator_sample" | +| period | double [s] | Duration of cycle | 10 (in `config/deviation_evaluator.yaml`) | +| cut | double [s] | Duration of ndt-cut-off | 9 (in `config/deviation_evaluator.yaml`) | + +## 4. Reflect the estimated parameters in Autoware + +The results of `deviation_estimator` is stored in two scripts: + +- `imu_corrector` param file (default: `$HOME/imu_corrector.param.yaml`) +- `vehicle_velocity_converter` param file (default: `$HOME/vehicle_velocity_converter.param.yaml`) + +Please modify your Autoware configuration so that it will launch using the above two parameter files. + +## 5. Known issues + +- The plot of `deviation_evaluator.png` generated by `deviation_evaluation_visualizer` may diverge, possibly due to the large covariance caused by a failure in localization. +- `ekf_localizer` in `deviation_evaluator` may not start properly. As for now, please launch `deviation_evaluator` first and then run `ros2 bag play` to provide pose and twist data. diff --git a/localization/deviation_estimation_tools/deviation_estimator/CMakeLists.txt b/localization/deviation_estimation_tools/deviation_estimator/CMakeLists.txt new file mode 100644 index 00000000..b36d43ad --- /dev/null +++ b/localization/deviation_estimation_tools/deviation_estimator/CMakeLists.txt @@ -0,0 +1,55 @@ +cmake_minimum_required(VERSION 3.5) +project(deviation_estimator) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(deviation_estimator_lib SHARED + src/deviation_estimator.cpp + src/utils.cpp + src/gyro_bias_module.cpp + src/velocity_coef_module.cpp + src/logger.cpp + src/validation_module.cpp +) + +# as a ros2 node +ament_auto_add_executable(deviation_estimator src/deviation_estimator_node.cpp) +target_compile_options(deviation_estimator PUBLIC -g -Wall -Wextra -Wpedantic -Werror) +target_link_libraries(deviation_estimator deviation_estimator_lib) +target_include_directories(deviation_estimator PUBLIC include) + +# as a static tool +ament_auto_add_executable(deviation_estimator_unit_tool src/deviation_estimator_main.cpp) +target_compile_options(deviation_estimator_unit_tool PUBLIC -g -Wall -Wextra -Wpedantic -Werror) +target_link_libraries(deviation_estimator_unit_tool deviation_estimator_lib) +target_include_directories(deviation_estimator_unit_tool PUBLIC include) + +if(BUILD_TESTING) + function(add_testcase filepath) + get_filename_component(filename ${filepath} NAME) + string(REGEX REPLACE ".cpp" "" test_name ${filename}) + + ament_add_gtest(${test_name} ${filepath}) + target_link_libraries("${test_name}" deviation_estimator_lib) + ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) + endfunction() + + find_package(ament_cmake_gtest REQUIRED) + + set(TEST_FILES + test/test_gyro_stddev.cpp + test/test_gyro_bias.cpp + test/test_utils.cpp) + + foreach(filepath ${TEST_FILES}) + add_testcase(${filepath}) + endforeach() +endif() + + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/localization/deviation_estimation_tools/deviation_estimator/config/deviation_estimator.param.yaml b/localization/deviation_estimation_tools/deviation_estimator/config/deviation_estimator.param.yaml new file mode 100644 index 00000000..1b170510 --- /dev/null +++ b/localization/deviation_estimation_tools/deviation_estimator/config/deviation_estimator.param.yaml @@ -0,0 +1,27 @@ +/**: + ros__parameters: + time_window: 4.0 + vx_threshold: 1.5 + wz_threshold: 0.01 + accel_threshold: 0.3 + output_frame: base_link + gyro_estimation: + only_use_straight: true + only_use_moving: false + only_use_constant_velocity: false + add_bias_uncertainty: false + velocity_estimation: + only_use_straight: true + only_use_moving: true + only_use_constant_velocity: true + add_bias_uncertainty: false + + # Used when adding bias-based uncertainty + dt_design: 10.0 # [s] + dx_design: 30.0 # [m] + + # Used for validating the results of deviation_estimator + thres_coef_vx: 0.01 # [.] + thres_stddev_vx: 0.05 # [.] + thres_bias_gyro: 0.001 # [rad/s] + thres_stddev_gyro: 0.01 # [rad/s] diff --git a/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/deviation_estimator.hpp b/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/deviation_estimator.hpp new file mode 100644 index 00000000..bf773638 --- /dev/null +++ b/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/deviation_estimator.hpp @@ -0,0 +1,124 @@ +// Copyright 2018-2019 Autoware Foundation +// +// 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 DEVIATION_ESTIMATOR__DEVIATION_ESTIMATOR_HPP_ +#define DEVIATION_ESTIMATOR__DEVIATION_ESTIMATOR_HPP_ + +#include "deviation_estimator/gyro_bias_module.hpp" +#include "deviation_estimator/logger.hpp" +#include "deviation_estimator/utils.hpp" +#include "deviation_estimator/validation_module.hpp" +#include "deviation_estimator/velocity_coef_module.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2/utils.h" +#include "tier4_autoware_utils/ros/transform_listener.hpp" + +#include "autoware_vehicle_msgs/msg/velocity_report.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/twist_with_covariance_stamped.hpp" +#include "sensor_msgs/msg/imu.hpp" +#include "std_msgs/msg/float64.hpp" +#include "tier4_debug_msgs/msg/float64_stamped.hpp" + +#include +#include +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +geometry_msgs::msg::Vector3 estimate_stddev_angular_velocity( + const std::vector & traj_data_list, + const geometry_msgs::msg::Vector3 & gyro_bias); + +double estimate_stddev_velocity( + const std::vector & traj_data_list, const double coef_vx); + +class DeviationEstimator : public rclcpp::Node +{ +public: + DeviationEstimator(const std::string & node_name, const rclcpp::NodeOptions & options); + +private: + rclcpp::Subscription::SharedPtr sub_pose_with_cov_; + rclcpp::Subscription::SharedPtr sub_wheel_odometry_; + rclcpp::Subscription::SharedPtr sub_imu_; + rclcpp::Publisher::SharedPtr pub_coef_vx_; + rclcpp::Publisher::SharedPtr pub_bias_angvel_; + rclcpp::Publisher::SharedPtr pub_stddev_vx_; + rclcpp::Publisher::SharedPtr pub_stddev_angvel_; + rclcpp::TimerBase::SharedPtr timer_; + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + std::string imu_link_frame_; + + std::vector vx_all_; + std::vector gyro_all_; + std::vector pose_buf_; + std::vector traj_data_list_for_gyro_; + std::vector traj_data_list_for_velocity_; + + double dt_design_; + double dx_design_; + double wz_threshold_; + double vx_threshold_; + double accel_threshold_; + double estimation_freq_; + double time_window_; + + bool gyro_only_use_straight_; + bool gyro_only_use_moving_; + bool gyro_only_use_constant_velocity_; + bool gyro_add_bias_uncertainty_; + bool velocity_only_use_straight_; + bool velocity_only_use_moving_; + bool velocity_only_use_constant_velocity_; + bool velocity_add_bias_uncertainty_; + + std::string imu_frame_; + const std::string output_frame_; + const std::string results_dir_; + const Logger results_logger_; + + std::unique_ptr gyro_bias_module_; + std::unique_ptr vel_coef_module_; + std::unique_ptr validation_module_; + + std::shared_ptr transform_listener_; + + void callback_pose_with_covariance(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + + void callback_wheel_odometry( + const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr wheel_odometry_msg_ptr); + + void callback_imu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr); + + void timer_callback(); + + double add_bias_uncertainty_on_velocity( + const double stddev_vx, const double stddev_coef_vx) const; + + geometry_msgs::msg::Vector3 add_bias_uncertainty_on_angular_velocity( + const geometry_msgs::msg::Vector3 stddev_angvel_base, + const geometry_msgs::msg::Vector3 stddev_angvel_bias_base) const; +}; +#endif // DEVIATION_ESTIMATOR__DEVIATION_ESTIMATOR_HPP_ diff --git a/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/gyro_bias_module.hpp b/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/gyro_bias_module.hpp new file mode 100644 index 00000000..cc391ca0 --- /dev/null +++ b/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/gyro_bias_module.hpp @@ -0,0 +1,40 @@ +// Copyright 2018-2019 Autoware Foundation +// +// 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 DEVIATION_ESTIMATOR__GYRO_BIAS_MODULE_HPP_ +#define DEVIATION_ESTIMATOR__GYRO_BIAS_MODULE_HPP_ + +#include "deviation_estimator/utils.hpp" + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/vector3_stamped.hpp" + +#include +#include + +class GyroBiasModule +{ +public: + GyroBiasModule() = default; + void update_bias(const TrajectoryData & traj_data); + geometry_msgs::msg::Vector3 get_bias_base_link() const; + geometry_msgs::msg::Vector3 get_bias_std() const; + bool empty() const; + +private: + std::vector gyro_bias_list_; + std::pair gyro_bias_pair_; +}; + +#endif // DEVIATION_ESTIMATOR__GYRO_BIAS_MODULE_HPP_ diff --git a/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/logger.hpp b/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/logger.hpp new file mode 100644 index 00000000..ffe58524 --- /dev/null +++ b/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/logger.hpp @@ -0,0 +1,43 @@ +// Copyright 2018-2019 Autoware Foundation +// +// 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 DEVIATION_ESTIMATOR__LOGGER_HPP_ +#define DEVIATION_ESTIMATOR__LOGGER_HPP_ + +#include "deviation_estimator/utils.hpp" +#include "deviation_estimator/validation_module.hpp" + +#include "geometry_msgs/msg/vector3.hpp" + +#include + +#include +#include + +class Logger +{ +public: + explicit Logger(const std::string & output_dir); + void log_estimated_result_section( + const double stddev_vx, const double coef_vx, + const geometry_msgs::msg::Vector3 & angular_velocity_stddev, + const geometry_msgs::msg::Vector3 & angular_velocity_offset) const; + void log_validation_result_section(const ValidationModule & validation_module) const; + +private: + const std::string output_log_path_; + const std::string output_imu_param_path_; + const std::string output_velocity_param_path_; +}; +#endif // DEVIATION_ESTIMATOR__LOGGER_HPP_ diff --git a/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/tier4_autoware_utils.hpp b/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/tier4_autoware_utils.hpp new file mode 100644 index 00000000..7e7fea9e --- /dev/null +++ b/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/tier4_autoware_utils.hpp @@ -0,0 +1,138 @@ +// Copyright 2018-2019 Autoware Foundation +// +// 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 DEVIATION_ESTIMATOR__TIER4_AUTOWARE_UTILS_HPP_ +#define DEVIATION_ESTIMATOR__TIER4_AUTOWARE_UTILS_HPP_ + +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/math/constants.hpp" +#include "tier4_autoware_utils/math/normalization.hpp" + +#include + +// ToDo (kminoda): Replace these functions with the one from tier4_autoware_utils. +// Currently these functions are declared here since this tool has to be compatible with older +// version of Autoware. + +inline geometry_msgs::msg::Vector3 createVector3(const double x, double y, double z) +{ + geometry_msgs::msg::Vector3 vec; + vec.x = x; + vec.y = y; + vec.z = z; + return vec; +} + +template +bool isDrivingForward(const Pose1 & src_pose, const Pose2 & dst_pose) +{ + // check the first point direction + const double src_yaw = tf2::getYaw(tier4_autoware_utils::getPose(src_pose).orientation); + const double pose_direction_yaw = tier4_autoware_utils::calcAzimuthAngle( + tier4_autoware_utils::getPoint(src_pose), tier4_autoware_utils::getPoint(dst_pose)); + return std::fabs(tier4_autoware_utils::normalizeRadian(src_yaw - pose_direction_yaw)) < + tier4_autoware_utils::pi / 2.0; +} + +/** + * @brief Calculate a point by linear interpolation. + * @param src source point + * @param dst destination point + * @param ratio interpolation ratio, which should be [0.0, 1.0] + * @return interpolated point + */ +template +geometry_msgs::msg::Point calcInterpolatedPoint( + const Point1 & src, const Point2 & dst, const double ratio) +{ + const auto src_point = tier4_autoware_utils::getPoint(src); + const auto dst_point = tier4_autoware_utils::getPoint(dst); + + tf2::Vector3 src_vec; + src_vec.setX(src_point.x); + src_vec.setY(src_point.y); + src_vec.setZ(src_point.z); + + tf2::Vector3 dst_vec; + dst_vec.setX(dst_point.x); + dst_vec.setY(dst_point.y); + dst_vec.setZ(dst_point.z); + + // Get pose by linear interpolation + const double clamped_ratio = std::clamp(ratio, 0.0, 1.0); + const auto & vec = tf2::lerp(src_vec, dst_vec, clamped_ratio); + + geometry_msgs::msg::Point point; + point.x = vec.x(); + point.y = vec.y(); + point.z = vec.z(); + + return point; +} + +/** + * @brief Calculate a pose by linear interpolation. + * Note that if dist(src_pose, dst_pose)<=0.01 + * the orientation of the output pose is same as the orientation + * of the dst_pose + * @param src source point + * @param dst destination point + * @param ratio interpolation ratio, which should be [0.0, 1.0] + * @param set_orientation_from_position_direction set position by spherical interpolation if false + * @return interpolated point + */ +template +geometry_msgs::msg::Pose calcInterpolatedPose( + const Pose1 & src_pose, const Pose2 & dst_pose, const double ratio, + const bool set_orientation_from_position_direction = true) +{ + const double clamped_ratio = std::clamp(ratio, 0.0, 1.0); + + geometry_msgs::msg::Pose output_pose; + output_pose.position = calcInterpolatedPoint( + tier4_autoware_utils::getPoint(src_pose), tier4_autoware_utils::getPoint(dst_pose), + clamped_ratio); + + if (set_orientation_from_position_direction) { + const double input_poses_dist = tier4_autoware_utils::calcDistance2d( + tier4_autoware_utils::getPoint(src_pose), tier4_autoware_utils::getPoint(dst_pose)); + const bool is_driving_forward = isDrivingForward(src_pose, dst_pose); + + // Get orientation from interpolated point and src_pose + if ((is_driving_forward && clamped_ratio > 1.0 - (1e-6)) || input_poses_dist < 1e-3) { + output_pose.orientation = tier4_autoware_utils::getPose(dst_pose).orientation; + } else if (!is_driving_forward && clamped_ratio < 1e-6) { + output_pose.orientation = tier4_autoware_utils::getPose(src_pose).orientation; + } else { + const auto & base_pose = is_driving_forward ? dst_pose : src_pose; + const double pitch = tier4_autoware_utils::calcElevationAngle( + tier4_autoware_utils::getPoint(output_pose), tier4_autoware_utils::getPoint(base_pose)); + const double yaw = tier4_autoware_utils::calcAzimuthAngle( + tier4_autoware_utils::getPoint(output_pose), tier4_autoware_utils::getPoint(base_pose)); + output_pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw); + } + } else { + // Get orientation by spherical linear interpolation + tf2::Transform src_tf; + tf2::Transform dst_tf; + tf2::fromMsg(tier4_autoware_utils::getPose(src_pose), src_tf); + tf2::fromMsg(tier4_autoware_utils::getPose(dst_pose), dst_tf); + const auto & quaternion = tf2::slerp(src_tf.getRotation(), dst_tf.getRotation(), clamped_ratio); + output_pose.orientation = tf2::toMsg(quaternion); + } + + return output_pose; +} + +#endif // DEVIATION_ESTIMATOR__TIER4_AUTOWARE_UTILS_HPP_ diff --git a/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/utils.hpp b/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/utils.hpp new file mode 100644 index 00000000..122c38b4 --- /dev/null +++ b/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/utils.hpp @@ -0,0 +1,191 @@ +// Copyright 2018-2019 Autoware Foundation +// +// 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 DEVIATION_ESTIMATOR__UTILS_HPP_ +#define DEVIATION_ESTIMATOR__UTILS_HPP_ + +#include "deviation_estimator/tier4_autoware_utils.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "tier4_debug_msgs/msg/float64_stamped.hpp" + +#include + +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif +#include + +struct TrajectoryData +{ + std::vector pose_list; + std::vector vx_list; + std::vector gyro_list; +}; + +double double_round(const double x, const int n); + +bool whether_to_use_data( + const bool is_straight, const bool is_moving, const bool is_constant_velocity, + const bool only_use_straight, const bool only_use_moving, const bool only_use_constant_velocity); + +template +double calculate_mean(const std::vector & v) +{ + if (v.size() == 0) { + return 0; + } + + double mean = 0; + for (const T & t : v) { + mean += t; + } + mean /= v.size(); + return mean; +} + +template +double calculate_std(const std::vector & v) +{ + if (v.size() == 0) { + return 0; + } + + const double mean = calculate_mean(v); + double error = 0; + for (const T & t : v) { + error += pow(t - mean, 2); + } + return std::sqrt(error / v.size()); +} + +template +double calculate_std_mean_const(const std::vector & v, const double mean) +{ + if (v.size() == 0) { + return 0; + } + + double error = 0; + for (const T & t : v) { + error += pow(t - mean, 2); + } + return std::sqrt(error / v.size()); +} + +struct CompareMsgTimestamp +{ + bool operator()(tier4_debug_msgs::msg::Float64Stamped const & t1, double const & t2) const + { + return rclcpp::Time(t1.stamp).seconds() < t2; + } + + bool operator()(tier4_debug_msgs::msg::Float64Stamped const & t1, rclcpp::Time const & t2) const + { + return rclcpp::Time(t1.stamp).seconds() < t2.seconds(); + } + + template + bool operator()(T1 const & t1, double const & t2) const + { + return rclcpp::Time(t1.header.stamp).seconds() < t2; + } + + template + bool operator()(double const & t1, T2 const & t2) const + { + return t1 < rclcpp::Time(t2.header.stamp).seconds(); + } + + template + bool operator()(T1 const & t1, T2 const & t2) const + { + return rclcpp::Time(t1.header.stamp).seconds() < rclcpp::Time(t2.header.stamp).seconds(); + } + + template + bool operator()(T1 const & t1, rclcpp::Time const & t2) const + { + return rclcpp::Time(t1.header.stamp).seconds() < t2.seconds(); + } + + template + bool operator()(rclcpp::Time const & t1, T2 const & t2) const + { + return t1.seconds() < rclcpp::Time(t2.header.stamp).seconds(); + } +}; + +geometry_msgs::msg::Vector3 interpolate_vector3_stamped( + const std::vector & vec_list, const double time, + const double tolerance_sec); + +template +std::vector extract_sub_trajectory( + const std::vector & msg_list, const rclcpp::Time & t0, const rclcpp::Time & t1) +{ + const auto start_iter = + std::lower_bound(msg_list.begin(), msg_list.end(), t0, CompareMsgTimestamp()); + const auto end_iter = + std::lower_bound(msg_list.begin(), msg_list.end(), t1, CompareMsgTimestamp()); + std::vector msg_list_sub(start_iter, end_iter); + return msg_list_sub; +} + +template +double norm_xy(const T p1, const U p2) +{ + const double dx = p1.x - p2.x; + const double dy = p1.y - p2.y; + return std::sqrt(dx * dx + dy * dy); +} + +double clip_radian(const double rad); + +geometry_msgs::msg::Point integrate_position( + const std::vector & vx_list, + const std::vector & gyro_list, const double coef_vx, + const double yaw_init); + +geometry_msgs::msg::Vector3 calculate_error_rpy( + const std::vector & pose_list, + const std::vector & gyro_list, + const geometry_msgs::msg::Vector3 & gyro_bias); + +geometry_msgs::msg::Vector3 integrate_orientation( + const std::vector & gyro_list, + const geometry_msgs::msg::Vector3 & gyro_bias); + +double get_mean_abs_vx(const std::vector & vx_list); +double get_mean_abs_wz(const std::vector & gyro_list); +double get_mean_accel(const std::vector & vx_list); + +geometry_msgs::msg::Vector3 transform_vector3( + const geometry_msgs::msg::Vector3 & vec, const geometry_msgs::msg::TransformStamped & transform); + +inline void myFromMsg(const geometry_msgs::msg::Transform & in, tf2::Transform & out); + +geometry_msgs::msg::TransformStamped inverse_transform( + const geometry_msgs::msg::TransformStamped & transform); + +#endif // DEVIATION_ESTIMATOR__UTILS_HPP_ diff --git a/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/validation_module.hpp b/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/validation_module.hpp new file mode 100644 index 00000000..c6c01567 --- /dev/null +++ b/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/validation_module.hpp @@ -0,0 +1,50 @@ +// Copyright 2018-2019 Autoware Foundation +// +// 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 DEVIATION_ESTIMATOR__VALIDATION_MODULE_HPP_ +#define DEVIATION_ESTIMATOR__VALIDATION_MODULE_HPP_ + +#include "deviation_estimator/utils.hpp" + +#include "geometry_msgs/msg/vector3_stamped.hpp" + +#include + +#include +#include +#include +#include +#include + +class ValidationModule +{ +public: + ValidationModule( + const double threshold_coef_vx, const double threshold_stddev_vx, + const double threshold_bias_gyro, const double threshold_stddev_gyro, const size_t num_history); + void set_velocity_data(const double coef_vx, const double stddev_vx); + void set_gyro_data( + const geometry_msgs::msg::Vector3 & bias_gyro, const geometry_msgs::msg::Vector3 & stddev_gyro); + + std::pair get_min_max(const std::string key) const; + bool is_valid(const std::string key) const; + +private: + std::map> data_list_dict_; + + std::map threshold_dict_; + const size_t num_history_; +}; + +#endif // DEVIATION_ESTIMATOR__VALIDATION_MODULE_HPP_ diff --git a/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/velocity_coef_module.hpp b/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/velocity_coef_module.hpp new file mode 100644 index 00000000..374ce03b --- /dev/null +++ b/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/velocity_coef_module.hpp @@ -0,0 +1,41 @@ +// Copyright 2018-2019 Autoware Foundation +// +// 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 DEVIATION_ESTIMATOR__VELOCITY_COEF_MODULE_HPP_ +#define DEVIATION_ESTIMATOR__VELOCITY_COEF_MODULE_HPP_ + +#include "deviation_estimator/utils.hpp" + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/vector3_stamped.hpp" +#include "tier4_debug_msgs/msg/float64_stamped.hpp" + +#include +#include + +class VelocityCoefModule +{ +public: + VelocityCoefModule() = default; + void update_coef(const TrajectoryData & traj_data); + double get_coef() const; + double get_coef_std() const; + bool empty() const; + +private: + std::vector coef_vx_list_; + std::pair coef_vx_; +}; + +#endif // DEVIATION_ESTIMATOR__VELOCITY_COEF_MODULE_HPP_ diff --git a/localization/deviation_estimation_tools/deviation_estimator/launch/deviation_estimator.launch.xml b/localization/deviation_estimation_tools/deviation_estimator/launch/deviation_estimator.launch.xml new file mode 100644 index 00000000..905f9aca --- /dev/null +++ b/localization/deviation_estimation_tools/deviation_estimator/launch/deviation_estimator.launch.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/localization/deviation_estimation_tools/deviation_estimator/media/sample_confidence_ellipse.png b/localization/deviation_estimation_tools/deviation_estimator/media/sample_confidence_ellipse.png new file mode 100644 index 00000000..b5c9873c Binary files /dev/null and b/localization/deviation_estimation_tools/deviation_estimator/media/sample_confidence_ellipse.png differ diff --git a/localization/deviation_estimation_tools/deviation_estimator/media/sample_rviz.png b/localization/deviation_estimation_tools/deviation_estimator/media/sample_rviz.png new file mode 100644 index 00000000..aebb42b2 Binary files /dev/null and b/localization/deviation_estimation_tools/deviation_estimator/media/sample_rviz.png differ diff --git a/localization/deviation_estimation_tools/deviation_estimator/package.xml b/localization/deviation_estimation_tools/deviation_estimator/package.xml new file mode 100644 index 00000000..21cc5504 --- /dev/null +++ b/localization/deviation_estimation_tools/deviation_estimator/package.xml @@ -0,0 +1,34 @@ + + + + deviation_estimator + 0.0.0 + The deviation estimator package + Koji Minoda + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_vehicle_msgs + geometry_msgs + nav_msgs + rclcpp + rosbag2 + rosbag2_cpp + sensor_msgs + std_msgs + tf2 + tf2_geometry_msgs + tf2_ros + tier4_autoware_utils + tier4_debug_msgs + + ament_cmake_gtest + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/localization/deviation_estimation_tools/deviation_estimator/src/deviation_estimator.cpp b/localization/deviation_estimation_tools/deviation_estimator/src/deviation_estimator.cpp new file mode 100644 index 00000000..cc3f255c --- /dev/null +++ b/localization/deviation_estimation_tools/deviation_estimator/src/deviation_estimator.cpp @@ -0,0 +1,352 @@ +// Copyright 2018-2019 Autoware Foundation +// +// 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 "deviation_estimator/deviation_estimator.hpp" + +#include "deviation_estimator/logger.hpp" +#include "deviation_estimator/utils.hpp" +#include "rclcpp/logging.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" + +#include +#include +#include +#include +#include +#include + +using std::placeholders::_1; + +geometry_msgs::msg::Vector3 estimate_stddev_angular_velocity( + const std::vector & traj_data_list, const geometry_msgs::msg::Vector3 & gyro_bias) +{ + if (traj_data_list.size() == 0) return geometry_msgs::msg::Vector3{}; + + double t_window = 0.0; + for (const TrajectoryData & traj_data : traj_data_list) { + const rclcpp::Time t0_rclcpp_time = rclcpp::Time(traj_data.pose_list.front().header.stamp); + const rclcpp::Time t1_rclcpp_time = rclcpp::Time(traj_data.pose_list.back().header.stamp); + t_window += t1_rclcpp_time.seconds() - t0_rclcpp_time.seconds(); + } + t_window /= traj_data_list.size(); + + std::vector delta_wx_list; + std::vector delta_wy_list; + std::vector delta_wz_list; + + for (const TrajectoryData & traj_data : traj_data_list) { + const auto t1_pose = rclcpp::Time(traj_data.pose_list.back().header.stamp); + const auto t0_pose = rclcpp::Time(traj_data.pose_list.front().header.stamp); + if (t0_pose > t1_pose) continue; + + const size_t n_twist = traj_data.gyro_list.size(); + + auto error_rpy = calculate_error_rpy(traj_data.pose_list, traj_data.gyro_list, gyro_bias); + const double dt_pose = (rclcpp::Time(traj_data.pose_list.back().header.stamp) - + rclcpp::Time(traj_data.pose_list.front().header.stamp)) + .seconds(); + const double dt_gyro = (rclcpp::Time(traj_data.gyro_list.back().header.stamp) - + rclcpp::Time(traj_data.gyro_list.front().header.stamp)) + .seconds(); + error_rpy.x *= dt_pose / dt_gyro; + error_rpy.y *= dt_pose / dt_gyro; + error_rpy.z *= dt_pose / dt_gyro; + + delta_wx_list.push_back(std::sqrt(n_twist / t_window) * error_rpy.x); + delta_wy_list.push_back(std::sqrt(n_twist / t_window) * error_rpy.y); + delta_wz_list.push_back(std::sqrt(n_twist / t_window) * error_rpy.z); + } + + geometry_msgs::msg::Vector3 stddev_angvel_base = createVector3( + calculate_std(delta_wx_list) / std::sqrt(t_window), + calculate_std(delta_wy_list) / std::sqrt(t_window), + calculate_std(delta_wz_list) / std::sqrt(t_window)); + return stddev_angvel_base; +} + +double estimate_stddev_velocity( + const std::vector & traj_data_list, const double coef_vx) +{ + if (traj_data_list.size() == 0) return 0.0; + + double t_window = 0.0; + for (const TrajectoryData & traj_data : traj_data_list) { + const rclcpp::Time t0_rclcpp_time = rclcpp::Time(traj_data.pose_list.front().header.stamp); + const rclcpp::Time t1_rclcpp_time = rclcpp::Time(traj_data.pose_list.back().header.stamp); + t_window += t1_rclcpp_time.seconds() - t0_rclcpp_time.seconds(); + } + t_window /= traj_data_list.size(); + + std::vector delta_x_list; + + for (const TrajectoryData & traj_data : traj_data_list) { + const auto t1_pose = rclcpp::Time(traj_data.pose_list.back().header.stamp); + const auto t0_pose = rclcpp::Time(traj_data.pose_list.front().header.stamp); + if (t0_pose > t1_pose) continue; + + const size_t n_twist = traj_data.vx_list.size(); + + const double distance = + norm_xy(traj_data.pose_list.front().pose.position, traj_data.pose_list.back().pose.position); + const auto d_pos = integrate_position( + traj_data.vx_list, traj_data.gyro_list, coef_vx, + tf2::getYaw(traj_data.pose_list.front().pose.orientation)); + + const double dt_pose = (rclcpp::Time(traj_data.pose_list.back().header.stamp) - + rclcpp::Time(traj_data.pose_list.front().header.stamp)) + .seconds(); + const double dt_velocity = + (rclcpp::Time(traj_data.vx_list.back().stamp) - rclcpp::Time(traj_data.vx_list.front().stamp)) + .seconds(); + const double distance_from_twist = + std::sqrt(d_pos.x * d_pos.x + d_pos.y * d_pos.y) * dt_pose / dt_velocity; + + const double delta = std::sqrt(n_twist / t_window) * (distance - distance_from_twist); + delta_x_list.push_back(delta); + } + return calculate_std(delta_x_list) / std::sqrt(t_window); +} + +DeviationEstimator::DeviationEstimator( + const std::string & node_name, const rclcpp::NodeOptions & node_options) +: rclcpp::Node(node_name, node_options), + tf_buffer_(this->get_clock()), + tf_listener_(tf_buffer_), + output_frame_(declare_parameter("output_frame")), + results_dir_(declare_parameter("results_dir")), + results_logger_(results_dir_) +{ + dt_design_ = declare_parameter("dt_design"); + dx_design_ = declare_parameter("dx_design"); + vx_threshold_ = declare_parameter("vx_threshold"); + wz_threshold_ = declare_parameter("wz_threshold"); + accel_threshold_ = declare_parameter("accel_threshold"); + time_window_ = declare_parameter("time_window"); + + // flags for deciding which trajectory to use + gyro_only_use_straight_ = declare_parameter("gyro_estimation.only_use_straight"); + gyro_only_use_moving_ = declare_parameter("gyro_estimation.only_use_moving"); + gyro_only_use_constant_velocity_ = + declare_parameter("gyro_estimation.only_use_constant_velocity"); + gyro_add_bias_uncertainty_ = declare_parameter("gyro_estimation.add_bias_uncertainty"); + velocity_only_use_straight_ = declare_parameter("velocity_estimation.only_use_straight"); + velocity_only_use_moving_ = declare_parameter("velocity_estimation.only_use_moving"); + velocity_only_use_constant_velocity_ = + declare_parameter("velocity_estimation.only_use_constant_velocity"); + velocity_add_bias_uncertainty_ = + declare_parameter("velocity_estimation.add_bias_uncertainty"); + + auto timer_callback = std::bind(&DeviationEstimator::timer_callback, this); + auto period_control = std::chrono::duration_cast( + std::chrono::duration(time_window_)); + timer_ = std::make_shared>( + this->get_clock(), period_control, std::move(timer_callback), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(timer_, nullptr); + + sub_pose_with_cov_ = create_subscription( + "in_pose_with_covariance", 1, + std::bind(&DeviationEstimator::callback_pose_with_covariance, this, _1)); + pub_coef_vx_ = create_publisher("estimated_coef_vx", 1); + pub_bias_angvel_ = + create_publisher("estimated_bias_angular_velocity", 1); + pub_stddev_vx_ = create_publisher("estimated_stddev_vx", 1); + pub_stddev_angvel_ = + create_publisher("estimated_stddev_angular_velocity", 1); + + sub_imu_ = create_subscription( + "in_imu", 1, std::bind(&DeviationEstimator::callback_imu, this, _1)); + sub_wheel_odometry_ = create_subscription( + "in_wheel_odometry", 1, std::bind(&DeviationEstimator::callback_wheel_odometry, this, _1)); + results_logger_.log_estimated_result_section( + 0.2, 0.0, geometry_msgs::msg::Vector3{}, geometry_msgs::msg::Vector3{}); + + gyro_bias_module_ = std::make_unique(); + vel_coef_module_ = std::make_unique(); + validation_module_ = std::make_unique( + declare_parameter("thres_coef_vx"), declare_parameter("thres_stddev_vx"), + declare_parameter("thres_bias_gyro"), declare_parameter("thres_stddev_gyro"), + 5); + transform_listener_ = std::make_shared(this); + + RCLCPP_INFO(this->get_logger(), "[Deviation Estimator] launch success"); +} + +/** + * @brief receive ground-truth pose (e.g. NDT pose) data + */ +void DeviationEstimator::callback_pose_with_covariance( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) +{ + // push pose_msg to queue + geometry_msgs::msg::PoseStamped pose; + pose.header = msg->header; + pose.pose = msg->pose.pose; + pose_buf_.push_back(pose); +} + +/** + * @brief receive velocity data and store it in a buffer + */ +void DeviationEstimator::callback_wheel_odometry( + const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr wheel_odometry_msg_ptr) +{ + tier4_debug_msgs::msg::Float64Stamped vx; + vx.stamp = wheel_odometry_msg_ptr->header.stamp; + vx.data = wheel_odometry_msg_ptr->longitudinal_velocity; + + vx_all_.push_back(vx); +} + +/** + * @brief receive IMU data, transform it into a required frame, and store it in a buffer + */ +void DeviationEstimator::callback_imu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr) +{ + imu_frame_ = imu_msg_ptr->header.frame_id; + geometry_msgs::msg::TransformStamped::ConstSharedPtr tf_imu2base_ptr = + transform_listener_->getLatestTransform(imu_frame_, output_frame_); + if (!tf_imu2base_ptr) { + RCLCPP_ERROR( + this->get_logger(), "Please publish TF %s to %s", output_frame_.c_str(), + (imu_frame_).c_str()); + return; + } + + geometry_msgs::msg::Vector3Stamped gyro; + gyro.header.stamp = imu_msg_ptr->header.stamp; + gyro.vector = transform_vector3(imu_msg_ptr->angular_velocity, *tf_imu2base_ptr); + + gyro_all_.push_back(gyro); +} + +/** + * @brief process the stored IMU, velocity, and pose data to estimate bias and standard deviation + */ +void DeviationEstimator::timer_callback() +{ + if (gyro_all_.empty()) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "No IMU data"); + return; + } + if (vx_all_.empty()) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "No wheel odometry"); + return; + } + if (pose_buf_.size() == 0) return; + rclcpp::Time t0_rclcpp_time = rclcpp::Time(pose_buf_.front().header.stamp); + rclcpp::Time t1_rclcpp_time = rclcpp::Time(pose_buf_.back().header.stamp); + if (t1_rclcpp_time <= t0_rclcpp_time) return; + + TrajectoryData traj_data; + traj_data.pose_list = pose_buf_; + traj_data.vx_list = extract_sub_trajectory(vx_all_, t0_rclcpp_time, t1_rclcpp_time); + traj_data.gyro_list = extract_sub_trajectory(gyro_all_, t0_rclcpp_time, t1_rclcpp_time); + bool is_straight = get_mean_abs_wz(traj_data.gyro_list) < wz_threshold_; + bool is_moving = get_mean_abs_vx(traj_data.vx_list) > vx_threshold_; + bool is_constant_velocity = std::abs(get_mean_accel(traj_data.vx_list)) < accel_threshold_; + + const bool use_gyro = whether_to_use_data( + is_straight, is_moving, is_constant_velocity, gyro_only_use_straight_, gyro_only_use_moving_, + gyro_only_use_constant_velocity_); + const bool use_velocity = whether_to_use_data( + is_straight, is_moving, is_constant_velocity, velocity_only_use_straight_, + velocity_only_use_moving_, velocity_only_use_constant_velocity_); + if (use_velocity) { + vel_coef_module_->update_coef(traj_data); + traj_data_list_for_velocity_.push_back(traj_data); + } + if (use_gyro) { + gyro_bias_module_->update_bias(traj_data); + traj_data_list_for_gyro_.push_back(traj_data); + } + + pose_buf_.clear(); + + double stddev_vx = + estimate_stddev_velocity(traj_data_list_for_velocity_, vel_coef_module_->get_coef()); + if (velocity_add_bias_uncertainty_) { + stddev_vx = add_bias_uncertainty_on_velocity(stddev_vx, vel_coef_module_->get_coef_std()); + } + + auto stddev_angvel_base = estimate_stddev_angular_velocity( + traj_data_list_for_gyro_, gyro_bias_module_->get_bias_base_link()); + if (gyro_add_bias_uncertainty_) { + stddev_angvel_base = add_bias_uncertainty_on_angular_velocity( + stddev_angvel_base, gyro_bias_module_->get_bias_std()); + } + + // publish messages + std_msgs::msg::Float64 coef_vx_msg; + coef_vx_msg.data = vel_coef_module_->get_coef(); + pub_coef_vx_->publish(coef_vx_msg); + + geometry_msgs::msg::TransformStamped::ConstSharedPtr tf_base2imu_ptr = + transform_listener_->getLatestTransform(output_frame_, imu_frame_); + if (!tf_base2imu_ptr) { + RCLCPP_ERROR( + this->get_logger(), "Please publish TF %s to %s", imu_frame_.c_str(), output_frame_.c_str()); + return; + } + const geometry_msgs::msg::Vector3 bias_angvel_imu = + transform_vector3(gyro_bias_module_->get_bias_base_link(), *tf_base2imu_ptr); + pub_bias_angvel_->publish(bias_angvel_imu); + + std_msgs::msg::Float64 stddev_vx_msg; + stddev_vx_msg.data = stddev_vx; + pub_stddev_vx_->publish(stddev_vx_msg); + + // For IMU link standard deviation, we use the yaw standard deviation in base_link. + // This is because the standard deviation estimation of x and y in base_link may not be accurate + // especially when the data contains a motion when the people are getting on/off the vehicle, + // which causes the vehicle to tilt in roll and pitch. In this case, we would like to use the + // standard deviation of yaw axis in base_link. + double stddev_angvel_imu = stddev_angvel_base.z; + geometry_msgs::msg::Vector3 stddev_angvel_imu_msg = + createVector3(stddev_angvel_imu, stddev_angvel_imu, stddev_angvel_imu); + pub_stddev_angvel_->publish(stddev_angvel_imu_msg); + + validation_module_->set_velocity_data(vel_coef_module_->get_coef(), stddev_vx); + validation_module_->set_gyro_data(bias_angvel_imu, stddev_angvel_imu_msg); + + results_logger_.log_estimated_result_section( + stddev_vx, vel_coef_module_->get_coef(), stddev_angvel_imu_msg, bias_angvel_imu); + results_logger_.log_validation_result_section(*validation_module_); +} + +/** + * @brief add uncertainty due to the deviation of speed scale factor on velocity standard deviation + */ +double DeviationEstimator::add_bias_uncertainty_on_velocity( + const double stddev_vx, const double stddev_coef_vx) const +{ + const double stddev_vx_prime = + std::sqrt(pow(stddev_vx, 2) + pow(stddev_coef_vx, 2) * pow(dx_design_, 2) / dt_design_); + return stddev_vx_prime; +} + +/** + * @brief add uncertainty due to the deviation of gyroscope bias on angular velocity standard + * deviation + */ +geometry_msgs::msg::Vector3 DeviationEstimator::add_bias_uncertainty_on_angular_velocity( + const geometry_msgs::msg::Vector3 stddev_angvel_base, + const geometry_msgs::msg::Vector3 stddev_angvel_bias_base) const +{ + geometry_msgs::msg::Vector3 stddev_angvel_prime_base = createVector3( + std::sqrt(pow(stddev_angvel_base.x, 2) + dt_design_ * pow(stddev_angvel_bias_base.x, 2)), + std::sqrt(pow(stddev_angvel_base.y, 2) + dt_design_ * pow(stddev_angvel_bias_base.y, 2)), + std::sqrt(pow(stddev_angvel_base.z, 2) + dt_design_ * pow(stddev_angvel_bias_base.z, 2))); + return stddev_angvel_prime_base; +} diff --git a/localization/deviation_estimation_tools/deviation_estimator/src/deviation_estimator_main.cpp b/localization/deviation_estimation_tools/deviation_estimator/src/deviation_estimator_main.cpp new file mode 100644 index 00000000..6346e8e0 --- /dev/null +++ b/localization/deviation_estimation_tools/deviation_estimator/src/deviation_estimator_main.cpp @@ -0,0 +1,273 @@ +// Copyright 2018-2019 Autoware Foundation +// +// 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 "deviation_estimator/deviation_estimator.hpp" + +#include +#include +#include + +int main(int argc, char ** argv) +{ + if (argc != 2) { + std::cout << "Usage: " << argv[0] << " " << std::endl; + return 1; + } + const std::string rosbag_path = argv[1]; + + std::cout << "deviation_estimator_unit_tool" << std::endl; + + // Load parameters + std::map param_map; + { + const std::string yaml_path = + ament_index_cpp::get_package_share_directory("deviation_estimator") + + "/config/deviation_estimator.param.yaml"; + rcl_params_t * params_st = rcl_yaml_node_struct_init(rcl_get_default_allocator()); + if (!rcl_parse_yaml_file(yaml_path.c_str(), params_st)) { + std::cerr << "Failed to parse yaml file: " << yaml_path << std::endl; + std::exit(1); + } + const std::vector parameters = + rclcpp::parameter_map_from(params_st, "").at(""); + rcl_yaml_node_struct_fini(params_st); + for (const rclcpp::Parameter & param : parameters) { + param_map[param.get_name()] = param; + } + } + + // Prepare rosbag reader + rosbag2_storage::StorageOptions storage_options; + storage_options.uri = rosbag_path; + storage_options.storage_id = "sqlite3"; + rosbag2_cpp::ConverterOptions converter_options; + converter_options.input_serialization_format = "cdr"; + converter_options.output_serialization_format = "cdr"; + rosbag2_cpp::readers::SequentialReader reader; + reader.open(storage_options, converter_options); + + // Prepare serialization + rclcpp::Serialization serialization_velocity_status; + rclcpp::Serialization serialization_tf; + rclcpp::Serialization serialization_imu; + rclcpp::Serialization serialization_pose; + + // Prepare tf_buffer + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_ROS_TIME); + tf2_ros::Buffer tf_buffer(clock); + + // Prepare variables + std::vector trajectory_data_list; + rclcpp::Time first_stamp(std::numeric_limits::max(), RCL_ROS_TIME); + const double time_window = param_map.at("time_window").as_double(); + std::string imu_frame_id; + + // ----------- // + // Read rosbag // + // ----------- // + while (reader.has_next()) { + const rosbag2_storage::SerializedBagMessageSharedPtr serialized_message = reader.read_next(); + const std::string topic_name = serialized_message->topic_name; + const rclcpp::SerializedMessage msg(*serialized_message->serialized_data); + + if (topic_name == "/vehicle/status/velocity_status") { + autoware_vehicle_msgs::msg::VelocityReport::SharedPtr velocity_status_msg = + std::make_shared(); + serialization_velocity_status.deserialize_message(&msg, velocity_status_msg.get()); + const rclcpp::Time curr_stamp = velocity_status_msg->header.stamp; + first_stamp = std::min(first_stamp, curr_stamp); + const double diff_sec = (curr_stamp - first_stamp).seconds(); + const int64_t index = static_cast(diff_sec / time_window); + if (index >= static_cast(trajectory_data_list.size())) { + trajectory_data_list.resize(index + 1); + } + tier4_debug_msgs::msg::Float64Stamped vx; + vx.stamp = velocity_status_msg->header.stamp; + vx.data = velocity_status_msg->longitudinal_velocity; + trajectory_data_list[index].vx_list.push_back(vx); + + } else if (topic_name == "/tf_static") { + tf2_msgs::msg::TFMessage::SharedPtr tf_msg = std::make_shared(); + serialization_tf.deserialize_message(&msg, tf_msg.get()); + for (const auto & transform : tf_msg->transforms) { + try { + tf_buffer.setTransform(transform, "default_authority", false); + } catch (const tf2::TransformException & ex) { + std::cerr << "Transform exception: " << ex.what() << std::endl; + std::exit(1); + } + } + + } else if (topic_name == "/sensing/imu/tamagawa/imu_raw") { + sensor_msgs::msg::Imu::SharedPtr imu_msg = std::make_shared(); + serialization_imu.deserialize_message(&msg, imu_msg.get()); + imu_frame_id = imu_msg->header.frame_id; + const geometry_msgs::msg::TransformStamped transform = + tf_buffer.lookupTransform("base_link", imu_msg->header.frame_id, tf2::TimePointZero); + geometry_msgs::msg::Vector3Stamped vec_stamped_transformed; + vec_stamped_transformed.header = imu_msg->header; + tf2::doTransform(imu_msg->angular_velocity, vec_stamped_transformed.vector, transform); + const rclcpp::Time curr_stamp = imu_msg->header.stamp; + first_stamp = std::min(first_stamp, curr_stamp); + const double diff_sec = (curr_stamp - first_stamp).seconds(); + const int64_t index = static_cast(diff_sec / time_window); + if (index >= static_cast(trajectory_data_list.size())) { + trajectory_data_list.resize(index + 1); + } + trajectory_data_list[index].gyro_list.push_back(vec_stamped_transformed); + + } else if (topic_name == "/localization/pose_estimator/pose_with_covariance") { + geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr pose_msg = + std::make_shared(); + serialization_pose.deserialize_message(&msg, pose_msg.get()); + const rclcpp::Time curr_stamp = pose_msg->header.stamp; + first_stamp = std::min(first_stamp, curr_stamp); + const double diff_sec = (curr_stamp - first_stamp).seconds(); + const int64_t index = static_cast(diff_sec / time_window); + if (index >= static_cast(trajectory_data_list.size())) { + trajectory_data_list.resize(index + 1); + } + geometry_msgs::msg::PoseStamped pose_stamped; + pose_stamped.header = pose_msg->header; + pose_stamped.pose = pose_msg->pose.pose; + trajectory_data_list[index].pose_list.push_back(pose_stamped); + + } else { + } + } + + // ------------------ // + // Estimate deviation // + // ------------------ // + const double wz_threshold = param_map.at("wz_threshold").as_double(); + const double vx_threshold = param_map.at("vx_threshold").as_double(); + const double accel_threshold = param_map.at("accel_threshold").as_double(); + const bool gyro_only_use_straight = param_map.at("gyro_estimation.only_use_straight").as_bool(); + const bool gyro_only_use_moving = param_map.at("gyro_estimation.only_use_moving").as_bool(); + const bool gyro_only_use_constant_velocity = + param_map.at("gyro_estimation.only_use_constant_velocity").as_bool(); + const bool gyro_add_bias_uncertainty = + param_map.at("gyro_estimation.add_bias_uncertainty").as_bool(); + const bool velocity_only_use_straight = + param_map.at("velocity_estimation.only_use_straight").as_bool(); + const bool velocity_only_use_moving = + param_map.at("velocity_estimation.only_use_moving").as_bool(); + const bool velocity_only_use_constant_velocity = + param_map.at("velocity_estimation.only_use_constant_velocity").as_bool(); + const bool velocity_add_bias_uncertainty = + param_map.at("velocity_estimation.add_bias_uncertainty").as_bool(); + std::unique_ptr gyro_bias_module = std::make_unique(); + std::unique_ptr vel_coef_module = std::make_unique(); + std::vector traj_data_list_for_velocity; + std::vector traj_data_list_for_gyro; + + if (gyro_add_bias_uncertainty) { + std::cerr << "gyro_add_bias_uncertainty is not supported yet." << std::endl; + std::exit(1); + } + if (velocity_add_bias_uncertainty) { + std::cerr << "velocity_add_bias_uncertainty is not supported yet." << std::endl; + std::exit(1); + } + + std::unique_ptr validation_module = std::make_unique( + param_map.at("thres_coef_vx").as_double(), param_map.at("thres_stddev_vx").as_double(), + param_map.at("thres_bias_gyro").as_double(), param_map.at("thres_stddev_gyro").as_double(), 5); + + Logger results_logger("."); + + for (const TrajectoryData & traj_data : trajectory_data_list) { + std::cout << "traj_data.pose_list.size(): " << traj_data.pose_list.size() << std::endl; + std::cout << "traj_data.gyro_list.size(): " << traj_data.gyro_list.size() << std::endl; + std::cout << "traj_data.vx_list.size(): " << traj_data.vx_list.size() << std::endl; + + // Skip if there is too little data such as terminal data + if ( + traj_data.pose_list.size() < 2 || traj_data.gyro_list.size() < 2 || + traj_data.vx_list.size() < 2) { + continue; + } + + const bool is_straight = get_mean_abs_wz(traj_data.gyro_list) < wz_threshold; + const bool is_moving = get_mean_abs_vx(traj_data.vx_list) > vx_threshold; + const bool is_constant_velocity = std::abs(get_mean_accel(traj_data.vx_list)) < accel_threshold; + + const bool use_gyro = whether_to_use_data( + is_straight, is_moving, is_constant_velocity, gyro_only_use_straight, gyro_only_use_moving, + gyro_only_use_constant_velocity); + const bool use_velocity = whether_to_use_data( + is_straight, is_moving, is_constant_velocity, velocity_only_use_straight, + velocity_only_use_moving, velocity_only_use_constant_velocity); + if (use_velocity) { + vel_coef_module->update_coef(traj_data); + traj_data_list_for_velocity.push_back(traj_data); + } + if (use_gyro) { + gyro_bias_module->update_bias(traj_data); + traj_data_list_for_gyro.push_back(traj_data); + } + + double stddev_vx = + estimate_stddev_velocity(traj_data_list_for_velocity, vel_coef_module->get_coef()); + + auto stddev_angvel_base = estimate_stddev_angular_velocity( + traj_data_list_for_gyro, gyro_bias_module->get_bias_base_link()); + + // print + const geometry_msgs::msg::Vector3 curr_gyro_bias = gyro_bias_module->get_bias_base_link(); + + std::cout << std::fixed // + << "is_straight=" << is_straight // + << ", is_moving=" << is_moving // + << ", is_constant_velocity=" << is_constant_velocity // + << ", use_gyro=" << use_gyro // + << ", use_velocity=" << use_velocity // + << ", vel_coef_module->get_coef()=" << vel_coef_module->get_coef() // + << ", curr_gyro_bias.x=" << curr_gyro_bias.x // + << ", curr_gyro_bias.y=" << curr_gyro_bias.y // + << ", curr_gyro_bias.z=" << curr_gyro_bias.z // + << ", stddev_vx=" << stddev_vx // + << ", stddev_angvel_base.x=" << stddev_angvel_base.x // + << ", stddev_angvel_base.y=" << stddev_angvel_base.y // + << ", stddev_angvel_base.z=" << stddev_angvel_base.z // + << std::endl; + + // For IMU link standard deviation, we use the yaw standard deviation in base_link. + // This is because the standard deviation estimation of x and y in base_link may not be accurate + // especially when the data contains a motion when the people are getting on/off the vehicle, + // which causes the vehicle to tilt in roll and pitch. In this case, we would like to use the + // standard deviation of yaw axis in base_link. + double stddev_angvel_imu = stddev_angvel_base.z; + geometry_msgs::msg::Vector3 stddev_angvel_imu_msg = + createVector3(stddev_angvel_imu, stddev_angvel_imu, stddev_angvel_imu); + + const geometry_msgs::msg::TransformStamped transform = + tf_buffer.lookupTransform(imu_frame_id, "base_link", tf2::TimePointZero); + geometry_msgs::msg::Vector3 bias_angvel_imu; + tf2::doTransform(gyro_bias_module->get_bias_base_link(), bias_angvel_imu, transform); + + validation_module->set_velocity_data(vel_coef_module->get_coef(), stddev_vx); + validation_module->set_gyro_data(bias_angvel_imu, stddev_angvel_imu_msg); + + results_logger.log_estimated_result_section( + stddev_vx, vel_coef_module->get_coef(), stddev_angvel_imu_msg, bias_angvel_imu); + results_logger.log_validation_result_section(*validation_module); + + std::cout << "saved to ./" << std::endl; + } + + std::cout << "count for velocity : " << traj_data_list_for_velocity.size() << std::endl; + std::cout << "count for gyro : " << traj_data_list_for_gyro.size() << std::endl; + std::cout << "Finished." << std::endl; +} diff --git a/localization/deviation_estimation_tools/deviation_estimator/src/deviation_estimator_node.cpp b/localization/deviation_estimation_tools/deviation_estimator/src/deviation_estimator_node.cpp new file mode 100644 index 00000000..40f34ffc --- /dev/null +++ b/localization/deviation_estimation_tools/deviation_estimator/src/deviation_estimator_node.cpp @@ -0,0 +1,28 @@ +// Copyright 2018-2019 Autoware Foundation +// +// 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 "deviation_estimator/deviation_estimator.hpp" + +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + auto node = std::make_shared("deviation_estimator", node_options); + + rclcpp::spin(node); + + return 0; +} diff --git a/localization/deviation_estimation_tools/deviation_estimator/src/gyro_bias_module.cpp b/localization/deviation_estimation_tools/deviation_estimator/src/gyro_bias_module.cpp new file mode 100644 index 00000000..82ef0058 --- /dev/null +++ b/localization/deviation_estimation_tools/deviation_estimator/src/gyro_bias_module.cpp @@ -0,0 +1,91 @@ +// Copyright 2018-2019 Autoware Foundation +// +// 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 "deviation_estimator/gyro_bias_module.hpp" + +#include "deviation_estimator/utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" + +/** + * @brief update gyroscope bias based on a given trajectory data + */ +void GyroBiasModule::update_bias(const TrajectoryData & traj_data) +{ + const rclcpp::Time t0_rclcpp_time = rclcpp::Time(traj_data.pose_list.front().header.stamp); + const rclcpp::Time t1_rclcpp_time = rclcpp::Time(traj_data.pose_list.back().header.stamp); + const double dt = t1_rclcpp_time.seconds() - t0_rclcpp_time.seconds(); + + auto error_rpy = + calculate_error_rpy(traj_data.pose_list, traj_data.gyro_list, geometry_msgs::msg::Vector3{}); + const double dt_pose = (rclcpp::Time(traj_data.pose_list.back().header.stamp) - + rclcpp::Time(traj_data.pose_list.front().header.stamp)) + .seconds(); + const double dt_gyro = (rclcpp::Time(traj_data.gyro_list.back().header.stamp) - + rclcpp::Time(traj_data.gyro_list.front().header.stamp)) + .seconds(); + error_rpy.x *= dt_pose / dt_gyro; + error_rpy.y *= dt_pose / dt_gyro; + error_rpy.z *= dt_pose / dt_gyro; + + gyro_bias_pair_.first.x += dt * error_rpy.x; + gyro_bias_pair_.first.y += dt * error_rpy.y; + gyro_bias_pair_.first.z += dt * error_rpy.z; + gyro_bias_pair_.second.x += dt * dt; + gyro_bias_pair_.second.y += dt * dt; + gyro_bias_pair_.second.z += dt * dt; + + geometry_msgs::msg::Vector3 gyro_bias; + gyro_bias.x = error_rpy.x / dt; + gyro_bias.y = error_rpy.y / dt; + gyro_bias.z = error_rpy.z / dt; + gyro_bias_list_.push_back(gyro_bias); +} + +/** + * @brief getter function for current estimated bias + */ +geometry_msgs::msg::Vector3 GyroBiasModule::get_bias_base_link() const +{ + geometry_msgs::msg::Vector3 gyro_bias_base; + gyro_bias_base.x = gyro_bias_pair_.first.x / gyro_bias_pair_.second.x; + gyro_bias_base.y = gyro_bias_pair_.first.y / gyro_bias_pair_.second.y; + gyro_bias_base.z = gyro_bias_pair_.first.z / gyro_bias_pair_.second.z; + return gyro_bias_base; +} + +/** + * @brief getter function for current standard deviation of estimated bias + */ +geometry_msgs::msg::Vector3 GyroBiasModule::get_bias_std() const +{ + std::vector stddev_bias_list_x, stddev_bias_list_y, stddev_bias_list_z; + for (const auto & e : gyro_bias_list_) { + stddev_bias_list_x.push_back(e.x); + stddev_bias_list_y.push_back(e.y); + stddev_bias_list_z.push_back(e.z); + } + geometry_msgs::msg::Vector3 stddev_bias; + stddev_bias.x = calculate_std_mean_const( + stddev_bias_list_x, gyro_bias_pair_.first.x / gyro_bias_pair_.second.x); + stddev_bias.y = calculate_std_mean_const( + stddev_bias_list_y, gyro_bias_pair_.first.y / gyro_bias_pair_.second.y); + stddev_bias.z = calculate_std_mean_const( + stddev_bias_list_z, gyro_bias_pair_.first.z / gyro_bias_pair_.second.z); + return stddev_bias; +} + +bool GyroBiasModule::empty() const +{ + return gyro_bias_list_.empty(); +} diff --git a/localization/deviation_estimation_tools/deviation_estimator/src/logger.cpp b/localization/deviation_estimation_tools/deviation_estimator/src/logger.cpp new file mode 100644 index 00000000..e3d8d3fd --- /dev/null +++ b/localization/deviation_estimation_tools/deviation_estimator/src/logger.cpp @@ -0,0 +1,107 @@ +// Copyright 2018-2019 Autoware Foundation +// +// 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 "deviation_estimator/logger.hpp" + +#include + +/** + * @brief constructor for Logger class + */ +Logger::Logger(const std::string & output_dir) +: output_log_path_(output_dir + "/output.txt"), + output_imu_param_path_(output_dir + "/imu_corrector.param.yaml"), + output_velocity_param_path_(output_dir + "/vehicle_velocity_converter.param.yaml") +{ + std::ofstream file_log(output_log_path_); + file_log.close(); + + std::ofstream file_imu_param(output_imu_param_path_); + file_imu_param.close(); + + std::ofstream file_velocity_param(output_velocity_param_path_); + file_velocity_param.close(); +} + +/** + * @brief log estimated results (IMU and velocity parameters) + */ +void Logger::log_estimated_result_section( + const double stddev_vx, const double coef_vx, + const geometry_msgs::msg::Vector3 & angular_velocity_stddev, + const geometry_msgs::msg::Vector3 & angular_velocity_offset) const +{ + std::ofstream file_velocity_param(output_velocity_param_path_); + file_velocity_param << "# Estimated by deviation_estimator\n"; + file_velocity_param << "/**:\n"; + file_velocity_param << " ros__parameters:\n"; + file_velocity_param << fmt::format(" speed_scale_factor: {:.5f}\n", coef_vx); + file_velocity_param << fmt::format(" velocity_stddev_xx: {:.5f}\n", stddev_vx); + file_velocity_param << " angular_velocity_stddev_zz: 0.1 # Default value\n"; + file_velocity_param << " frame_id: base_link # Default value\n"; + file_velocity_param.close(); + + std::ofstream file_imu_param(output_imu_param_path_); + file_imu_param << "# Estimated by deviation_estimator\n"; + file_imu_param << "/**:\n"; + file_imu_param << " ros__parameters:\n"; + file_imu_param << fmt::format( + " angular_velocity_offset_x: {:.5f}\n", angular_velocity_offset.x); + file_imu_param << fmt::format( + " angular_velocity_offset_y: {:.5f}\n", angular_velocity_offset.y); + file_imu_param << fmt::format( + " angular_velocity_offset_z: {:.5f}\n", angular_velocity_offset.z); + file_imu_param << fmt::format( + " angular_velocity_stddev_xx: {:.5f}\n", angular_velocity_stddev.x); + file_imu_param << fmt::format( + " angular_velocity_stddev_yy: {:.5f}\n", angular_velocity_stddev.y); + file_imu_param << fmt::format( + " angular_velocity_stddev_zz: {:.5f}\n", angular_velocity_stddev.z); +} + +/** + * @brief log validation results + */ +void Logger::log_validation_result_section(const ValidationModule & validation_module) const +{ + std::ofstream file(output_log_path_); + file << "# Validation results\n"; + file << "# value: [min, max]\n"; + + std::vector keys{ + "coef_vx", + "stddev_vx", + "angular_velocity_offset_x", + "angular_velocity_offset_y", + "angular_velocity_offset_z", + "angular_velocity_stddev_xx", + "angular_velocity_stddev_yy", + "angular_velocity_stddev_zz"}; + + for (const std::string & key : keys) { + try { + const auto min_max = validation_module.get_min_max(key); + if (validation_module.is_valid(key)) { + file << "[OK] "; + } else { + file << "[NG] "; + } + file << fmt::format( + "{}: [{}, {}]\n", key, double_round(min_max.first, 5), double_round(min_max.second, 5)); + } catch (std::domain_error & e) { // if the data is not enough + file << fmt::format("[NG] {}: Not enough data provided yet\n", key); + } + } + file.close(); +} diff --git a/localization/deviation_estimation_tools/deviation_estimator/src/utils.cpp b/localization/deviation_estimation_tools/deviation_estimator/src/utils.cpp new file mode 100644 index 00000000..1fb84235 --- /dev/null +++ b/localization/deviation_estimation_tools/deviation_estimator/src/utils.cpp @@ -0,0 +1,225 @@ +// Copyright 2018-2019 Autoware Foundation +// +// 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 "deviation_estimator/utils.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" + +#include +#include + +#include +#include + +double double_round(const double x, const int n) +{ + return std::round(x * std::pow(10, n)) / std::pow(10, n); +} + +double clip_radian(const double rad) +{ + if (rad < -M_PI) { + return rad + 2 * M_PI; + } else if (rad >= M_PI) { + return rad - 2 * M_PI; + } else { + return rad; + } +} + +bool whether_to_use_data( + const bool is_straight, const bool is_moving, const bool is_constant_velocity, + const bool only_use_straight, const bool only_use_moving, const bool only_use_constant_velocity) +{ + return (is_straight || !only_use_straight) && (is_moving || !only_use_moving) && + (is_constant_velocity || !only_use_constant_velocity); +} + +/** + * @brief calculate the vector3 value at the given "time", based on the interpolation on "vec_list" + */ +geometry_msgs::msg::Vector3 interpolate_vector3_stamped( + const std::vector & vec_list, const double time, + const double tolerance_sec = 0.1) +{ + std::vector time_list{}; + for (const auto & vec : vec_list) { + time_list.push_back(rclcpp::Time(vec.header.stamp).seconds()); + } + + const int next_idx = + std::upper_bound(time_list.begin(), time_list.end(), time) - time_list.begin(); + + if (next_idx == 0) { + if (time_list.front() - time > tolerance_sec) { + throw std::domain_error("interpolate_vector3_stamped failed! Query time is too small."); + } + return vec_list.front().vector; + } else if (next_idx == time_list.end() - time_list.begin()) { + if (time - time_list.back() > tolerance_sec) { + throw std::domain_error("interpolate_vector3_stamped failed! Query time is too large."); + } + return vec_list.back().vector; + } else { + const int prev_idx = next_idx - 1; + const double ratio = (time - time_list[prev_idx]) / (time_list[next_idx] - time_list[prev_idx]); + geometry_msgs::msg::Point interpolated_vec = + calcInterpolatedPoint(vec_list[prev_idx].vector, vec_list[next_idx].vector, ratio); + + return createVector3(interpolated_vec.x, interpolated_vec.y, interpolated_vec.z); + } +} + +/** + * @brief perform a simple dead reckoning and return a relative position of end point from start + * point + */ +geometry_msgs::msg::Point integrate_position( + const std::vector & vx_list, + const std::vector & gyro_list, const double coef_vx, + const double yaw_init) +{ + double t_prev = rclcpp::Time(vx_list.front().stamp).seconds(); + double yaw = yaw_init; + geometry_msgs::msg::Point d_pos = tier4_autoware_utils::createPoint(0.0, 0.0, 0.0); + for (std::size_t i = 0; i < vx_list.size() - 1; ++i) { + const double t_cur = rclcpp::Time(vx_list[i + 1].stamp).seconds(); + const geometry_msgs::msg::Vector3 gyro_interpolated = + interpolate_vector3_stamped(gyro_list, rclcpp::Time(vx_list[i + 1].stamp).seconds()); + yaw += gyro_interpolated.z * (t_cur - t_prev); + + d_pos.x += (t_cur - t_prev) * vx_list[i].data * std::cos(yaw) * coef_vx; + d_pos.y += (t_cur - t_prev) * vx_list[i].data * std::sin(yaw) * coef_vx; + + t_prev = t_cur; + } + return d_pos; +} + +/** + * @brief calculate RPY error on dead-reckoning (calculated from "gyro_list") compared to the + * ground-truth pose from "pose_list". + */ +geometry_msgs::msg::Vector3 calculate_error_rpy( + const std::vector & pose_list, + const std::vector & gyro_list, + const geometry_msgs::msg::Vector3 & gyro_bias) +{ + const geometry_msgs::msg::Vector3 rpy_0 = + tier4_autoware_utils::getRPY(pose_list.front().pose.orientation); + const geometry_msgs::msg::Vector3 rpy_1 = + tier4_autoware_utils::getRPY(pose_list.back().pose.orientation); + const geometry_msgs::msg::Vector3 d_rpy = integrate_orientation(gyro_list, gyro_bias); + + geometry_msgs::msg::Vector3 error_rpy = createVector3( + clip_radian(-rpy_1.x + rpy_0.x + d_rpy.x), clip_radian(-rpy_1.y + rpy_0.y + d_rpy.y), + clip_radian(-rpy_1.z + rpy_0.z + d_rpy.z)); + return error_rpy; +} + +/** + * @brief perform dead reckoning based on "gyro_list" and return a relative pose (in RPY) + */ +geometry_msgs::msg::Vector3 integrate_orientation( + const std::vector & gyro_list, + const geometry_msgs::msg::Vector3 & gyro_bias) +{ + geometry_msgs::msg::Vector3 d_rpy = createVector3(0.0, 0.0, 0.0); + double t_prev = rclcpp::Time(gyro_list.front().header.stamp).seconds(); + for (std::size_t i = 0; i < gyro_list.size() - 1; ++i) { + double t_cur = rclcpp::Time(gyro_list[i + 1].header.stamp).seconds(); + + d_rpy.x += (t_cur - t_prev) * (gyro_list[i].vector.x - gyro_bias.x); + d_rpy.y += (t_cur - t_prev) * (gyro_list[i].vector.y - gyro_bias.y); + d_rpy.z += (t_cur - t_prev) * (gyro_list[i].vector.z - gyro_bias.z); + + t_prev = t_cur; + } + return d_rpy; +} + +/** + * @brief calculate mean of |vx| + */ +double get_mean_abs_vx(const std::vector & vx_list) +{ + double mean_abs_vx = 0; + for (const auto & msg : vx_list) { + mean_abs_vx += abs(msg.data); + } + mean_abs_vx /= vx_list.size(); + return mean_abs_vx; +} + +/** + * @brief calculate mean of |wz| + */ +double get_mean_abs_wz(const std::vector & gyro_list) +{ + double mean_abs_wz = 0; + for (auto & msg : gyro_list) { + mean_abs_wz += abs(msg.vector.z); + } + mean_abs_wz /= gyro_list.size(); + return mean_abs_wz; +} + +/** + * @brief calculate mean of acceleration + */ +double get_mean_accel(const std::vector & vx_list) +{ + const double dt = + (rclcpp::Time(vx_list.back().stamp) - rclcpp::Time(vx_list.front().stamp)).seconds(); + return (vx_list.back().data - vx_list.front().data) / dt; +} + +/** + * @brief transform a vector by "transform" + */ +geometry_msgs::msg::Vector3 transform_vector3( + const geometry_msgs::msg::Vector3 & vec, const geometry_msgs::msg::TransformStamped & transform) +{ + geometry_msgs::msg::Vector3Stamped vec_stamped; + vec_stamped.vector = vec; + + geometry_msgs::msg::Vector3Stamped vec_stamped_transformed; + tf2::doTransform(vec_stamped, vec_stamped_transformed, transform); + return vec_stamped_transformed.vector; +} + +/* This function is copied from tf2_geometry_msgs package, which should ideally be avoided. + However, at the time of implementation, we got the "undefined reference" error for fromMsg, + and thus TEMPORARILY ported the function here. */ +inline void myFromMsg(const geometry_msgs::msg::Transform & in, tf2::Transform & out) +{ + out.setOrigin(tf2::Vector3(in.translation.x, in.translation.y, in.translation.z)); + // w at the end in the constructor + out.setRotation(tf2::Quaternion(in.rotation.x, in.rotation.y, in.rotation.z, in.rotation.w)); +} + +/** + * @brief inverse a transform + */ +geometry_msgs::msg::TransformStamped inverse_transform( + const geometry_msgs::msg::TransformStamped & transform) +{ + tf2::Transform tf; + myFromMsg(transform.transform, tf); + geometry_msgs::msg::TransformStamped transform_inv; + transform_inv.header = transform.header; + transform_inv.transform = tf2::toMsg(tf.inverse()); + return transform_inv; +} diff --git a/localization/deviation_estimation_tools/deviation_estimator/src/validation_module.cpp b/localization/deviation_estimation_tools/deviation_estimator/src/validation_module.cpp new file mode 100644 index 00000000..7c089bc7 --- /dev/null +++ b/localization/deviation_estimation_tools/deviation_estimator/src/validation_module.cpp @@ -0,0 +1,107 @@ +// Copyright 2018-2019 Autoware Foundation +// +// 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 "deviation_estimator/validation_module.hpp" + +/** + * @brief ValidationModule validates if estimated parameters are properly converged, given a + * predefined threshold in this constructor arguments + */ +ValidationModule::ValidationModule( + const double threshold_coef_vx, const double threshold_stddev_vx, + const double threshold_bias_gyro, const double threshold_stddev_gyro, const size_t num_history) +: num_history_(num_history) +{ + threshold_dict_["coef_vx"] = threshold_coef_vx; + threshold_dict_["stddev_vx"] = threshold_stddev_vx; + threshold_dict_["angular_velocity_offset_x"] = threshold_bias_gyro; + threshold_dict_["angular_velocity_offset_y"] = threshold_bias_gyro; + threshold_dict_["angular_velocity_offset_z"] = threshold_bias_gyro; + threshold_dict_["angular_velocity_stddev_xx"] = threshold_stddev_gyro; + threshold_dict_["angular_velocity_stddev_yy"] = threshold_stddev_gyro; + threshold_dict_["angular_velocity_stddev_zz"] = threshold_stddev_gyro; +} + +/** + * @brief get a min and max of a given vector + */ +std::pair get_min_max_from_vector(const std::vector & vec, const int num) +{ + double min_val = std::numeric_limits::max(); + double max_val = -std::numeric_limits::max(); + for (int i = 0; i < num; ++i) { + min_val = std::min(min_val, vec[vec.size() - i - 1]); + max_val = std::max(max_val, vec[vec.size() - i - 1]); + } + return std::pair(min_val, max_val); +} + +/** + * @brief add a newly-estimated velocity related parameters + */ +void ValidationModule::set_velocity_data(const double coef_vx, const double stddev_vx) +{ + if (data_list_dict_.count("coef_vx")) { + if (coef_vx == data_list_dict_["coef_vx"].back()) { + return; + } + } + + data_list_dict_["coef_vx"].push_back(coef_vx); + data_list_dict_["stddev_vx"].push_back(stddev_vx); +} + +/** + * @brief add a newly-estimated gyroscope related parameters + */ +void ValidationModule::set_gyro_data( + const geometry_msgs::msg::Vector3 & bias_gyro, const geometry_msgs::msg::Vector3 & stddev_gyro) +{ + if (data_list_dict_.count("angular_velocity_offset_x")) { + if (bias_gyro.x == data_list_dict_["angular_velocity_offset_x"].back()) { + return; + } + } + + data_list_dict_["angular_velocity_offset_x"].push_back(bias_gyro.x); + data_list_dict_["angular_velocity_offset_y"].push_back(bias_gyro.y); + data_list_dict_["angular_velocity_offset_z"].push_back(bias_gyro.z); + data_list_dict_["angular_velocity_stddev_xx"].push_back(stddev_gyro.x); + data_list_dict_["angular_velocity_stddev_yy"].push_back(stddev_gyro.y); + data_list_dict_["angular_velocity_stddev_zz"].push_back(stddev_gyro.z); +} + +/** + * @brief get a min and max of a certain vector (designated by a key) + */ +std::pair ValidationModule::get_min_max(const std::string key) const +{ + if (data_list_dict_.count(key)) { + if (data_list_dict_.at(key).size() < num_history_) { + throw std::domain_error("The data is not enough. Provide more data for valid results."); + } + return get_min_max_from_vector(data_list_dict_.at(key), num_history_); + } else { + throw std::runtime_error("Invalid key in ValidationModule::get_min_max"); + } +} + +/** + * @brief check if the given item (="key") is valid + */ +bool ValidationModule::is_valid(const std::string key) const +{ + const auto min_max = this->get_min_max(key); + return min_max.second - min_max.first < threshold_dict_.at(key); +} diff --git a/localization/deviation_estimation_tools/deviation_estimator/src/velocity_coef_module.cpp b/localization/deviation_estimation_tools/deviation_estimator/src/velocity_coef_module.cpp new file mode 100644 index 00000000..c8346ed7 --- /dev/null +++ b/localization/deviation_estimation_tools/deviation_estimator/src/velocity_coef_module.cpp @@ -0,0 +1,73 @@ +// Copyright 2018-2019 Autoware Foundation +// +// 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 "deviation_estimator/velocity_coef_module.hpp" + +#include "deviation_estimator/utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" + +/** + * @brief update speed scale factor (or velocity coefficient) based on a given trajectory data + */ +void VelocityCoefModule::update_coef(const TrajectoryData & traj_data) +{ + const rclcpp::Time t0_rclcpp_time = rclcpp::Time(traj_data.pose_list.front().header.stamp); + const rclcpp::Time t1_rclcpp_time = rclcpp::Time(traj_data.pose_list.back().header.stamp); + + auto d_pos = integrate_position( + traj_data.vx_list, traj_data.gyro_list, 1.0, + tf2::getYaw(traj_data.pose_list.front().pose.orientation)); + const double dt_pose = (rclcpp::Time(traj_data.pose_list.back().header.stamp) - + rclcpp::Time(traj_data.pose_list.front().header.stamp)) + .seconds(); + const double dt_velocity = + (rclcpp::Time(traj_data.vx_list.back().stamp) - rclcpp::Time(traj_data.vx_list.front().stamp)) + .seconds(); + d_pos.x *= dt_pose / dt_velocity; + d_pos.y *= dt_pose / dt_velocity; + + const double dx = + traj_data.pose_list.back().pose.position.x - traj_data.pose_list.front().pose.position.x; + const double dy = + traj_data.pose_list.back().pose.position.y - traj_data.pose_list.front().pose.position.y; + if (d_pos.x * d_pos.x + d_pos.y * d_pos.y == 0) return; + + const double d_coef_vx = (d_pos.x * dx + d_pos.y * dy) / (d_pos.x * d_pos.x + d_pos.y * d_pos.y); + + coef_vx_.first += d_coef_vx; + coef_vx_.second += 1; + coef_vx_list_.push_back(d_coef_vx); +} + +/** + * @brief getter function for current estimated coefficient + */ +double VelocityCoefModule::get_coef() const +{ + if (coef_vx_.second == 0) return 1.0; + return coef_vx_.first / coef_vx_.second; +} + +/** + * @brief getter function for current estimated standard deviation of coefficient + */ +double VelocityCoefModule::get_coef_std() const +{ + return calculate_std(coef_vx_list_); +} + +bool VelocityCoefModule::empty() const +{ + return coef_vx_.second == 0; +} diff --git a/localization/deviation_estimation_tools/deviation_estimator/test/test_gyro_bias.cpp b/localization/deviation_estimation_tools/deviation_estimator/test/test_gyro_bias.cpp new file mode 100644 index 00000000..fc8bb471 --- /dev/null +++ b/localization/deviation_estimation_tools/deviation_estimator/test/test_gyro_bias.cpp @@ -0,0 +1,71 @@ +// Copyright 2022 Autoware Foundation +// +// 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 "deviation_estimator/gyro_bias_module.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" + +#include + +#include + +TEST(DeviationEstimatorGyroBias, SmokeTestDefault) +{ + const geometry_msgs::msg::Vector3 gyro_bias = createVector3(0.01, -0.005, -0.015); + + const double ERROR_RATE = 0.1; + const int num_data = 1000; + const double dt = 1.0; + + const int gyro_rate = 100; + const int ndt_rate = 10; + const double stddev_gyro = 0.02; + const rclcpp::Time t_start = rclcpp::Time(0, 0); + + GyroBiasModule gyro_bias_module; + + std::mt19937 engine; + engine.seed(); + std::normal_distribution<> dist(0.0, stddev_gyro); + + for (int i = 0; i < num_data; ++i) { + std::vector gyro_data_while_stopped; + for (int i = 0; i <= gyro_rate * dt; ++i) { + geometry_msgs::msg::Vector3Stamped gyro; + gyro.header.stamp = t_start + rclcpp::Duration::from_seconds(1.0 * i / gyro_rate); + gyro.vector.x = dist(engine) + gyro_bias.x; + gyro.vector.y = dist(engine) + gyro_bias.y; + gyro.vector.z = dist(engine) + gyro_bias.z; + gyro_data_while_stopped.push_back(gyro); + } + + std::vector pose_list; + for (int i = 0; i <= ndt_rate * dt; ++i) { + geometry_msgs::msg::PoseStamped pose; + pose.header.stamp = t_start + rclcpp::Duration::from_seconds(1.0 * i / ndt_rate); + pose.pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(0.0, 0.0, 0.0); + pose_list.push_back(pose); + } + + TrajectoryData traj_data; + traj_data.pose_list = pose_list; + traj_data.gyro_list = gyro_data_while_stopped; + + gyro_bias_module.update_bias(traj_data); + } + + const geometry_msgs::msg::Vector3 estimated_gyro_bias = gyro_bias_module.get_bias_base_link(); + EXPECT_NEAR(estimated_gyro_bias.x, gyro_bias.x, std::abs(gyro_bias.x) * ERROR_RATE); + EXPECT_NEAR(estimated_gyro_bias.y, gyro_bias.y, std::abs(gyro_bias.y) * ERROR_RATE); + EXPECT_NEAR(estimated_gyro_bias.z, gyro_bias.z, std::abs(gyro_bias.z) * ERROR_RATE); +} diff --git a/localization/deviation_estimation_tools/deviation_estimator/test/test_gyro_stddev.cpp b/localization/deviation_estimation_tools/deviation_estimator/test/test_gyro_stddev.cpp new file mode 100644 index 00000000..0e40c27f --- /dev/null +++ b/localization/deviation_estimation_tools/deviation_estimator/test/test_gyro_stddev.cpp @@ -0,0 +1,123 @@ +// Copyright 2022 Autoware Foundation +// +// 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 "deviation_estimator/deviation_estimator.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" + +#include + +#include +#include + +TEST(DeviationEstimatorGyroStddev, SmokeTestDefault) +{ + const double ERROR_RATE = 0.1; + + const int n_all = 10000; + const rclcpp::Time t_start = rclcpp::Time(0, 0); + const double stddev_gyro = 0.03141592; + const int gyro_rate = 100; + const int ndt_rate = 10; + const double t_window = 5; + const geometry_msgs::msg::Vector3 gyro_bias = createVector3(0.0, 0.0, 0.0); + + std::mt19937 engine; + engine.seed(); + std::normal_distribution<> dist(0.0, stddev_gyro); + + std::vector traj_data_list; + + for (int tmp = 0; tmp < n_all; ++tmp) { + std::vector gyro_data_while_stopped; + for (int i = 0; i <= gyro_rate * t_window; ++i) { + geometry_msgs::msg::Vector3Stamped gyro; + gyro.header.stamp = t_start + rclcpp::Duration::from_seconds(1.0 * i / gyro_rate); + gyro.vector.x = dist(engine) + gyro_bias.x; + gyro.vector.y = dist(engine) + gyro_bias.y; + gyro.vector.z = dist(engine) + gyro_bias.z; + gyro_data_while_stopped.push_back(gyro); + } + + std::vector pose_list; + for (int i = 0; i <= ndt_rate * t_window; ++i) { + geometry_msgs::msg::PoseStamped pose; + pose.header.stamp = t_start + rclcpp::Duration::from_seconds(1.0 * i / ndt_rate); + pose.pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(0.0, 0.0, 0.0); + pose_list.push_back(pose); + } + + TrajectoryData traj_data; + traj_data.pose_list = pose_list; + traj_data.gyro_list = gyro_data_while_stopped; + traj_data_list.push_back(traj_data); + } + + geometry_msgs::msg::Vector3 estimated_gyro_stddev = + estimate_stddev_angular_velocity(traj_data_list, gyro_bias); + + EXPECT_NEAR(estimated_gyro_stddev.x, stddev_gyro, stddev_gyro * ERROR_RATE); + EXPECT_NEAR(estimated_gyro_stddev.y, stddev_gyro, stddev_gyro * ERROR_RATE); + EXPECT_NEAR(estimated_gyro_stddev.z, stddev_gyro, stddev_gyro * ERROR_RATE); +} + +TEST(DeviationEstimatorGyroStddev, SmokeTestWithBias) +{ + const double ERROR_RATE = 0.1; + + const int n_all = 10000; + const rclcpp::Time t_start = rclcpp::Time(0, 0); + const double stddev_gyro = 0.03141592; + const int gyro_rate = 30; + const int ndt_rate = 10; + const double t_window = 5; + const geometry_msgs::msg::Vector3 gyro_bias = createVector3(0.005, 0.001, -0.01); + + std::mt19937 engine; + engine.seed(); + std::normal_distribution<> dist(0.0, stddev_gyro); + + std::vector traj_data_list; + + for (int tmp = 0; tmp < n_all; ++tmp) { + std::vector gyro_data_while_stopped; + for (int i = 0; i <= gyro_rate * t_window; ++i) { + geometry_msgs::msg::Vector3Stamped gyro; + gyro.header.stamp = t_start + rclcpp::Duration::from_seconds(1.0 * i / gyro_rate); + gyro.vector.x = dist(engine) + gyro_bias.x; + gyro.vector.y = dist(engine) + gyro_bias.y; + gyro.vector.z = dist(engine) + gyro_bias.z; + gyro_data_while_stopped.push_back(gyro); + } + + std::vector pose_list; + for (int i = 0; i <= ndt_rate * t_window; ++i) { + geometry_msgs::msg::PoseStamped pose; + pose.header.stamp = t_start + rclcpp::Duration::from_seconds(1.0 * i / ndt_rate); + pose.pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(0.0, 0.0, 0.0); + pose_list.push_back(pose); + } + + TrajectoryData traj_data; + traj_data.pose_list = pose_list; + traj_data.gyro_list = gyro_data_while_stopped; + traj_data_list.push_back(traj_data); + } + + geometry_msgs::msg::Vector3 estimated_gyro_stddev = + estimate_stddev_angular_velocity(traj_data_list, gyro_bias); + + EXPECT_NEAR(estimated_gyro_stddev.x, stddev_gyro, stddev_gyro * ERROR_RATE); + EXPECT_NEAR(estimated_gyro_stddev.y, stddev_gyro, stddev_gyro * ERROR_RATE); + EXPECT_NEAR(estimated_gyro_stddev.z, stddev_gyro, stddev_gyro * ERROR_RATE); +} diff --git a/localization/deviation_estimation_tools/deviation_estimator/test/test_utils.cpp b/localization/deviation_estimation_tools/deviation_estimator/test/test_utils.cpp new file mode 100644 index 00000000..49c492d6 --- /dev/null +++ b/localization/deviation_estimation_tools/deviation_estimator/test/test_utils.cpp @@ -0,0 +1,108 @@ +// Copyright 2022 Autoware Foundation +// +// 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 "deviation_estimator/utils.hpp" + +#include + +TEST(DeviationEstimatorUtils, WhetherToUseData1) +{ + const bool is_straight = false; + const bool is_moving = false; + const bool is_constant_velocity = false; + const bool only_use_straight = false; + const bool only_use_moving = false; + const bool only_use_constant_velocity = false; + EXPECT_TRUE(whether_to_use_data( + is_straight, is_moving, is_constant_velocity, only_use_straight, only_use_moving, + only_use_constant_velocity)); +} + +TEST(DeviationEstimatorUtils, WhetherToUseData2) +{ + const bool is_straight = true; + const bool is_moving = false; + const bool is_constant_velocity = false; + const bool only_use_straight = false; + const bool only_use_moving = false; + const bool only_use_constant_velocity = false; + EXPECT_TRUE(whether_to_use_data( + is_straight, is_moving, is_constant_velocity, only_use_straight, only_use_moving, + only_use_constant_velocity)); +} + +TEST(DeviationEstimatorUtils, WhetherToUseData3) +{ + const bool is_straight = false; + const bool is_moving = false; + const bool is_constant_velocity = false; + const bool only_use_straight = true; + const bool only_use_moving = false; + const bool only_use_constant_velocity = false; + EXPECT_FALSE(whether_to_use_data( + is_straight, is_moving, is_constant_velocity, only_use_straight, only_use_moving, + only_use_constant_velocity)); +} + +TEST(DeviationEstimatorUtils, WhetherToUseData4) +{ + const bool is_straight = true; + const bool is_moving = false; + const bool is_constant_velocity = false; + const bool only_use_straight = true; + const bool only_use_moving = false; + const bool only_use_constant_velocity = false; + EXPECT_TRUE(whether_to_use_data( + is_straight, is_moving, is_constant_velocity, only_use_straight, only_use_moving, + only_use_constant_velocity)); +} + +TEST(DeviationEstimatorUtils, WhetherToUseData5) +{ + const bool is_straight = true; + const bool is_moving = true; + const bool is_constant_velocity = false; + const bool only_use_straight = true; + const bool only_use_moving = false; + const bool only_use_constant_velocity = false; + EXPECT_TRUE(whether_to_use_data( + is_straight, is_moving, is_constant_velocity, only_use_straight, only_use_moving, + only_use_constant_velocity)); +} + +TEST(DeviationEstimatorUtils, WhetherToUseData6) +{ + const bool is_straight = true; + const bool is_moving = false; + const bool is_constant_velocity = false; + const bool only_use_straight = true; + const bool only_use_moving = true; + const bool only_use_constant_velocity = false; + EXPECT_FALSE(whether_to_use_data( + is_straight, is_moving, is_constant_velocity, only_use_straight, only_use_moving, + only_use_constant_velocity)); +} + +TEST(DeviationEstimatorUtils, WhetherToUseData7) +{ + const bool is_straight = true; + const bool is_moving = true; + const bool is_constant_velocity = false; + const bool only_use_straight = true; + const bool only_use_moving = true; + const bool only_use_constant_velocity = false; + EXPECT_TRUE(whether_to_use_data( + is_straight, is_moving, is_constant_velocity, only_use_straight, only_use_moving, + only_use_constant_velocity)); +} diff --git a/localization/deviation_estimation_tools/deviation_evaluator/CMakeLists.txt b/localization/deviation_estimation_tools/deviation_evaluator/CMakeLists.txt new file mode 100644 index 00000000..6e0d2a85 --- /dev/null +++ b/localization/deviation_estimation_tools/deviation_evaluator/CMakeLists.txt @@ -0,0 +1,54 @@ +cmake_minimum_required(VERSION 3.5) +project(deviation_evaluator) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +### Compile options +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror) +endif() + +# find dependencies +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +find_package(Eigen3 REQUIRED) + +ament_auto_add_executable(deviation_evaluator + src/deviation_evaluator_node.cpp + src/deviation_evaluator.cpp +) +ament_target_dependencies(deviation_evaluator) + +# if(BUILD_TESTING) +# find_package(ament_cmake_gtest REQUIRED) +# ament_add_gtest(deviation_evaluator-test test/test_deviation_evaluator.test +# test/src/test_deviation_evaluator.cpp +# src/deviation_evaluator.cpp +# ) +# target_include_directories(deviation_evaluator-test +# PRIVATE +# include +# ) +# ament_target_dependencies(deviation_evaluator-test geometry_msgs rclcpp tf2 tf2_ros) +# endif() + +install(PROGRAMS scripts/deviation_evaluation_visualizer.py DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch + rviz + config +) diff --git a/localization/deviation_estimation_tools/deviation_evaluator/config/deviation_evaluator.param.yaml b/localization/deviation_estimation_tools/deviation_evaluator/config/deviation_evaluator.param.yaml new file mode 100644 index 00000000..908344bc --- /dev/null +++ b/localization/deviation_estimation_tools/deviation_evaluator/config/deviation_evaluator.param.yaml @@ -0,0 +1,7 @@ +/**: + ros__parameters: + # Dead Reckoning configuration + wait_duration: 6.0 # [s] + wait_scale: 1.3 + + need_ekf_initial_trigger: true diff --git a/localization/deviation_estimation_tools/deviation_evaluator/include/deviation_evaluator/deviation_evaluator.hpp b/localization/deviation_estimation_tools/deviation_evaluator/include/deviation_evaluator/deviation_evaluator.hpp new file mode 100644 index 00000000..b148e6cc --- /dev/null +++ b/localization/deviation_estimation_tools/deviation_evaluator/include/deviation_evaluator/deviation_evaluator.hpp @@ -0,0 +1,100 @@ +// Copyright 2018-2019 Autoware Foundation +// +// 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 DEVIATION_EVALUATOR__DEVIATION_EVALUATOR_HPP_ +#define DEVIATION_EVALUATOR__DEVIATION_EVALUATOR_HPP_ + +#include "deviation_evaluator/tier4_autoware_utils.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2/LinearMath/Quaternion.h" +#include "tier4_autoware_utils/ros/transform_listener.hpp" + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" +#include "geometry_msgs/msg/twist_with_covariance_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "sensor_msgs/msg/imu.hpp" +#include "std_msgs/msg/float64.hpp" +#include "std_srvs/srv/set_bool.hpp" +#include "tier4_debug_msgs/msg/float64_stamped.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +class DeviationEvaluator : public rclcpp::Node +{ +private: + using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; + using TwistWithCovarianceStamped = geometry_msgs::msg::TwistWithCovarianceStamped; + using PoseStamped = geometry_msgs::msg::PoseStamped; + using Odometry = nav_msgs::msg::Odometry; + struct Errors + { + double lateral; + double long_radius; + Errors() : lateral(0), long_radius(0) {} + }; + +public: + DeviationEvaluator(const std::string & node_name, const rclcpp::NodeOptions & options); + +private: + rclcpp::Subscription::SharedPtr sub_ndt_pose_with_cov_; + rclcpp::Subscription::SharedPtr sub_dr_odom_; + rclcpp::Subscription::SharedPtr sub_gt_odom_; + rclcpp::Publisher::SharedPtr pub_pose_with_cov_dr_; + rclcpp::Publisher::SharedPtr pub_pose_with_cov_gt_; + rclcpp::Publisher::SharedPtr pub_init_pose_with_cov_; + + rclcpp::Client::SharedPtr client_trigger_ekf_dr_; + rclcpp::Client::SharedPtr client_trigger_ekf_gt_; + + bool show_debug_info_; + std::string save_dir_; + double start_time_; + + double wait_duration_; + Errors errors_threshold_; + Errors current_errors_; + + std::deque dr_pose_queue_; + + PoseStamped::SharedPtr last_gt_pose_ptr_; + + PoseStamped::SharedPtr current_ekf_gt_pose_ptr_; + PoseStamped::SharedPtr current_ndt_pose_ptr_; + + std::shared_ptr transform_listener_; + + bool has_published_initial_pose_; + + // void callbackWheelOdometry(const TwistWithCovarianceStamped::SharedPtr msg); + + void callbackNDTPoseWithCovariance(const PoseWithCovarianceStamped::SharedPtr msg); + + // cspell:words EKFDR, EKFGT + void callbackEKFDROdom(const Odometry::SharedPtr msg); + + void callbackEKFGTOdom(const Odometry::SharedPtr msg); + + geometry_msgs::msg::Pose interpolatePose(const double timestamp_seconds); +}; + +#endif // DEVIATION_EVALUATOR__DEVIATION_EVALUATOR_HPP_ diff --git a/localization/deviation_estimation_tools/deviation_evaluator/include/deviation_evaluator/tier4_autoware_utils.hpp b/localization/deviation_estimation_tools/deviation_evaluator/include/deviation_evaluator/tier4_autoware_utils.hpp new file mode 100644 index 00000000..a40b3112 --- /dev/null +++ b/localization/deviation_estimation_tools/deviation_evaluator/include/deviation_evaluator/tier4_autoware_utils.hpp @@ -0,0 +1,129 @@ +// Copyright 2018-2019 Autoware Foundation +// +// 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 DEVIATION_EVALUATOR__TIER4_AUTOWARE_UTILS_HPP_ +#define DEVIATION_EVALUATOR__TIER4_AUTOWARE_UTILS_HPP_ + +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/math/constants.hpp" +#include "tier4_autoware_utils/math/normalization.hpp" + +#include + +// ToDo (kminoda): Replace these functions with the one from tier4_autoware_utils. +// Currently these functions are declared here since this tool has to be compatible with older +// version of Autoware. + +template +bool isDrivingForward(const Pose1 & src_pose, const Pose2 & dst_pose) +{ + // check the first point direction + const double src_yaw = tf2::getYaw(tier4_autoware_utils::getPose(src_pose).orientation); + const double pose_direction_yaw = tier4_autoware_utils::calcAzimuthAngle( + tier4_autoware_utils::getPoint(src_pose), tier4_autoware_utils::getPoint(dst_pose)); + return std::fabs(tier4_autoware_utils::normalizeRadian(src_yaw - pose_direction_yaw)) < + tier4_autoware_utils::pi / 2.0; +} + +/** + * @brief Calculate a point by linear interpolation. + * @param src source point + * @param dst destination point + * @param ratio interpolation ratio, which should be [0.0, 1.0] + * @return interpolated point + */ +template +geometry_msgs::msg::Point calcInterpolatedPoint( + const Point1 & src, const Point2 & dst, const double ratio) +{ + const auto src_point = tier4_autoware_utils::getPoint(src); + const auto dst_point = tier4_autoware_utils::getPoint(dst); + + tf2::Vector3 src_vec; + src_vec.setX(src_point.x); + src_vec.setY(src_point.y); + src_vec.setZ(src_point.z); + + tf2::Vector3 dst_vec; + dst_vec.setX(dst_point.x); + dst_vec.setY(dst_point.y); + dst_vec.setZ(dst_point.z); + + // Get pose by linear interpolation + const double clamped_ratio = std::clamp(ratio, 0.0, 1.0); + const auto & vec = tf2::lerp(src_vec, dst_vec, clamped_ratio); + + geometry_msgs::msg::Point point; + point.x = vec.x(); + point.y = vec.y(); + point.z = vec.z(); + + return point; +} + +/** + * @brief Calculate a pose by linear interpolation. + * Note that if dist(src_pose, dst_pose)<=0.01 + * the orientation of the output pose is same as the orientation + * of the dst_pose + * @param src source point + * @param dst destination point + * @param ratio interpolation ratio, which should be [0.0, 1.0] + * @param set_orientation_from_position_direction set position by spherical interpolation if false + * @return interpolated point + */ +template +geometry_msgs::msg::Pose calcInterpolatedPose( + const Pose1 & src_pose, const Pose2 & dst_pose, const double ratio, + const bool set_orientation_from_position_direction = true) +{ + const double clamped_ratio = std::clamp(ratio, 0.0, 1.0); + + geometry_msgs::msg::Pose output_pose; + output_pose.position = calcInterpolatedPoint( + tier4_autoware_utils::getPoint(src_pose), tier4_autoware_utils::getPoint(dst_pose), + clamped_ratio); + + if (set_orientation_from_position_direction) { + const double input_poses_dist = tier4_autoware_utils::calcDistance2d( + tier4_autoware_utils::getPoint(src_pose), tier4_autoware_utils::getPoint(dst_pose)); + const bool is_driving_forward = isDrivingForward(src_pose, dst_pose); + + // Get orientation from interpolated point and src_pose + if ((is_driving_forward && clamped_ratio > 1.0 - (1e-6)) || input_poses_dist < 1e-3) { + output_pose.orientation = tier4_autoware_utils::getPose(dst_pose).orientation; + } else if (!is_driving_forward && clamped_ratio < 1e-6) { + output_pose.orientation = tier4_autoware_utils::getPose(src_pose).orientation; + } else { + const auto & base_pose = is_driving_forward ? dst_pose : src_pose; + const double pitch = tier4_autoware_utils::calcElevationAngle( + tier4_autoware_utils::getPoint(output_pose), tier4_autoware_utils::getPoint(base_pose)); + const double yaw = tier4_autoware_utils::calcAzimuthAngle( + tier4_autoware_utils::getPoint(output_pose), tier4_autoware_utils::getPoint(base_pose)); + output_pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw); + } + } else { + // Get orientation by spherical linear interpolation + tf2::Transform src_tf; + tf2::Transform dst_tf; + tf2::fromMsg(tier4_autoware_utils::getPose(src_pose), src_tf); + tf2::fromMsg(tier4_autoware_utils::getPose(dst_pose), dst_tf); + const auto & quaternion = tf2::slerp(src_tf.getRotation(), dst_tf.getRotation(), clamped_ratio); + output_pose.orientation = tf2::toMsg(quaternion); + } + + return output_pose; +} + +#endif // DEVIATION_EVALUATOR__TIER4_AUTOWARE_UTILS_HPP_ diff --git a/localization/deviation_estimation_tools/deviation_evaluator/launch/deviation_evaluation_visualizer.launch.xml b/localization/deviation_estimation_tools/deviation_evaluator/launch/deviation_evaluation_visualizer.launch.xml new file mode 100644 index 00000000..8b56e716 --- /dev/null +++ b/localization/deviation_estimation_tools/deviation_evaluator/launch/deviation_evaluation_visualizer.launch.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/localization/deviation_estimation_tools/deviation_evaluator/launch/deviation_evaluator.launch.xml b/localization/deviation_estimation_tools/deviation_evaluator/launch/deviation_evaluator.launch.xml new file mode 100644 index 00000000..15ee21ac --- /dev/null +++ b/localization/deviation_estimation_tools/deviation_evaluator/launch/deviation_evaluator.launch.xml @@ -0,0 +1,127 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/localization/deviation_estimation_tools/deviation_evaluator/media/deviation_evaluator.drawio.svg b/localization/deviation_estimation_tools/deviation_evaluator/media/deviation_evaluator.drawio.svg new file mode 100644 index 00000000..f2b90f05 --- /dev/null +++ b/localization/deviation_estimation_tools/deviation_evaluator/media/deviation_evaluator.drawio.svg @@ -0,0 +1,266 @@ + + + + + + + + + + + + + + + + + + + +
+
+
+ initial pose +
+
+
+ + initial pose + + + + + + +
+
+
+ deviation_evaluator +
+
+
+ + deviation_evaluator + + + + + + + + +
+
+
+ ekf_localizer +
+ (for ground truth) +
+
+
+ + ekf_localizer... + + + + + + + + +
+
+
+ ekf_localizer +
+ (for partially dead reckoning) +
+
+
+ + ekf_localizer... + + + + + + + + +
+
+
+ rosbag +
+
+
+ + rosbag + + + + + + +
+
+
+ odometry +
+
+
+ + odometry + + + + + + +
+
+
+ odometry +
+
+
+ + odometry + + + + + + +
+
+
IMU & wheel odometry
+
pose
+
+
+ + IMU & wheel odometry... + + + + + + + + + +
+
+
+ twist +
+
+
+ + twist + + + + + + +
+
+
+ gyro_odometer +
+
+
+ + gyro_odometer + + + + + +
+
+
+ IMU & wheel odometry +
+ (calibrated) +
+
+
+ + IMU & wheel odometry... + + + + + +
+
+
+ pose +
+
+
+ + pose + + + + + +
+
+
+ pose +
+ (partially cut) +
+
+
+ + pose... + + + + + + + Text is not SVG - cannot display + + + diff --git a/localization/deviation_estimation_tools/deviation_evaluator/media/rviz_sample.png b/localization/deviation_estimation_tools/deviation_evaluator/media/rviz_sample.png new file mode 100644 index 00000000..8760ccf5 Binary files /dev/null and b/localization/deviation_estimation_tools/deviation_evaluator/media/rviz_sample.png differ diff --git a/localization/deviation_estimation_tools/deviation_evaluator/package.xml b/localization/deviation_estimation_tools/deviation_evaluator/package.xml new file mode 100644 index 00000000..7f28543d --- /dev/null +++ b/localization/deviation_estimation_tools/deviation_evaluator/package.xml @@ -0,0 +1,32 @@ + + + + deviation_evaluator + 0.0.0 + The deviation evaluator package + Koji Minoda + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + geometry_msgs + nav_msgs + rclcpp + rclpy + sensor_msgs + std_msgs + std_srvs + tf2 + tf2_ros + tier4_autoware_utils + tier4_debug_msgs + + ament_cmake_gtest + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/localization/deviation_estimation_tools/deviation_evaluator/requirements.txt b/localization/deviation_estimation_tools/deviation_evaluator/requirements.txt new file mode 100644 index 00000000..4d1e48db --- /dev/null +++ b/localization/deviation_estimation_tools/deviation_evaluator/requirements.txt @@ -0,0 +1,5 @@ +numpy +scipy +pysqlite3 +tqdm +matplotlib diff --git a/localization/deviation_estimation_tools/deviation_evaluator/rviz/deviation_evaluator.rviz b/localization/deviation_estimation_tools/deviation_evaluator/rviz/deviation_evaluator.rviz new file mode 100644 index 00000000..050fa3f5 --- /dev/null +++ b/localization/deviation_estimation_tools/deviation_evaluator/rviz/deviation_evaluator.rviz @@ -0,0 +1,225 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.557669460773468 + Tree Height: 551 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: ~ + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + - /ThirdPersonFollower1 + Name: Views + Splitter Ratio: 0.5 + - Class: tier4_localization_rviz_plugin/InitialPoseButtonPanel + Name: InitialPoseButtonPanel +Visualization Manager: + Class: "" + Displays: + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Lanelet2VectorMap + Namespaces: + center_lane_line: true + center_line_arrows: true + crosswalk_lanelets: true + lane_start_bound: true + lanelet direction: true + lanelet_id: true + left_lane_bound: true + pedestrian_marking: true + right_lane_bound: true + road_lanelets: false + stop_lines: true + traffic_light: true + traffic_light_id: true + traffic_light_triangle: true + walkway_lanelets: true + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map/vector_map_marker + Value: true + Enabled: true + Name: Map + - Angle Tolerance: 0.10000000149011612 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: false + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: false + Value: false + Enabled: true + Keep: 1 + Name: Ground Truth + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 52; 101; 164 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /deviation_evaluator/ground_truth/ekf_localizer/kinematic_state + Value: true + - Angle Tolerance: 0.10000000149011612 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: false + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 3 + Value: true + Value: true + Enabled: true + Keep: 1 + Name: Estimated + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /deviation_evaluator/dead_reckoning/ekf_localizer/kinematic_state + Value: true + Enabled: true + Global Options: + Background Color: 10; 10; 10 + Fixed Frame: map + Frame Rate: 5 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/ThirdPersonFollower + Distance: 5.540429592132568 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.5697963237762451 + Target Frame: base_link + Value: ThirdPersonFollower (rviz) + Yaw: 4.711589813232422 + Saved: + - Class: rviz_default_plugins/ThirdPersonFollower + Distance: 18 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: ThirdPersonFollower + Near Clip Distance: 0.009999999776482582 + Pitch: 1.2999999523162842 + Target Frame: base_link + Value: ThirdPersonFollower (rviz) + Yaw: 3.141592025756836 + - Angle: 0 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: TopDownOrtho + Near Clip Distance: 0.009999999776482582 + Scale: 10 + Target Frame: viewer + Value: TopDownOrtho (rviz) + X: 0 + Y: 0 +Window Geometry: + Displays: + collapsed: false + Height: 973 + Hide Left Dock: false + Hide Right Dock: false + InitialPoseButtonPanel: + collapsed: false + QMainWindow State: 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 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 1920 + Y: 0 diff --git a/localization/deviation_estimation_tools/deviation_evaluator/scripts/bag_load_utils.py b/localization/deviation_estimation_tools/deviation_evaluator/scripts/bag_load_utils.py new file mode 100644 index 00000000..baeb57bd --- /dev/null +++ b/localization/deviation_estimation_tools/deviation_evaluator/scripts/bag_load_utils.py @@ -0,0 +1,384 @@ +#! /usr/bin/env python3 +# -*- coding: utf-8 -*- + +# Copyright 2020 Tier IV, Inc. +# +# 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. + +# cSpell:ignore covs + +import bisect +import dataclasses +import sqlite3 + +from constants import THRESHOLD_FOR_INITIALIZED_ERROR +from geometry_msgs.msg import PoseWithCovarianceStamped +from geometry_msgs.msg import TwistWithCovarianceStamped +from nav_msgs.msg import Odometry +import numpy as np +from rclpy.serialization import deserialize_message +from rosidl_runtime_py.utilities import get_message +from scipy.spatial.transform import Rotation +from tqdm import tqdm + + +@dataclasses.dataclass +class ErrorResults: + direction_name: str + error: np.ndarray + expected_error: np.ndarray + lower_bound: float = None + + +@dataclasses.dataclass +class DeviationEvaluatorResults: + long_radius: ErrorResults + lateral: ErrorResults + longitudinal: ErrorResults + timestamps: np.ndarray + ndt_timestamps: np.ndarray + twist_list: np.ndarray + + +def calc_stddev_rotated(P, theta): + e_vec = np.array([[np.cos(theta)], [np.sin(theta)]]) + d = np.sqrt(np.dot(np.dot(e_vec.T, P), e_vec) / np.linalg.det(P)) + return d[0][0] + + +class BagFileParser: + def __init__(self, bag_file): + self.conn = sqlite3.connect(bag_file) + self.cursor = self.conn.cursor() + + topics_data = self.cursor.execute("SELECT id, name, type FROM topics").fetchall() + self.topic_type = {name_of: type_of for id_of, name_of, type_of in topics_data} + self.topic_id = {name_of: id_of for id_of, name_of, type_of in topics_data} + self.topic_msg_message = { + name_of: get_message(type_of) for id_of, name_of, type_of in topics_data + } + + def __del__(self): + self.conn.close() + + def get_messages(self, topic_name): + topic_id = self.topic_id[topic_name] + rows = self.cursor.execute( + "SELECT timestamp, data FROM messages WHERE topic_id = {}".format(topic_id) + ).fetchall() + return [ + (timestamp, deserialize_message(data, self.topic_msg_message[topic_name])) + for timestamp, data in rows + ] + + +class EKFBagFileParser(BagFileParser): + def __init__(self, bag_file): + super().__init__(bag_file) + + def get_messages(self, topic_name): + topic_id = self.topic_id[topic_name] + rows = self.cursor.execute( + "SELECT timestamp, data FROM messages WHERE topic_id = {}".format(topic_id) + ).fetchall() + sample = deserialize_message(rows[0][1], self.topic_msg_message[topic_name]) + if isinstance(sample, PoseWithCovarianceStamped): + return self.parse_pose_with_covariance_stamped(rows, topic_name) + elif isinstance(sample, TwistWithCovarianceStamped): + return self.parse_twist_with_covariance_stamped(rows, topic_name) + elif isinstance(sample, Odometry): + return self.parse_odometry(rows, topic_name) + else: + print(topic_name + " not found.") + return [] + + def parse_pose_with_covariance_stamped(self, rows, topic_name): + pose_list, pose_cov_list = [], [] + for timestamp, data in rows: + msg = deserialize_message(data, self.topic_msg_message[topic_name]) + R = Rotation.from_quat( + [ + msg.pose.pose.orientation.x, + msg.pose.pose.orientation.y, + msg.pose.pose.orientation.z, + msg.pose.pose.orientation.w, + ] + ) + _, _, yaw = R.as_euler("xyz", degrees=False) + pose_list.append( + [ + msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9, + msg.pose.pose.position.x, + msg.pose.pose.position.y, + msg.pose.pose.position.z, + yaw, + ] + ) + pose_cov_list.append(msg.pose.covariance.reshape(6, 6)[(0, 1, 5), :][:, (0, 1, 5)]) + sorted_idxs = np.argsort(np.array(pose_list)[:, 0]) + pose_list = np.array(pose_list)[sorted_idxs] + pose_cov_list = np.array(pose_cov_list)[sorted_idxs] + pose_list = np.array(pose_list) + pose_cov_list = np.array(pose_cov_list) + return pose_list, pose_cov_list + + def parse_twist_with_covariance_stamped(self, rows, topic_name): + twist_list = [] + for timestamp, data in tqdm(rows, desc="Loading rosbag"): + msg = deserialize_message(data, self.topic_msg_message[topic_name]) + twist_list.append( + [ + msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9, + msg.twist.twist.linear.x, + msg.twist.twist.angular.z, + ] + ) + twist_list = np.array(twist_list) + return np.array(sorted(twist_list, key=lambda x: x[0])) + + def parse_odometry(self, rows, topic_name): + pose_list, pose_cov_list = [], [] + for timestamp, data in rows: + msg = deserialize_message(data, self.topic_msg_message[topic_name]) + R = Rotation.from_quat( + [ + msg.pose.pose.orientation.x, + msg.pose.pose.orientation.y, + msg.pose.pose.orientation.z, + msg.pose.pose.orientation.w, + ] + ) + _, _, yaw = R.as_euler("xyz", degrees=False) + pose_list.append( + [ + msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9, + msg.pose.pose.position.x, + msg.pose.pose.position.y, + msg.pose.pose.position.z, + yaw, + ] + ) + pose_cov_list.append(msg.pose.covariance.reshape(6, 6)[(0, 1, 5), :][:, (0, 1, 5)]) + sorted_idxs = np.argsort(np.array(pose_list)[:, 0]) + pose_list = np.array(pose_list)[sorted_idxs] + pose_cov_list = np.array(pose_cov_list)[sorted_idxs] + return pose_list, pose_cov_list + + +class BagFileEvaluator: + def __init__(self, bagfile, params, use_normal_ekf=False, bagfile_base=None): + bag_parser = EKFBagFileParser(bagfile) + + pose_list, _ = bag_parser.get_messages(params["pose_topic"]) + ekf_gt_pose_list, ekf_gt_pose_cov_list = bag_parser.get_messages( + params["ekf_gt_odom_topic"] + ) + ekf_dr_pose_list, ekf_dr_pose_cov_list = bag_parser.get_messages( + params["ekf_dr_odom_topic"] + ) + + ekf_gt_pose_list_interpolated, valid_idxs = calc_interpolate( + ekf_gt_pose_list[:, 1:3], + ekf_gt_pose_list[:, 0].tolist(), + ekf_dr_pose_list[:, 0].tolist(), + ) + + long_axis_angles = get_long_axis_angles_from_covs(ekf_dr_pose_cov_list[:, :2, :2]) + errors = ekf_gt_pose_list_interpolated[:, 1:3] - ekf_dr_pose_list[:, 1:3] + errors_along_elliptical_axis = transform_errors(errors, long_axis_angles) + errors_along_body_frame = transform_errors(errors, ekf_dr_pose_list[:, 4]) + + stddev_longitudinal_2d, stddev_lateral_2d = calc_body_frame_length( + ekf_dr_pose_cov_list, + ekf_dr_pose_list, + ) + stddev_longitudinal_2d_gt, stddev_lateral_2d_gt = calc_body_frame_length( + ekf_gt_pose_cov_list, + ekf_gt_pose_list, + ) + + stddev_long_2d, stddev_short_2d = calc_long_short_radius(ekf_dr_pose_cov_list) + stddev_long_2d_gt, stddev_short_2d_gt = calc_long_short_radius(ekf_gt_pose_cov_list) + + ignore_index = np.where(stddev_long_2d_gt < THRESHOLD_FOR_INITIALIZED_ERROR)[0][0] + + long_radius_results = ErrorResults( + "long_radius", + np.linalg.norm(errors_along_elliptical_axis, axis=1)[valid_idxs], + stddev_long_2d[valid_idxs] * params["scale"], + np.max(stddev_long_2d_gt[ignore_index:]) * params["scale"], + ) + lateral_results = ErrorResults( + "lateral", + np.abs(errors_along_body_frame[valid_idxs, 1]), + stddev_lateral_2d[valid_idxs] * params["scale"], + np.max(stddev_lateral_2d_gt[ignore_index:]) * params["scale"], + ) + longitudinal_results = ErrorResults( + "longitudinal", + np.abs(errors_along_body_frame[valid_idxs, 0]), + stddev_longitudinal_2d[valid_idxs] * params["scale"], + ) + + self.results = DeviationEvaluatorResults( + long_radius_results, + lateral_results, + longitudinal_results, + ekf_dr_pose_list[valid_idxs, 0], + pose_list[:, 0], + bag_parser.get_messages(params["twist_topic"]), + ) + + # def calc_roc_curve_lateral(self, threshold): + # recall_list = calc_roc_curve(threshold, self.results.lateral) + # return recall_list + + # def calc_roc_curve_long_radius(self, threshold): + # recall_list = calc_roc_curve(threshold, self.results.long_radius) + # return recall_list + + def calc_recall_lateral(self, threshold): + positives = self.results.lateral.error > threshold + if np.sum(positives) == 0: + return np.inf + true_positives = positives & (self.results.lateral.expected_error > threshold) + return np.sum(true_positives) / np.sum(positives) + + def calc_recall_long_radius(self, threshold): + positives = self.results.long_radius.error > threshold + if np.sum(positives) == 0: + return np.inf + true_positives = positives & (self.results.long_radius.expected_error > threshold) + return np.sum(true_positives) / np.sum(positives) + + +def calc_interpolate(poses, timestamps, timestamps_target): + poses_interpolated = [] + + for i in range(len(timestamps_target)): + t = timestamps_target[i] + idx = bisect.bisect_left(timestamps, t, 0, -1) + if idx < 0: + interpolated = poses[0] + elif idx >= len(timestamps): + interpolated = poses[-1] + else: + t0 = timestamps[idx - 1] + t1 = timestamps[idx] + x0 = poses[idx - 1] + x1 = poses[idx] + + interpolated = ((t1 - t) * x0 + (t - t0) * x1) / (t1 - t0) + poses_interpolated.append([t, interpolated[0], interpolated[1]]) + poses_interpolated = np.array(poses_interpolated) + + valid_idxs = np.array(timestamps_target) < min(timestamps[-1], timestamps_target[-1]) * ( + np.array(timestamps_target) > max(timestamps[0], timestamps_target[0]) + ) + return poses_interpolated, valid_idxs + + +def get_long_axis_angles_from_covs(covs): + angles = [] + for cov in covs: + eigen_values, eigen_vectors = np.linalg.eig(np.linalg.inv(cov)) + + idx = eigen_values.argsort()[::-1] + eigen_values = eigen_values[idx] + eigen_vectors = eigen_vectors[:, idx] + angle = np.arctan2(eigen_vectors[0, 0], eigen_vectors[0, 1]) + angles.append(angle) + return angles + + +def transform_errors(errors, angles): + assert len(errors) == len(angles), "Length of errors and angles should be the same" + assert errors.shape[1] == 2, "Dimension of errors should be 2 (x&y)" + errors_transformed = [] + for error, angle in zip(errors, angles): + mat = np.array([[np.cos(angle), np.sin(angle)], [-np.sin(angle), np.cos(angle)]]) + error_transformed = (mat @ error.reshape(-1, 1)).reshape(-1) + errors_transformed.append(error_transformed) + errors_transformed = np.array(errors_transformed) + return errors_transformed + + +def calc_body_frame_length(cov_list, pose_list): + inv_2d = np.linalg.inv(cov_list[:, :2, :2]) + stddev_longitudinal_2d = [] + stddev_lateral_2d = [] + + for i in range(len(cov_list)): + cov_matrix = inv_2d[i] + yaw = pose_list[i, 4] + stddev_longitudinal_2d.append(calc_stddev_rotated(cov_matrix, yaw - np.pi / 2)) + stddev_lateral_2d.append(calc_stddev_rotated(cov_matrix, yaw)) + + stddev_longitudinal_2d = np.array(stddev_longitudinal_2d) + stddev_lateral_2d = np.array(stddev_lateral_2d) + return stddev_longitudinal_2d, stddev_lateral_2d + + +def calc_long_short_radius(cov_list): + cov_eig_val_list = np.linalg.eig(cov_list[:, :2, :2])[0] + cov_eig_val_list[cov_eig_val_list < 0] = 1e-5 + stddev_long_2d = np.sqrt(np.max(cov_eig_val_list, axis=1)) + stddev_short_2d = np.sqrt(np.min(cov_eig_val_list, axis=1)) + return stddev_long_2d, stddev_short_2d + + +def calc_roc_curve(threshold_error, error_results: ErrorResults): + positives = error_results.error > threshold_error + if not positives.any(): + return None + recall_list = [] + threshold_stddev_list = np.arange(0, 0.8, 0.02) + for threshold_stddev in threshold_stddev_list: + true_positives = positives & (error_results.expected_error > threshold_stddev) + recall = np.sum(true_positives) / np.sum(positives) + recall_list.append([threshold_stddev, recall]) + return np.array(recall_list) + + +def get_duration_to_error(timestamps, ndt_timestamps, error_lateral, ndt_freq=10): + duration_to_error = [] + for timestamp, error in zip(timestamps, error_lateral): + idx = bisect.bisect_left(ndt_timestamps, timestamp) + if idx > 0 and error < 2: + duration = timestamp - ndt_timestamps[idx - 1] + if duration > 5.0 / ndt_freq: # Only count if NDT hasn't come for 5 steps. + duration_to_error.append([duration, error]) + return np.array(duration_to_error) + + +if __name__ == "__main__": + # Apply some test here as a temporary measure. + # Ideally this test should be implemented as a rostest. + + # get_long_axis_angles_from_covs + cov0 = np.array([[1, 0], [0, 2]]) + cov1 = np.array([[2, 0], [0, 1]]) + cov2 = np.linalg.inv(np.array([[2, -1], [-1, 2]])) + covs = [cov0, cov1, cov2] + angles_answer = [90, 0, 45] + np.testing.assert_array_almost_equal( + get_long_axis_angles_from_covs(covs), np.deg2rad(angles_answer) + ) + + # transform_errors + error = np.array([2, 0]) + errors = np.array([error, error, error, error]) + angles = np.deg2rad([0, 45, 90, 180]) + errors_transformed_answer = np.array([[2, 0], [np.sqrt(2), -np.sqrt(2)], [0, -2], [-2, 0]]) + errors_transformed_calculated = transform_errors(errors, angles) + np.testing.assert_array_almost_equal(errors_transformed_calculated, errors_transformed_answer) diff --git a/localization/deviation_estimation_tools/deviation_evaluator/scripts/constants.py b/localization/deviation_estimation_tools/deviation_evaluator/scripts/constants.py new file mode 100644 index 00000000..523b8b8d --- /dev/null +++ b/localization/deviation_estimation_tools/deviation_evaluator/scripts/constants.py @@ -0,0 +1,20 @@ +#! /usr/bin/env python3 +# -*- coding: utf-8 -*- + +# Copyright 2023 TIER IV, Inc. +# +# 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. + +# Threshold used for detecting invalid values for expected errors which tends to have significantly large values at the beginning of the bag file +# which is around 1e8 [m]. This value is used to ignore the initial part of the bag file. +THRESHOLD_FOR_INITIALIZED_ERROR = 100.0 # [m] diff --git a/localization/deviation_estimation_tools/deviation_evaluator/scripts/deviation_evaluation_visualizer.py b/localization/deviation_estimation_tools/deviation_evaluator/scripts/deviation_evaluation_visualizer.py new file mode 100755 index 00000000..7d1ee6a0 --- /dev/null +++ b/localization/deviation_estimation_tools/deviation_evaluator/scripts/deviation_evaluation_visualizer.py @@ -0,0 +1,123 @@ +#! /usr/bin/env python3 +# -*- coding: utf-8 -*- + +# Copyright 2020 Tier IV, Inc. +# +# 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. + +# cspell:words lowerbound + +from pathlib import Path +from threading import Thread + +from bag_load_utils import BagFileEvaluator +import numpy as np +from plot_utils import plot_bag_compare +import rclpy +from rclpy.node import Node + +PARAMS = { + "twist_topic": "/deviation_evaluator/twist_estimator/twist_with_covariance", + "pose_topic": "/deviation_evaluator/dead_reckoning/pose_estimator/pose_with_covariance", + "ekf_gt_odom_topic": "/deviation_evaluator/ground_truth/ekf_localizer/kinematic_state", + "ekf_dr_odom_topic": "/deviation_evaluator/dead_reckoning/ekf_localizer/kinematic_state", + "scale": 3, + "ndt_freq": 10, +} + + +def validate_threshold( + recall: float, threshold: float, lowerbound: float, recall_threshold: float = 0.99 +): + if threshold < lowerbound: + print("Threshold is too small for this vehicle.") + elif recall == np.inf: + print( + f"No error larger than {threshold:.3f} [m] observed. Provide more data on deviation_evaluator." + ) + elif recall > recall_threshold: + print("Valid threshold!") + else: + print("Covariance seems to be too optimistic.") + print("Consider increasing the covariances of the dead reckoning sensors.") + + if threshold > lowerbound: + print(f" [OK] lowerbound = {lowerbound:.3f} < {threshold:.3f} = threshold [m]") + else: + print(f" [NG] lowerbound = {lowerbound:.3f} > {threshold:.3f} = threshold [m]") + + if recall == np.inf: + print(f" [NG] recall = {recall}") + elif recall > recall_threshold: + print(f" [OK] recall = {recall:.3f} > {recall_threshold:.3f}") + else: + print(f" [NG] recall = {recall:.3f} < {recall_threshold:.3f}") + + +class DeviationEvaluationVisualizer(Node): + def __init__(self): + super().__init__("deviation_evaluation_visualizer") + self.declare_parameter("save_dir", "") + self.declare_parameter("warn_ellipse_size", 0.0) + self.declare_parameter("warn_ellipse_size_lateral_direction", 0.0) + + save_dir = self.get_parameter("save_dir").get_parameter_value().string_value + threshold_long_radius = ( + self.get_parameter("warn_ellipse_size").get_parameter_value().double_value + ) + threshold_lateral = ( + self.get_parameter("warn_ellipse_size_lateral_direction") + .get_parameter_value() + .double_value + ) + + bagfile = Path(save_dir) / "ros2bag/ros2bag_0.db3" + output_dir = Path(save_dir) + + bag_file_evaluator = BagFileEvaluator(str(bagfile), PARAMS) + + recall_lateral = bag_file_evaluator.calc_recall_lateral(threshold_lateral) + print("============================================================") + print("Results for lateral error monitoring:") + validate_threshold( + recall_lateral, threshold_lateral, bag_file_evaluator.results.lateral.lower_bound + ) + + print("============================================================") + print("Results for long radius error monitoring:") + recall_long_radius = bag_file_evaluator.calc_recall_long_radius(threshold_long_radius) + validate_threshold( + recall_long_radius, + threshold_long_radius, + bag_file_evaluator.results.long_radius.lower_bound, + ) + print("============================================================") + + _ = plot_bag_compare( + output_dir / "deviation_evaluator.png", + bag_file_evaluator.results, + ) + # plt.show() + print("Visualization completed! Press ctrl-C to exit.") + + +def main(args=None): + rclpy.init(args=args) + print("Loading rosbag. This may take a while...") + node = DeviationEvaluationVisualizer() + spin_thread = Thread(target=rclpy.spin, args=(node,)) + spin_thread.start() + + +if __name__ == "__main__": + main() diff --git a/localization/deviation_estimation_tools/deviation_evaluator/scripts/plot_utils.py b/localization/deviation_estimation_tools/deviation_evaluator/scripts/plot_utils.py new file mode 100644 index 00000000..4ef3752e --- /dev/null +++ b/localization/deviation_estimation_tools/deviation_evaluator/scripts/plot_utils.py @@ -0,0 +1,118 @@ +#! /usr/bin/env python3 +# -*- coding: utf-8 -*- + +# Copyright 2020 Tier IV, Inc. +# +# 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. + +from constants import THRESHOLD_FOR_INITIALIZED_ERROR +import matplotlib.pyplot as plt +import numpy as np + + +def plot_thresholds(recall_list, lower_bound, threshold, scale, save_path=None): + fig = plt.figure(figsize=(8, 6)) + ax = fig.add_subplot(111, xlabel="threshold [m]", ylabel="recall") + ax.text( + 0.1, 0.1, "Threshold lower bound = {:.3f} [m]".format(lower_bound), transform=ax.transAxes + ) + + if recall_list is not None: + ax.plot(recall_list[:, 0], recall_list[:, 1], label="Upper bound") + ax.legend() + else: + ax.text(0.3, 0.5, "Error larger than {:.3f} [m] was not observed".format(threshold)) + ax.grid() + ax.set_title( + "Recall for detecting localization anomalies (over {0:.3f} [m], {1}-sigma)".format( + threshold, scale + ) + ) + if save_path is not None: + plt.savefig(save_path) + plt.close() + + +def plot_bag_compare(save_path, results): + # Ignore the initial part larger than this value, since the values right after launch may diverge. + is_smaller_than_thres = results.long_radius.expected_error < THRESHOLD_FOR_INITIALIZED_ERROR + ignore_index = np.where(is_smaller_than_thres)[0][0] + error_maximum = np.max( + np.hstack( + [ + results.long_radius.expected_error[ignore_index:], + results.lateral.expected_error[ignore_index:], + results.longitudinal.expected_error[ignore_index:], + ] + ) + ) + + fig = plt.figure(figsize=(12, 12)) + ax1 = fig.add_subplot(411, xlabel="time [s]", ylabel="error [m]") + plot_error_analysis(ax1, results.timestamps, results.long_radius, error_maximum) + + # Plot error along body-frame-y-axis + ax2 = fig.add_subplot(412, xlabel="time [s]", ylabel="error [m]") + plot_error_analysis(ax2, results.timestamps, results.lateral, error_maximum) + + # Plot error along body-frame-x-axis + ax3 = fig.add_subplot(413, xlabel="time [s]", ylabel="error [m]") + plot_error_analysis(ax3, results.timestamps, results.longitudinal, error_maximum) + + # Plot velocity and yaw_rate + timestamp_twist = results.twist_list[:, 0] + velocity = results.twist_list[:, 1] + yaw_rate = results.twist_list[:, 2] + + ax4_vel = fig.add_subplot(414, xlabel="time [s]", ylabel="velocity [m/s]") + ax4_vel.plot(timestamp_twist, velocity, label=r"velocity (left axis)", color="c") + ax4_yaw_rate = ax4_vel.twinx() + ax4_yaw_rate.plot( + timestamp_twist, + yaw_rate, + label=r"yaw rate (right axis)", + color="red", + ) + handler1, label1 = ax4_vel.get_legend_handles_labels() + handler2, label2 = ax4_yaw_rate.get_legend_handles_labels() + ax4_vel.legend(handler1 + handler2, label1 + label2, loc=2, borderaxespad=0.0) + + plt.savefig(save_path, bbox_inches="tight") + + return fig + + +def plot_error_analysis(ax, timestamp, results, error_maximum): + warning_timestamps = timestamp[results.error > results.expected_error] + ax.vlines( + warning_timestamps, + 0, + max(results.expected_error), + color=(1, 0.7, 0.7), + label="Danger zone", + ) + ax.plot( + timestamp, + results.error, + label="error along {}".format(results.direction_name), + ) + ax.plot( + timestamp, + results.expected_error, + label=r"expected error (3-sigma)", + linestyle="--", + color="gray", + ) + ax.legend(loc=[1.0, 0.7]) + ax.set_ylim([0, error_maximum * 1.1]) + ax.grid() diff --git a/localization/deviation_estimation_tools/deviation_evaluator/src/deviation_evaluator.cpp b/localization/deviation_estimation_tools/deviation_evaluator/src/deviation_evaluator.cpp new file mode 100644 index 00000000..3fa24802 --- /dev/null +++ b/localization/deviation_estimation_tools/deviation_evaluator/src/deviation_evaluator.cpp @@ -0,0 +1,213 @@ +// Copyright 2018-2019 Autoware Foundation +// +// 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. + +// cspell:words EKFDR, EKFGT + +#include "deviation_evaluator/deviation_evaluator.hpp" + +#include "rclcpp/logging.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" + +#include +#include +#include +#include +#include + +// clang-format off +#define PRINT_MAT(X) std::cout << #X << ":\n" << X << std::endl << std::endl +#define DEBUG_INFO(...) {if (show_debug_info_) {RCLCPP_INFO(__VA_ARGS__);}} +#define DEBUG_PRINT_MAT(X) {if (show_debug_info_) {std::cout << #X << ": " << X << std::endl;}} + +// clang-format on +using std::placeholders::_1; + +using std::chrono_literals::operator""s; + +double double_round(const double x, const int n) +{ + return std::round(x * pow(10, n)) / pow(10, n); +} + +double norm_xy(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2) +{ + double dx = p1.x - p2.x; + double dy = p1.y - p2.y; + return std::sqrt(dx * dx + dy * dy); +} + +double norm_xy_lateral( + const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, const double yaw) +{ + double dx = p1.x - p2.x; + double dy = p1.y - p2.y; + return std::abs(dx * std::sin(yaw) - dy * std::cos(yaw)); +} + +DeviationEvaluator::DeviationEvaluator( + const std::string & node_name, const rclcpp::NodeOptions & node_options) +: rclcpp::Node(node_name, node_options) +{ + show_debug_info_ = declare_parameter("show_debug_info", false); + save_dir_ = declare_parameter("save_dir"); + wait_duration_ = declare_parameter("wait_duration"); + double wait_scale = declare_parameter("wait_scale"); + errors_threshold_.lateral = + declare_parameter("warn_ellipse_size_lateral_direction") * wait_scale; + errors_threshold_.long_radius = declare_parameter("warn_ellipse_size") * wait_scale; + + client_trigger_ekf_dr_ = + create_client("out_ekf_dr_trigger", rmw_qos_profile_services_default); + client_trigger_ekf_gt_ = + create_client("out_ekf_gt_trigger", rmw_qos_profile_services_default); + + if (declare_parameter("need_ekf_initial_trigger")) { + while (!client_trigger_ekf_dr_->wait_for_service(1s)) { + if (!rclcpp::ok()) break; + RCLCPP_INFO_STREAM(this->get_logger(), "Waiting for EKF trigger service..."); + } + auto req = std::make_shared(); + req->data = true; + client_trigger_ekf_dr_->async_send_request( + req, [this]([[maybe_unused]] rclcpp::Client::SharedFuture result) {}); + + while (!client_trigger_ekf_gt_->wait_for_service(1s)) { + if (!rclcpp::ok()) break; + RCLCPP_INFO_STREAM(this->get_logger(), "Waiting for EKF trigger service..."); + } + client_trigger_ekf_gt_->async_send_request( + req, [this]([[maybe_unused]] rclcpp::Client::SharedFuture result) {}); + RCLCPP_INFO(this->get_logger(), "EKF initialization finished"); + } + + sub_ndt_pose_with_cov_ = create_subscription( + "in_ndt_pose_with_covariance", 1, + std::bind(&DeviationEvaluator::callbackNDTPoseWithCovariance, this, _1)); + sub_dr_odom_ = create_subscription( + "in_ekf_dr_odom", 1, std::bind(&DeviationEvaluator::callbackEKFDROdom, this, _1)); + sub_gt_odom_ = create_subscription( + "in_ekf_gt_odom", 1, std::bind(&DeviationEvaluator::callbackEKFGTOdom, this, _1)); + + pub_pose_with_cov_dr_ = create_publisher( + "out_pose_with_covariance_dr", 1); + pub_pose_with_cov_gt_ = create_publisher( + "out_pose_with_covariance_gt", 1); + pub_init_pose_with_cov_ = create_publisher( + "out_initial_pose_with_covariance", 1); + + transform_listener_ = std::make_shared(this); + + current_ndt_pose_ptr_ = nullptr; + has_published_initial_pose_ = false; +} + +void DeviationEvaluator::callbackNDTPoseWithCovariance( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) +{ + if (!has_published_initial_pose_) { + geometry_msgs::msg::PoseWithCovarianceStamped pose_with_cov; + pose_with_cov = *msg; + const double initial_position_stddev = 0.05; + const double initial_angle_stddev = 0.01; + pose_with_cov.pose.covariance[0 * 6 + 0] = initial_position_stddev * initial_position_stddev; + pose_with_cov.pose.covariance[1 * 6 + 1] = initial_position_stddev * initial_position_stddev; + pose_with_cov.pose.covariance[2 * 6 + 2] = initial_position_stddev * initial_position_stddev; + pose_with_cov.pose.covariance[3 * 6 + 3] = initial_angle_stddev * initial_angle_stddev; + pose_with_cov.pose.covariance[4 * 6 + 4] = initial_angle_stddev * initial_angle_stddev; + pose_with_cov.pose.covariance[5 * 6 + 5] = initial_angle_stddev * initial_angle_stddev; + pub_init_pose_with_cov_->publish(pose_with_cov); + has_published_initial_pose_ = true; + start_time_ = rclcpp::Time(msg->header.stamp).seconds(); + return; + } + + const double msg_time = rclcpp::Time(msg->header.stamp).seconds(); + + if (msg_time - start_time_ < wait_duration_) { + pub_pose_with_cov_dr_->publish(*msg); + } + pub_pose_with_cov_gt_->publish(*msg); + + if ( + (current_errors_.lateral > errors_threshold_.lateral) & + (current_errors_.long_radius > errors_threshold_.long_radius)) { + RCLCPP_INFO(this->get_logger(), "Errors are large enough. Publish EKF initialization poses."); + start_time_ = msg_time; + has_published_initial_pose_ = false; + current_errors_ = Errors(); + } +} + +void DeviationEvaluator::callbackEKFDROdom(const Odometry::SharedPtr msg) +{ + PoseStamped::SharedPtr pose_ptr(new PoseStamped); + pose_ptr->pose = msg->pose.pose; + pose_ptr->header = msg->header; + if (!dr_pose_queue_.empty()) { + if ( + rclcpp::Time(dr_pose_queue_.back()->header.stamp).seconds() > + rclcpp::Time(pose_ptr->header.stamp).seconds()) { + dr_pose_queue_.clear(); + RCLCPP_ERROR_STREAM(this->get_logger(), "Timestamp jump detected!"); + } + } + dr_pose_queue_.push_back(pose_ptr); +} + +void DeviationEvaluator::callbackEKFGTOdom(const Odometry::SharedPtr msg) +{ + if (last_gt_pose_ptr_ == nullptr) { + last_gt_pose_ptr_.reset(new PoseStamped); + last_gt_pose_ptr_->header = msg->header; + last_gt_pose_ptr_->pose = msg->pose.pose; + } + if (dr_pose_queue_.size() < 2) return; + + double start_time = rclcpp::Time(dr_pose_queue_.front()->header.stamp).seconds(); + double target_time = rclcpp::Time(last_gt_pose_ptr_->header.stamp).seconds(); + if (start_time > target_time) { + last_gt_pose_ptr_ = nullptr; + return; + } + + geometry_msgs::msg::Pose target_pose; + try { + target_pose = interpolatePose(target_time); + } catch (const std::runtime_error & exception) { + return; + } + current_errors_.long_radius = norm_xy(target_pose.position, last_gt_pose_ptr_->pose.position); + current_errors_.lateral = norm_xy_lateral( + target_pose.position, last_gt_pose_ptr_->pose.position, tf2::getYaw(target_pose.orientation)); + last_gt_pose_ptr_ = nullptr; + dr_pose_queue_.clear(); +} + +geometry_msgs::msg::Pose DeviationEvaluator::interpolatePose(const double time) +{ + const auto iter_next = std::upper_bound( + dr_pose_queue_.begin(), dr_pose_queue_.end(), time, + [](double const & t1, geometry_msgs::msg::PoseStamped::SharedPtr const & t2) -> bool { + return t1 < rclcpp::Time(t2->header.stamp).seconds(); + }); + + if ((iter_next == dr_pose_queue_.begin()) | (iter_next == dr_pose_queue_.end())) { + throw std::runtime_error("Interpolation failed"); + } + + const double time_start = rclcpp::Time((*(iter_next - 1))->header.stamp).seconds(); + const double time_end = rclcpp::Time((*iter_next)->header.stamp).seconds(); + const double ratio = (time - time_start) / (time_end - time_start); + return calcInterpolatedPose((*(iter_next - 1))->pose, (*iter_next)->pose, ratio); +} diff --git a/localization/deviation_estimation_tools/deviation_evaluator/src/deviation_evaluator_node.cpp b/localization/deviation_estimation_tools/deviation_evaluator/src/deviation_evaluator_node.cpp new file mode 100644 index 00000000..ae112331 --- /dev/null +++ b/localization/deviation_estimation_tools/deviation_evaluator/src/deviation_evaluator_node.cpp @@ -0,0 +1,28 @@ +// Copyright 2018-2019 Autoware Foundation +// +// 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 "deviation_evaluator/deviation_evaluator.hpp" + +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + auto node = std::make_shared("deviation_evaluator", node_options); + + rclcpp::spin(node); + + return 0; +} diff --git a/vehicle/calibration_adapter/CMakeLists.txt b/vehicle/calibration_adapter/CMakeLists.txt new file mode 100644 index 00000000..f2655e7d --- /dev/null +++ b/vehicle/calibration_adapter/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.5) +project(calibration_adapter) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +ament_auto_add_executable(pacmod_calibration_adapter + src/pacmod_calibration_adapter_node.cpp + src/calibration_adapter_node_base.cpp +) +ament_target_dependencies(pacmod_calibration_adapter) + +ament_auto_add_executable(calibration_adapter + src/calibration_adapter_node.cpp + src/calibration_adapter_node_base.cpp +) +ament_target_dependencies(calibration_adapter) + +ament_auto_package( + INSTALL_TO_SHARE + launch +) diff --git a/vehicle/calibration_adapter/README.md b/vehicle/calibration_adapter/README.md new file mode 100644 index 00000000..478aab36 --- /dev/null +++ b/vehicle/calibration_adapter/README.md @@ -0,0 +1,17 @@ +# calibration_adapter + +## Purpose + +This package relay topic to `Float32Stamped` type of "autoware_calibration_msgs" to generalize calibration topics. + +### Details + +- `calibration_adapter_node_base` + This node has general calibration topics for all vehicle interface + +- `calibration_adapter` + This node has vehicle specific or temporary topics to calibrate and this node inherit `calibration_adapter_node_base`. + +## Assumptions / Known limits + +TBD. diff --git a/vehicle/calibration_adapter/include/calibration_adapter/calibration_adapter_node.hpp b/vehicle/calibration_adapter/include/calibration_adapter/calibration_adapter_node.hpp new file mode 100644 index 00000000..32befa12 --- /dev/null +++ b/vehicle/calibration_adapter/include/calibration_adapter/calibration_adapter_node.hpp @@ -0,0 +1,62 @@ +// +// Copyright 2020 Tier IV, Inc. All rights reserved. +// +// 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 CALIBRATION_ADAPTER__CALIBRATION_ADAPTER_NODE_HPP_ +#define CALIBRATION_ADAPTER__CALIBRATION_ADAPTER_NODE_HPP_ + +#include "calibration_adapter/calibration_adapter_node_base.hpp" +#include "estimator_utils/math_utils.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "autoware_control_msgs/msg/control.hpp" +#include "autoware_vehicle_msgs/msg/velocity_report.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "tier4_calibration_msgs/msg/float32_stamped.hpp" + +#include + +class CalibrationAdapterNode : public CalibrationAdapterNodeBase +{ + using Velocity = autoware_vehicle_msgs::msg::VelocityReport; + using ControlCommandStamped = autoware_control_msgs::msg::Control; + using TwistStamped = geometry_msgs::msg::TwistStamped; + +public: + CalibrationAdapterNode(); + +private: + double acceleration_ = 0.0; + const double dif_twist_time_ = 0.2; // 200ms + const std::size_t twist_vec_max_size_ = 100; + double lowpass_cutoff_value_; + std::vector twist_vec_; + rclcpp::Publisher::SharedPtr pub_acceleration_status_; + rclcpp::Publisher::SharedPtr pub_acceleration_cmd_; + rclcpp::Publisher::SharedPtr pub_steering_angle_cmd_; + rclcpp::Subscription::SharedPtr sub_control_cmd_; + rclcpp::Subscription::SharedPtr sub_twist_; + template + T getNearestTimeDataFromVec( + const T base_data, const double back_time, const std::vector & vec); + double getAccel( + const TwistStamped & prev_twist, const TwistStamped & current_twist, const double dt); + template + void pushDataToVec(const T data, const std::size_t max_size, std::vector * vec); + void callbackControlCmd(const ControlCommandStamped::ConstSharedPtr msg); + void callbackTwistStatus(const Velocity::ConstSharedPtr msg); +}; + +#endif // CALIBRATION_ADAPTER__CALIBRATION_ADAPTER_NODE_HPP_ diff --git a/vehicle/calibration_adapter/include/calibration_adapter/calibration_adapter_node_base.hpp b/vehicle/calibration_adapter/include/calibration_adapter/calibration_adapter_node_base.hpp new file mode 100644 index 00000000..53d34c1a --- /dev/null +++ b/vehicle/calibration_adapter/include/calibration_adapter/calibration_adapter_node_base.hpp @@ -0,0 +1,60 @@ +// +// Copyright 2020 Tier IV, Inc. All rights reserved. +// +// 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 CALIBRATION_ADAPTER__CALIBRATION_ADAPTER_NODE_BASE_HPP_ +#define CALIBRATION_ADAPTER__CALIBRATION_ADAPTER_NODE_BASE_HPP_ + +#include "rclcpp/rclcpp.hpp" + +#include "autoware_vehicle_msgs/msg/engage.hpp" +#include "autoware_vehicle_msgs/msg/steering_report.hpp" +#include "tier4_calibration_msgs/msg/bool_stamped.hpp" +#include "tier4_calibration_msgs/msg/float32_stamped.hpp" +#include "tier4_vehicle_msgs/msg/actuation_command_stamped.hpp" +#include "tier4_vehicle_msgs/msg/actuation_status_stamped.hpp" + +class CalibrationAdapterNodeBase : public rclcpp::Node +{ +public: + using ActuationCommandStamped = tier4_vehicle_msgs::msg::ActuationCommandStamped; + using ActuationStatusStamped = tier4_vehicle_msgs::msg::ActuationStatusStamped; + using Float32Stamped = tier4_calibration_msgs::msg::Float32Stamped; + using BoolStamped = tier4_calibration_msgs::msg::BoolStamped; + using EngageStatus = autoware_vehicle_msgs::msg::Engage; + using SteeringAngleStatus = autoware_vehicle_msgs::msg::SteeringReport; + CalibrationAdapterNodeBase(); + +private: + rclcpp::Publisher::SharedPtr pub_accel_status_; + rclcpp::Publisher::SharedPtr pub_brake_status_; + rclcpp::Publisher::SharedPtr pub_steer_status_; + rclcpp::Publisher::SharedPtr pub_accel_cmd_; + rclcpp::Publisher::SharedPtr pub_brake_cmd_; + rclcpp::Publisher::SharedPtr pub_steer_cmd_; + rclcpp::Publisher::SharedPtr pub_steering_angle_status_; + rclcpp::Publisher::SharedPtr pub_is_engage_; + + rclcpp::Subscription::SharedPtr sub_actuation_command_; + rclcpp::Subscription::SharedPtr sub_actuation_status_; + rclcpp::Subscription::SharedPtr sub_steering_angle_status_; + rclcpp::Subscription::SharedPtr sub_engage_status_; + void callbackSteeringAngleStatus(const SteeringAngleStatus::ConstSharedPtr msg); + void onActuationCmd(const ActuationCommandStamped::ConstSharedPtr msg); + void onActuationStatus(const ActuationStatusStamped::ConstSharedPtr msg); + void onEngageStatus(const EngageStatus::SharedPtr msg); +}; + +#endif // CALIBRATION_ADAPTER__CALIBRATION_ADAPTER_NODE_BASE_HPP_ diff --git a/vehicle/calibration_adapter/include/calibration_adapter/pacmod_calibration_adapter_node.hpp b/vehicle/calibration_adapter/include/calibration_adapter/pacmod_calibration_adapter_node.hpp new file mode 100644 index 00000000..ae34a151 --- /dev/null +++ b/vehicle/calibration_adapter/include/calibration_adapter/pacmod_calibration_adapter_node.hpp @@ -0,0 +1,38 @@ +// +// Copyright 2020 Tier IV, Inc. All rights reserved. +// +// 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 CALIBRATION_ADAPTER__PACMOD_CALIBRATION_ADAPTER_NODE_HPP_ +#define CALIBRATION_ADAPTER__PACMOD_CALIBRATION_ADAPTER_NODE_HPP_ + +#include "calibration_adapter/calibration_adapter_node_base.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "tier4_calibration_msgs/msg/float32_stamped.hpp" +#include "tier4_vehicle_msgs/msg/steering_wheel_status_stamped.hpp" + +class PacmodCalibrationAdapterNode : public CalibrationAdapterNodeBase +{ +public: + using SteeringWheelStatusStamped = tier4_vehicle_msgs::msg::SteeringWheelStatusStamped; + PacmodCalibrationAdapterNode(); + +private: + rclcpp::Publisher::SharedPtr pub_handle_status_; + rclcpp::Subscription::SharedPtr sub_handle_status_; + void callbackSteeringWheelStatus(const SteeringWheelStatusStamped::ConstSharedPtr msg); +}; + +#endif // CALIBRATION_ADAPTER__PACMOD_CALIBRATION_ADAPTER_NODE_HPP_ diff --git a/vehicle/calibration_adapter/launch/calibration_adapter.launch.xml b/vehicle/calibration_adapter/launch/calibration_adapter.launch.xml new file mode 100644 index 00000000..9b4f006e --- /dev/null +++ b/vehicle/calibration_adapter/launch/calibration_adapter.launch.xml @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/vehicle/calibration_adapter/launch/pacmod_calibration_adapter.launch.xml b/vehicle/calibration_adapter/launch/pacmod_calibration_adapter.launch.xml new file mode 100644 index 00000000..51e123d2 --- /dev/null +++ b/vehicle/calibration_adapter/launch/pacmod_calibration_adapter.launch.xml @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/vehicle/calibration_adapter/package.xml b/vehicle/calibration_adapter/package.xml new file mode 100644 index 00000000..3f1b91db --- /dev/null +++ b/vehicle/calibration_adapter/package.xml @@ -0,0 +1,28 @@ + + + calibration_adapter + 0.1.0 + The calibration_adapter + Tomoya Kimura + Taiki Tanaka + Apache 2 + + ament_cmake_auto + + autoware_cmake + + autoware_control_msgs + autoware_vehicle_msgs + estimator_utils + rclcpp + tf2 + tier4_calibration_msgs + tier4_vehicle_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/vehicle/calibration_adapter/src/calibration_adapter_node.cpp b/vehicle/calibration_adapter/src/calibration_adapter_node.cpp new file mode 100644 index 00000000..650b8d29 --- /dev/null +++ b/vehicle/calibration_adapter/src/calibration_adapter_node.cpp @@ -0,0 +1,131 @@ +// +// Copyright 2020 Tier IV, Inc. All rights reserved. +// +// 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 "calibration_adapter/calibration_adapter_node.hpp" + +#include + +#include +#include +#include + +CalibrationAdapterNode::CalibrationAdapterNode() +{ + using std::placeholders::_1; + + // QoS setup + static constexpr std::size_t queue_size = 1; + rclcpp::QoS durable_qos(queue_size); + durable_qos.transient_local(); // option for latching + lowpass_cutoff_value_ = this->declare_parameter("lowpass_cutoff_value", 0.033); + + pub_steering_angle_cmd_ = + create_publisher("~/output/steering_angle_cmd", durable_qos); + pub_acceleration_status_ = + create_publisher("~/output/acceleration_status", durable_qos); + pub_acceleration_cmd_ = + create_publisher("~/output/acceleration_cmd", durable_qos); + + sub_control_cmd_ = create_subscription( + "~/input/control_cmd", queue_size, + std::bind(&CalibrationAdapterNode::callbackControlCmd, this, _1)); + sub_twist_ = create_subscription( + "~/input/twist_status", queue_size, + std::bind(&CalibrationAdapterNode::callbackTwistStatus, this, _1)); +} + +template +T CalibrationAdapterNode::getNearestTimeDataFromVec( + const T base_data, const double back_time, const std::vector & vec) +{ + double nearest_time = std::numeric_limits::max(); + const double target_time = rclcpp::Time(base_data.header.stamp).seconds() - back_time; + T nearest_time_data; + for (const auto & data : vec) { + const double data_time = rclcpp::Time(data.header.stamp).seconds(); + const auto delta_time = std::abs(target_time - data_time); + if (nearest_time > delta_time) { + nearest_time_data = data; + nearest_time = delta_time; + } + } + return nearest_time_data; +} + +double CalibrationAdapterNode::getAccel( + const TwistStamped & prev_twist, const TwistStamped & current_twist, const double dt) +{ + if (dt < 1e-03) { + // invalid twist. return prev acceleration + return acceleration_; + } + const double dv = current_twist.twist.linear.x - prev_twist.twist.linear.x; + return dv / dt; +} + +template +void CalibrationAdapterNode::pushDataToVec( + const T data, const std::size_t max_size, std::vector * vec) +{ + vec->emplace_back(data); + while (vec->size() > max_size) { + vec->erase(vec->begin()); + } +} + +void CalibrationAdapterNode::callbackControlCmd(const ControlCommandStamped::ConstSharedPtr msg) +{ + Float32Stamped steer_angle_msg; + steer_angle_msg.header.stamp = msg->stamp; + steer_angle_msg.header.frame_id = "base_link"; + steer_angle_msg.data = msg->lateral.steering_tire_angle; + pub_steering_angle_cmd_->publish(steer_angle_msg); + + Float32Stamped accel_msg; + accel_msg.header.stamp = msg->stamp; + accel_msg.header.frame_id = "base_link"; + accel_msg.data = msg->longitudinal.acceleration; + pub_acceleration_cmd_->publish(accel_msg); +} + +void CalibrationAdapterNode::callbackTwistStatus(const Velocity::ConstSharedPtr msg) +{ + TwistStamped twist; + twist.header = msg->header; + twist.twist.linear.x = msg->longitudinal_velocity; + if (!twist_vec_.empty()) { + const auto past_msg = getNearestTimeDataFromVec(twist, dif_twist_time_, twist_vec_); + const double dt = + (rclcpp::Time(msg->header.stamp) - rclcpp::Time(past_msg.header.stamp)).seconds(); + const double raw_acceleration = getAccel(past_msg, twist, dt); + acceleration_ = + math_utils::lowpassFilter(acceleration_, raw_acceleration, lowpass_cutoff_value_, dt); + } + pushDataToVec(twist, twist_vec_max_size_, &twist_vec_); + + Float32Stamped accel_status_msg; + accel_status_msg.header.stamp = msg->header.stamp; + accel_status_msg.data = acceleration_; + pub_acceleration_status_->publish(accel_status_msg); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/vehicle/calibration_adapter/src/calibration_adapter_node_base.cpp b/vehicle/calibration_adapter/src/calibration_adapter_node_base.cpp new file mode 100644 index 00000000..15673d36 --- /dev/null +++ b/vehicle/calibration_adapter/src/calibration_adapter_node_base.cpp @@ -0,0 +1,116 @@ +// +// Copyright 2020 Tier IV, Inc. All rights reserved. +// +// 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 "calibration_adapter/calibration_adapter_node_base.hpp" + +#include + +#include + +CalibrationAdapterNodeBase::CalibrationAdapterNodeBase() : Node("calibration_adapter") +{ + using std::placeholders::_1; + + // QoS setup + static constexpr std::size_t queue_size = 1; + rclcpp::QoS durable_qos(queue_size); + durable_qos.transient_local(); // option for latching + + pub_accel_status_ = create_publisher( + "~/output/accel_status", durable_qos); + pub_brake_status_ = create_publisher( + "~/output/brake_status", durable_qos); + pub_steer_status_ = create_publisher( + "~/output/steer_status", durable_qos); + pub_accel_cmd_ = create_publisher( + "~/output/accel_cmd", durable_qos); + pub_brake_cmd_ = create_publisher( + "~/output/brake_cmd", durable_qos); + pub_steer_cmd_ = create_publisher( + "~/output/steer_cmd", durable_qos); + + // steering angle + pub_steering_angle_status_ = + create_publisher("~/output/steering_angle_status", durable_qos); + sub_steering_angle_status_ = create_subscription( + "~/input/steering_angle_status", queue_size, + std::bind(&CalibrationAdapterNodeBase::callbackSteeringAngleStatus, this, _1)); + + pub_is_engage_ = + create_publisher("~/output/is_engage", durable_qos); + + sub_engage_status_ = create_subscription( + "~/input/is_engage", queue_size, + std::bind(&CalibrationAdapterNodeBase::onEngageStatus, this, _1)); + sub_actuation_status_ = create_subscription( + "~/input/actuation_status", queue_size, + std::bind(&CalibrationAdapterNodeBase::onActuationStatus, this, _1)); + sub_actuation_command_ = create_subscription( + "~/input/actuation_command", queue_size, + std::bind(&CalibrationAdapterNodeBase::onActuationCmd, this, _1)); +} + +void CalibrationAdapterNodeBase::callbackSteeringAngleStatus( + const SteeringAngleStatus::ConstSharedPtr msg) +{ + Float32Stamped steer_status_msg; + steer_status_msg.header.stamp = msg->stamp; + steer_status_msg.data = msg->steering_tire_angle; + pub_steering_angle_status_->publish(steer_status_msg); +} + +void CalibrationAdapterNodeBase::onActuationCmd(const ActuationCommandStamped::ConstSharedPtr msg) +{ + Float32Stamped brake_msgs; + brake_msgs.header = msg->header; + brake_msgs.data = msg->actuation.brake_cmd; + pub_brake_cmd_->publish(brake_msgs); + + Float32Stamped accel_msgs; + accel_msgs.header = msg->header; + accel_msgs.data = msg->actuation.accel_cmd; + pub_accel_cmd_->publish(accel_msgs); + + Float32Stamped steer_msgs; + steer_msgs.header = msg->header; + steer_msgs.data = msg->actuation.steer_cmd; + pub_steer_cmd_->publish(steer_msgs); +} + +void CalibrationAdapterNodeBase::onActuationStatus(const ActuationStatusStamped::ConstSharedPtr msg) +{ + Float32Stamped accel_msgs; + accel_msgs.header = msg->header; + accel_msgs.data = msg->status.accel_status; + pub_accel_status_->publish(accel_msgs); + + Float32Stamped brake_msgs; + brake_msgs.header = msg->header; + brake_msgs.data = msg->status.brake_status; + pub_brake_status_->publish(brake_msgs); + + Float32Stamped steer_msgs; + steer_msgs.header = msg->header; + steer_msgs.data = msg->status.steer_status; + pub_steer_status_->publish(steer_msgs); +} + +void CalibrationAdapterNodeBase::onEngageStatus(const EngageStatus::SharedPtr msg) +{ + BoolStamped engage_msgs; + engage_msgs.data = msg->engage; + pub_is_engage_->publish(engage_msgs); +} diff --git a/vehicle/calibration_adapter/src/pacmod_calibration_adapter_node.cpp b/vehicle/calibration_adapter/src/pacmod_calibration_adapter_node.cpp new file mode 100644 index 00000000..b7b1e8a4 --- /dev/null +++ b/vehicle/calibration_adapter/src/pacmod_calibration_adapter_node.cpp @@ -0,0 +1,54 @@ +// +// Copyright 2020 Tier IV, Inc. All rights reserved. +// +// 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 "calibration_adapter/pacmod_calibration_adapter_node.hpp" + +#include + +#include + +PacmodCalibrationAdapterNode::PacmodCalibrationAdapterNode() +{ + using std::placeholders::_1; + + // QoS setup + static constexpr std::size_t queue_size = 1; + rclcpp::QoS durable_qos(queue_size); + durable_qos.transient_local(); // option for latching + + pub_handle_status_ = create_publisher("~/output/handle_status", durable_qos); + sub_handle_status_ = create_subscription( + "~/input/handle_status", queue_size, + std::bind(&PacmodCalibrationAdapterNode::callbackSteeringWheelStatus, this, _1)); +} + +void PacmodCalibrationAdapterNode::callbackSteeringWheelStatus( + const SteeringWheelStatusStamped::ConstSharedPtr msg) +{ + tier4_calibration_msgs::msg::Float32Stamped steer_msgs; + steer_msgs.header.stamp = msg->stamp; + steer_msgs.header.frame_id = "base_link"; + steer_msgs.data = msg->data; + pub_handle_status_->publish(steer_msgs); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/vehicle/estimator_utils/CMakeLists.txt b/vehicle/estimator_utils/CMakeLists.txt new file mode 100644 index 00000000..f43fc895 --- /dev/null +++ b/vehicle/estimator_utils/CMakeLists.txt @@ -0,0 +1,34 @@ +# cSpell:ignore utest +cmake_minimum_required(VERSION 3.5) +project(estimator_utils) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() + + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror) +endif() + + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +ament_auto_add_library(${PROJECT_NAME} + src/estimator_utils.cpp) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + find_package(ament_cmake_gmock REQUIRED) + file(GLOB_RECURSE test_files test/**/*.cpp) + ament_add_gmock(test_estimator_utils test/utest_launch.test ${test_files}) + target_link_libraries(test_estimator_utils + estimator_utils + ) +endif() + +ament_auto_package() diff --git a/vehicle/estimator_utils/include/estimator_utils/estimator_base.hpp b/vehicle/estimator_utils/include/estimator_utils/estimator_base.hpp new file mode 100644 index 00000000..80d64574 --- /dev/null +++ b/vehicle/estimator_utils/include/estimator_utils/estimator_base.hpp @@ -0,0 +1,114 @@ +// +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 ESTIMATOR_UTILS__ESTIMATOR_BASE_HPP_ +#define ESTIMATOR_UTILS__ESTIMATOR_BASE_HPP_ + +#include "estimator_utils/math_utils.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "tier4_calibration_msgs/msg/estimation_result.hpp" + +#include +#include + +class EstimatorBase +{ +public: + EstimatorBase() {} + virtual ~EstimatorBase() {} + virtual bool estimate() = 0; + virtual void preprocessData() = 0; + virtual bool checkIsValidData() = 0; + virtual void postprocessOutput() = 0; + virtual void publishData() = 0; + bool getIsValidData() { return is_valid_data_; } + bool getIsValidEstimation() { return is_valid_estimation_; } + using outputType = tier4_calibration_msgs::msg::EstimationResult; + void createPublisher(const std::string & name, rclcpp::Node * node) + { + // QoS setup + static constexpr std::size_t queue_size = 1; + rclcpp::QoS durable_qos(queue_size); + durable_qos.transient_local(); // option for latching + + pub_estimated_ = rclcpp::create_publisher(node, "output/" + name, durable_qos); + + result_msgs_.result.resize(5, 0); + result_msgs_.result_mean.resize(5, 0); + result_msgs_.result_stddev.resize(5, 0); + result_msgs_.absolute_error.resize(5, 0); + result_msgs_.mean_absolute_error.resize(5, 0); + result_msgs_.stddev_absolute_error.resize(5, 0); + } + void calcStatistics() + { + math_utils::calcSequentialStddev(result_statistics_); + math_utils::calcSequentialStddev(error_statistics_); + } + void publishResult() + { + result_msgs_.result = result_statistics_.value; + result_msgs_.result_mean = result_statistics_.mean; + result_msgs_.result_stddev = result_statistics_.stddev; + result_msgs_.absolute_error = error_statistics_.value; + result_msgs_.mean_absolute_error = error_statistics_.mean; + result_msgs_.stddev_absolute_error = error_statistics_.stddev; + pub_estimated_->publish(result_msgs_); + } + rclcpp::Publisher::SharedPtr pub_estimated_; + outputType result_msgs_; + math_utils::Statistics result_statistics_; + math_utils::Statistics error_statistics_; + int seq_ = 0; + bool is_valid_data_ = false; + bool is_valid_estimation_ = false; + + void processData() { is_valid_data_ = checkIsValidData(); } + /** + * data flow + * if data is valid data: + * 1. preprocess data + * 2. estimate parameter + * 3. post process output + * publish output + **/ + void Run() + { + try { + if (is_valid_data_) { + is_valid_estimation_ = estimate(); + if (is_valid_data_ && is_valid_estimation_) { + postprocessOutput(); + if (seq_ > 50) { + calcStatistics(); + } + seq_++; + } + } + publishData(); + publishResult(); + } catch (std::runtime_error & e) { // Handle runtime errors + std::cerr << "[parameter_estimator] runtime_error: " << e.what() << std::endl; + } catch (std::logic_error & e) { + std::cerr << "[parameter_estimator] logic_error: " << e.what() << std::endl; + } catch (...) { // Handle all unexpected exceptions + std::cerr << "[parameter_estimator] unkown error" << std::endl; + } + } +}; + +#endif // ESTIMATOR_UTILS__ESTIMATOR_BASE_HPP_ diff --git a/vehicle/estimator_utils/include/estimator_utils/estimator_utils.hpp b/vehicle/estimator_utils/include/estimator_utils/estimator_utils.hpp new file mode 100644 index 00000000..9d5b2356 --- /dev/null +++ b/vehicle/estimator_utils/include/estimator_utils/estimator_utils.hpp @@ -0,0 +1,22 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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 ESTIMATOR_UTILS__ESTIMATOR_UTILS_HPP_ +#define ESTIMATOR_UTILS__ESTIMATOR_UTILS_HPP_ + +#include "estimator_utils/estimator_base.hpp" +#include "estimator_utils/math_utils.hpp" +#include "estimator_utils/optimization_utils.hpp" + +#endif // ESTIMATOR_UTILS__ESTIMATOR_UTILS_HPP_ diff --git a/vehicle/estimator_utils/include/estimator_utils/math_utils.hpp b/vehicle/estimator_utils/include/estimator_utils/math_utils.hpp new file mode 100644 index 00000000..c29c3a14 --- /dev/null +++ b/vehicle/estimator_utils/include/estimator_utils/math_utils.hpp @@ -0,0 +1,393 @@ +// +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 ESTIMATOR_UTILS__MATH_UTILS_HPP_ +#define ESTIMATOR_UTILS__MATH_UTILS_HPP_ + +#include +#include +#include +#include +#include +#include + +namespace math_utils +{ +inline double saturation(const double val, const double min, const double max) +{ + return std::min(std::max(val, min), max); +} +static double interpolate(const double val, const double ref, const double w) +{ + return val * (1 - w) + w * ref; +} + +inline double normalize(const double val, const double min, const double max) +{ + return (val - min) / (max - min); +} + +inline std::vector getLinearInterpolation(const std::vector & arr, int num_interp) +{ + std::vector interp; + for (size_t i = 1; i < arr.size(); i++) { + double a = arr[i - 1]; + double b = arr[i]; + for (int j = 0; j < num_interp; j++) { + double weight = j / static_cast(num_interp); + double val = interpolate(a, b, weight); + interp.emplace_back(val); + } + } + interp.emplace_back(arr.back()); + return interp; +} + +template +int getMaximumIndexFromVector(const T & x) +{ + const auto iter = std::max_element(x.begin(), x.end()); + return std::distance(x.begin(), iter); +} +template +double getAverageFromVector(const T & arr) +{ + if (arr.size() == 0) { + return 0; + } + return std::accumulate((arr).begin(), (arr).end(), 0.0) / static_cast(arr.size()); +} + +template +double getStddevFromVector(const T & arr) +{ + if (arr.empty()) { + return 0; + } + double average = getAverageFromVector(arr); + double diff = 0; + for (const auto & v : arr) { + diff += std::pow(v - average, 2); + } + return std::sqrt(diff / static_cast(arr.size())); +} + +template +double getCorrelationCoefficientFromVector(const T & x, const T & y) +{ + if (x.empty()) { + return 0; + } + int sz = x.size(); + double coeff = 0; + double x_avg = getAverageFromVector(x); + double y_avg = getAverageFromVector(y); + double x_stddev = getStddevFromVector(x); + double y_stddev = getStddevFromVector(y); + if (x_stddev < 0.0001 || y_stddev < 0.0001) { + return 0; + } + for (int i = 0; i < x.size(); i++) { + coeff += (x[i] - x_avg) * (y[i] - y_avg); + } + coeff /= (x_stddev * y_stddev * sz); + return coeff; +} + +template +double getAverageFromVector(const T & arr, const T & w) +{ + if (arr.empty()) { + return 0; + } + double sum = 0; + double sum_w = 0; + for (size_t i = 0; i < arr.size(); ++i) { + sum += w[i] * arr[i]; + sum_w += w[i]; + } + return sum / sum_w; +} + +template +double getCovarianceFromVector(const T & x, const T & y, const T & w) +{ + if (x.empty()) { + return 0; + } + double cov = 0; + double sum_w = 0; + double avg_x = getAverageFromVector(x, w); + double avg_y = getAverageFromVector(y, w); + for (size_t i = 0; i < x.size(); i++) { + cov += w[i] * (x[i] - avg_x) * (y[i] - avg_y); + sum_w += w[i]; + } + return cov / sum_w; +} +template +double getStddevFromVector(const T & arr, const T & w) +{ + if (arr.empty()) { + return 0; + } + double average = getAverageFromVector(arr, w); + double var = 0; + double w_sum = 0; + for (size_t i = 0; i < arr.size(); i++) { + var += w[i] * std::pow((arr[i] - average), 2); + w_sum += w[i]; + } + return std::sqrt(var / w_sum); +} + +template +double getCorrelationCoefficientFromVector(const T & x, const T & y, const T & w) +{ + if (x.empty()) { + return 0; + } + double x_stddev = getStddevFromVector(x, w); + double y_stddev = getStddevFromVector(y, w); + double xy_cov = getCovarianceFromVector(x, y, w); + return xy_cov / (x_stddev * y_stddev); +} + +inline double lowpassFilter( + const double current_value, const double prev_value, double cutoff, const double dt) +{ + const double tau = 1 / (2 * M_PI * cutoff); + const double a = tau / (dt + tau); + return prev_value * a + (1 - a) * current_value; +} + +/** + * @param : arr vector like container + * @return : avg_arr processed arr vector + */ +template +std::vector getAveragedVector(const T & arr) +{ + if (arr.empty()) { + return arr; + } + std::vector avg_arr = {arr.begin(), arr.end()}; + double avg = std::accumulate((arr).begin(), (arr).end(), 0.0) / static_cast(arr.size()); + for (auto & v : avg_arr) { + v -= avg; + } + return avg_arr; +} + +/** + * @param : arr vector like container + * @return : avg_arr processed arr vector + */ +template +std::vector arrToVector(const T & arr) +{ + std::vector vec = {arr.begin(), arr.end()}; + return vec; +} + +/** + * @param : arr vector like container + * @param : slide move vector placement 0 or 1 + * @return : avg_arr processed arr vector + */ +template +std::vector fitToTheSizeOfVector(const T & arr, const int size, const int num_slide = 0) +{ + if (static_cast(arr.size()) < (size + num_slide)) { + return {arr.begin(), arr.end()}; + } + return {arr.end() - size - num_slide, arr.end() - num_slide}; +} + +/** + * @param : input_stamp : deque type input stamp + * @param : input_stamp : deque type input stamp + * @param : input : deque type input + * @param : response : deque type response + * @param : size : size of final output + */ +template +void fitToTheSizeOfVector( + const T & input_stamp, const T & response_stamp, std::vector & input, + std::vector & response, const int size, const int num_slide) +{ + const size_t max_slide = num_slide; + if (input.size() < (size + max_slide) && response.size() < (size + max_slide)) { + return; + } + double in0 = *(input_stamp.end() - 1); + double in1 = *(input_stamp.end() - 2); + double re0 = *(response_stamp.end() - 1); + double re1 = *(response_stamp.end() - 2); + double d00 = std::abs(in0 - re0); + double d10 = std::abs(in1 - re0); + double d01 = std::abs(in0 - re1); + double input_slide = 0; + double response_slide = 0; + if (d01 < d00 && d01 < d10) { + response_slide = num_slide; + } else if (d10 < d00 && d10 < d01) { + input_slide = num_slide; + } + input = fitToTheSizeOfVector(input, size, input_slide); + response = fitToTheSizeOfVector(response, size, response_slide); +} +/** + * + * @param input : input signal + * @param response : output signal + * @param valid_delay_index_ratio : number of shift to compare rate + * @return corr : size of (input+num_shift) , type T correlation value + */ +template +T calcCrossCorrelationCoefficient( + const T & input, const T & response, const double valid_delay_index_ratio) +{ + T r_input = input; + T r_response = response; + std::reverse(r_input.begin(), r_input.end()); + std::reverse(r_response.begin(), r_response.end()); + int T_interval = static_cast(input.size() * valid_delay_index_ratio); + T CorrCoeff(T_interval + 1, 0.0); + /** + * Correlation Coefficient Method + * CorrCoeff = Cov(x1x2)/Stddev(x1)*Stddev(x2) + */ + for (int tau = 0; tau < T_interval; tau++) { + T cmp_input(input.size()); + T cmp_response(response.size()); + copy(r_input.begin() + tau, r_input.end(), cmp_input.begin()); + copy(r_response.begin(), r_response.end() - tau, cmp_response.begin()); + CorrCoeff.at(tau) = getCorrelationCoefficientFromVector(cmp_input, cmp_response); + } + return CorrCoeff; +} + +/** + * + * @param input : input signal + * @param response : output signal + * @param weight : weight for correlation + * @param valid_delay_index_ratio : number of shift to compare rate + * @return corr : size of (input+num_shift) , type T correlation value + */ +template +T calcCrossCorrelationCoefficient( + const T & input, const T & response, const T & weight, const double valid_delay_index_ratio) +{ + T r_input = input; + T r_response = response; + std::reverse(r_input.begin(), r_input.end()); + std::reverse(r_response.begin(), r_response.end()); + int T_interval = static_cast(input.size() * valid_delay_index_ratio); + T CorrCoeff(T_interval, 0.0); + /** + * Correlation Coefficient Method + * CorrCoeff = Cov(x1x2)/Stddev(x1)*Stddev(x2) + */ + for (int tau = 0; tau < T_interval - 1; tau++) { + T cmp_input(input.size()); + T cmp_response(response.size()); + copy(r_input.begin() + tau, r_input.end(), cmp_input.begin()); + copy(r_response.begin(), r_response.end() - tau, cmp_response.begin()); + CorrCoeff.at(tau) = getCorrelationCoefficientFromVector(cmp_input, cmp_response, weight); + } + return CorrCoeff; +} + +template +double calcMAE(const T & input, const T & response, const int delay_index) +{ + size_t sz = input.size() / 2; + T r_input = input; + T r_response = response; + std::reverse(r_input.begin(), r_input.end()); + std::reverse(r_response.begin(), r_response.end()); + std::vector errors; // input - response + for (size_t i = 0; i < sz; i++) { + errors.emplace_back(r_input[i + delay_index] - r_response[i]); + } + double abs_sum = 0; + for (size_t i = 0; i < sz; i++) { + abs_sum += std::abs(errors[i]); + } + double mae = abs_sum / static_cast(sz); + return mae; +} + +struct Statistics +{ + Statistics() {} + explicit Statistics(int dim) + { + value = std::vector(dim, 0.0); + mean = std::vector(dim, 0.0); + variance = std::vector(dim, 0.0); + stddev = std::vector(dim, 0.0); + } + std::vector value; + std::vector mean; + std::vector variance; + std::vector stddev; + int count = 0; +}; + +inline void calcSequentialStddev(Statistics & stat) +{ + auto & cnt = stat.count; + double seq = static_cast(cnt); + for (size_t i = 0; i < stat.value.size(); i++) { + auto & mean = stat.mean[i]; + auto & variance = stat.variance[i]; + auto & val = stat.value[i]; + auto & stddev = stat.stddev[i]; + const auto old_avg = stat.mean[i]; + mean = (seq * mean + val) / (seq + 1.0); + variance = (seq * (variance + std::pow(old_avg, 2)) + std::pow(val, 2)) / (seq + 1.0) - + std::pow(mean, 2); + stddev = std::sqrt(variance); + } + cnt++; +} + +struct Statistic +{ + int cnt = 0; + double mean = 0; + double variance = 0; + double stddev = 0; + // mean absolute error + double mae = 0; + // O(1) speed stddev & mean + double calcSequentialStddev(const double val) + { + double old_avg = mean; + double seq = static_cast(cnt); + mean = (seq * mean + val) / (seq + 1.0); + variance = (seq * (variance + std::pow(old_avg, 2)) + std::pow(val, 2)) / (seq + 1.0) - + std::pow(mean, 2); + cnt++; + return std::sqrt(variance); + } +}; +} // namespace math_utils + +#endif // ESTIMATOR_UTILS__MATH_UTILS_HPP_ diff --git a/vehicle/estimator_utils/include/estimator_utils/optimization_utils.hpp b/vehicle/estimator_utils/include/estimator_utils/optimization_utils.hpp new file mode 100644 index 00000000..1e2a5c90 --- /dev/null +++ b/vehicle/estimator_utils/include/estimator_utils/optimization_utils.hpp @@ -0,0 +1,178 @@ +// +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 ESTIMATOR_UTILS__OPTIMIZATION_UTILS_HPP_ +#define ESTIMATOR_UTILS__OPTIMIZATION_UTILS_HPP_ + +#include +#include +#include +#include + +#include + +namespace optimization_utils +{ +inline void estimateByRLS( + double & est, double & cov, const double zn, const double ff, const double y) +{ + const double coef = (cov * zn) / (ff + zn * cov * zn); + cov = (cov - coef * zn * cov) / ff; + const double error = y - zn * est; + est = est + coef * error; +} + +inline void estimateByRLS( + Eigen::MatrixXd & est, Eigen::MatrixXd & cov, const Eigen::MatrixXd & zn, + const Eigen::MatrixXd & ff, const Eigen::MatrixXd & y) +{ + /** + * coef_n=(cov_n-1*zn_n)/(rho_n+zn^T_n*cov_n-1*zn_n) + * cov_n=[cov_n-1 - coef*zn^T*cov_n-1)] + * th_n=th_n-1+coef_n*epsilon_n + * eps_n=y_n-zn^T_n*th_n-1 + */ + Eigen::MatrixXd znT = zn.transpose(); + const Eigen::MatrixXd coef = (cov * zn) / ((ff + znT * cov * zn)(0, 0)); + cov = (cov - coef * znT * cov) / ff(0, 0); + const Eigen::MatrixXd error = y - znT * est; + est = est + coef * error(0, 0); +} + +/** + * @param x_t latter value + * @param t_x previous value + * @param dt delta time + * @return secondary central difference + */ +inline double getSecondaryCentralDifference(const double x_t, const double t_x, const double dt) +{ + return (x_t - t_x) / (2.0 * dt); +} + +inline double getSecondaryCentralDifference( + const double x_t, const double u, const double t_x, const double dt) +{ + return (x_t - u * 2.0 + t_x) / std::pow(dt, 2); +} + +static void vectorToMatrix( + const std::vector & x_dot, const std::vector & x, const std::vector & u, + Eigen::MatrixXd & Y, Eigen::MatrixXd & X) +{ + int dim_t = x_dot.size(); + X = Eigen::MatrixXd::Zero(dim_t, 2); + Y = Eigen::MatrixXd::Zero(dim_t, 1); + for (size_t i = 0; i < x_dot.size(); ++i) { + X(i, 0) = x_dot[i]; + X(i, 1) = x[i]; + Y(i, 0) = u[i]; + } +} + +static void vectorToMatrix( + const std::vector & x2dot, const std::vector & x_dot, + const std::vector & x, const std::vector & u, Eigen::MatrixXd & Y, + Eigen::MatrixXd & X) +{ + int dim_t = x_dot.size(); + X = Eigen::MatrixXd::Zero(dim_t, 3); + Y = Eigen::MatrixXd::Zero(dim_t, 1); + for (size_t i = 0; i < x_dot.size(); ++i) { + X(i, 0) = x2dot[i]; + X(i, 1) = x_dot[i]; + X(i, 2) = x[i]; + Y(i, 0) = u[i]; + } +} + +/** + * @brief get solution by least squared (LS) method + * @param X : differential matrix + * @param Y : target matrix + * @param w : solved by LS + * + * E = || Xw - Y ||^2 + * to get minimum E -> E' = 0 + * Xt * X * w = Xt * Y + */ +static void getSolutionByLeastSquared( + const Eigen::MatrixXd & X, const Eigen::MatrixXd & Y, Eigen::VectorXd & w) +{ + Eigen::MatrixXd XtX = X.transpose() * X; + // print(XtX); + Eigen::MatrixXd b = X.transpose() * Y; + // solve Ax=b in this case A=XtX u=w(desired) b=b + Eigen::FullPivLU lu(XtX); + w = lu.solve(b); +} + +/** + * @brief get error norm with optimized coefficient + * @param X : differential matrix + * @param Y : target matrix + * @param w : solved by LS + * @return : normalized error + */ +static double getErrorNorm( + const Eigen::MatrixXd & X, const Eigen::MatrixXd & Y, const Eigen::VectorXd & w) +{ + Eigen::MatrixXd E = X * w - Y; + double error = 0; + for (size_t i = 0; i < static_cast(E.rows()); i++) { + error += std::abs(E(i, 0)); + } + error /= static_cast(E.rows()); + return error; +} + +inline double getLeastSquaredError( + const std::vector & x_dot, const std::vector & x, const std::vector & u, + Eigen::VectorXd & w) +{ + int dim_t = x_dot.size(); + Eigen::MatrixXd X = Eigen::MatrixXd::Zero(dim_t, 2); + Eigen::MatrixXd Y = Eigen::MatrixXd::Zero(dim_t, 1); + vectorToMatrix(x_dot, x, u, Y, X); + getSolutionByLeastSquared(X, Y, w); + return getErrorNorm(X, Y, w); +} + +inline double getLeastSquaredError( + const std::vector & x2dot, const std::vector & x_dot, + const std::vector & x, const std::vector & u, Eigen::VectorXd & w) +{ + int dim_t = u.size(); + Eigen::MatrixXd X = Eigen::MatrixXd::Zero(dim_t, 3); + Eigen::MatrixXd Y = Eigen::MatrixXd::Zero(dim_t, 1); + vectorToMatrix(x2dot, x_dot, x, u, Y, X); + getSolutionByLeastSquared(X, Y, w); + return getErrorNorm(X, Y, w); +} + +template +bool change_abs_min(T & a, const T & b) +{ + if (std::abs(b) < std::abs(a)) { + a = std::abs(b); + return true; + } + return false; +} + +} // namespace optimization_utils + +#endif // ESTIMATOR_UTILS__OPTIMIZATION_UTILS_HPP_ diff --git a/vehicle/estimator_utils/package.xml b/vehicle/estimator_utils/package.xml new file mode 100644 index 00000000..53666647 --- /dev/null +++ b/vehicle/estimator_utils/package.xml @@ -0,0 +1,28 @@ + + + + estimator_utils + 0.1.0 + The estimator utils + Taiki Tanaka + Apache 2 + + ament_cmake_auto + + + eigen + rclcpp + rosidl_default_runtime + tier4_calibration_msgs + + + + ament_cmake_gmock + ament_lint_auto + autoware_lint_common + libgmock-dev + + + ament_cmake + + diff --git a/vehicle/estimator_utils/src/estimator_utils.cpp b/vehicle/estimator_utils/src/estimator_utils.cpp new file mode 100644 index 00000000..4d8b3df7 --- /dev/null +++ b/vehicle/estimator_utils/src/estimator_utils.cpp @@ -0,0 +1,20 @@ +// +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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. +// + +int main() +{ + return 0; +} diff --git a/vehicle/estimator_utils/test/src/test_math_utils.cpp b/vehicle/estimator_utils/test/src/test_math_utils.cpp new file mode 100644 index 00000000..72095e70 --- /dev/null +++ b/vehicle/estimator_utils/test/src/test_math_utils.cpp @@ -0,0 +1,111 @@ +// +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 "estimator_utils/math_utils.hpp" + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +TEST(math_utils, saturation) +{ + using math_utils::saturation; + double min = 1.0; + double max = 2.0; + double val = 2.0; + const double expect = 2.0; + EXPECT_DOUBLE_EQ(saturation(val, min, max), expect); +} + +TEST(math_utils, normalize) +{ + using math_utils::normalize; + double min = -1.0; + double max = 1.0; + double val = 0.0; + const double expect = 0.5; + EXPECT_DOUBLE_EQ(normalize(val, min, max), expect); +} + +TEST(math_utils, getLinearInterpolation) +{ + using math_utils::getLinearInterpolation; + using testing::ElementsAre; + std::vector input = {0, 3}; + int num_interpolation = 3; + std::vector output = getLinearInterpolation(input, num_interpolation); + ASSERT_THAT(output, ElementsAre(0, 1, 2, 3)); +} + +TEST(math_utils, calcCrossCorrelationCoefficient) +{ + using math_utils::calcCrossCorrelationCoefficient; + std::vector input = {1, 2, 3, 2, 1, 0}; + std::vector response = {0, 1, 2, 3, 2, 1}; + std::vector weights = {1, 1, 1, 1, 1}; + std::vector output = calcCrossCorrelationCoefficient(input, response, weights, 0.5); + int delay_index = math_utils::getMaximumIndexFromVector(output); + EXPECT_EQ(delay_index, 1); +} + +TEST(math_utils, Statistics) +{ + using ::testing::ElementsAre; + math_utils::Statistics stat(1); + std::vector result = {0, 0.5, 0.81649658092772603}; + std::vector x = {1, 2, 3}; + for (int i = 0; i < 3; ++i) { + stat.value[0] = x[i]; + math_utils::calcSequentialStddev(stat); + EXPECT_DOUBLE_EQ(stat.stddev[0], result[i]); + } +} + +TEST(math_utils, getAveragedVector) +{ + using ::testing::ElementsAre; + std::vector result = {}; + std::vector x = {}; + std::vector empty = math_utils::getAveragedVector(x); + ASSERT_THAT(empty, ElementsAre()); +} + +TEST(math_utils, getAveragedVector2) +{ + using ::testing::ElementsAre; + std::vector result = {-1, 0, 1}; + std::vector x = {1, 2, 3}; + std::vector avg = math_utils::getAveragedVector(x); + ASSERT_THAT(result, avg); +} + +TEST(math_utils, fitToTheSizeOfVector) +{ + std::vector input_stamp = {1, 2, 3, 4, 5, 5, 6, 10}; + std::vector response_stamp = {1, 2, 3, 4, 5, 5, 6, 7}; + std::vector input = {1, 2, 3, 4, 5, 5, 6, 7}; + std::vector response = {1, 2, 3, 4, 5, 5, 6, 7}; + math_utils::fitToTheSizeOfVector(input_stamp, response_stamp, input, response, 5, 1); + ASSERT_THAT(input, testing::ElementsAre(3, 4, 5, 5, 6)); +} diff --git a/vehicle/estimator_utils/test/src/test_optimization_util.cpp b/vehicle/estimator_utils/test/src/test_optimization_util.cpp new file mode 100644 index 00000000..afe451ae --- /dev/null +++ b/vehicle/estimator_utils/test/src/test_optimization_util.cpp @@ -0,0 +1,103 @@ +// +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 "estimator_utils/optimization_utils.hpp" + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +TEST(optimization_utils, change_abs_min) +{ + using optimization_utils::change_abs_min; + int dim_t = 3; + int diff_t = 3; + Eigen::VectorXd w = Eigen::MatrixXd::Zero(diff_t, 1); + Eigen::MatrixXd X = Eigen::MatrixXd::Zero(diff_t, dim_t); + Eigen::MatrixXd Y = Eigen::MatrixXd::Zero(diff_t, 1); + // 13^2 14^2 15^2 + // 4*13^2 4*14^2 4*15^2 + // Y = 4*X^2 -> w = [0,0,4] + X << 2, 24, 169, 2, 26, 196, 2, 28, 225; + Y << 676, 784, 900; + optimization_utils::getSolutionByLeastSquared(X, Y, w); + EXPECT_DOUBLE_EQ(w(0), 0.0); + EXPECT_DOUBLE_EQ(w(1), 0.0); + EXPECT_DOUBLE_EQ(w(2), 4.0); +} + +TEST(optimization_utils, getSolutionByLeastSquared) +{ + using optimization_utils::getSolutionByLeastSquared; + int dim_t = 3; + int diff_t = 3; + Eigen::VectorXd w = Eigen::MatrixXd::Zero(diff_t, 1); + Eigen::MatrixXd X = Eigen::MatrixXd::Zero(diff_t, dim_t); + Eigen::MatrixXd Y = Eigen::MatrixXd::Zero(diff_t, 1); + // 13^2 14^2 15^2 + // 4*13^2 4*14^2 4*15^2 + // Y = 4*X^2 -> w = [0,0,4] + X << 2, 24, 169, 2, 26, 196, 2, 28, 225; + Y << 676, 784, 900; + getSolutionByLeastSquared(X, Y, w); + EXPECT_DOUBLE_EQ(w(0), 0.0); + EXPECT_DOUBLE_EQ(w(1), 0.0); + EXPECT_DOUBLE_EQ(w(2), 4.0); +} + +TEST(optimization_utils, getErrorNorm) +{ + using optimization_utils::getErrorNorm; + int dim_t = 3; + int diff_t = 3; + Eigen::VectorXd w = Eigen::MatrixXd::Zero(diff_t, 1); + Eigen::MatrixXd X = Eigen::MatrixXd::Zero(diff_t, dim_t); + Eigen::MatrixXd Y = Eigen::MatrixXd::Zero(diff_t, 1); + // 13^2 14^2 15^2 + // 4*13^2 4*14^2 4*15^2 + // Y = 4*X^2 -> w = [0,0,4] + X << 2, 24, 169, 2, 26, 196, 2, 28, 225; + Y << 676, 784, 900; + w << 0, 0, 4; + double error = getErrorNorm(X, Y, w); + EXPECT_DOUBLE_EQ(error, 0.0); +} + +TEST(optimization_utils, getSecondaryCentralDifference) +{ + using optimization_utils::getSecondaryCentralDifference; + double x0 = 0; + double x1 = 2; + double x2 = 4; + double dt = 1; + double xd = getSecondaryCentralDifference(x2, x0, dt); + double xdd = getSecondaryCentralDifference(x2, x1, x0, dt); + + EXPECT_DOUBLE_EQ(xd, 2); + EXPECT_DOUBLE_EQ(xdd, 0); +} diff --git a/vehicle/estimator_utils/test/utest.cpp b/vehicle/estimator_utils/test/utest.cpp new file mode 100644 index 00000000..28b495c3 --- /dev/null +++ b/vehicle/estimator_utils/test/utest.cpp @@ -0,0 +1,25 @@ +// +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 "estimator_utils/estimator_utils.hpp" + +#include + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/vehicle/estimator_utils/test/utest_launch.test b/vehicle/estimator_utils/test/utest_launch.test new file mode 100755 index 00000000..4a377885 --- /dev/null +++ b/vehicle/estimator_utils/test/utest_launch.test @@ -0,0 +1,5 @@ + + + + + diff --git a/vehicle/parameter_estimator/CMakeLists.txt b/vehicle/parameter_estimator/CMakeLists.txt new file mode 100644 index 00000000..119b7822 --- /dev/null +++ b/vehicle/parameter_estimator/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 3.5) +project(parameter_estimator) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +ament_auto_add_executable(parameter_estimator + src/parameter_estimator_node.cpp + src/wheel_base_estimator.cpp + src/steer_offset_estimator.cpp + src/gear_ratio_estimator.cpp + src/main.cpp) +ament_target_dependencies(parameter_estimator) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package(INSTALL_TO_SHARE + launch + config) diff --git a/vehicle/parameter_estimator/README.md b/vehicle/parameter_estimator/README.md new file mode 100644 index 00000000..5574427d --- /dev/null +++ b/vehicle/parameter_estimator/README.md @@ -0,0 +1,116 @@ +# ParameterEstimation + +This parameter estimation node estimates a default parameters from inputs for steer offset,wheel base and gear ratio. + +

+ +

+ +## I/O + +### input + +The following topics are used to estimate the parameters. + +- `/sensing/imu/imu_data`: used as vehicle angular velocity +- `/vehicle/status/twist`: used as vehicle velocity +- `/vehicle/status/steering`: used as vehicle steering angle (**Only used in Steer Offset Estimator & Wheel Base Estimator**) +- `/calibration/vehicle/handle_status`: used as vehicle handle angle (**Only used in Gear Estimator**) +- `/vehicle/engage`: used to check the driving operation status + +### output + +The following topics are the output + +- `/vehicle/status/gear_ratio` +- `/vehicle/status/steering_offset` +- `/vehicle/status/wheel_base` + +For users, the `EstimationResult.msg` output contains the following items: + +- _result_: Estimated result. +- _result_mean_: Average value of result. +- _result_stddev_: Standard deviation value of the estimated parameter +- _absolute_error_: Absolute error of the estimated parameters +- _mean_absolute_error_: Mean absolute error of the estimated parameters +- _stddev_absolute_error_: Standard Deviation of absolute error of the estimated parameters + +### These values can be confirmed in [plot_juggler](https://plotjuggler.io/) + +- To display in the plot juggler, you need to drag the result into the table, currently plot juggler cannot auto display the value + +## How to Run Parameter Estimator + +Note: You need to build the Autoware beforehand. + +The following command will start the parameter estimation node. + +```sh +ros2 launch parameter_estimator parameter_estimator.launch.xml vehicle_model:=lexus +``` + +- The following arguments are using to select the estimator, default is true + - select_gear_ratio_estimator + - select_steer_offset_estimator + - select_wheel_base_estimator +- To deselect the specific estimator, you need to set estimator to false +- Example, the following command will only launch gear ratio estimator + +```sh +ros2 launch parameter_estimator parameter_estimator.launch.xml vehicle_model:=lexus select_steer_offset_estimator:=false select_wheel_base_estimator:=false +``` + +If you want to launch with Rviz, use the following launch file. **Currently unavailable** + +```sh +# Launch parameter Estimator with the Autoware +$ ros2 launch parameter_estimator parameter_estimator_with_simulation.launch.xml map_path:=.../kashiwanoha2/ vehicle_model:=jpntaxi sensor_model:=aip_xx1 rviz:=true +``` + +### How to check the estimated parameters + +The necessary information is plotted in the plot_juggler, which displays the following information from top to bottom. + +- First row: Estimation results confirmation +- Second row: error for estimation + +You need to adjust the value of `(valid_min_)` or `(valid_max_)`. according to the standard deviation to determine the validity of the data. + +#### Estimation results confirmation + +Check the estimation results. + +- _result_ : parameter estimated at each step +- _result_mean_ : Average of the parameters +- _result_stddev_ : Standard deviation of the estimated parameters + +

+ +

+ +It is preferable to use the _\_mean_ for the calibration results. + +The parameters estimation starts when enough data is stored. The output value is zero until it is ready. + +#### Error for parameters estimation + +Check the statistics of the errors in the input/output data after the parameter estimation. + +- _absolute_error_ : absolute error of measured and estimated of the parameters estimation +- _mean_absolute_error_ : Mean of the absolute error calculated in each estimation step +- _stddev_absolute_error_ : Standard deviation of the absolute error calculated in each estimation step + +

+ +

+ +If these values are large, the model needs to be reconsidered. + +## Data preprocessing + +### Examine the results of processing the input data + +Data that do not satisfy the following conditions are considered invalid and will not be used for estimation. + +- Data variation (evaluated by standard deviation) is smaller than the threshold +- The car is not in automatic operation mode (judged from Autoware's Engage, Vehicle's Engage, etc.) diff --git a/vehicle/parameter_estimator/config/parameter_estimator_param.yaml b/vehicle/parameter_estimator/config/parameter_estimator_param.yaml new file mode 100644 index 00000000..56594635 --- /dev/null +++ b/vehicle/parameter_estimator/config/parameter_estimator_param.yaml @@ -0,0 +1,9 @@ +parameter_estimator: + ros__parameters: + update_hz: 10.0 # Used for the timer + initial_covariance: 1.0 # Used in RLS(Recursive least squares) to get nearest estimate value + forgetting_factor: 0.999 # Used in RLS(Recursive least squares) to get nearest estimate value + valid_max_steer_rad: 0.05 # Used as steer data validation, the data should be less than this value + valid_min_velocity: 0.5 # Used as velocity validation, the data should be more than this value + valid_min_angular_velocity: 0.1 # Used in gear ratio estimator, the angular should be more than this value + gear_ratio: [15.7, 0.053, 0.047] # Initial estimated gear ratio diff --git a/vehicle/parameter_estimator/config/plot_juggler_parameter_estimator.xml b/vehicle/parameter_estimator/config/plot_juggler_parameter_estimator.xml new file mode 100644 index 00000000..4006f0fe --- /dev/null +++ b/vehicle/parameter_estimator/config/plot_juggler_parameter_estimator.xml @@ -0,0 +1,115 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/vehicle/parameter_estimator/include/parameter_estimator/debugger.hpp b/vehicle/parameter_estimator/include/parameter_estimator/debugger.hpp new file mode 100644 index 00000000..b042293e --- /dev/null +++ b/vehicle/parameter_estimator/include/parameter_estimator/debugger.hpp @@ -0,0 +1,46 @@ +// +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 PARAMETER_ESTIMATOR__DEBUGGER_HPP_ +#define PARAMETER_ESTIMATOR__DEBUGGER_HPP_ + +#include "rclcpp/rclcpp.hpp" + +#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" + +#include +#include + +struct Debugger +{ + explicit Debugger(std::string name, rclcpp::Node * node) + { + // QoS setup + static constexpr std::size_t queue_size = 1; + rclcpp::QoS durable_qos(queue_size); + durable_qos.transient_local(); // option for latching + + pub_debug_ = rclcpp::create_publisher( + node, "~/debug_values/" + name, durable_qos); + debug_values_.data.resize(num_debug_values_, 0.0); + } + rclcpp::Publisher::SharedPtr pub_debug_; + void publishDebugValue() { pub_debug_->publish(debug_values_); } + static constexpr std::uint8_t num_debug_values_ = 20; + mutable tier4_debug_msgs::msg::Float32MultiArrayStamped debug_values_; +}; + +#endif // PARAMETER_ESTIMATOR__DEBUGGER_HPP_ diff --git a/vehicle/parameter_estimator/include/parameter_estimator/gear_ratio_estimator.hpp b/vehicle/parameter_estimator/include/parameter_estimator/gear_ratio_estimator.hpp new file mode 100644 index 00000000..c76fbfbd --- /dev/null +++ b/vehicle/parameter_estimator/include/parameter_estimator/gear_ratio_estimator.hpp @@ -0,0 +1,60 @@ +// +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 PARAMETER_ESTIMATOR__GEAR_RATIO_ESTIMATOR_HPP_ +#define PARAMETER_ESTIMATOR__GEAR_RATIO_ESTIMATOR_HPP_ + +#include "estimator_utils/estimator_base.hpp" +#include "estimator_utils/math_utils.hpp" +#include "estimator_utils/optimization_utils.hpp" +#include "parameter_estimator/debugger.hpp" +#include "parameter_estimator/parameters.hpp" +#include "rclcpp/rclcpp.hpp" + +#include +#include +#include +#include + +#include +#include + +class GearRatioEstimator : public EstimatorBase +{ +public: + GearRatioEstimator( + rclcpp::Node * node, const Params & p, const double & cov, const double ff, + const std::vector & est); + ~GearRatioEstimator() {} + void setData(const VehicleData & v) { data_ = v; } + +private: + VehicleData data_; + Params params_; + int dim_x_; + std::unique_ptr debugger_; + Eigen::MatrixXd estimated_; + Eigen::MatrixXd covariance_; + Eigen::MatrixXd forgetting_factor_; + double error_; + bool estimate(); + bool checkIsValidData(); + void preprocessData() {} + void postprocessOutput(); + void publishData(); +}; + +#endif // PARAMETER_ESTIMATOR__GEAR_RATIO_ESTIMATOR_HPP_ diff --git a/vehicle/parameter_estimator/include/parameter_estimator/parameter_estimator_node.hpp b/vehicle/parameter_estimator/include/parameter_estimator/parameter_estimator_node.hpp new file mode 100644 index 00000000..2e721f81 --- /dev/null +++ b/vehicle/parameter_estimator/include/parameter_estimator/parameter_estimator_node.hpp @@ -0,0 +1,88 @@ +// +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 PARAMETER_ESTIMATOR__PARAMETER_ESTIMATOR_NODE_HPP_ +#define PARAMETER_ESTIMATOR__PARAMETER_ESTIMATOR_NODE_HPP_ + +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" +#include "estimator_utils/estimator_base.hpp" +#include "parameter_estimator/gear_ratio_estimator.hpp" +#include "parameter_estimator/parameters.hpp" +#include "parameter_estimator/steer_offset_estimator.hpp" +#include "parameter_estimator/wheel_base_estimator.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "autoware_vehicle_msgs/msg/control_mode_report.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "sensor_msgs/msg/imu.hpp" +#include "tier4_calibration_msgs/msg/float32_stamped.hpp" + +#include + +class ParameterEstimatorNode : public rclcpp::Node +{ +private: + rclcpp::Subscription::SharedPtr sub_vehicle_twist_; + rclcpp::Subscription::SharedPtr sub_steer_; + rclcpp::Subscription::SharedPtr sub_steer_wheel_; + rclcpp::Subscription::SharedPtr + sub_control_mode_report_; + rclcpp::Subscription::SharedPtr sub_imu_; + + // Timer + rclcpp::TimerBase::SharedPtr timer_; + void initTimer(double period_s); + + geometry_msgs::msg::TwistStamped::ConstSharedPtr vehicle_twist_ptr_; + sensor_msgs::msg::Imu::ConstSharedPtr imu_ptr_; + tier4_calibration_msgs::msg::Float32Stamped::ConstSharedPtr steer_ptr_; + tier4_calibration_msgs::msg::Float32Stamped::ConstSharedPtr steer_wheel_ptr_; + autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_ptr_; + + /** + * ros parameters + **/ + Params params_; + double use_auto_mode_; + double wheel_base_; + double update_hz_; + double covariance_; + double forgetting_factor_; + bool select_gear_ratio_estimator; + bool select_steer_offset_estimator; + bool select_wheel_base_estimator; + + double auto_mode_duration_ = 0; + double last_manual_time_ = 0; + + std::unique_ptr steer_offset_estimator_; + std::unique_ptr wheel_base_estimator_; + std::unique_ptr gear_ratio_estimator_; + + void callbackVehicleTwist(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg); + void callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr msg); + void callbackSteer(const tier4_calibration_msgs::msg::Float32Stamped::ConstSharedPtr msg); + void callbackSteerWheel(const tier4_calibration_msgs::msg::Float32Stamped::ConstSharedPtr msg); + void callbackControlModeReport( + const autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg); + void timerCallback(); + bool updateGearRatio(); + +public: + explicit ParameterEstimatorNode(const rclcpp::NodeOptions & node_options); +}; + +#endif // PARAMETER_ESTIMATOR__PARAMETER_ESTIMATOR_NODE_HPP_ diff --git a/vehicle/parameter_estimator/include/parameter_estimator/parameters.hpp b/vehicle/parameter_estimator/include/parameter_estimator/parameters.hpp new file mode 100644 index 00000000..b3f154e5 --- /dev/null +++ b/vehicle/parameter_estimator/include/parameter_estimator/parameters.hpp @@ -0,0 +1,37 @@ +// +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 PARAMETER_ESTIMATOR__PARAMETERS_HPP_ +#define PARAMETER_ESTIMATOR__PARAMETERS_HPP_ + +struct Params +{ + double valid_max_steer_rad; + double valid_min_velocity; + double valid_min_angular_velocity; + bool is_showing_debug_info; +}; + +struct VehicleData +{ + double velocity; + double angular_velocity; + double steer; + double wheel_base; + double handle; +}; + +#endif // PARAMETER_ESTIMATOR__PARAMETERS_HPP_ diff --git a/vehicle/parameter_estimator/include/parameter_estimator/steer_offset_estimator.hpp b/vehicle/parameter_estimator/include/parameter_estimator/steer_offset_estimator.hpp new file mode 100644 index 00000000..04f90969 --- /dev/null +++ b/vehicle/parameter_estimator/include/parameter_estimator/steer_offset_estimator.hpp @@ -0,0 +1,52 @@ +// +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 PARAMETER_ESTIMATOR__STEER_OFFSET_ESTIMATOR_HPP_ +#define PARAMETER_ESTIMATOR__STEER_OFFSET_ESTIMATOR_HPP_ + +#include "estimator_utils/estimator_base.hpp" +#include "estimator_utils/math_utils.hpp" +#include "estimator_utils/optimization_utils.hpp" +#include "parameter_estimator/debugger.hpp" +#include "parameter_estimator/parameters.hpp" +#include "rclcpp/rclcpp.hpp" + +#include + +class SteerOffsetEstimator : public EstimatorBase +{ +public: + explicit SteerOffsetEstimator( + rclcpp::Node * node, const Params & p, double cov, const double ff, const double est); + ~SteerOffsetEstimator() {} + void setData(const VehicleData & v) { data_ = v; } + +private: + VehicleData data_; + Params params_; + std::unique_ptr debugger_; + double covariance_; + double forgetting_factor_; + double estimated_; + double error_; + bool estimate(); + bool checkIsValidData(); + void preprocessData() {} + void postprocessOutput(); + void publishData(); +}; + +#endif // PARAMETER_ESTIMATOR__STEER_OFFSET_ESTIMATOR_HPP_ diff --git a/vehicle/parameter_estimator/include/parameter_estimator/wheel_base_estimator.hpp b/vehicle/parameter_estimator/include/parameter_estimator/wheel_base_estimator.hpp new file mode 100644 index 00000000..3c8c4d80 --- /dev/null +++ b/vehicle/parameter_estimator/include/parameter_estimator/wheel_base_estimator.hpp @@ -0,0 +1,52 @@ +// +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 PARAMETER_ESTIMATOR__WHEEL_BASE_ESTIMATOR_HPP_ +#define PARAMETER_ESTIMATOR__WHEEL_BASE_ESTIMATOR_HPP_ + +#include "estimator_utils/estimator_base.hpp" +#include "estimator_utils/math_utils.hpp" +#include "estimator_utils/optimization_utils.hpp" +#include "parameter_estimator/debugger.hpp" +#include "parameter_estimator/parameters.hpp" +#include "rclcpp/rclcpp.hpp" + +#include + +class WheelBaseEstimator : public EstimatorBase +{ +public: + explicit WheelBaseEstimator( + rclcpp::Node * node, const Params & p, double cov, const double ff, const double est); + ~WheelBaseEstimator() {} + void setData(const VehicleData & v) { data_ = v; } + +private: + VehicleData data_; + Params params_; + std::unique_ptr debugger_; + double covariance_; + double forgetting_factor_; + double estimated_; + double error_; + bool estimate(); + bool checkIsValidData(); + void preprocessData() {} + void postprocessOutput(); + void publishData(); +}; + +#endif // PARAMETER_ESTIMATOR__WHEEL_BASE_ESTIMATOR_HPP_ diff --git a/vehicle/parameter_estimator/launch/parameter_estimator.launch.xml b/vehicle/parameter_estimator/launch/parameter_estimator.launch.xml new file mode 100644 index 00000000..5cdffa7c --- /dev/null +++ b/vehicle/parameter_estimator/launch/parameter_estimator.launch.xml @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/vehicle/parameter_estimator/launch/parameter_estimator_with_simulation.launch.xml b/vehicle/parameter_estimator/launch/parameter_estimator_with_simulation.launch.xml new file mode 100644 index 00000000..f3c57375 --- /dev/null +++ b/vehicle/parameter_estimator/launch/parameter_estimator_with_simulation.launch.xml @@ -0,0 +1,37 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/vehicle/parameter_estimator/media/error.png b/vehicle/parameter_estimator/media/error.png new file mode 100644 index 00000000..4acfd58f Binary files /dev/null and b/vehicle/parameter_estimator/media/error.png differ diff --git a/vehicle/parameter_estimator/media/parameter_estimation.png b/vehicle/parameter_estimator/media/parameter_estimation.png new file mode 100644 index 00000000..2b78df85 Binary files /dev/null and b/vehicle/parameter_estimator/media/parameter_estimation.png differ diff --git a/vehicle/parameter_estimator/media/parameter_estimator.gif b/vehicle/parameter_estimator/media/parameter_estimator.gif new file mode 100644 index 00000000..69a7a8f6 Binary files /dev/null and b/vehicle/parameter_estimator/media/parameter_estimator.gif differ diff --git a/vehicle/parameter_estimator/media/result.png b/vehicle/parameter_estimator/media/result.png new file mode 100644 index 00000000..b4c7f11f Binary files /dev/null and b/vehicle/parameter_estimator/media/result.png differ diff --git a/vehicle/parameter_estimator/package.xml b/vehicle/parameter_estimator/package.xml new file mode 100644 index 00000000..6f8023d2 --- /dev/null +++ b/vehicle/parameter_estimator/package.xml @@ -0,0 +1,30 @@ + + + + parameter_estimator + 0.1.0 + The parameter_estimator + Taiki Tanaka + Tomoya Kimura + Apache 2 + + ament_cmake_auto + + autoware_vehicle_info_utils + autoware_vehicle_msgs + estimator_utils + geometry_msgs + rclcpp + sensor_msgs + tier4_calibration_msgs + tier4_debug_msgs + plotjuggler + plotjuggler_ros + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/vehicle/parameter_estimator/src/gear_ratio_estimator.cpp b/vehicle/parameter_estimator/src/gear_ratio_estimator.cpp new file mode 100644 index 00000000..5e4b1388 --- /dev/null +++ b/vehicle/parameter_estimator/src/gear_ratio_estimator.cpp @@ -0,0 +1,117 @@ +// +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 "parameter_estimator/gear_ratio_estimator.hpp" + +#include +#include + +GearRatioEstimator::GearRatioEstimator( + rclcpp::Node * node, const Params & p, const double & cov, const double ff, + const std::vector & est) +{ + error_ = 0; + dim_x_ = est.size(); + estimated_ = Eigen::MatrixXd::Zero(dim_x_, 1); + for (size_t i = 0; i < est.size(); i++) { + estimated_(i, 0) = est.at(i); + } + covariance_ = + (Eigen::MatrixXd::Identity(dim_x_, dim_x_) + 0.1 * Eigen::MatrixXd::Ones(dim_x_, dim_x_)) * cov; + forgetting_factor_ = Eigen::MatrixXd::Identity(1, 1) * ff; + params_ = p; + debugger_ = std::make_unique("gear_ratio", node); + // in estimator_base.h + createPublisher("gear_ratio", node); + result_statistics_ = math_utils::Statistics(dim_x_); + error_statistics_ = math_utils::Statistics(1); +} + +bool GearRatioEstimator::estimate() +{ + const auto & vel = data_.velocity; + const auto & wheel_base = data_.wheel_base; + const auto & wz = data_.angular_velocity; + const auto & handle = data_.handle; + const auto & ff = forgetting_factor_; + auto & est = estimated_; + auto & cov = covariance_; + // steering = handle / gear_ratio + // gear_ratio=(a+b*v^2-c*abs(handle)) + Eigen::MatrixXd zn = Eigen::MatrixXd::Zero(dim_x_, 1); + Eigen::MatrixXd yn = Eigen::MatrixXd::Zero(1, 1); + Eigen::MatrixXd th = Eigen::MatrixXd::Zero(dim_x_, 1); + + if (dim_x_ == 3) { + yn(0, 0) = handle / std::atan2(wz * wheel_base, vel); + zn(0, 0) = 1.0; + zn(1, 0) = vel * vel; + zn(2, 0) = -std::fabs(handle); + } else if (dim_x_ == 4) { + yn(0, 0) = handle / std::atan2(wz * wheel_base, vel); + zn(0, 0) = 1.0; + zn(1, 0) = vel; + zn(2, 0) = vel * vel; + zn(3, 0) = -std::fabs(handle); + } + optimization_utils::estimateByRLS(est, cov, zn, ff, yn); + error_ = (yn - zn.transpose() * est)(0, 0); + auto & de = debugger_->debug_values_; + { + th << 15.713, 0.053, 0.042; + double gear = (zn.transpose() * est)(0, 0); + de.data[0] = gear; + de.data[1] = yn(0, 0); + de.data[2] = (zn.transpose() * th)(0, 0); + } + // if (error > 1.0) return false; + return true; +} + +bool GearRatioEstimator::checkIsValidData() +{ + const auto & d = data_; + debugger_->debug_values_.data[5] = 0; + if ( + std::fabs(d.velocity) < params_.valid_min_velocity || + std::fabs(d.angular_velocity) < params_.valid_min_angular_velocity) { + return false; + } + debugger_->debug_values_.data[5] = 1; + return true; +} + +void GearRatioEstimator::postprocessOutput() +{ + result_statistics_.value = {estimated_(0, 0), estimated_(1, 0), estimated_(2, 0)}; + error_statistics_.value = {std::fabs(error_)}; + std::vector covariance = {covariance_(0, 0), covariance_(1, 1), covariance_(2, 2)}; + result_msgs_.covariance = covariance; +} + +void GearRatioEstimator::publishData() +{ + auto & de = debugger_->debug_values_; + const auto & vel = data_.velocity; + const auto & wheel_base = data_.wheel_base; + const auto & wz = data_.angular_velocity; + const auto & handle = data_.handle; + double gear = + (1.0 * estimated_(0, 0) + vel * vel * estimated_(1, 0) - std::fabs(handle) * estimated_(2, 0)); + de.data[7] = vel * std::tan(handle / gear) / wheel_base; + de.data[8] = wz; + debugger_->publishDebugValue(); +} diff --git a/vehicle/parameter_estimator/src/main.cpp b/vehicle/parameter_estimator/src/main.cpp new file mode 100644 index 00000000..c37d82d2 --- /dev/null +++ b/vehicle/parameter_estimator/src/main.cpp @@ -0,0 +1,28 @@ +// +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 "parameter_estimator/parameter_estimator_node.hpp" + +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + rclcpp::spin(std::make_shared(node_options)); + rclcpp::shutdown(); + return 0; +} diff --git a/vehicle/parameter_estimator/src/parameter_estimator_node.cpp b/vehicle/parameter_estimator/src/parameter_estimator_node.cpp new file mode 100644 index 00000000..60bf2f2d --- /dev/null +++ b/vehicle/parameter_estimator/src/parameter_estimator_node.cpp @@ -0,0 +1,191 @@ +// +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 "parameter_estimator/parameter_estimator_node.hpp" + +#include +#include +#include + +ParameterEstimatorNode::ParameterEstimatorNode(const rclcpp::NodeOptions & node_options) +: Node("parameter_estimator", node_options) +{ + using std::placeholders::_1; + + // QoS setup + static constexpr std::size_t queue_size = 1; + rclcpp::QoS durable_qos(queue_size); + durable_qos.transient_local(); // option for latching + + // get parameter + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); + wheel_base_ = vehicle_info.wheel_base_m; + use_auto_mode_ = this->declare_parameter("use_auto_mode", true); + update_hz_ = this->declare_parameter("update_hz", 10.0); + covariance_ = this->declare_parameter("initial_covariance", 1.0); + forgetting_factor_ = this->declare_parameter("forgetting_factor", 0.999); + select_gear_ratio_estimator = this->declare_parameter("select_gear_ratio_estimator", true); + select_steer_offset_estimator = + this->declare_parameter("select_steer_offset_estimator", true); + select_wheel_base_estimator = this->declare_parameter("select_wheel_base_estimator", true); + params_.valid_max_steer_rad = this->declare_parameter("valid_max_steer_rad", 0.05); + params_.valid_min_velocity = this->declare_parameter("valid_min_velocity", 0.5); + params_.valid_min_angular_velocity = + this->declare_parameter("valid_min_angular_velocity", 0.1); + params_.is_showing_debug_info = this->declare_parameter("is_showing_debug_info", true); + + const auto estimated_gear_ratio = + this->declare_parameter>("gear_ratio", {15.7, 0.053, 0.047}); + steer_offset_estimator_ = + std::make_unique(this, params_, covariance_, forgetting_factor_, 0); + wheel_base_estimator_ = std::make_unique( + this, params_, covariance_, forgetting_factor_, wheel_base_); + gear_ratio_estimator_ = std::make_unique( + this, params_, covariance_, forgetting_factor_, estimated_gear_ratio); + + // subscriber + sub_imu_ = create_subscription( + "input/imu_twist", queue_size, std::bind(&ParameterEstimatorNode::callbackImu, this, _1)); + sub_vehicle_twist_ = create_subscription( + "input/vehicle_twist", queue_size, + std::bind(&ParameterEstimatorNode::callbackVehicleTwist, this, _1)); + + if (select_gear_ratio_estimator) { + sub_steer_wheel_ = create_subscription( + "input/handle_status", queue_size, + std::bind(&ParameterEstimatorNode::callbackSteerWheel, this, _1)); + } + + if (select_steer_offset_estimator || select_wheel_base_estimator) { + sub_steer_ = create_subscription( + "input/steer", queue_size, std::bind(&ParameterEstimatorNode::callbackSteer, this, _1)); + } + sub_control_mode_report_ = create_subscription( + "input/control_mode", queue_size, + std::bind(&ParameterEstimatorNode::callbackControlModeReport, this, _1)); + + initTimer(1.0 / update_hz_); +} + +void ParameterEstimatorNode::initTimer(double period_s) +{ + auto timer_callback = std::bind(&ParameterEstimatorNode::timerCallback, this); + const auto period_ns = + std::chrono::duration_cast(std::chrono::duration(period_s)); + timer_ = std::make_shared>( + this->get_clock(), period_ns, std::move(timer_callback), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(timer_, nullptr); +} + +void ParameterEstimatorNode::timerCallback() +{ + auto & clk = *this->get_clock(); + const auto & is_debug = params_.is_showing_debug_info; + if (!vehicle_twist_ptr_) { + RCLCPP_INFO_EXPRESSION( + rclcpp::get_logger("parameter_estimator"), is_debug, "vehicle_twist_ptr_ is null"); + return; + } else if (!steer_ptr_ && (select_steer_offset_estimator || select_wheel_base_estimator)) { + RCLCPP_INFO_EXPRESSION( + rclcpp::get_logger("parameter_estimator"), is_debug, "steer_ptr_ is null"); + return; + } else if (!steer_wheel_ptr_ && select_gear_ratio_estimator) { + RCLCPP_INFO_EXPRESSION( + rclcpp::get_logger("parameter_estimator"), is_debug, "steer_wheel_ptr_ is null"); + return; + } else if (!imu_ptr_) { + RCLCPP_INFO_EXPRESSION(rclcpp::get_logger("parameter_estimator"), is_debug, "imu_ptr_ is null"); + return; + } else if (auto_mode_duration_ < 0.5 && use_auto_mode_) { + RCLCPP_INFO_STREAM_THROTTLE( + rclcpp::get_logger("parameter_estimator"), clk, 5000, "not auto mode"); + return; + } else { + RCLCPP_INFO_STREAM_THROTTLE( + rclcpp::get_logger("parameter_estimator"), clk, 5000, "running parameter estimator"); + } + + { + VehicleData v = {}; + v.velocity = vehicle_twist_ptr_->twist.linear.x; + v.angular_velocity = -imu_ptr_->angular_velocity.z; + if (select_steer_offset_estimator || select_wheel_base_estimator) { + v.steer = steer_ptr_->data; + } + if (select_gear_ratio_estimator) { + v.handle = steer_wheel_ptr_->data; + } + v.wheel_base = wheel_base_; + if (select_steer_offset_estimator) { + steer_offset_estimator_->setData(v); + steer_offset_estimator_->processData(); + steer_offset_estimator_->Run(); + } + if (select_wheel_base_estimator) { + wheel_base_estimator_->setData(v); + wheel_base_estimator_->processData(); + wheel_base_estimator_->Run(); + } + if (select_gear_ratio_estimator) { + gear_ratio_estimator_->setData(v); + gear_ratio_estimator_->processData(); + gear_ratio_estimator_->Run(); + } + } +} + +void ParameterEstimatorNode::callbackVehicleTwist( + const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) +{ + vehicle_twist_ptr_ = msg; +} + +void ParameterEstimatorNode::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr msg) +{ + imu_ptr_ = msg; +} + +void ParameterEstimatorNode::callbackSteer( + const tier4_calibration_msgs::msg::Float32Stamped::ConstSharedPtr msg) +{ + steer_ptr_ = msg; +} + +void ParameterEstimatorNode::callbackSteerWheel( + const tier4_calibration_msgs::msg::Float32Stamped::ConstSharedPtr msg) +{ + steer_wheel_ptr_ = msg; +} + +void ParameterEstimatorNode::callbackControlModeReport( + const autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg) +{ + auto & clk = *this->get_clock(); + control_mode_ptr_ = msg; + if (control_mode_ptr_->mode == autoware_vehicle_msgs::msg::ControlModeReport::AUTONOMOUS) { + auto_mode_duration_ = (this->now().seconds() - last_manual_time_); + RCLCPP_DEBUG_STREAM_THROTTLE( + rclcpp::get_logger("parameter_estimator"), clk, 5000, + "[parameter_estimator] control mode duration: " << auto_mode_duration_); + } else { + auto_mode_duration_ = 0; + last_manual_time_ = this->now().seconds(); + RCLCPP_DEBUG_STREAM_THROTTLE( + rclcpp::get_logger("parameter_estimator"), clk, 5000, + "[parameter_estimator] control mode : manual"); + } +} diff --git a/vehicle/parameter_estimator/src/steer_offset_estimator.cpp b/vehicle/parameter_estimator/src/steer_offset_estimator.cpp new file mode 100644 index 00000000..ff86d3d6 --- /dev/null +++ b/vehicle/parameter_estimator/src/steer_offset_estimator.cpp @@ -0,0 +1,77 @@ +// +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 "parameter_estimator/steer_offset_estimator.hpp" + +#include +#include +SteerOffsetEstimator::SteerOffsetEstimator( + rclcpp::Node * node, const Params & p, double cov, const double ff, const double est) +{ + error_ = 0; + covariance_ = cov; + forgetting_factor_ = ff; + estimated_ = est; + params_ = p; + debugger_ = std::make_unique("steer_offset", node); + createPublisher("steer_offset", node); + result_statistics_ = math_utils::Statistics(1); + error_statistics_ = math_utils::Statistics(1); +} + +bool SteerOffsetEstimator::estimate() +{ + // use following approximation: tan(a+b) = tan(a) + tan(b) = a + b + const auto & vel = data_.velocity; + const auto & wz = data_.angular_velocity; + const auto & wheel_base = data_.wheel_base; + const auto & steer = data_.steer; + const double phi = vel / wheel_base; + const double yn = wz - phi * steer; + auto & cov = covariance_; + const auto & ff = forgetting_factor_; + auto & est = estimated_; + optimization_utils::estimateByRLS(est, cov, phi, ff, yn); + error_ = yn - phi * est; + // if (error > 1.0) return false; + return true; +} + +bool SteerOffsetEstimator::checkIsValidData() +{ + const auto & d = data_; + debugger_->debug_values_.data[5] = 0; + if ( + std::fabs(d.velocity) < params_.valid_min_velocity || + std::fabs(d.steer) > params_.valid_max_steer_rad) { + return false; + } + debugger_->debug_values_.data[5] = 1; + return true; +} + +void SteerOffsetEstimator::postprocessOutput() +{ + result_statistics_.value = {estimated_}; + error_statistics_.value = {std::fabs(error_)}; + std::vector covariance = {covariance_}; + result_msgs_.covariance = covariance; +} + +void SteerOffsetEstimator::publishData() +{ + debugger_->publishDebugValue(); +} diff --git a/vehicle/parameter_estimator/src/wheel_base_estimator.cpp b/vehicle/parameter_estimator/src/wheel_base_estimator.cpp new file mode 100644 index 00000000..329db451 --- /dev/null +++ b/vehicle/parameter_estimator/src/wheel_base_estimator.cpp @@ -0,0 +1,80 @@ +// +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 "parameter_estimator/wheel_base_estimator.hpp" + +#include +#include + +WheelBaseEstimator::WheelBaseEstimator( + rclcpp::Node * node, const Params & p, double cov, const double ff, const double est) +{ + error_ = 0; + covariance_ = cov; + forgetting_factor_ = ff; + estimated_ = est; + params_ = p; + debugger_ = std::make_unique("wheel_base", node); + createPublisher("wheel_base", node); + result_statistics_ = math_utils::Statistics(1); + error_statistics_ = math_utils::Statistics(1); +} + +bool WheelBaseEstimator::estimate() +{ + const auto & vel = data_.velocity; + const auto & wz = data_.angular_velocity; + const auto & wheel_base = data_.wheel_base; + const auto & steer = data_.steer; + const double zn = wz; + const double yn = vel * std::tan(steer); + auto & cov = covariance_; + const auto & ff = forgetting_factor_; + auto & est = estimated_; + // wz * wheel_base = vel * tan(steer) + optimization_utils::estimateByRLS(est, cov, zn, ff, yn); + error_ = yn - zn * est; + auto & de = debugger_->debug_values_; + de.data[0] = zn * wheel_base; + de.data[1] = yn; + de.data[2] = 1; + // if (error > 1.0) return false; + return true; +} + +void WheelBaseEstimator::postprocessOutput() +{ + result_statistics_.value = {estimated_}; + error_statistics_.value = {std::fabs(error_)}; + std::vector covariance = {covariance_}; + result_msgs_.covariance = covariance; +} + +bool WheelBaseEstimator::checkIsValidData() +{ + const auto & d = data_; + debugger_->debug_values_.data[5] = 0; + if (std::fabs(d.velocity) < params_.valid_min_velocity) { + return false; + } + debugger_->debug_values_.data[5] = 1; + return true; +} + +void WheelBaseEstimator::publishData() +{ + debugger_->publishDebugValue(); +} diff --git a/vehicle/pitch_checker/CMakeLists.txt b/vehicle/pitch_checker/CMakeLists.txt new file mode 100644 index 00000000..76112201 --- /dev/null +++ b/vehicle/pitch_checker/CMakeLists.txt @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 3.5) +project(pitch_checker) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_executable(pitch_checker + src/pitch_checker_node.cpp + src/main.cpp +) +ament_target_dependencies(pitch_checker) + +ament_auto_add_library(pitch_compare SHARED + src/pitch_compare.cpp + src/pitch_reader.cpp +) +ament_target_dependencies(pitch_compare) + +install( + PROGRAMS + scripts/view_pitch.py + DESTINATION + lib/${PROJECT_NAME} +) + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/vehicle/pitch_checker/README.md b/vehicle/pitch_checker/README.md new file mode 100644 index 00000000..f08900a9 --- /dev/null +++ b/vehicle/pitch_checker/README.md @@ -0,0 +1,31 @@ +# pitch checker + +The role of this node is to visualize pitch of driving route. The source of pitch is tf (`map->base_link`). + +## How to visualize + +### Collect data + +#### launch data collector node + +```sh +ros2 launch pitch_checker pitch_checker.launch.xml +``` + +#### save file to data + +```sh +ros2 service call /pitch_checker/save_flag std_srvs/srv/Trigger {} +``` + +(The pitch data is saved at `/install/pitch_checker/share/pitch_checker/pitch.csv`) + +### Visualize data + +```sh +ros2 launch pitch_checker view_pitch.launch.xml +``` + +The `view_pitch.launch` loads the data stored in the default path and visualize it is as below. The pitch angle [rad] is shown on the left plot, the value of the z-coordinate [m] on the right plot. + +![sample pic](data/pitch_sample.png) diff --git a/vehicle/pitch_checker/config/pitch_checker_param.yaml b/vehicle/pitch_checker/config/pitch_checker_param.yaml new file mode 100644 index 00000000..c652527d --- /dev/null +++ b/vehicle/pitch_checker/config/pitch_checker_param.yaml @@ -0,0 +1,3 @@ +pitch_checker: + ros__parameters: + update_hz: 10.0 # Used for the timer diff --git a/vehicle/pitch_checker/data/pitch_sample.png b/vehicle/pitch_checker/data/pitch_sample.png new file mode 100644 index 00000000..efc70389 Binary files /dev/null and b/vehicle/pitch_checker/data/pitch_sample.png differ diff --git a/vehicle/pitch_checker/include/pitch_checker/pitch_checker.hpp b/vehicle/pitch_checker/include/pitch_checker/pitch_checker.hpp new file mode 100644 index 00000000..f076fab9 --- /dev/null +++ b/vehicle/pitch_checker/include/pitch_checker/pitch_checker.hpp @@ -0,0 +1,83 @@ +// +// Copyright 2020 Tier IV, Inc. All rights reserved. +// +// 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 PITCH_CHECKER__PITCH_CHECKER_HPP_ +#define PITCH_CHECKER__PITCH_CHECKER_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "tf2/utils.h" + +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "std_msgs/msg/bool.hpp" +#include "std_srvs/srv/trigger.hpp" + +#include +#include +#include +#include +#include +#include +#ifdef ROS_DISTRO_GALACTIC +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#else +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#endif +#include "tier4_autoware_utils/ros/transform_listener.hpp" + +struct TfInfo +{ + double x; + double y; + double z; + double yaw; + double pitch; +}; + +class PitchChecker : public rclcpp::Node +{ +public: + explicit PitchChecker(const rclcpp::NodeOptions & node_options); + +private: + std::shared_ptr transform_listener_; + + // Timer + rclcpp::TimerBase::SharedPtr timer_; + void initTimer(double period_s); + + // Service + rclcpp::Service::SharedPtr save_flag_server_; + + std::vector tf_info_vec_; + std::map, std::vector> tf_map_; + std::map, std::vector> tf_map_unified_; + std::string output_file_; + double update_hz_; + int get_tf_count_ = 0; + static constexpr int MAX_TF_COUNT_ = 1e08; + + void timerCallback(); + bool onSaveService( + const std::shared_ptr req_header, + const std::shared_ptr req, + const std::shared_ptr res); + bool getTf(); + void pitchInfoToMap(); + bool writeMap(); + TfInfo getMedianPitchTfInfo(const std::vector & tf_info_vec); +}; + +#endif // PITCH_CHECKER__PITCH_CHECKER_HPP_ diff --git a/vehicle/pitch_checker/include/pitch_checker/pitch_compare.hpp b/vehicle/pitch_checker/include/pitch_checker/pitch_compare.hpp new file mode 100644 index 00000000..65757f97 --- /dev/null +++ b/vehicle/pitch_checker/include/pitch_checker/pitch_compare.hpp @@ -0,0 +1,31 @@ +// +// Copyright 2020 Tier IV, Inc. All rights reserved. +// +// 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 PITCH_CHECKER__PITCH_COMPARE_HPP_ +#define PITCH_CHECKER__PITCH_COMPARE_HPP_ + +#include "rclcpp/rclcpp.hpp" + +namespace pitch_compare +{ +class PitchCompareNode : public rclcpp::Node +{ +public: + explicit PitchCompareNode(const rclcpp::NodeOptions & node_options); +}; +} // namespace pitch_compare + +#endif // PITCH_CHECKER__PITCH_COMPARE_HPP_ diff --git a/vehicle/pitch_checker/include/pitch_checker/pitch_reader.hpp b/vehicle/pitch_checker/include/pitch_checker/pitch_reader.hpp new file mode 100644 index 00000000..1a01423d --- /dev/null +++ b/vehicle/pitch_checker/include/pitch_checker/pitch_reader.hpp @@ -0,0 +1,54 @@ +// +// Copyright 2020 Tier IV, Inc. All rights reserved. +// +// 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 PITCH_CHECKER__PITCH_READER_HPP_ +#define PITCH_CHECKER__PITCH_READER_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include + +struct TfInfo +{ + double x; + double y; + double z; + double yaw; + double pitch; +}; + +class PitchReader +{ +public: + explicit PitchReader(const std::string input_file); + bool getPitch( + double * pitch, const double x, const double y, const double yaw, + const double dist_thresh = 10.0, const double yaw_thresh = M_PI_4); + std::vector comparePitch(const std::string comp_input_file); + +private: + std::vector tf_infos_; + bool readCSV(const std::string csv_path, std::vector * tf_infos); + std::vector split(const std::string & s, char delim); + bool read_csv_ = false; +}; + +#endif // PITCH_CHECKER__PITCH_READER_HPP_ diff --git a/vehicle/pitch_checker/launch/pitch_checker.launch.xml b/vehicle/pitch_checker/launch/pitch_checker.launch.xml new file mode 100644 index 00000000..1676bd91 --- /dev/null +++ b/vehicle/pitch_checker/launch/pitch_checker.launch.xml @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/vehicle/pitch_checker/launch/view_pitch.launch.xml b/vehicle/pitch_checker/launch/view_pitch.launch.xml new file mode 100644 index 00000000..8818a426 --- /dev/null +++ b/vehicle/pitch_checker/launch/view_pitch.launch.xml @@ -0,0 +1,5 @@ + + + + + diff --git a/vehicle/pitch_checker/package.xml b/vehicle/pitch_checker/package.xml new file mode 100644 index 00000000..434db35d --- /dev/null +++ b/vehicle/pitch_checker/package.xml @@ -0,0 +1,32 @@ + + + + pitch_checker + 0.1.0 + The pitch_checker + Tomoya Kimura + Apache 2 + + ament_cmake_auto + + autoware_cmake + + geometry_msgs + rclcpp + std_msgs + std_srvs + tf2 + tf2_geometry_msgs + tf2_ros + tier4_autoware_utils + + python3-matplotlib + python3-pandas + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/vehicle/pitch_checker/scripts/view_pitch.py b/vehicle/pitch_checker/scripts/view_pitch.py new file mode 100755 index 00000000..933a0218 --- /dev/null +++ b/vehicle/pitch_checker/scripts/view_pitch.py @@ -0,0 +1,61 @@ +#! /usr/bin/env python3 +# -*- coding: utf-8 -*- + +# Copyright 2021 Tier IV, Inc. All rights reserved. +# +# 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. + +from ament_index_python.packages import get_package_share_directory +import matplotlib.pyplot as plt +import pandas as pd +import rclpy +from rclpy.node import Node + + +class ViewPlot(Node): + def __init__(self): + super().__init__("pitch_viewer") + package_path = get_package_share_directory("pitch_checker") + self.declare_parameter("pitch_file_name", package_path + "/pitch.csv") + file_name = self.get_parameter("pitch_file_name").get_parameter_value().string_value + data = pd.read_csv(file_name) + + x = data["x"] + y = data["y"] + z = data["z"] + pitch = data["pitch"] + + fig1 = plt.subplot(121) + # cmap... cmap or hsv + sc1 = fig1.scatter(x, y, c=pitch, label="pitch", cmap="gist_rainbow") + sc1.set_clim(-0.025, 0.025) + plt.colorbar(sc1) + fig1.set_title("pitch") + + fig2 = plt.subplot(122) + sc2 = fig2.scatter(x, y, c=z, label="z", cmap="gist_rainbow") + plt.colorbar(sc2) + fig2.set_title("z-axis") + plt.show() + + +def main(args=None): + rclpy.init(args=args) + node = ViewPlot() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/vehicle/pitch_checker/src/main.cpp b/vehicle/pitch_checker/src/main.cpp new file mode 100644 index 00000000..71db1469 --- /dev/null +++ b/vehicle/pitch_checker/src/main.cpp @@ -0,0 +1,28 @@ +// +// Copyright 2020 Tier IV, Inc. All rights reserved. +// +// 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 "pitch_checker/pitch_checker.hpp" + +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + rclcpp::spin(std::make_shared(node_options)); + rclcpp::shutdown(); + return 0; +} diff --git a/vehicle/pitch_checker/src/pitch_checker_node.cpp b/vehicle/pitch_checker/src/pitch_checker_node.cpp new file mode 100644 index 00000000..66a9a016 --- /dev/null +++ b/vehicle/pitch_checker/src/pitch_checker_node.cpp @@ -0,0 +1,191 @@ +// +// Copyright 2020 Tier IV, Inc. All rights reserved. +// +// 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 "pitch_checker/pitch_checker.hpp" + +#include +#include +#include +#include + +PitchChecker::PitchChecker(const rclcpp::NodeOptions & node_options) +: Node("pitch_checker", node_options) +{ + transform_listener_ = std::make_shared(this); + using std::placeholders::_1; + using std::placeholders::_2; + using std::placeholders::_3; + + update_hz_ = this->declare_parameter("update_hz", 10.0); + output_file_ = this->declare_parameter("output_file", "pitch.csv"); + save_flag_server_ = this->create_service( + "/pitch_checker/save_flag", std::bind(&PitchChecker::onSaveService, this, _1, _2, _3)); + initTimer(1.0 / update_hz_); +} + +void PitchChecker::initTimer(double period_s) +{ + auto timer_callback = std::bind(&PitchChecker::timerCallback, this); + const auto period_ns = + std::chrono::duration_cast(std::chrono::duration(period_s)); + timer_ = std::make_shared>( + this->get_clock(), period_ns, std::move(timer_callback), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(timer_, nullptr); +} + +bool PitchChecker::onSaveService( + [[maybe_unused]] const std::shared_ptr req_header, + [[maybe_unused]] const std::shared_ptr req, + const std::shared_ptr res) +{ + pitchInfoToMap(); + if (writeMap()) { + res->success = true; + res->message = "Data has been successfully saved on " + output_file_; + } else { + res->success = false; + res->message = "Failed to save data. Maybe wrong path?"; + } + + return true; +} + +void PitchChecker::timerCallback() +{ + getTf(); + if (get_tf_count_ >= MAX_TF_COUNT_) { + RCLCPP_INFO_STREAM( + rclcpp::get_logger("pitch_compare"), + "Save data and stop recording due to capacity limitation."); + pitchInfoToMap(); + writeMap(); + } +} + +bool PitchChecker::getTf() +{ + geometry_msgs::msg::TransformStamped::ConstSharedPtr transform; + try { + transform = transform_listener_->getTransform( + "map", "base_link", rclcpp::Time(0), rclcpp::Duration::from_seconds(0.5)); + } catch (tf2::TransformException & ex) { + auto & clk = *this->get_clock(); + RCLCPP_WARN_SKIPFIRST_THROTTLE( + rclcpp::get_logger("pitch_checker"), clk, 5000, "cannot get map to base_link transform. %s", + ex.what()); + return false; + } + double roll, pitch, yaw; + tf2::getEulerYPR(transform->transform.rotation, roll, pitch, yaw); + TfInfo tf_info; + tf_info.pitch = pitch; + tf_info.yaw = yaw; + tf_info.x = transform->transform.translation.x; + tf_info.y = transform->transform.translation.y; + tf_info.z = transform->transform.translation.z; + tf_info_vec_.emplace_back(tf_info); + get_tf_count_++; + return true; +} + +void PitchChecker::pitchInfoToMap() +{ + tf_map_.clear(); + tf_map_unified_.clear(); + for (const auto tf_info : tf_info_vec_) { + const int x_i = static_cast(std::nearbyint(tf_info.x)); + const int y_i = static_cast(std::nearbyint(tf_info.y)); + const auto xy_i = std::pair(x_i, y_i); + // create new key + if (tf_map_.count(xy_i) == 0) { + tf_map_[xy_i] = std::vector(); + } + tf_map_[xy_i].emplace_back(tf_info); + } + + for (const auto & map_val : tf_map_) { + const auto key = map_val.first; + const auto tf_info_vec = map_val.second; + if (tf_info_vec.empty()) { + continue; + } + std::vector vec1; + std::vector vec2; + // separate vec by yaw + for (const auto & tf_info : tf_info_vec) { + const double dyaw = std::fabs(tf_info.yaw - tf_info_vec.front().yaw); + if (dyaw < M_PI_2) { + vec1.emplace_back(tf_info); + } else { + vec2.emplace_back(tf_info); + } + tf_map_unified_[key] = std::vector(); + if (!vec1.empty()) { + tf_map_unified_[key].emplace_back(getMedianPitchTfInfo(vec1)); + } + if (!vec2.empty()) { + tf_map_unified_[key].emplace_back(getMedianPitchTfInfo(vec2)); + } + } + } +} + +TfInfo PitchChecker::getMedianPitchTfInfo(const std::vector & tf_info_vec) +{ + auto tf_info_vec_local = tf_info_vec; + std::sort( + tf_info_vec_local.begin(), tf_info_vec_local.end(), + [](const TfInfo & x, const TfInfo & y) { return x.pitch < y.pitch; }); + + if (tf_info_vec_local.size() % 2 == 0) { + const int mid_idx_1 = (tf_info_vec_local.size()) / 2; + const int mid_idx_2 = (tf_info_vec_local.size()) / 2 - 1; + TfInfo tf_info; + tf_info.x = (tf_info_vec_local.at(mid_idx_1).x + tf_info_vec_local.at(mid_idx_2).x) / 2.0; + tf_info.y = (tf_info_vec_local.at(mid_idx_1).y + tf_info_vec_local.at(mid_idx_2).y) / 2.0; + tf_info.z = (tf_info_vec_local.at(mid_idx_1).z + tf_info_vec_local.at(mid_idx_2).z) / 2.0; + tf_info.pitch = + (tf_info_vec_local.at(mid_idx_1).pitch + tf_info_vec_local.at(mid_idx_2).pitch) / 2.0; + tf_info.yaw = (tf_info_vec_local.at(mid_idx_1).yaw + tf_info_vec_local.at(mid_idx_2).yaw) / 2.0; + return tf_info; + } else { + const int mid_idx = (tf_info_vec_local.size() - 1) / 2; + return tf_info_vec_local.at(mid_idx); + } +} + +bool PitchChecker::writeMap() +{ + std::ofstream of(output_file_); + if (!of.is_open()) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("pitch_checker"), "cannot open the file: " << output_file_); + return false; + } + + of << "x,y,z,yaw,pitch" << std::endl; + for (const auto & map_val : tf_map_unified_) { + const auto key = map_val.first; + const int x = key.first; + const int y = key.second; + for (const auto tf_info : map_val.second) { + of << x << "," << y << "," << tf_info.z << "," << tf_info.yaw << "," << tf_info.pitch + << std::endl; + } + } + return true; +} diff --git a/vehicle/pitch_checker/src/pitch_compare.cpp b/vehicle/pitch_checker/src/pitch_compare.cpp new file mode 100644 index 00000000..962f9102 --- /dev/null +++ b/vehicle/pitch_checker/src/pitch_compare.cpp @@ -0,0 +1,39 @@ +// +// Copyright 2020 Tier IV, Inc. All rights reserved. +// +// 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 "pitch_checker/pitch_compare.hpp" + +#include "pitch_checker/pitch_reader.hpp" + +#include + +namespace pitch_compare +{ +PitchCompareNode::PitchCompareNode(const rclcpp::NodeOptions & node_options) +: Node("pitch_compare", node_options) +{ + const auto base_pitch_csv = + this->declare_parameter("base_pitch_csv", "base_pitch.csv"); + const auto pitch_csv = this->declare_parameter("pitch_csv", "pitch.csv"); + + PitchReader node(base_pitch_csv); + const auto dif_pitch_vec = node.comparePitch(pitch_csv); + + for (const auto dif_pitch : dif_pitch_vec) { + RCLCPP_INFO_STREAM(rclcpp::get_logger("pitch_compare"), dif_pitch); + } +} +} // namespace pitch_compare diff --git a/vehicle/pitch_checker/src/pitch_reader.cpp b/vehicle/pitch_checker/src/pitch_reader.cpp new file mode 100644 index 00000000..13c4c656 --- /dev/null +++ b/vehicle/pitch_checker/src/pitch_reader.cpp @@ -0,0 +1,106 @@ +// +// Copyright 2020 Tier IV, Inc. All rights reserved. +// +// 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. +// + +// cSpell:ignore strvec + +#include "pitch_checker/pitch_reader.hpp" + +#include +#include +#include + +PitchReader::PitchReader(const std::string input_file) +{ + read_csv_ = (readCSV(input_file, &tf_infos_)); +} + +bool PitchReader::getPitch( + double * pitch, const double x, const double y, const double yaw, const double dist_thresh, + const double yaw_thresh) +{ + if (!read_csv_) { + return false; + } + + double min_dist = std::numeric_limits::max(); + bool success_search = false; + for (const auto & tf_info : tf_infos_) { + const double dist = std::hypot(x - tf_info.x, y - tf_info.y); + if (dist < dist_thresh && dist < min_dist && std::fabs(yaw - tf_info.yaw) < yaw_thresh) { + min_dist = dist; + *pitch = tf_info.pitch; + success_search = true; + } + } + return success_search; +} + +std::vector PitchReader::comparePitch(const std::string comp_input_file) +{ + std::vector pitches; + std::vector comp_tf_infos; + if (!readCSV(comp_input_file, &comp_tf_infos)) { + return pitches; + } + + for (const auto & tf_info : comp_tf_infos) { + double pitch; + if (getPitch(&pitch, tf_info.x, tf_info.y, tf_info.yaw)) { + pitches.emplace_back(pitch - tf_info.pitch); + } + } + return pitches; +} + +bool PitchReader::readCSV(const std::string csv_path, std::vector * tf_infos) +{ + std::vector> result; + std::ifstream ifs(csv_path); + if (!ifs.is_open()) { + return false; + } + + std::string line; + while (std::getline(ifs, line)) { + std::vector strvec = split(line, ','); + result.emplace_back(strvec); + } + + for (size_t i = 1; i < result.size(); i++) { + TfInfo tf_info; + tf_info.x = std::stod(result.at(i).at(0)); + tf_info.y = std::stod(result.at(i).at(1)); + tf_info.z = std::stod(result.at(i).at(2)); + tf_info.yaw = std::stod(result.at(i).at(3)); + tf_info.pitch = std::stod(result.at(i).at(4)); + tf_infos->emplace_back(tf_info); + } + + return true; +} + +std::vector PitchReader::split(const std::string & original, char delim) +{ + std::vector elems; + std::stringstream sstr(original); + std::string elem; + while (getline(sstr, elem, delim)) { + if (!elem.empty()) { + elems.push_back(elem); + } + } + return elems; +} diff --git a/vehicle/time_delay_estimator/.gitignore b/vehicle/time_delay_estimator/.gitignore new file mode 100644 index 00000000..0d20b648 --- /dev/null +++ b/vehicle/time_delay_estimator/.gitignore @@ -0,0 +1 @@ +*.pyc diff --git a/vehicle/time_delay_estimator/CMakeLists.txt b/vehicle/time_delay_estimator/CMakeLists.txt new file mode 100644 index 00000000..e2dd8e7c --- /dev/null +++ b/vehicle/time_delay_estimator/CMakeLists.txt @@ -0,0 +1,42 @@ +cmake_minimum_required(VERSION 3.5) +project(time_delay_estimator) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +ament_auto_add_executable(time_delay_estimator + src/time_delay_estimator_node.cpp + src/time_delay_estimator.cpp + src/data_processor.cpp + src/estimator.cpp + src/main.cpp) +ament_target_dependencies(time_delay_estimator) + +ament_auto_add_executable(general_time_delay_estimator + src/general_time_delay_estimator_node.cpp + src/time_delay_estimator.cpp + src/data_processor.cpp + src/estimator.cpp + src/main.cpp) +ament_target_dependencies(general_time_delay_estimator) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +install(PROGRAMS scripts/correlation_visualizer.py DESTINATION lib/${PROJECT_NAME}) + +ament_auto_package(INSTALL_TO_SHARE + launch + config) diff --git a/vehicle/time_delay_estimator/README.md b/vehicle/time_delay_estimator/README.md new file mode 100644 index 00000000..c26ceb98 --- /dev/null +++ b/vehicle/time_delay_estimator/README.md @@ -0,0 +1,162 @@ +# TimeDelayEstimation + +This delay estimation node estimates a time delay from inputs to outputs for accel, brake, and steer. + +

+ +

+ +## Input / Response + +The following topics are used to estimate the delay. + +- `/vehicle/raw_vehicle_cmd`: used as accel/brake target value +- `/control/control_cmd`: used as steer target value +- `/calibration/vehicle/accel_status`: used as accel observed value +- `/calibration/vehicle/brake_status`: used as brake observed value +- `/vehicle/status/steering`: used as steer observed value +- `/calibration/vehicle/is_engage`: used to check the driving operation status + +**output**. + +For users, the `TimeDelay.msg` output contains the following items: + +- _time_delay_: Estimated time delay. +- _mean_: Mean value of the estimated time delay +- _stddev_: Standard deviation of the estimated time delay +- _is_valid_data_: Validity determination of the current data +- _first_order_model_coefficients_：size 2 array of model coefficients(b,k,t) +- _second_order_model_coefficients_：size 2 array of model coefficients(m,b,k,t) + +In addition, the following items are output for developers. + +- Mean average error of delay estimation +- Standard deviation of the mean error of delay estimation + +These values can be confirmed in [rqt_multiplot](http://wiki.ros.org/rqt_multiplot), described below. + +## How to Run Time Delay Estimator + +Note: You need to build the Autoware beforehand. + +The following command will start the delay estimation node. + +```bash +ros2 launch time_delay_estimator time_delay_estimator.launch.xml is_showing_debug_graph:=true +``` + +

+ +

+ +### Change the estimator type + +You can decide the estimator_type with the following parameters + +- "cc" : Cross Correlation +- "ls" : Least Squared +- "ls2" : Least Squared Second + +Note: Only "cc" Cross Correlation will display the debug graph + +### How to check the estimated delay + +The necessary information is plotted in the rqt_multiplot, which displays the following information from top to bottom. + +- First row: Input data processing results examination +- Second row: Estimation results confirmation +- Third row: The confidence level of the estimation results +- Fourth row: Input/output error after delay compensation + +### Input data processing results examination + +Check the input and output data. It is also used to adjust parameters of the estimation logic. + +- _input raw_ : input data +- _response raw_ : response data +- _input processed_ : input data after compensation (\*) +- _response processed_ : response data after compensation (\*) +- _data stddev_ : standard deviation of the data used for estimation (used to determine validity) +- _is valid data_ : Whether the data is valid or not + - 0.0 : invalid because the standard deviation is less than the threshold (`*_min_stddev_threshold`). + - 1.0 : valid because the standard deviation is greater than the threshold (`*_min_stddev_threshold`). + +(\*) Smoothing, normalization, and resampling are applied as preprocessing. + +

+ +

+ +You need to adjust the value of `*_min_stddev_threshold` according to the standard deviation to determine the validity of the data. + +#### Estimation results confirmation + +Check the estimation results. + +- _delay_ : Time delay estimated at each step +- _average_ : Average of the time delays +- _stddev_ : Standard deviation of the estimated delay + +

+ +

+ +It is preferable to use the _average_ for the calibration results. + +The delay estimation starts when enough data is stored. The output value is zero until it is ready. + +#### Confidence level of the estimation results + +The reliability of the estimated time delay can be analyzed by the correlation coefficient. + +- _correlation peak_ : correlation coefficient between input and output (if this value is low (about 0.7 or less), the estimation may not be successful) +- _detection result_ : Whether the cross-correlation resulted in a good estimation or not. + - 0.00 : Invalid estimation due to low correlation (output the previous value) + - 0.50 : Estimation has not started yet or received invalid data + - 1.00 : Estimation has been done properly + +

+ +

+ +#### Input/output error after delay compensation + +Check the statistics of the errors in the input/output data after the time delay compensation. + +- _mae raw_ : Mean absolute error of input/output after the delay compensation +- _mae mean_ : Mean of the mean absolute error calculated in each estimation step +- _mae stddev_ : Standard deviation of mean absolute error + +

+ +

+ +If these values are large, the input/output model needs to be reconsidered. + +## Data preprocessing + +### Examine the results of processing the input data + +Data that do not satisfy the following conditions are considered invalid and will not be used for estimation. + +- Data variation (evaluated by standard deviation) is smaller than the threshold +- The car is not in automatic operation mode (judged from Autoware's Engage, Vehicle's Engage, etc.) + +### Visualization of delay estimation results + +Before running the node, you need to set the `is_showing_debug_info` parameter in the yaml file to true for a visualization. +Then the internal values of accel/brake/steer/test are plotted on the python visualization tool. +If the superposition of input and response is good, we can say that we have a good estimation. + +

+ +

+ +### Test **WIP** + +Execute the following command to perform an estimation on the sample data. +This test should be used to see the characteristics when the parameters are changed. + +```sh +roslaunch time_delay_estimator test_time_delay_estimator.launch +``` diff --git a/vehicle/time_delay_estimator/config/general_time_delay_estimator_param.yaml b/vehicle/time_delay_estimator/config/general_time_delay_estimator_param.yaml new file mode 100644 index 00000000..76cc6800 --- /dev/null +++ b/vehicle/time_delay_estimator/config/general_time_delay_estimator_param.yaml @@ -0,0 +1,16 @@ +general_time_delay_estimator: + ros__parameters: + data: # data size + sampling_hz: 30.0 # data sampling hz + estimation_hz: 10.0 # estimation hz + sampling_duration: 5.0 # sampling duration to estimate delay (range 5~20 sec) + validation_duration: 1.0 # to check if it's valid data or not (range 0.5~2 sec) + valid_peak_cross_correlation_threshold: 0.8 # above 0.8 is preferred + valid_delay_index_ratio: 0.1 # below 0.2 is usual + num_interpolation: 3 # number of interpolation to data (range 3~5) + filter: # filtering + cutoff_hz_input: 0.5 # smooth input (range 0.01~7.0) + cutoff_hz_output: 0.1 # smooth output (range 0.01~7.0) + reset_at_disengage: false # default false + is_showing_debug_info: true # set false to test at pubic road + use_weight_for_cross_correlation: false diff --git a/vehicle/time_delay_estimator/config/plot_juggler_time_delay_estimator.xml b/vehicle/time_delay_estimator/config/plot_juggler_time_delay_estimator.xml new file mode 100644 index 00000000..7fc9891c --- /dev/null +++ b/vehicle/time_delay_estimator/config/plot_juggler_time_delay_estimator.xml @@ -0,0 +1,137 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/vehicle/time_delay_estimator/config/time_delay_estimator_param.yaml b/vehicle/time_delay_estimator/config/time_delay_estimator_param.yaml new file mode 100644 index 00000000..e0f12242 --- /dev/null +++ b/vehicle/time_delay_estimator/config/time_delay_estimator_param.yaml @@ -0,0 +1,34 @@ +time_delay_estimator: + ros__parameters: + data: # data size + sampling_hz: 30.0 # data sampling hz + estimation_hz: 10.0 # estimation hz + sampling_duration: 5.0 # sampling duration to estimate delay (range 5~20 sec) + validation_duration: 1.0 # to check if it's valid data or not (range 0.5~2 sec) + valid_peak_cross_correlation_threshold: 0.8 # above 0.8 is preferred + valid_delay_index_ratio: 0.1 # below 0.2 is usual + num_interpolation: 3 # number of interpolation to data (range 3~5) + filter: # filtering + cutoff_hz_input: 0.5 # smooth input (range 0.01~7.0) + cutoff_hz_output: 0.1 # smooth output (range 0.01~7.0) + accel: # useful data range + valid_min_accel: 0.05 + valid_max_accel: 1.0 + offset_value: 0.0 + min_stddev_threshold: 0.005 + brake: # useful data range + valid_min_brake: 0.05 + valid_max_brake: 1.0 + offset_value: 0.0 + min_stddev_threshold: 0.005 + steer: # useful data range + valid_min_steer: 0.05 + valid_max_steer: 1.0 + offset_value: 0.0 + min_stddev_threshold: 0.0025 + reset_at_disengage: false # default false + is_showing_debug_info: true # set false to test at pubic road + use_weight_for_cross_correlation: false + test: # test option + is_test_mode: false + test_min_stddev_threshold: 0.0 diff --git a/vehicle/time_delay_estimator/include/time_delay_estimator/data_processor.hpp b/vehicle/time_delay_estimator/include/time_delay_estimator/data_processor.hpp new file mode 100644 index 00000000..5f420c59 --- /dev/null +++ b/vehicle/time_delay_estimator/include/time_delay_estimator/data_processor.hpp @@ -0,0 +1,131 @@ +// +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 TIME_DELAY_ESTIMATOR__DATA_PROCESSOR_HPP_ +#define TIME_DELAY_ESTIMATOR__DATA_PROCESSOR_HPP_ + +#include "estimator_utils/math_utils.hpp" +#include "estimator_utils/optimization_utils.hpp" +#include "rclcpp/rclcpp.hpp" +#include "time_delay_estimator/parameters.hpp" + +#include "std_msgs/msg/float32_multi_array.hpp" +#include "std_msgs/msg/float64_multi_array.hpp" + +#include +#include +#include +#include +#include +#include + +struct Data +{ + Data() : value{0.0}, p_value{0.0} {} + double value; + double p_value = 0; + double stamp = 0; + std::deque stamps = {}; + std::deque validation = {}; + std::deque raw = {}; + std::deque filtered = {}; + std::vector processed; + + void setValue(const double val, const double time) + { + value = val; + stamp = time; + } +}; + +struct Result +{ + int estimated_delay_index = 0; + double time_delay = 0; + double time_delay_stddev = 0; + double time_delay_prev = 0; + double time_delay_default = 0; + bool is_valid_data = false; +}; + +struct Evaluation +{ + math_utils::Statistic stat_delay; + math_utils::Statistic stat_error; + // root squared error + double mae = 0; + double mae_stddev = 0; + double mae_mean = 0; + double mae_max = 0; + double mae_min = 0; +}; + +struct Estimator : public Result, Evaluation +{ + std::vector cross_correlation; + double peak_correlation = 0; + Eigen::VectorXd w = Eigen::MatrixXd::Zero(3, 1); +}; + +struct MinMax +{ + double min; + double max; +}; + +namespace data_processor +{ +enum class ForgetResult : std::int8_t { + CURRENT = 0, + NONE = 1, + OLD = 2, + EXCEPTION = -1, +}; +/** + * @brief : emplace back new data and filtering + * @param data : filtered data + * @param data_dot : filtered diff data + * @param data_2dot : filtered diff2 data + * @param params : config for preprocessor + * @param buffer data for fitting + * @return : has enough data to estimate + **/ +bool processResponseData( + rclcpp::Node * node, Data & data, Data & data_dot, Data & data_2dot, const Params & params, + const int buffer = 0); + +/** + * @brief : emplace back new data and filtering + * @param data : raw filtered processed data + * @param params : config for preprocessor + * @param buffer data for fitting + * @return : has enough data to estimate + **/ +bool processInputData(Data & data, const Params & params, const int buffer = 0); + +/** + * @brief : forget no feature data to avoid over fitting + * @param input : input data + * @param response : response data + * @return : forget result 0: None 1:OLD 2:NEW + **/ +bool checkIsValidData( + Data & data, Data & response, const Params & params, double & max_stddev, + const double & ignore_thresh); + +} // namespace data_processor + +#endif // TIME_DELAY_ESTIMATOR__DATA_PROCESSOR_HPP_ diff --git a/vehicle/time_delay_estimator/include/time_delay_estimator/debugger.hpp b/vehicle/time_delay_estimator/include/time_delay_estimator/debugger.hpp new file mode 100644 index 00000000..826d946b --- /dev/null +++ b/vehicle/time_delay_estimator/include/time_delay_estimator/debugger.hpp @@ -0,0 +1,103 @@ +// +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 TIME_DELAY_ESTIMATOR__DEBUGGER_HPP_ +#define TIME_DELAY_ESTIMATOR__DEBUGGER_HPP_ + +// ros depend +#include "rclcpp/rclcpp.hpp" + +#include "std_msgs/msg/float32_multi_array.hpp" +#include "std_msgs/msg/float64_multi_array.hpp" + +#include +// ros pkg depend +#include "time_delay_estimator/parameters.hpp" + +class Debugger +{ +public: + Debugger(rclcpp::Node * node, const std::string & name) + { + // QoS setup + static constexpr std::size_t queue_size = 1; + rclcpp::QoS durable_qos(queue_size); + durable_qos.transient_local(); // option for latching + + pub_debug_ = node->create_publisher( + "~/debug_values/" + name, durable_qos); + pub_debug_input_ = node->create_publisher( + "~/debug_values/" + name + "_input", durable_qos); + pub_debug_response_ = node->create_publisher( + "~/debug_values/" + name + "_response", durable_qos); + pub_debug_estimated_ = node->create_publisher( + "~/debug_values/" + name + "_estimated", durable_qos); + pub_debug_corr_ = node->create_publisher( + "~/debug_values/" + name + "_correlation", durable_qos); + debug_values_.data.resize(num_debug_values_, 0.0); + } + + /* Debug */ + rclcpp::Publisher::SharedPtr pub_debug_; + rclcpp::Publisher::SharedPtr pub_debug_input_; + rclcpp::Publisher::SharedPtr pub_debug_response_; + rclcpp::Publisher::SharedPtr pub_debug_estimated_; + rclcpp::Publisher::SharedPtr pub_debug_corr_; + + /** + * @brief : for rqt_multiplot + * @return : if publishing correlation data or not + **/ + // debug processed data + static constexpr std::uint8_t num_debug_values_ = 40; + mutable std_msgs::msg::Float32MultiArray debug_values_; + enum DBGVAL : std::uint8_t { + INPUT_RAW = 0, // [0] raw input + INPUT_FILTERED = 1, // [1] filtered input + INPUT_PROCESSED = 2, // [2] processed input + RESPONSE_RAW = 3, // [3] raw response + RESPONSE_FILTERED = 4, // [4] filtered response + RESPONSE_PROCESSED = 5, // [5] processed response + MAX_STDDEV = 6, // [6] input/response stddev + IS_VALID_DATA = 7, // [7] ignore current data 0 ,old data 1 + MIN_IGNORE_THRESH = 8, // [8] min ignore threshold + }; + enum CROSS_CORRELATION : std::uint8_t { + CC_DELAY_RAW = 10, // [10] estimate delay raw + CC_DELAY = 11, // [11] estimate delay processed + CC_CORRELATION_PEAK = 12, // [12] correlation coefficient + CC_MAE_AT_TIME = 13, // [13] root squared error + CC_MAE_MEAN = 14, // [14] accumulated mae mean + CC_MAE_STDDEV = 15, // [15] accumulated mae stddev + CC_DETECTION_RESULT = 16, // [15] detection result + CC_DELAY_STDDEV = 17, // [15] accumulated mae stddev + }; + + enum LEAST_SQUARED : std::uint8_t { + LS_DELAY_RAW = 20, // [20] estimate delay + LS_DELAY = 21, // [21] estimate delay + LS_MAE_AT_TIME = 23, // [23] root squared error + }; + enum LEAST_SQUARED_SECOND : std::uint8_t { + LS2_DELAY_RAW = 30, // [30] estimate delay + LS2_DELAY = 31, // [31] estimate delay + LS2_MAE_AT_TIME = 33, // [33] root squared error + }; + void publishDebugValue() { pub_debug_->publish(debug_values_); } + ~Debugger() {} +}; + +#endif // TIME_DELAY_ESTIMATOR__DEBUGGER_HPP_ diff --git a/vehicle/time_delay_estimator/include/time_delay_estimator/general_time_delay_estimator_node.hpp b/vehicle/time_delay_estimator/include/time_delay_estimator/general_time_delay_estimator_node.hpp new file mode 100644 index 00000000..54f0bd6d --- /dev/null +++ b/vehicle/time_delay_estimator/include/time_delay_estimator/general_time_delay_estimator_node.hpp @@ -0,0 +1,105 @@ +// +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 TIME_DELAY_ESTIMATOR__GENERAL_TIME_DELAY_ESTIMATOR_NODE_HPP_ +#define TIME_DELAY_ESTIMATOR__GENERAL_TIME_DELAY_ESTIMATOR_NODE_HPP_ + +#include "estimator_utils/math_utils.hpp" +#include "rclcpp/rclcpp.hpp" +#include "time_delay_estimator/data_processor.hpp" +#include "time_delay_estimator/parameters.hpp" +#include "time_delay_estimator/time_delay_estimator.hpp" + +#include "autoware_vehicle_msgs/msg/control_mode_report.hpp" +#include "std_msgs/msg/bool.hpp" +#include "std_msgs/msg/float32.hpp" +#include "std_msgs/msg/float32_multi_array.hpp" +#include "std_msgs/msg/float64_multi_array.hpp" +#include "std_msgs/msg/string.hpp" +#include "tier4_calibration_msgs/msg/bool_stamped.hpp" +#include "tier4_calibration_msgs/msg/float32_stamped.hpp" +#include "tier4_calibration_msgs/msg/time_delay.hpp" + +#include +#include +#include +#include +#include +#include +#include + +class TimeDelayEstimatorNode : public rclcpp::Node +{ + using Float32Stamped = tier4_calibration_msgs::msg::Float32Stamped; + using ControlModeReport = autoware_vehicle_msgs::msg::ControlModeReport; + using IsEngaged = tier4_calibration_msgs::msg::BoolStamped; + using BoolStamped = tier4_calibration_msgs::msg::BoolStamped; + using TimeDelay = tier4_calibration_msgs::msg::TimeDelay; + +private: + // output delay + rclcpp::Publisher::SharedPtr pub_time_delay_; + + // input subscription + rclcpp::Subscription::SharedPtr + sub_control_mode_report_; + + // response subscription + rclcpp::Subscription::SharedPtr sub_input_cmd_; + rclcpp::Subscription::SharedPtr sub_input_status_; + rclcpp::Subscription::SharedPtr sub_is_engaged_; + + // Timer + rclcpp::TimerBase::SharedPtr timer_estimation_; + rclcpp::TimerBase::SharedPtr timer_data_processing_; + + Float32Stamped::ConstSharedPtr input_cmd_ptr_; + Float32Stamped::ConstSharedPtr input_status_ptr_; + ControlModeReport::ConstSharedPtr control_mode_ptr_; + std::string estimator_type_; + std::string data_name_; + + void callbackInputCmd(const Float32Stamped::ConstSharedPtr msg); + void callbackInputStatus(const Float32Stamped::ConstSharedPtr msg); + void callbackControlModeReport(const ControlModeReport::ConstSharedPtr msg); + void callbackVehicleEngage(const ControlModeReport::ConstSharedPtr msg); + void callbackEngage(const IsEngaged::ConstSharedPtr msg); + + // use diff instead of raw data + double auto_mode_duration_ = 0; + double last_manual_time_; + double last_disengage_time_; + double engage_duration_; + bool detect_manual_engage_; + bool engage_mode_ = false; + // saturation + MinMax valid_input_; + double input_offset_; + + std::unique_ptr collected_data_; + // for ros parameters + Params params_; + + void timerCallback(); + void timerDataCollector(); + bool estimateTimeDelay(); + +public: + explicit TimeDelayEstimatorNode(const rclcpp::NodeOptions & node_options); + ~TimeDelayEstimatorNode() {} +}; + +#endif // TIME_DELAY_ESTIMATOR__GENERAL_TIME_DELAY_ESTIMATOR_NODE_HPP_ diff --git a/vehicle/time_delay_estimator/include/time_delay_estimator/parameters.hpp b/vehicle/time_delay_estimator/include/time_delay_estimator/parameters.hpp new file mode 100644 index 00000000..c6edf0fa --- /dev/null +++ b/vehicle/time_delay_estimator/include/time_delay_estimator/parameters.hpp @@ -0,0 +1,43 @@ +// +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 TIME_DELAY_ESTIMATOR__PARAMETERS_HPP_ +#define TIME_DELAY_ESTIMATOR__PARAMETERS_HPP_ + +struct Params +{ + double sampling_hz; + double estimation_hz; + double sampling_duration; + double validation_duration; + int data_size; + int total_data_size; + int validation_size; + double valid_peak_cross_correlation_threshold; + double valid_delay_index_ratio; + double sampling_delta_time; + double estimation_delta_time; + double cutoff_hz_input; + double cutoff_hz_output; + bool reset_at_disengage; + bool is_showing_debug_info; + bool use_interpolation; + int num_interpolation; + int estimation_method; + bool is_test_mode; +}; + +#endif // TIME_DELAY_ESTIMATOR__PARAMETERS_HPP_ diff --git a/vehicle/time_delay_estimator/include/time_delay_estimator/time_delay_estimator.hpp b/vehicle/time_delay_estimator/include/time_delay_estimator/time_delay_estimator.hpp new file mode 100644 index 00000000..6d77543d --- /dev/null +++ b/vehicle/time_delay_estimator/include/time_delay_estimator/time_delay_estimator.hpp @@ -0,0 +1,115 @@ +// +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 TIME_DELAY_ESTIMATOR__TIME_DELAY_ESTIMATOR_HPP_ +#define TIME_DELAY_ESTIMATOR__TIME_DELAY_ESTIMATOR_HPP_ + +#include "estimator_utils/math_utils.hpp" +#include "estimator_utils/optimization_utils.hpp" +#include "rclcpp/rclcpp.hpp" +#include "time_delay_estimator/data_processor.hpp" +#include "time_delay_estimator/debugger.hpp" +#include "time_delay_estimator/parameters.hpp" + +#include "std_msgs/msg/float32_multi_array.hpp" +#include "std_msgs/msg/float64_multi_array.hpp" +#include "tier4_calibration_msgs/msg/time_delay.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +class TimeDelayEstimator +{ +private: + enum class DetectionResult : std::int8_t { + BELOW_THRESH = 0, + SLEEP = 1, + DETECTED = 2, + }; + bool has_enough_input_ = false; + bool has_enough_response_ = false; + tier4_calibration_msgs::msg::TimeDelay time_delay_; + int loop_count_ = 0; + Estimator cc_estimator_; + Estimator ls_estimator_; + Estimator ls2_estimator_; + std::vector weights_for_data_; + std::string name_; + bool is_valid_data_ = false; + double max_current_stddev_ = 0; + double ignore_thresh_; + DetectionResult detection_result_; + data_processor::ForgetResult forget_result_; + + void postprocessOutput(Estimator & estimator); + void setOutput(Estimator & estimator); + bool calcStatistics( + math_utils::Statistic & stat_delay, math_utils::Statistic & stat_error, const double delay, + const double mae); + + /** + * @brief : get time delay from stored input & response + * @param input : vector like array + * @param response : vector like array + * @param corr : correlation value + * @param params : parameters for setting + * @return Correlation type value + **/ + enum DetectionResult estimateDelayByCrossCorrelation( + rclcpp::Node * node, const std::vector & input, const std::vector & response, + Estimator & corr, std::string name, const Params & params); + + enum DetectionResult estimateDelayByLeastSquared( + const std::vector & x2dot, const std::vector & x_dot, + const std::vector & x, const std::vector & u, const Params & params); + enum DetectionResult estimateDelayByLeastSquared( + const std::vector & x_dot, const std::vector & x, const std::vector & u, + const Params & params); + +public: + Data input_; + Data response_; + Data response_dot_; + Data response_2dot_; + bool checkIsValidData(); + void estimate(); + void preprocessData(rclcpp::Node * node); + void processDebugData(rclcpp::Node * node); + void resetEstimator(); + Params params_; + /** + * @brief : initialize module + * @param name : which delay to estimate + * @param total_data_size : total sample data to estimate + * @param use_weight_for_cross_correlation : weight decay + * @return : success + **/ + TimeDelayEstimator( + rclcpp::Node * node, const Params & params, const std::string & name, int total_data_size, + bool use_weight_for_cross_correlation); + tier4_calibration_msgs::msg::TimeDelay estimateTimeDelay( + rclcpp::Node * node, std::string estimator_type); + virtual ~TimeDelayEstimator() {} + + std::unique_ptr debugger_; +}; +#endif // TIME_DELAY_ESTIMATOR__TIME_DELAY_ESTIMATOR_HPP_ diff --git a/vehicle/time_delay_estimator/include/time_delay_estimator/time_delay_estimator_node.hpp b/vehicle/time_delay_estimator/include/time_delay_estimator/time_delay_estimator_node.hpp new file mode 100644 index 00000000..84742362 --- /dev/null +++ b/vehicle/time_delay_estimator/include/time_delay_estimator/time_delay_estimator_node.hpp @@ -0,0 +1,129 @@ +// +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 TIME_DELAY_ESTIMATOR__TIME_DELAY_ESTIMATOR_NODE_HPP_ +#define TIME_DELAY_ESTIMATOR__TIME_DELAY_ESTIMATOR_NODE_HPP_ + +#include "estimator_utils/math_utils.hpp" +#include "rclcpp/rclcpp.hpp" +#include "time_delay_estimator/data_processor.hpp" +#include "time_delay_estimator/parameters.hpp" +#include "time_delay_estimator/time_delay_estimator.hpp" + +#include "autoware_vehicle_msgs/msg/control_mode_report.hpp" +#include "std_msgs/msg/bool.hpp" +#include "std_msgs/msg/float32.hpp" +#include "std_msgs/msg/float32_multi_array.hpp" +#include "std_msgs/msg/float64_multi_array.hpp" +#include "std_msgs/msg/string.hpp" +#include "tier4_calibration_msgs/msg/bool_stamped.hpp" +#include "tier4_calibration_msgs/msg/float32_stamped.hpp" +#include "tier4_calibration_msgs/msg/time_delay.hpp" + +#include +#include +#include +#include +#include +#include +#include + +class TimeDelayEstimatorNode : public rclcpp::Node +{ + using Float32Stamped = tier4_calibration_msgs::msg::Float32Stamped; + using ControlModeReport = autoware_vehicle_msgs::msg::ControlModeReport; + using IsEngaged = tier4_calibration_msgs::msg::BoolStamped; + using BoolStamped = tier4_calibration_msgs::msg::BoolStamped; + using TimeDelay = tier4_calibration_msgs::msg::TimeDelay; + +private: + // output delay + rclcpp::Publisher::SharedPtr pub_time_delay_test_; + rclcpp::Publisher::SharedPtr pub_time_delay_accel_; + rclcpp::Publisher::SharedPtr pub_time_delay_brake_; + rclcpp::Publisher::SharedPtr pub_time_delay_steer_; + + // input subscription + rclcpp::Subscription::SharedPtr sub_control_mode_; + + // response subscription + rclcpp::Subscription::SharedPtr sub_accel_cmd_; + rclcpp::Subscription::SharedPtr sub_brake_cmd_; + rclcpp::Subscription::SharedPtr sub_steer_cmd_; + rclcpp::Subscription::SharedPtr sub_accel_status_; + rclcpp::Subscription::SharedPtr sub_brake_status_; + rclcpp::Subscription::SharedPtr sub_steer_status_; + rclcpp::Subscription::SharedPtr sub_is_engaged_; + + // Timer + rclcpp::TimerBase::SharedPtr timer_estimation_; + rclcpp::TimerBase::SharedPtr timer_data_processing_; + + Float32Stamped::ConstSharedPtr accel_cmd_ptr_; + Float32Stamped::ConstSharedPtr brake_cmd_ptr_; + Float32Stamped::ConstSharedPtr steer_cmd_ptr_; + Float32Stamped::ConstSharedPtr accel_status_ptr_; + Float32Stamped::ConstSharedPtr brake_status_ptr_; + Float32Stamped::ConstSharedPtr steer_status_ptr_; + ControlModeReport::ConstSharedPtr control_mode_ptr_; + std::string estimator_type_; + + void callbackAccelCmd(const Float32Stamped::ConstSharedPtr msg); + void callbackBrakeCmd(const Float32Stamped::ConstSharedPtr msg); + void callbackSteerCmd(const Float32Stamped::ConstSharedPtr msg); + void callbackAccelStatus(const Float32Stamped::ConstSharedPtr msg); + void callbackBrakeStatus(const Float32Stamped::ConstSharedPtr msg); + void callbackSteerStatus(const Float32Stamped::ConstSharedPtr msg); + void callbackControlModeReport(const ControlModeReport::ConstSharedPtr msg); + void callbackVehicleEngage(const ControlModeReport::ConstSharedPtr msg); + void callbackEngage(const IsEngaged::ConstSharedPtr msg); + + // use diff instead of raw data + double auto_mode_duration_ = 0; + double last_manual_time_; + double last_disengage_time_; + double engage_duration_; + bool detect_manual_engage_; + bool engage_mode_ = false; + // saturation + MinMax valid_steer_; + MinMax valid_accel_; + MinMax valid_brake_; + double steer_offset_; + double accel_offset_; + double brake_offset_; + + std::unique_ptr accel_data_; + std::unique_ptr brake_data_; + std::unique_ptr steer_data_; + // for ros parameters + Params params_; + + void timerCallback(); + void timerDataCollector(); + bool estimateTimeDelay(); + + // for testing + std::shared_ptr test_data_; + int cnt_ = 0; + void callbackTestData(); + +public: + explicit TimeDelayEstimatorNode(const rclcpp::NodeOptions & node_options); + ~TimeDelayEstimatorNode() {} +}; + +#endif // TIME_DELAY_ESTIMATOR__TIME_DELAY_ESTIMATOR_NODE_HPP_ diff --git a/vehicle/time_delay_estimator/launch/general_time_delay_estimator.launch.xml b/vehicle/time_delay_estimator/launch/general_time_delay_estimator.launch.xml new file mode 100644 index 00000000..37eb45bb --- /dev/null +++ b/vehicle/time_delay_estimator/launch/general_time_delay_estimator.launch.xml @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/vehicle/time_delay_estimator/launch/pacmod_time_delay_estimator.launch.xml b/vehicle/time_delay_estimator/launch/pacmod_time_delay_estimator.launch.xml new file mode 100644 index 00000000..069595ea --- /dev/null +++ b/vehicle/time_delay_estimator/launch/pacmod_time_delay_estimator.launch.xml @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/vehicle/time_delay_estimator/launch/time_delay_estimator.launch.xml b/vehicle/time_delay_estimator/launch/time_delay_estimator.launch.xml new file mode 100644 index 00000000..1b0826ff --- /dev/null +++ b/vehicle/time_delay_estimator/launch/time_delay_estimator.launch.xml @@ -0,0 +1,57 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/vehicle/time_delay_estimator/media/delay_diagram.png b/vehicle/time_delay_estimator/media/delay_diagram.png new file mode 100644 index 00000000..faa091b8 Binary files /dev/null and b/vehicle/time_delay_estimator/media/delay_diagram.png differ diff --git a/vehicle/time_delay_estimator/media/detection_result.png b/vehicle/time_delay_estimator/media/detection_result.png new file mode 100644 index 00000000..693ec93d Binary files /dev/null and b/vehicle/time_delay_estimator/media/detection_result.png differ diff --git a/vehicle/time_delay_estimator/media/estimation.png b/vehicle/time_delay_estimator/media/estimation.png new file mode 100644 index 00000000..f797626a Binary files /dev/null and b/vehicle/time_delay_estimator/media/estimation.png differ diff --git a/vehicle/time_delay_estimator/media/evaluation.png b/vehicle/time_delay_estimator/media/evaluation.png new file mode 100644 index 00000000..e3f37214 Binary files /dev/null and b/vehicle/time_delay_estimator/media/evaluation.png differ diff --git a/vehicle/time_delay_estimator/media/processed_data.png b/vehicle/time_delay_estimator/media/processed_data.png new file mode 100644 index 00000000..4da61c85 Binary files /dev/null and b/vehicle/time_delay_estimator/media/processed_data.png differ diff --git a/vehicle/time_delay_estimator/media/time_delay_estimator.gif b/vehicle/time_delay_estimator/media/time_delay_estimator.gif new file mode 100644 index 00000000..e91d4b3a Binary files /dev/null and b/vehicle/time_delay_estimator/media/time_delay_estimator.gif differ diff --git a/vehicle/time_delay_estimator/media/viz.png b/vehicle/time_delay_estimator/media/viz.png new file mode 100644 index 00000000..fd7c92c7 Binary files /dev/null and b/vehicle/time_delay_estimator/media/viz.png differ diff --git a/vehicle/time_delay_estimator/package.xml b/vehicle/time_delay_estimator/package.xml new file mode 100644 index 00000000..83b4cc11 --- /dev/null +++ b/vehicle/time_delay_estimator/package.xml @@ -0,0 +1,33 @@ + + + + time_delay_estimator + 0.1.0 + The time_delay_estimator + Taiki Tanaka + Apache 2 + + ament_cmake_auto + + + autoware_vehicle_msgs + calibration_adapter + eigen + estimator_utils + rclcpp + rclpy + std_msgs + tier4_calibration_msgs + + + + plotjuggler + plotjuggler_ros + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/vehicle/time_delay_estimator/scripts/correlation_visualizer.py b/vehicle/time_delay_estimator/scripts/correlation_visualizer.py new file mode 100755 index 00000000..3a486f0a --- /dev/null +++ b/vehicle/time_delay_estimator/scripts/correlation_visualizer.py @@ -0,0 +1,207 @@ +#! /usr/bin/env python3 +# -*- coding: utf-8 -*- + +# Copyright 2021 Tier IV, Inc. All rights reserved. +# +# 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. + +import copy +from threading import Thread + +from matplotlib import animation +import matplotlib.pyplot as plt +import numpy as np +import rclpy +from rclpy.node import Node +from std_msgs.msg import Float32MultiArray +from std_msgs.msg import Float64MultiArray + +plt.style.use("dark_background") + + +class CorrData: + def __init__(self, name, node): + self.print_debug = False + self.input_received = False + self.response_received = False + self.correlation_received = False + self.log_corr = np.arange(1, 0.0) + self.cross_correlation = np.arange(1, 0.0) + self.input = np.arange(1, 0.0) + self.response = np.arange(1, 0.0) + self.estimated = np.arange(1, 0.0) + self._node = node + + self.sub_input = node.create_subscription( + Float32MultiArray, "/debug_values/" + name + "_input", self.CallBackInput, 10 + ) + self.sub_response = node.create_subscription( + Float32MultiArray, "/debug_values/" + name + "_response", self.CallBackResponse, 10 + ) + self.sub_estimated = node.create_subscription( + Float32MultiArray, "/debug_values/" + name + "_estimated", self.CallBackEstimated, 10 + ) + self.sub_correlation = node.create_subscription( + Float64MultiArray, "/debug_values/" + name + "_correlation", self.CallBackCorr, 10 + ) + + def CallBackCorr(self, msg): + self.cross_correlation = msg.data + self.correlation_received = True + # self.log_corr = np.log10(self.cross_correlation) + if self.print_debug: + print(self.cross_correlation[::1]) + + def CallBackInput(self, msg): + self.input = msg.data + self.input_received = True + if self.print_debug: + self._node.get_logger().info("in") + print(self.input[::1]) + + def CallBackResponse(self, msg): + self.response = msg.data + self.response_received = True + if self.print_debug: + self._node.get_logger().info("resp") + print(self.response[::1]) + + def CallBackEstimated(self, msg): + self.estimated = msg.data + self.estimated_received = True + if self.print_debug: + self._node.get_logger().info("est") + + +class PlotData: + def __init__(self): + self.ax_corr = None + self.ax_val = None + self.im_corr = None + self.im_input = None + self.im_response = None + self.im_estimated = None + + +class TimeDelayAnimator(Node): + def __init__(self): + super().__init__("correlation_visualizer") + self.debugMode() + + def debugMode(self): + self.accel_plt = PlotData() # accel + self.brake_plt = PlotData() # brake + self.steer_plt = PlotData() # steer + self.fig, ( + self.accel_plt.ax_corr, + self.brake_plt.ax_corr, + self.steer_plt.ax_corr, + ) = plt.subplots(3, 1, figsize=(6, 12)) + + self.accel_data = CorrData("accel", self) + self.brake_data = CorrData("brake", self) + self.steer_data = CorrData("steer", self) + self.ani = animation.FuncAnimation( + self.fig, self.animationCallback, interval=200, blit=True + ) + min_max = [0, 1000] + self.setPlotGraph(self.accel_plt, "accel", min_max, [0, 1]) + self.setPlotGraph(self.brake_plt, "brake", min_max, [0, 1]) + self.setPlotGraph(self.steer_plt, "steer", min_max, [-1, 1]) + return + + def setPlotGraph(self, plotter, name, min_max, range_list): + plotter.ax_val = plotter.ax_corr.twinx() + plotter.ax_corr.set_xlim(min_max) + plotter.ax_corr.set_ylim(range_list) + plotter.ax_val.set_xlim(min_max) + plotter.ax_val.set_ylim(range_list) + (plotter.im_corr,) = plotter.ax_corr.plot( + [], [], label="0: cross correlation", color="red", marker="" + ) + (plotter.im_input,) = plotter.ax_val.plot( + [], [], label="1: input (right axis)", color="green", marker="" + ) + (plotter.im_response,) = plotter.ax_val.plot( + [], [], label="2: response (right axis)", color="purple", marker="" + ) + (plotter.im_estimated,) = plotter.ax_val.plot( + [], [], label="3: estimated (right axis)", color="orange", marker="" + ) + handler1, label1 = plotter.ax_corr.get_legend_handles_labels() + handler2, label2 = plotter.ax_val.get_legend_handles_labels() + plotter.ax_corr.legend(handler1 + handler2, label1 + label2, loc=2, borderaxespad=0.0) + plotter.ax_corr.set_xlabel(name + " correlation index / time") + plotter.ax_corr.set_ylabel(name + " correlation") + plotter.ax_val.set_ylabel(name + " value") + return + + def animationCallback(self, data): + try: + self.timerCallback() + except KeyboardInterrupt: + exit(0) + return ( + self.accel_plt.im_corr, + self.accel_plt.im_input, + self.accel_plt.im_response, + self.accel_plt.im_estimated, + self.brake_plt.im_corr, + self.brake_plt.im_input, + self.brake_plt.im_response, + self.brake_plt.im_estimated, + self.steer_plt.im_corr, + self.steer_plt.im_input, + self.steer_plt.im_response, + self.steer_plt.im_estimated, + ) + + def setData(self, data, plotter, resample_rate=1): + corr_data_y = copy.deepcopy(np.array(data.cross_correlation[::resample_rate])) + corr_data_x = np.arange(len(corr_data_y)) + input_data_y = copy.deepcopy(np.array(data.input[::-resample_rate])) + input_data_x = np.arange(len(input_data_y)) + response_data_y = copy.deepcopy(np.array(data.response[::-resample_rate])) + response_data_x = np.arange(len(response_data_y)) + estimated_data_y = copy.deepcopy(np.array(data.estimated[::-resample_rate])) + estimated_data_x = np.arange(len(estimated_data_y)) + if len(corr_data_x) != len(corr_data_y): + print("size mismatch for I/O") + return + plotter.im_corr.set_data(corr_data_x, corr_data_y) + plotter.im_input.set_data(input_data_x, input_data_y) + plotter.im_response.set_data(response_data_x, response_data_y) + plotter.im_estimated.set_data(estimated_data_x, estimated_data_y) + return + + def timerCallback(self): + # print(len(input_x), len(response_x), len(corr_data)) + self.setData(self.accel_data, self.accel_plt) + self.setData(self.brake_data, self.brake_plt) + self.setData(self.steer_data, self.steer_plt) + return + + def closeFigure(self): + plt.close(self.fig) + + +def main(args=None): + rclpy.init(args=args) + node = TimeDelayAnimator() + spin_thread = Thread(target=rclpy.spin, args=(node,)) + spin_thread.start() + plt.show() + + +if __name__ == "__main__": + main() diff --git a/vehicle/time_delay_estimator/src/data_processor.cpp b/vehicle/time_delay_estimator/src/data_processor.cpp new file mode 100644 index 00000000..9edefde6 --- /dev/null +++ b/vehicle/time_delay_estimator/src/data_processor.cpp @@ -0,0 +1,169 @@ +// +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 "time_delay_estimator/data_processor.hpp" + +#include + +namespace data_processor +{ +bool checkIsValidData( + Data & input, Data & response, const Params & params, double & max_stddev, + const double & ignore_thresh) +{ + bool is_valid_data = false; + if (input.validation.empty()) { + input.validation.emplace_back(input.value); + response.validation.emplace_back(response.value); + } else { + const double filtered_input = math_utils::lowpassFilter( + input.value, input.validation.back(), params.cutoff_hz_input, params.sampling_delta_time); + input.validation.emplace_back(filtered_input); + const double filtered_response = math_utils::lowpassFilter( + response.value, response.validation.back(), params.cutoff_hz_input, + params.sampling_delta_time); + response.validation.emplace_back(filtered_response); + } + + const auto input_val_size = static_cast(input.validation.size()); + const auto response_val_size = static_cast(response.validation.size()); + if (input_val_size > params.validation_size && response_val_size > params.validation_size) { + // Ignore None featured data or Too much deviation data + double input_stddev = math_utils::getStddevFromVector(input.validation); + double response_stddev = math_utils::getStddevFromVector(response.validation); + max_stddev = std::max(input_stddev, response_stddev); + if (ignore_thresh < max_stddev) { + is_valid_data = true; + } + } + if (input_val_size > params.validation_size) { + input.validation.pop_front(); + } + if (response_val_size > params.validation_size) { + response.validation.pop_front(); + } + return is_valid_data; +} +bool processInputData(Data & input, const Params & params, const int buffer) +{ + bool has_enough_input = true; + input.raw.emplace_back(input.value); + input.stamps.emplace_back(input.stamp); + if (input.filtered.size() == 0) { + input.filtered.emplace_back(input.value); + } else { + const double filt = math_utils::lowpassFilter( + input.raw.back(), input.filtered.back(), params.cutoff_hz_input, params.sampling_delta_time); + // Filtered + input.filtered.emplace_back(filt); + input.processed = math_utils::arrToVector(input.filtered); + if (params.num_interpolation > 1 && input.processed.size() > 5) { + input.processed = + math_utils::getLinearInterpolation(input.processed, params.num_interpolation); + } + } + + const auto input_raw_size = static_cast(input.raw.size()); + // size of raw = filtered + if (input_raw_size < params.data_size + buffer) { + has_enough_input = false; + } else { + input.stamps.pop_front(); + input.raw.pop_front(); + input.filtered.pop_front(); + } + // fail safe + if (input_raw_size > params.data_size + buffer) { + RCLCPP_ERROR( + rclcpp::get_logger("time_delay_estimator"), "invalid index size raw input > data_size"); + input.stamps.pop_front(); + input.raw.pop_front(); + input.filtered.pop_front(); + } + return has_enough_input; +} + +bool processResponseData( + rclcpp::Node * node, Data & data, Data & data_dot, Data & data_2dot, const Params & params, + const int buffer) +{ + bool has_enough_data = true; + data.raw.emplace_back(data.value); + data.stamps.emplace_back(data.stamp); + if (data.filtered.size() == 0) { + data.filtered.emplace_back(data.value); + } else { + const double filt = math_utils::lowpassFilter( + data.raw.back(), data.filtered.back(), params.cutoff_hz_input, params.sampling_delta_time); + // Filtered + data.filtered.emplace_back(filt); + data.processed = math_utils::arrToVector(data.filtered); + if (params.num_interpolation > 1 && data.processed.size() > 5) { + data.processed = math_utils::getLinearInterpolation(data.processed, params.num_interpolation); + } + } + if (data.filtered.size() < 3) { + data_dot.filtered.emplace_back(0); + data_2dot.filtered.emplace_back(0); + return false; + } else { + const double x0 = *(data.filtered.end() - 3); + const double x1 = *(data.filtered.end() - 2); + const double x2 = *(data.filtered.end() - 1); + double diff = + optimization_utils::getSecondaryCentralDifference(x0, x2, params.sampling_delta_time); + double diff2 = + optimization_utils::getSecondaryCentralDifference(x0, x1, x2, params.sampling_delta_time); + + auto & clk = *node->get_clock(); + RCLCPP_DEBUG_STREAM_THROTTLE( + rclcpp::get_logger("time_delay_estimator"), clk, 3000, + "[time delay estimator] diff : " << diff << " diff2 : " << diff2); + data_dot.filtered.emplace_back(diff); + data_2dot.filtered.emplace_back(diff2); + // Filtered + data_dot.processed = math_utils::arrToVector(data_dot.filtered); + data_2dot.processed = math_utils::arrToVector(data_2dot.filtered); + data_dot.processed = + math_utils::getLinearInterpolation(data_dot.processed, params.num_interpolation); + data_2dot.processed = + math_utils::getLinearInterpolation(data_2dot.processed, params.num_interpolation); + } + + const auto data_raw_size = static_cast(data.raw.size()); + // size of raw = filtered + if (data_raw_size < params.data_size + buffer) { + has_enough_data = false; + } else { + data.raw.pop_front(); + data.filtered.pop_front(); + data_dot.filtered.pop_front(); + data_2dot.filtered.pop_front(); + data.stamps.pop_front(); + } + // fail safe + if (data_raw_size > params.data_size + buffer) { + data.raw.pop_front(); + data.filtered.pop_front(); + data_dot.filtered.pop_front(); + data_2dot.filtered.pop_front(); + data.stamps.pop_front(); + RCLCPP_ERROR( + rclcpp::get_logger("time_delay_estimator"), "invalid index size raw input > data_size"); + } + return has_enough_data; +} +} // namespace data_processor diff --git a/vehicle/time_delay_estimator/src/estimator.cpp b/vehicle/time_delay_estimator/src/estimator.cpp new file mode 100644 index 00000000..5312870b --- /dev/null +++ b/vehicle/time_delay_estimator/src/estimator.cpp @@ -0,0 +1,113 @@ +// +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 "estimator_utils/math_utils.hpp" +#include "time_delay_estimator/time_delay_estimator.hpp" + +#include +#include +#include + +TimeDelayEstimator::DetectionResult TimeDelayEstimator::estimateDelayByLeastSquared( + const std::vector & x_dot, const std::vector & x, const std::vector & u, + const Params & params) +{ + int num_sample = static_cast(u.size() * (1.0 - params.valid_delay_index_ratio)); + int maximum_delay = static_cast(u.size() * params.valid_delay_index_ratio); + std::vector resample_y = {x.end() - num_sample, x.end()}; + std::vector resample_y_dot = {x_dot.end() - num_sample, x_dot.end()}; + std::vector resample_x; + double min_error = std::numeric_limits::max(); + + int min_error_index = 0; + for (int d = 0; d < maximum_delay; d++) { + // assume std::vector(old,....,new) + resample_x = {u.end() - num_sample - d, u.end() - d}; + double error_norm = optimization_utils::getLeastSquaredError( + resample_y_dot, resample_y, resample_x, ls_estimator_.w); + if (optimization_utils::change_abs_min(min_error, error_norm)) { + min_error = error_norm; + min_error_index = d; + } + } + ls_estimator_.mae = min_error; + ls_estimator_.estimated_delay_index = min_error_index; + ls_estimator_.time_delay = static_cast(min_error_index) * params.sampling_delta_time / + static_cast(params.num_interpolation); + const double valid_mae_threshold = 0.1; + if (ls_estimator_.mae < valid_mae_threshold) { + return DetectionResult::BELOW_THRESH; + } + return DetectionResult::DETECTED; +} + +TimeDelayEstimator::DetectionResult TimeDelayEstimator::estimateDelayByLeastSquared( + const std::vector & x2dot, const std::vector & x_dot, + const std::vector & x, const std::vector & u, const Params & params) +{ + int num_sample = static_cast(u.size() * (1.0 - params.valid_delay_index_ratio)); + int maximum_delay = static_cast(u.size() * params.valid_delay_index_ratio); + std::vector resample_y = {x.end() - num_sample, x.end()}; + std::vector resample_y_dot = {x_dot.end() - num_sample, x_dot.end()}; + std::vector resample_y2dot = {x2dot.end() - num_sample, x2dot.end()}; + std::vector resample_x; + double min_error = std::numeric_limits::max(); + int min_error_index = 0; + min_error = std::numeric_limits::max(); + for (int d = 0; d < maximum_delay; d++) { + // assume std::vector(old,....,new) + resample_x = {u.end() - num_sample - d, u.end() - d}; + double error_norm = optimization_utils::getLeastSquaredError( + resample_y2dot, resample_y_dot, resample_y, resample_x, ls2_estimator_.w); + if (optimization_utils::change_abs_min(min_error, error_norm)) { + min_error = error_norm; + min_error_index = d; + } + } + ls2_estimator_.mae = min_error; + ls2_estimator_.estimated_delay_index = min_error_index; + ls2_estimator_.time_delay = static_cast(min_error_index) * params.sampling_delta_time / + static_cast(params.num_interpolation); + const double valid_mae_threshold = 0.1; + if (ls2_estimator_.mae < valid_mae_threshold) { + return DetectionResult::DETECTED; + } + return DetectionResult::BELOW_THRESH; +} + +TimeDelayEstimator::DetectionResult TimeDelayEstimator::estimateDelayByCrossCorrelation( + rclcpp::Node * node, const std::vector & input, const std::vector & response, + Estimator & cc_estimator, std::string name, const Params & params) +{ + auto & cross_corr = cc_estimator.cross_correlation; + cross_corr = math_utils::calcCrossCorrelationCoefficient( + input, response, weights_for_data_, params.valid_delay_index_ratio); + auto & peak_index = cc_estimator.estimated_delay_index; + peak_index = math_utils::getMaximumIndexFromVector(cross_corr); + auto & peak_corr = cc_estimator.peak_correlation; + peak_corr = cross_corr[peak_index]; + if (peak_corr < params.valid_peak_cross_correlation_threshold) { + auto & clk = *node->get_clock(); + RCLCPP_DEBUG_STREAM_THROTTLE( + rclcpp::get_logger("time_delay_estimator"), clk, 5000, + name << "[time_delay_estimator] : correlation peak less than thresh: " << peak_corr); + return DetectionResult::BELOW_THRESH; + } + cc_estimator.mae = math_utils::calcMAE(input, response, cc_estimator.estimated_delay_index); + cc_estimator.time_delay = static_cast(peak_index) * params.sampling_delta_time / + static_cast(params.num_interpolation); + return DetectionResult::DETECTED; +} diff --git a/vehicle/time_delay_estimator/src/general_time_delay_estimator_node.cpp b/vehicle/time_delay_estimator/src/general_time_delay_estimator_node.cpp new file mode 100644 index 00000000..242b8376 --- /dev/null +++ b/vehicle/time_delay_estimator/src/general_time_delay_estimator_node.cpp @@ -0,0 +1,221 @@ +// +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 "time_delay_estimator/general_time_delay_estimator_node.hpp" + +#include +#include +#include +#include + +double validateRange( + rclcpp::Node * node, const double min, const double max, const double val, std::string name) +{ + auto & clk = *node->get_clock(); + if (min < std::abs(val) && std::abs(val) < max) { + RCLCPP_DEBUG_STREAM_THROTTLE( + rclcpp::get_logger("time_delay_estimator"), clk, 5000, + "[time_delay_estimator] " << name << ": " << val); + return val; + } + RCLCPP_DEBUG_STREAM_THROTTLE( + rclcpp::get_logger("time_delay_estimator"), clk, 5000, name << " is out of range: " << val); + return 0.0; +} + +double addOffset(const double val, const double offset) +{ + return val + offset; +} + +TimeDelayEstimatorNode::TimeDelayEstimatorNode(const rclcpp::NodeOptions & node_options) +: Node("time_delay_estimator", node_options) +{ + using std::placeholders::_1; + + // QoS setup + static constexpr std::size_t queue_size = 1; + rclcpp::QoS durable_qos(queue_size); + durable_qos.transient_local(); // option for latching + + // get parameter + detect_manual_engage_ = this->declare_parameter("detect_manual_engage", true); + params_.sampling_hz = this->declare_parameter("data/sampling_hz", 30.0); + params_.estimation_hz = this->declare_parameter("data/estimation_hz", 10.0); + params_.sampling_duration = this->declare_parameter("data/sampling_duration", 5.0); + params_.validation_duration = this->declare_parameter("data/validation_duration", 1.0); + params_.valid_peak_cross_correlation_threshold = + this->declare_parameter("data/valid_peak_cross_correlation_threshold", 0.8); + params_.valid_delay_index_ratio = + this->declare_parameter("data/valid_delay_index_ratio", 0.1); + params_.cutoff_hz_input = this->declare_parameter("filter/cutoff_hz_input", 0.5); + params_.cutoff_hz_output = this->declare_parameter("filter/cutoff_hz_output", 0.1); + params_.is_showing_debug_info = this->declare_parameter("is_showing_debug_info", true); + // params_.is_test_mode = this->declare_parameter("test/is_test_mode", false); + params_.num_interpolation = this->declare_parameter("data/num_interpolation", 3); + params_.reset_at_disengage = this->declare_parameter("reset_at_disengage", false); + bool use_weight_for_cross_correlation = + this->declare_parameter("use_weight_for_cross_correlation", false); + params_.sampling_delta_time = 1.0 / params_.sampling_hz; + params_.estimation_delta_time = 1.0 / params_.estimation_hz; + params_.data_size = static_cast(params_.sampling_hz * params_.sampling_duration); + params_.validation_size = static_cast(params_.sampling_hz * params_.validation_duration); + valid_input_.min = this->declare_parameter("min_valid_value", 0.05); + valid_input_.max = this->declare_parameter("max_valid_value", 1.00); + input_offset_ = this->declare_parameter("offset_value", 0.0); + data_name_ = this->declare_parameter("data_name", "test"); + + last_manual_time_ = this->now().seconds(); + // input + sub_control_mode_report_ = create_subscription( + "~/input/control_mode", queue_size, + std::bind(&TimeDelayEstimatorNode::callbackControlModeReport, this, _1)); + + // response + sub_input_cmd_ = create_subscription( + "~/input/input_cmd", queue_size, + std::bind(&TimeDelayEstimatorNode::callbackInputCmd, this, _1)); + sub_input_status_ = create_subscription( + "~/input/input_status", queue_size, + std::bind(&TimeDelayEstimatorNode::callbackInputStatus, this, _1)); + sub_is_engaged_ = create_subscription( + "~/input/is_engage", queue_size, std::bind(&TimeDelayEstimatorNode::callbackEngage, this, _1)); + + params_.total_data_size = + static_cast(params_.sampling_duration * params_.sampling_hz * params_.num_interpolation) + + 1; + collected_data_ = std::make_unique( + this, params_, data_name_, params_.total_data_size, use_weight_for_cross_correlation); + + pub_time_delay_ = create_publisher("~/output/time_delay", durable_qos); + + const auto period_s = params_.sampling_delta_time; + // data processing callback + { + auto data_processing_callback = std::bind(&TimeDelayEstimatorNode::timerDataCollector, this); + const auto period_ns = + std::chrono::duration_cast(std::chrono::duration(period_s)); + timer_data_processing_ = + std::make_shared>( + this->get_clock(), period_ns, std::move(data_processing_callback), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(timer_data_processing_, nullptr); + } + // estimation callback + { + auto estimation_callback = std::bind(&TimeDelayEstimatorNode::estimateTimeDelay, this); + const auto period_ns = + std::chrono::duration_cast(std::chrono::duration(period_s)); + timer_estimation_ = std::make_shared>( + this->get_clock(), period_ns, std::move(estimation_callback), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(timer_estimation_, nullptr); + } +} + +void TimeDelayEstimatorNode::timerDataCollector() +{ + if (std::min(auto_mode_duration_, engage_duration_) < 5.0 && detect_manual_engage_) { + if (params_.reset_at_disengage) { + collected_data_->resetEstimator(); + } + return; + } + if (input_status_ptr_ && input_cmd_ptr_) { + collected_data_->preprocessData(this); + } +} +bool TimeDelayEstimatorNode::estimateTimeDelay() +{ + std::chrono::system_clock::time_point start, end; + start = std::chrono::system_clock::now(); + + if (std::min(auto_mode_duration_, engage_duration_) < 5.0 && detect_manual_engage_) { + return false; + } + auto & clk = *this->get_clock(); + + // ====data delay estimation + if (!input_status_ptr_ || !input_cmd_ptr_) { + RCLCPP_INFO_STREAM_THROTTLE( + rclcpp::get_logger("time_delay_estimator"), clk, 5000, + "[time_delay_estimator] : empty input ptr"); + } else { + pub_time_delay_->publish(collected_data_->estimateTimeDelay(this, "cc")); + } + + end = std::chrono::system_clock::now(); + double elapsed = std::chrono::duration_cast(end - start).count(); + RCLCPP_DEBUG_STREAM_THROTTLE( + rclcpp::get_logger("time_delay_estimator"), clk, 10000, "duration[microSec]: " << elapsed); + return true; +} + +void TimeDelayEstimatorNode::callbackInputCmd(const Float32Stamped::ConstSharedPtr msg) +{ + input_cmd_ptr_ = msg; + const auto & t = this->get_clock()->now().seconds(); + double input = + validateRange(this, valid_input_.min, valid_input_.max, input_cmd_ptr_->data, data_name_); + const double input_offset = addOffset(input, input_offset_); + collected_data_->input_.setValue(input_offset, t); +} + +void TimeDelayEstimatorNode::callbackInputStatus(const Float32Stamped::ConstSharedPtr msg) +{ + input_status_ptr_ = msg; + const auto & t = this->get_clock()->now().seconds(); + double input_response = validateRange( + this, valid_input_.min, valid_input_.max, input_status_ptr_->data, data_name_ + " response"); + const double input_response_offset = addOffset(input_response, input_offset_); + collected_data_->response_.setValue(input_response_offset, t); +} + +void TimeDelayEstimatorNode::callbackControlModeReport(const ControlModeReport::ConstSharedPtr msg) +{ + auto & clk = *this->get_clock(); + control_mode_ptr_ = msg; + if (control_mode_ptr_->mode == autoware_vehicle_msgs::msg::ControlModeReport::AUTONOMOUS) { + auto_mode_duration_ = (this->now().seconds() - last_manual_time_); + RCLCPP_DEBUG_STREAM_THROTTLE( + rclcpp::get_logger("time_delay_estimator"), clk, 5000, + "[time_delay_estimator] control mode duration: " << auto_mode_duration_); + } else { + auto_mode_duration_ = 0; + last_manual_time_ = this->now().seconds(); + RCLCPP_DEBUG_STREAM_THROTTLE( + rclcpp::get_logger("time_delay_estimator"), clk, 5000, + "[time_delay_estimator] control mode : manual"); + } +} + +void TimeDelayEstimatorNode::callbackEngage(const IsEngaged::ConstSharedPtr msg) +{ + engage_mode_ = msg->data; + auto & clk = *this->get_clock(); + if (engage_mode_) { + engage_duration_ = (this->now().seconds() - last_disengage_time_); + RCLCPP_DEBUG_STREAM_THROTTLE( + rclcpp::get_logger("time_delay_estimator"), clk, 5000, + "[time_delay_estimator] engage duration: " << engage_duration_); + } else { + engage_duration_ = 0; + last_disengage_time_ = this->now().seconds(); + RCLCPP_INFO_STREAM_THROTTLE( + rclcpp::get_logger("time_delay_estimator"), clk, 5000, + "[time_delay_estimator] engage mode : disengage"); + } +} diff --git a/vehicle/time_delay_estimator/src/main.cpp b/vehicle/time_delay_estimator/src/main.cpp new file mode 100644 index 00000000..4c9f3111 --- /dev/null +++ b/vehicle/time_delay_estimator/src/main.cpp @@ -0,0 +1,28 @@ +// +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 "time_delay_estimator/time_delay_estimator_node.hpp" + +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + rclcpp::spin(std::make_shared(node_options)); + rclcpp::shutdown(); + return 0; +} diff --git a/vehicle/time_delay_estimator/src/time_delay_estimator.cpp b/vehicle/time_delay_estimator/src/time_delay_estimator.cpp new file mode 100644 index 00000000..4f4e6b17 --- /dev/null +++ b/vehicle/time_delay_estimator/src/time_delay_estimator.cpp @@ -0,0 +1,248 @@ +// +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 "time_delay_estimator/time_delay_estimator.hpp" + +#include "time_delay_estimator/data_processor.hpp" +#include "time_delay_estimator/parameters.hpp" + +#include +#include + +TimeDelayEstimator::TimeDelayEstimator( + rclcpp::Node * node, const Params & params, const std::string & name, int total_data_size, + bool use_weight_for_cross_correlation) +{ + params_ = params; + debugger_ = std::make_unique(node, name); + this->name_ = name; + ignore_thresh_ = node->declare_parameter(name + "/min_stddev_threshold", 0.005); + weights_for_data_.clear(); + for (size_t i = total_data_size; i > 0; i--) { + if (use_weight_for_cross_correlation) { + weights_for_data_.emplace_back(static_cast(i)); + } else { + weights_for_data_.emplace_back(static_cast(1.0)); + } + } +} + +void TimeDelayEstimator::resetEstimator() +{ + cc_estimator_ = Estimator(); + ls_estimator_ = Estimator(); + ls2_estimator_ = Estimator(); + loop_count_ = 0; + has_enough_input_ = false; + has_enough_response_ = false; + max_current_stddev_ = 0; + input_ = {}; + response_ = {}; + response_dot_ = {}; + response_2dot_ = {}; + time_delay_ = tier4_calibration_msgs::msg::TimeDelay(); +} + +void TimeDelayEstimator::preprocessData(rclcpp::Node * node) +{ + try { + is_valid_data_ = data_processor::checkIsValidData( + input_, response_, params_, max_current_stddev_, ignore_thresh_); + if (is_valid_data_) { + const int buffer = 2; + has_enough_input_ = data_processor::processInputData(input_, params_, buffer); + has_enough_response_ = data_processor::processResponseData( + node, response_, response_dot_, response_2dot_, params_, buffer); + } + } catch (std::runtime_error & e) { // Handle runtime errors + std::cerr << "[time_delay_estimator] at preprocessData runtime_error: " << e.what() + << std::endl; + } catch (std::logic_error & e) { + std::cerr << "[time_delay_estimator] at preprocessData logic_error: " << e.what() << std::endl; + } catch (...) { // Handle all unexpected exceptions + RCLCPP_ERROR(node->get_logger(), "[time_delay_estimator] at preprocessData unkown error"); + } +} + +void TimeDelayEstimator::processDebugData(rclcpp::Node * node) +{ + auto & de = debugger_->debug_values_; + if (input_.raw.size() <= 5 && response_.raw.size() <= 5) { + auto & clk = *node->get_clock(); + RCLCPP_DEBUG_STREAM_THROTTLE( + rclcpp::get_logger("time_delay_estimator"), clk, 5000, + "[time_delay_estimator] current input data size: " << input_.processed.size()); + } else { + // debug I/O + de.data[debugger_->DBGVAL::INPUT_RAW] = input_.raw.back(); + de.data[debugger_->DBGVAL::INPUT_FILTERED] = input_.filtered.back(); + de.data[debugger_->DBGVAL::INPUT_PROCESSED] = input_.processed.back(); + de.data[debugger_->DBGVAL::RESPONSE_RAW] = response_.raw.back(); + de.data[debugger_->DBGVAL::RESPONSE_FILTERED] = response_.filtered.back(); + de.data[debugger_->DBGVAL::RESPONSE_PROCESSED] = response_.processed.back(); + de.data[debugger_->DBGVAL::MAX_STDDEV] = static_cast(max_current_stddev_ * 10.0); + de.data[debugger_->DBGVAL::IS_VALID_DATA] = static_cast(is_valid_data_); + de.data[debugger_->DBGVAL::MIN_IGNORE_THRESH] = ignore_thresh_ * 10.0; + // cross correlation + de.data[debugger_->CROSS_CORRELATION::CC_DELAY] = cc_estimator_.time_delay; + de.data[debugger_->CROSS_CORRELATION::CC_MAE_AT_TIME] = cc_estimator_.mae; + de.data[debugger_->CROSS_CORRELATION::CC_MAE_MEAN] = cc_estimator_.stat_error.mean; + de.data[debugger_->CROSS_CORRELATION::CC_MAE_STDDEV] = cc_estimator_.stat_error.stddev; + de.data[debugger_->CROSS_CORRELATION::CC_DELAY_STDDEV] = cc_estimator_.stat_delay.stddev; + de.data[debugger_->CROSS_CORRELATION::CC_DETECTION_RESULT] = + 0.5 * static_cast(detection_result_); + // primary order + de.data[debugger_->LEAST_SQUARED::LS_DELAY] = ls_estimator_.time_delay; + de.data[debugger_->LEAST_SQUARED::LS_MAE_AT_TIME] = ls_estimator_.mae; + // secondary order + de.data[debugger_->LEAST_SQUARED_SECOND::LS2_DELAY] = ls2_estimator_.time_delay; + de.data[debugger_->LEAST_SQUARED_SECOND::LS2_MAE_AT_TIME] = ls2_estimator_.mae; + debugger_->publishDebugValue(); + std_msgs::msg::Float32MultiArray input_data; + std_msgs::msg::Float32MultiArray response_data; + std_msgs::msg::Float32MultiArray estimated_data; + auto & input_dim = input_data.layout.dim; + input_dim.resize(1); + input_dim[0].size = params_.total_data_size; + auto & response_dim = response_data.layout.dim; + response_dim.resize(1); + response_dim[0].size = params_.total_data_size; + auto & estimated_dim = estimated_data.layout.dim; + estimated_dim.resize(1); + estimated_dim[0].size = params_.total_data_size; + int size = static_cast(input_.processed.size() * (1.0 + params_.valid_delay_index_ratio)); + if (params_.is_showing_debug_info) { + // for input & response + for (int i = 0; i < size; i += 1) { + if (i < static_cast(input_.processed.size())) { + input_data.data.emplace_back(input_.processed[i]); + response_data.data.emplace_back(response_.processed[i]); + } else { + input_data.data.emplace_back(0); + response_data.data.emplace_back(0); + } + int slide_index = i + cc_estimator_.estimated_delay_index; + if (slide_index < static_cast(response_.processed.size())) { + estimated_data.data.emplace_back(response_.processed[slide_index]); + } else { + estimated_data.data.emplace_back(0); + } + } + // for cross correlation + std_msgs::msg::Float64MultiArray corr_data; + if (cc_estimator_.cross_correlation.size() > 0) { + for (int i = 0; i < size; i += 1) { + if (i == cc_estimator_.estimated_delay_index) { + corr_data.data.emplace_back(1); + } else { + corr_data.data.emplace_back(0); + } + } + // cross correlation + debugger_->pub_debug_input_->publish(input_data); + debugger_->pub_debug_response_->publish(response_data); + debugger_->pub_debug_estimated_->publish(estimated_data); + debugger_->pub_debug_corr_->publish(corr_data); + } + } + } +} + +void TimeDelayEstimator::postprocessOutput(Estimator & estimator) +{ + // stash initial result + const int ignore_count = 50; + estimator.time_delay = math_utils::lowpassFilter( + estimator.time_delay, estimator.time_delay_prev, params_.cutoff_hz_output, + params_.estimation_delta_time); + if (loop_count_ < ignore_count) { + estimator.time_delay_prev = cc_estimator_.time_delay; + } else { + estimator.time_delay_prev = estimator.time_delay; + auto & est_d = estimator.stat_delay; + auto & est_e = estimator.stat_error; + est_d.stddev = est_d.calcSequentialStddev(estimator.time_delay); + est_e.stddev = est_e.calcSequentialStddev(estimator.mae); + } + loop_count_++; +} + +void TimeDelayEstimator::setOutput(Estimator & estimator) +{ + time_delay_.time_delay = estimator.time_delay; + time_delay_.correlation_peak = estimator.peak_correlation; + time_delay_.time_delay_by_cross_correlation = {estimator.time_delay}; + time_delay_.first_order_model_coefficients = { + estimator.w(1), estimator.w(0), estimator.time_delay}; + time_delay_.mean = estimator.stat_delay.mean; + time_delay_.stddev = estimator.stat_delay.stddev; +} + +tier4_calibration_msgs::msg::TimeDelay TimeDelayEstimator::estimateTimeDelay( + rclcpp::Node * node, std::string estimator_type) +{ + try { + const bool use_time_fitting = false; + auto & clk = *node->get_clock(); + // use this for real time estimation only + if (use_time_fitting) { + math_utils::fitToTheSizeOfVector( + input_.stamps, response_.stamps, input_.processed, response_.processed, + params_.total_data_size, params_.num_interpolation); + } + // ---- Estimate Time Delay + if (is_valid_data_ && has_enough_input_ && has_enough_response_) { + if (estimator_type == "cc") { + detection_result_ = estimateDelayByCrossCorrelation( + node, input_.processed, response_.processed, cc_estimator_, name_, params_); + postprocessOutput(cc_estimator_); + setOutput(cc_estimator_); + } else if (estimator_type == "ls") { + detection_result_ = estimateDelayByLeastSquared( + response_dot_.processed, response_.processed, input_.processed, params_); + postprocessOutput(ls_estimator_); + setOutput(ls_estimator_); + } else if (estimator_type == "ls2") { + detection_result_ = estimateDelayByLeastSquared( + response_2dot_.processed, response_dot_.processed, response_.processed, input_.processed, + params_); + postprocessOutput(ls2_estimator_); + setOutput(ls2_estimator_); + } else { + RCLCPP_ERROR(node->get_logger(), "[time_delay_estimator] the estimator_type is invalid"); + } + processDebugData(node); + } else { + detection_result_ = DetectionResult::SLEEP; + RCLCPP_DEBUG_STREAM_THROTTLE( + rclcpp::get_logger("time_delay_estimator"), clk, 2000, + "[time_delay_estimator] less than data size: " << input_.processed.size()); + } + // for publish statistics + time_delay_.is_valid_data = is_valid_data_; + RCLCPP_DEBUG_STREAM_THROTTLE( + rclcpp::get_logger("time_delay_estimator"), clk, 5000, + name_ << ": (" << time_delay_.time_delay << "," << time_delay_.correlation_peak << "," + << time_delay_.mean << "," << time_delay_.stddev << ")"); + } catch (std::runtime_error & e) { // Handle runtime errors + std::cerr << "[time_delay_estimator] at estimate runtime_error: " << e.what() << std::endl; + } catch (std::logic_error & e) { + std::cerr << "[time_delay_estimator] at estimate logic_error: " << e.what() << std::endl; + } catch (...) { // Handle all unexpected exceptions + RCLCPP_ERROR(node->get_logger(), "[time_delay_estimator] at estimate unkown error"); + } + return time_delay_; +} diff --git a/vehicle/time_delay_estimator/src/time_delay_estimator_node.cpp b/vehicle/time_delay_estimator/src/time_delay_estimator_node.cpp new file mode 100644 index 00000000..9fffa917 --- /dev/null +++ b/vehicle/time_delay_estimator/src/time_delay_estimator_node.cpp @@ -0,0 +1,358 @@ +// +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 "time_delay_estimator/time_delay_estimator_node.hpp" + +#include +#include +#include +#include + +double validateRange( + rclcpp::Node * node, const double min, const double max, const double val, std::string name) +{ + auto & clk = *node->get_clock(); + if (min < std::abs(val) && std::abs(val) < max) { + RCLCPP_DEBUG_STREAM_THROTTLE( + rclcpp::get_logger("time_delay_estimator"), clk, 5000, + "[time_delay_estimator] " << name << ": " << val); + return val; + } + RCLCPP_DEBUG_STREAM_THROTTLE( + rclcpp::get_logger("time_delay_estimator"), clk, 5000, name << " is out of range: " << val); + return 0.0; +} + +double addOffset(const double val, const double offset) +{ + return val + offset; +} + +TimeDelayEstimatorNode::TimeDelayEstimatorNode(const rclcpp::NodeOptions & node_options) +: Node("time_delay_estimator", node_options) +{ + using std::placeholders::_1; + + // QoS setup + static constexpr std::size_t queue_size = 1; + rclcpp::QoS durable_qos(queue_size); + durable_qos.transient_local(); // option for latching + + // get parameter + detect_manual_engage_ = this->declare_parameter("detect_manual_engage", true); + params_.sampling_hz = this->declare_parameter("data/sampling_hz", 30.0); + params_.estimation_hz = this->declare_parameter("data/estimation_hz", 10.0); + params_.sampling_duration = this->declare_parameter("data/sampling_duration", 5.0); + params_.validation_duration = this->declare_parameter("data/validation_duration", 1.0); + params_.valid_peak_cross_correlation_threshold = + this->declare_parameter("data/valid_peak_cross_correlation_threshold", 0.8); + params_.valid_delay_index_ratio = + this->declare_parameter("data/valid_delay_index_ratio", 0.1); + params_.cutoff_hz_input = this->declare_parameter("filter/cutoff_hz_input", 0.5); + params_.cutoff_hz_output = this->declare_parameter("filter/cutoff_hz_output", 0.1); + params_.is_showing_debug_info = this->declare_parameter("is_showing_debug_info", true); + params_.is_test_mode = this->declare_parameter("test/is_test_mode", false); + params_.num_interpolation = this->declare_parameter("data/num_interpolation", 3); + params_.reset_at_disengage = this->declare_parameter("reset_at_disengage", false); + estimator_type_ = this->declare_parameter("estimator_type", "cc"); + bool use_weight_for_cross_correlation = + this->declare_parameter("use_weight_for_cross_correlation", false); + params_.sampling_delta_time = 1.0 / params_.sampling_hz; + params_.estimation_delta_time = 1.0 / params_.estimation_hz; + params_.data_size = static_cast(params_.sampling_hz * params_.sampling_duration); + params_.validation_size = static_cast(params_.sampling_hz * params_.validation_duration); + valid_steer_.min = this->declare_parameter("steer/valid_min_steer", 0.05); + valid_steer_.max = this->declare_parameter("steer/valid_max_steer", 1.0); + steer_offset_ = this->declare_parameter("steer/offset_value", 0.0); + valid_accel_.min = this->declare_parameter("accel/valid_min_accel", 0.05); + valid_accel_.max = this->declare_parameter("accel/valid_max_accel", 1.0); + accel_offset_ = this->declare_parameter("accel/offset_value", 0.0); + valid_brake_.min = this->declare_parameter("brake/valid_min_brake", 0.05); + valid_brake_.max = this->declare_parameter("brake/valid_max_brake", 1.0); + brake_offset_ = this->declare_parameter("brake/offset_value", 0.0); + + last_manual_time_ = this->now().seconds(); + // input + sub_control_mode_ = create_subscription( + "~/input/control_mode", queue_size, + std::bind(&TimeDelayEstimatorNode::callbackControlModeReport, this, _1)); + + // response + sub_accel_cmd_ = create_subscription( + "~/input/accel_cmd", queue_size, + std::bind(&TimeDelayEstimatorNode::callbackAccelCmd, this, _1)); + sub_brake_cmd_ = create_subscription( + "~/input/brake_cmd", queue_size, + std::bind(&TimeDelayEstimatorNode::callbackBrakeCmd, this, _1)); + sub_steer_cmd_ = create_subscription( + "~/input/steer_cmd", queue_size, + std::bind(&TimeDelayEstimatorNode::callbackSteerCmd, this, _1)); + sub_accel_status_ = create_subscription( + "~/input/accel_status", queue_size, + std::bind(&TimeDelayEstimatorNode::callbackAccelStatus, this, _1)); + sub_brake_status_ = create_subscription( + "~/input/brake_status", queue_size, + std::bind(&TimeDelayEstimatorNode::callbackBrakeStatus, this, _1)); + sub_steer_status_ = create_subscription( + "~/input/steer_status", queue_size, + std::bind(&TimeDelayEstimatorNode::callbackSteerStatus, this, _1)); + sub_is_engaged_ = create_subscription( + "~/input/is_engage", queue_size, std::bind(&TimeDelayEstimatorNode::callbackEngage, this, _1)); + + params_.total_data_size = + static_cast(params_.sampling_duration * params_.sampling_hz * params_.num_interpolation) + + 1; + accel_data_ = std::make_unique( + this, params_, "accel", params_.total_data_size, use_weight_for_cross_correlation); + brake_data_ = std::make_unique( + this, params_, "brake", params_.total_data_size, use_weight_for_cross_correlation); + steer_data_ = std::make_unique( + this, params_, "steer", params_.total_data_size, use_weight_for_cross_correlation); + if (params_.is_test_mode) { + test_data_ = std::make_unique( + this, params_, "test", params_.total_data_size, use_weight_for_cross_correlation); + } + + pub_time_delay_test_ = create_publisher("~/output/test_time_delay", durable_qos); + pub_time_delay_accel_ = create_publisher("~/output/accel_cmd_delay", durable_qos); + pub_time_delay_steer_ = create_publisher("~/output/steer_cmd_delay", durable_qos); + pub_time_delay_brake_ = create_publisher("~/output/brake_cmd_delay", durable_qos); + + const auto period_s = params_.sampling_delta_time; + // data processing callback + { + auto data_processing_callback = std::bind(&TimeDelayEstimatorNode::timerDataCollector, this); + const auto period_ns = + std::chrono::duration_cast(std::chrono::duration(period_s)); + timer_data_processing_ = + std::make_shared>( + this->get_clock(), period_ns, std::move(data_processing_callback), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(timer_data_processing_, nullptr); + } + // estimation callback + { + auto estimation_callback = std::bind(&TimeDelayEstimatorNode::estimateTimeDelay, this); + const auto period_ns = + std::chrono::duration_cast(std::chrono::duration(period_s)); + timer_estimation_ = std::make_shared>( + this->get_clock(), period_ns, std::move(estimation_callback), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(timer_estimation_, nullptr); + } +} + +void TimeDelayEstimatorNode::timerDataCollector() +{ + if (params_.is_test_mode) { + callbackTestData(); + test_data_->preprocessData(this); + } + if (std::min(auto_mode_duration_, engage_duration_) < 5.0 && detect_manual_engage_) { + if (params_.reset_at_disengage) { + accel_data_->resetEstimator(); + brake_data_->resetEstimator(); + steer_data_->resetEstimator(); + } + return; + } + // ====accel delay estimation + if (accel_status_ptr_ && accel_cmd_ptr_) { + accel_data_->preprocessData(this); + } + if (brake_status_ptr_ && brake_cmd_ptr_) { + brake_data_->preprocessData(this); + } + if (steer_status_ptr_ && steer_cmd_ptr_) { + steer_data_->preprocessData(this); + } +} +bool TimeDelayEstimatorNode::estimateTimeDelay() +{ + std::chrono::system_clock::time_point start, end; + start = std::chrono::system_clock::now(); + + if (params_.is_test_mode) { + // ====test delay estimation + pub_time_delay_test_->publish(test_data_->estimateTimeDelay(this, estimator_type_)); + } + + if (std::min(auto_mode_duration_, engage_duration_) < 5.0 && detect_manual_engage_) { + return false; + } + auto & clk = *this->get_clock(); + + // ====accel delay estimation + if (!accel_status_ptr_ || !accel_cmd_ptr_) { + RCLCPP_INFO_STREAM_THROTTLE( + rclcpp::get_logger("time_delay_estimator"), clk, 5000, + "[time_delay_estimator] : empty accel ptr"); + } else { + pub_time_delay_accel_->publish(accel_data_->estimateTimeDelay(this, estimator_type_)); + } + + // ====brake delay estimation + if (!brake_status_ptr_ || !brake_cmd_ptr_) { + RCLCPP_INFO_STREAM_THROTTLE( + rclcpp::get_logger("time_delay_estimator"), clk, 5000, + "[time_delay_estimator] : empty brake ptr"); + } else { + pub_time_delay_brake_->publish(brake_data_->estimateTimeDelay(this, estimator_type_)); + } + // ====steer delay estimation + if (!steer_status_ptr_ || !steer_cmd_ptr_) { + RCLCPP_INFO_STREAM_THROTTLE( + rclcpp::get_logger("time_delay_estimator"), clk, 5000, + "[time_delay_estimator] : empty steer ptr"); + } else { + pub_time_delay_steer_->publish(steer_data_->estimateTimeDelay(this, estimator_type_)); + } + + end = std::chrono::system_clock::now(); + double elapsed = std::chrono::duration_cast(end - start).count(); + RCLCPP_DEBUG_STREAM_THROTTLE( + rclcpp::get_logger("time_delay_estimator"), clk, 10000, "duration[microSec]: " << elapsed); + return true; +} + +void TimeDelayEstimatorNode::callbackAccelCmd(const Float32Stamped::ConstSharedPtr msg) +{ + accel_cmd_ptr_ = msg; + const auto & t = this->get_clock()->now().seconds(); + // accel + double accel_input = + validateRange(this, valid_accel_.min, valid_accel_.max, accel_cmd_ptr_->data, "accel input"); + const double accel_input_offset = addOffset(accel_input, accel_offset_); + accel_data_->input_.setValue(accel_input_offset, t); +} + +void TimeDelayEstimatorNode::callbackBrakeCmd(const Float32Stamped::ConstSharedPtr msg) +{ + brake_cmd_ptr_ = msg; + const auto & t = this->get_clock()->now().seconds(); + // brake + double brake_input = + validateRange(this, valid_brake_.min, valid_brake_.max, brake_cmd_ptr_->data, "brake input"); + const double brake_input_offset = addOffset(brake_input, brake_offset_); + brake_data_->input_.setValue(brake_input_offset, t); +} + +void TimeDelayEstimatorNode::callbackSteerCmd(const Float32Stamped::ConstSharedPtr msg) +{ + steer_cmd_ptr_ = msg; + const auto & t = this->get_clock()->now().seconds(); + double steer_input = + validateRange(this, valid_steer_.min, valid_steer_.max, steer_cmd_ptr_->data, "steer input"); + steer_input = math_utils::normalize(steer_input, -1.0, 1.0); + const double steer_input_offset = addOffset(steer_input, steer_offset_); + steer_data_->input_.setValue(steer_input_offset, t); +} + +void TimeDelayEstimatorNode::callbackAccelStatus(const Float32Stamped::ConstSharedPtr msg) +{ + accel_status_ptr_ = msg; + const auto & t = this->get_clock()->now().seconds(); + double accel_response = validateRange( + this, valid_accel_.min, valid_accel_.max, accel_status_ptr_->data, "accel response"); + const double accel_response_offset = addOffset(accel_response, accel_offset_); + accel_data_->response_.setValue(accel_response_offset, t); +} + +void TimeDelayEstimatorNode::callbackBrakeStatus(const Float32Stamped::ConstSharedPtr msg) +{ + brake_status_ptr_ = msg; + const auto & t = this->get_clock()->now().seconds(); + double brake_response = validateRange( + this, valid_brake_.min, valid_brake_.max, brake_status_ptr_->data, "brake response"); + const double brake_response_offset = addOffset(brake_response, brake_offset_); + brake_data_->response_.setValue(brake_response_offset, t); +} + +void TimeDelayEstimatorNode::callbackSteerStatus(const Float32Stamped::ConstSharedPtr msg) +{ + steer_status_ptr_ = msg; + const auto & t = this->get_clock()->now().seconds(); + double steer_response = validateRange( + this, valid_steer_.min, valid_steer_.max, steer_status_ptr_->data, "steer response"); + steer_response = math_utils::normalize(steer_response, -1.0, 1.0); + const double steer_response_offset = addOffset(steer_response, steer_offset_); + steer_data_->response_.setValue(steer_response_offset, t); +} + +void TimeDelayEstimatorNode::callbackControlModeReport(const ControlModeReport::ConstSharedPtr msg) +{ + auto & clk = *this->get_clock(); + control_mode_ptr_ = msg; + if (control_mode_ptr_->mode == autoware_vehicle_msgs::msg::ControlModeReport::AUTONOMOUS) { + auto_mode_duration_ = (this->now().seconds() - last_manual_time_); + RCLCPP_DEBUG_STREAM_THROTTLE( + rclcpp::get_logger("time_delay_estimator"), clk, 5000, + "[time_delay_estimator] control mode duration: " << auto_mode_duration_); + } else { + auto_mode_duration_ = 0; + last_manual_time_ = this->now().seconds(); + RCLCPP_DEBUG_STREAM_THROTTLE( + rclcpp::get_logger("time_delay_estimator"), clk, 5000, + "[time_delay_estimator] control mode : manual"); + } +} + +void TimeDelayEstimatorNode::callbackEngage(const IsEngaged::ConstSharedPtr msg) +{ + engage_mode_ = msg->data; + auto & clk = *this->get_clock(); + if (engage_mode_) { + engage_duration_ = (this->now().seconds() - last_disengage_time_); + RCLCPP_DEBUG_STREAM_THROTTLE( + rclcpp::get_logger("time_delay_estimator"), clk, 5000, + "[time_delay_estimator] engage duration: " << engage_duration_); + } else { + engage_duration_ = 0; + last_disengage_time_ = this->now().seconds(); + RCLCPP_INFO_STREAM_THROTTLE( + rclcpp::get_logger("time_delay_estimator"), clk, 5000, + "[time_delay_estimator] engage mode : disengage"); + } +} + +void TimeDelayEstimatorNode::callbackTestData() +{ + cnt_++; + double dt = 1.0 / params_.sampling_hz; + static double delay; + if (cnt_ % 500 == 0) { + if (delay < 0.6) { + delay += 0.1; + } else { + delay = 0; + } + } + auto & clk = *this->get_clock(); + RCLCPP_DEBUG_STREAM_THROTTLE( + rclcpp::get_logger("time_delay_estimator"), clk, 1000, + "[time_delay_estimator] delay: " << delay << " cnt_: " << cnt_); + double t = cnt_ * dt; + const double w = 2.0 * M_PI * 0.5; + const double Amp = 0.5; + const double multiplier = 0.1 * (cnt_ % 100); + const double base = 0.5; + double input = Amp * multiplier * std::sin(w * t) + base; + double response = Amp * multiplier * std::sin(w * (t - delay)) + base; + input = math_utils::normalize(input, Amp * 10 + base, -Amp * 10 + base); + response = math_utils::normalize(response, Amp * 10 + base, -Amp * 10 + base); + test_data_->input_.setValue(input, t); + test_data_->response_.setValue(response, t); +}