From e91efde1236474ddf1270c9dd455a9f3bf5260f6 Mon Sep 17 00:00:00 2001 From: kminoda Date: Thu, 7 Sep 2023 11:21:32 +0900 Subject: [PATCH 1/5] resolve conflict Signed-off-by: kminoda --- .../deviation_estimator/CMakeLists.txt | 3 +- .../config/deviation_estimator.param.yaml | 8 ++ .../deviation_estimator.hpp | 7 ++ .../include/deviation_estimator/utils.hpp | 3 + .../src/deviation_estimator.cpp | 24 +++- .../deviation_estimator/src/utils.cpp | 8 ++ .../deviation_estimator/test/test_utils.cpp | 108 ++++++++++++++++++ 7 files changed, 155 insertions(+), 6 deletions(-) create mode 100644 localization/deviation_estimation_tools/deviation_estimator/test/test_utils.cpp diff --git a/localization/deviation_estimation_tools/deviation_estimator/CMakeLists.txt b/localization/deviation_estimation_tools/deviation_estimator/CMakeLists.txt index 8b112ead..e77cb150 100644 --- a/localization/deviation_estimation_tools/deviation_estimator/CMakeLists.txt +++ b/localization/deviation_estimation_tools/deviation_estimator/CMakeLists.txt @@ -36,7 +36,8 @@ if(BUILD_TESTING) set(TEST_FILES test/test_gyro_stddev.cpp - test/test_gyro_bias.cpp) + test/test_gyro_bias.cpp + test/test_utils.cpp) foreach(filepath ${TEST_FILES}) add_testcase(${filepath}) 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 index 0b720bbe..9836e549 100644 --- a/localization/deviation_estimation_tools/deviation_estimator/config/deviation_estimator.param.yaml +++ b/localization/deviation_estimation_tools/deviation_estimator/config/deviation_estimator.param.yaml @@ -1,3 +1,11 @@ /**: ros__parameters: time_window: 4.0 + gyro_estimation: + only_use_straight: true + only_use_moving: false + only_use_constant_velocity: false + velocity_estimation: + only_use_straight: true + only_use_moving: true + only_use_constant_velocity: true 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 index 834573cf..865be2d1 100644 --- 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 @@ -90,6 +90,13 @@ class DeviationEstimator : public rclcpp::Node double estimation_freq_; double time_window_; bool add_bias_uncertainty_; + + bool gyro_only_use_straight_; + bool gyro_only_use_moving_; + bool gyro_only_use_constant_velocity_; + bool velocity_only_use_straight_; + bool velocity_only_use_moving_; + bool velocity_only_use_constant_velocity_; std::string imu_frame_; const std::string output_frame_; 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 index f1dc96ae..d0be362f 100644 --- a/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/utils.hpp +++ b/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/utils.hpp @@ -45,6 +45,9 @@ struct TrajectoryData 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) { diff --git a/localization/deviation_estimation_tools/deviation_estimator/src/deviation_estimator.cpp b/localization/deviation_estimation_tools/deviation_estimator/src/deviation_estimator.cpp index 388b3c72..4bf8a074 100644 --- a/localization/deviation_estimation_tools/deviation_estimator/src/deviation_estimator.cpp +++ b/localization/deviation_estimation_tools/deviation_estimator/src/deviation_estimator.cpp @@ -144,6 +144,16 @@ DeviationEstimator::DeviationEstimator( time_window_ = declare_parameter("time_window", 4.0); add_bias_uncertainty_ = declare_parameter("add_bias_uncertainty", false); + // 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"); + 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"); + auto timer_callback = std::bind(&DeviationEstimator::timer_callback, this); auto period_control = std::chrono::duration_cast( std::chrono::duration(time_window_)); @@ -255,16 +265,20 @@ void DeviationEstimator::timer_callback() 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_stopped = get_mean_abs_vx(traj_data.vx_list) < vx_threshold_; + 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_; - if (is_straight & !is_stopped & is_constant_velocity) { + 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); } - gyro_bias_module_->update_bias(traj_data); - traj_data_list_for_gyro_.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(); diff --git a/localization/deviation_estimation_tools/deviation_estimator/src/utils.cpp b/localization/deviation_estimation_tools/deviation_estimator/src/utils.cpp index d8d51f8b..36e61ddc 100644 --- a/localization/deviation_estimation_tools/deviation_estimator/src/utils.cpp +++ b/localization/deviation_estimation_tools/deviation_estimator/src/utils.cpp @@ -39,6 +39,14 @@ double clip_radian(const double 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" */ 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..4fcc59ac --- /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)); +} \ No newline at end of file From d94615b76518f0ff49e6f7a9524f8d37923ac56d Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 7 Sep 2023 02:22:41 +0000 Subject: [PATCH 2/5] ci(pre-commit): autofix --- .../include/deviation_estimator/deviation_estimator.hpp | 2 +- .../include/deviation_estimator/utils.hpp | 3 ++- .../deviation_estimator/src/deviation_estimator.cpp | 8 ++++++-- .../deviation_estimator/src/utils.cpp | 8 ++++---- .../deviation_estimator/test/test_utils.cpp | 2 +- 5 files changed, 14 insertions(+), 9 deletions(-) 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 index 865be2d1..e92ee010 100644 --- 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 @@ -90,7 +90,7 @@ class DeviationEstimator : public rclcpp::Node double estimation_freq_; double time_window_; bool add_bias_uncertainty_; - + bool gyro_only_use_straight_; bool gyro_only_use_moving_; bool gyro_only_use_constant_velocity_; 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 index d0be362f..122c38b4 100644 --- a/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/utils.hpp +++ b/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/utils.hpp @@ -45,7 +45,8 @@ struct TrajectoryData 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, +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 diff --git a/localization/deviation_estimation_tools/deviation_estimator/src/deviation_estimator.cpp b/localization/deviation_estimation_tools/deviation_estimator/src/deviation_estimator.cpp index 4bf8a074..93e3cb4e 100644 --- a/localization/deviation_estimation_tools/deviation_estimator/src/deviation_estimator.cpp +++ b/localization/deviation_estimation_tools/deviation_estimator/src/deviation_estimator.cpp @@ -269,8 +269,12 @@ void DeviationEstimator::timer_callback() 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_); + 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); diff --git a/localization/deviation_estimation_tools/deviation_estimator/src/utils.cpp b/localization/deviation_estimation_tools/deviation_estimator/src/utils.cpp index 36e61ddc..1fb84235 100644 --- a/localization/deviation_estimation_tools/deviation_estimator/src/utils.cpp +++ b/localization/deviation_estimation_tools/deviation_estimator/src/utils.cpp @@ -39,12 +39,12 @@ double clip_radian(const double rad) } } -bool whether_to_use_data(const bool is_straight, const bool is_moving, const bool is_constant_velocity, +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); + return (is_straight || !only_use_straight) && (is_moving || !only_use_moving) && + (is_constant_velocity || !only_use_constant_velocity); } /** diff --git a/localization/deviation_estimation_tools/deviation_estimator/test/test_utils.cpp b/localization/deviation_estimation_tools/deviation_estimator/test/test_utils.cpp index 4fcc59ac..49c492d6 100644 --- a/localization/deviation_estimation_tools/deviation_estimator/test/test_utils.cpp +++ b/localization/deviation_estimation_tools/deviation_estimator/test/test_utils.cpp @@ -105,4 +105,4 @@ TEST(DeviationEstimatorUtils, WhetherToUseData7) EXPECT_TRUE(whether_to_use_data( is_straight, is_moving, is_constant_velocity, only_use_straight, only_use_moving, only_use_constant_velocity)); -} \ No newline at end of file +} From fe8daeb929ec2e8dc01f09ec15e68d88f9126a95 Mon Sep 17 00:00:00 2001 From: kminoda Date: Thu, 7 Sep 2023 11:25:40 +0900 Subject: [PATCH 3/5] fix precommit Signed-off-by: kminoda --- .../deviation_estimator/include/deviation_estimator/logger.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index c88b9ddf..ffe58524 100644 --- a/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/logger.hpp +++ b/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/logger.hpp @@ -28,7 +28,7 @@ class Logger { public: - Logger(const std::string & output_dir); + 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, From 175bd49f9ee852982a05e2d6b5ea34454f82bbb4 Mon Sep 17 00:00:00 2001 From: kminoda Date: Thu, 7 Sep 2023 11:31:59 +0900 Subject: [PATCH 4/5] update readme and test Signed-off-by: kminoda --- localization/deviation_estimation_tools/ReadMe.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/localization/deviation_estimation_tools/ReadMe.md b/localization/deviation_estimation_tools/ReadMe.md index 71356210..b50d62e2 100644 --- a/localization/deviation_estimation_tools/ReadMe.md +++ b/localization/deviation_estimation_tools/ReadMe.md @@ -194,6 +194,12 @@ The parameters and input topic names can be seen in the `deviation_estimator.lau | 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 From 1b4cd9e7575436397c228ee1fa6ca31a2ac746c3 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 7 Sep 2023 02:32:27 +0000 Subject: [PATCH 5/5] ci(pre-commit): autofix --- .../deviation_estimation_tools/ReadMe.md | 26 +++++++++---------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/localization/deviation_estimation_tools/ReadMe.md b/localization/deviation_estimation_tools/ReadMe.md index b50d62e2..4d2ebfcf 100644 --- a/localization/deviation_estimation_tools/ReadMe.md +++ b/localization/deviation_estimation_tools/ReadMe.md @@ -187,19 +187,19 @@ The parameters and input topic names can be seen in the `deviation_estimator.lau ### 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 | +| 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