feat(deviation_estimator): parametrize which data to use

localization/deviation_estimation_tools/ReadMe.md

@@ -187,13 +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)" |
+| 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
 

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})

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

localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/deviation_estimator.hpp

@@ -91,6 +91,13 @@ class DeviationEstimator : public rclcpp::Node
   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_;
   const std::string results_dir_;

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,

localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/utils.hpp

@@ -45,6 +45,10 @@ 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 <typename T>
 double calculate_mean(const std::vector<T> & v)
 {

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<bool>("gyro_estimation.only_use_straight");
+  gyro_only_use_moving_ = declare_parameter<bool>("gyro_estimation.only_use_moving");
+  gyro_only_use_constant_velocity_ =
+    declare_parameter<bool>("gyro_estimation.only_use_constant_velocity");
+  velocity_only_use_straight_ = declare_parameter<bool>("velocity_estimation.only_use_straight");
+  velocity_only_use_moving_ = declare_parameter<bool>("velocity_estimation.only_use_moving");
+  velocity_only_use_constant_velocity_ =
+    declare_parameter<bool>("velocity_estimation.only_use_constant_velocity");
+
   auto timer_callback = std::bind(&DeviationEstimator::timer_callback, this);
   auto period_control = std::chrono::duration_cast<std::chrono::nanoseconds>(
     std::chrono::duration<double>(time_window_));
@@ -255,16 +265,24 @@ 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();
 

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"
  */

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 <gtest/gtest.h>
+
+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));
+}