resolve conflict

Signed-off-by: kminoda <[email protected]>

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

@@ -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_;

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 <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,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();
 

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