Merge branch 'main' into fix/mpc_controller

Signed-off-by: kyoichi-sugahara <[email protected]>

.cspell-partial.json

@@ -5,5 +5,5 @@
     "perception/bytetrack/lib/**"
   ],
   "ignoreRegExpList": [],
-  "words": []
+  "words": ["dltype", "tvmgen", "quantizer", "imageio", "mimsave"]
 }

.github/CODEOWNERS

@@ -56,7 +56,7 @@ common/traffic_light_recognition_marker_publisher/** [email protected]
 common/traffic_light_utils/** [email protected] [email protected]
 common/trtexec_vendor/** [email protected] [email protected]
 common/tvm_utility/** [email protected] [email protected]
-control/autonomous_emergency_braking/** [email protected] [email protected]
+control/autonomous_emergency_braking/** [email protected] [email protected] [email protected]
 control/control_performance_analysis/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 control/control_validator/** [email protected] [email protected] [email protected]
 control/external_cmd_selector/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
@@ -66,6 +66,7 @@ control/mpc_lateral_controller/** [email protected] takayuki.murooka@tier
 control/obstacle_collision_checker/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 control/operation_mode_transition_manager/** [email protected] [email protected]
 control/pid_longitudinal_controller/** [email protected] [email protected]
+control/predicted_path_checker/** [email protected]
 control/pure_pursuit/** [email protected]
 control/shift_decider/** [email protected]
 control/trajectory_follower_base/** [email protected] [email protected]
@@ -87,13 +88,15 @@ launch/tier4_system_launch/** [email protected] [email protected]
 launch/tier4_vehicle_launch/** [email protected]
 localization/ekf_localizer/** [email protected] [email protected] [email protected] [email protected] [email protected]
 localization/gyro_odometer/** [email protected] [email protected]
-localization/landmark_based_localizer/ar_tag_based_localizer/** [email protected] [email protected]
-localization/landmark_based_localizer/landmark_tf_caster/** [email protected] [email protected]
+localization/landmark_based_localizer/ar_tag_based_localizer/** [email protected] [email protected] shintaro.sakoda@tier4.jp yamato.ando@tier4.jp
+localization/landmark_based_localizer/landmark_parser/** [email protected] [email protected] shintaro.sakoda@tier4.jp yamato.ando@tier4.jp
 localization/localization_error_monitor/** [email protected] [email protected] [email protected]
+localization/localization_util/** [email protected] [email protected] [email protected] [email protected]
 localization/ndt_scan_matcher/** [email protected] [email protected] [email protected] [email protected]
 localization/pose2twist/** [email protected] [email protected]
 localization/pose_initializer/** [email protected] [email protected] [email protected]
 localization/stop_filter/** [email protected] [email protected] [email protected]
+localization/tree_structured_parzen_estimator/** [email protected] [email protected] [email protected] [email protected]
 localization/twist2accel/** [email protected] [email protected] [email protected]
 localization/yabloc/yabloc_common/** [email protected] [email protected]
 localization/yabloc/yabloc_image_processing/** [email protected] [email protected]
@@ -140,6 +143,7 @@ perception/simple_object_merger/** [email protected] shunsuke.miura@tier4.
 perception/tensorrt_classifier/** [email protected]
 perception/tensorrt_yolo/** [email protected]
 perception/tensorrt_yolox/** [email protected] [email protected] [email protected]
+perception/tracking_object_merger/** [email protected] [email protected]
 perception/traffic_light_arbiter/** [email protected] [email protected]
 perception/traffic_light_classifier/** [email protected] [email protected]
 perception/traffic_light_fine_detector/** [email protected] [email protected]
@@ -150,7 +154,7 @@ perception/traffic_light_ssd_fine_detector/** [email protected]
 perception/traffic_light_visualization/** [email protected]
 planning/behavior_path_planner/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/behavior_velocity_blind_spot_module/** [email protected] [email protected] [email protected]
-planning/behavior_velocity_crosswalk_module/** [email protected] [email protected] [email protected] [email protected]
+planning/behavior_velocity_crosswalk_module/** [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/behavior_velocity_detection_area_module/** [email protected] [email protected] [email protected]
 planning/behavior_velocity_intersection_module/** [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/behavior_velocity_no_drivable_lane_module/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
@@ -177,11 +181,10 @@ planning/obstacle_cruise_planner/** [email protected] [email protected]
 planning/obstacle_stop_planner/** [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/obstacle_velocity_limiter/** [email protected]
 planning/path_smoother/** [email protected] [email protected]
-planning/planning_debug_tools/** [email protected] [email protected]
+planning/planning_debug_tools/** [email protected] [email protected] [email protected]
 planning/planning_test_utils/** [email protected] [email protected]
 planning/planning_validator/** [email protected] [email protected]
 planning/route_handler/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
-planning/rtc_auto_mode_manager/** [email protected] [email protected]
 planning/rtc_interface/** [email protected] [email protected]
 planning/rtc_replayer/** [email protected] [email protected]
 planning/sampling_based_planner/bezier_sampler/** [email protected]
@@ -215,11 +218,11 @@ system/default_ad_api_helpers/ad_api_visualizers/** [email protected] kahhoo
 system/default_ad_api_helpers/automatic_pose_initializer/** [email protected] [email protected] [email protected] [email protected] [email protected]
 system/dummy_diag_publisher/** [email protected] [email protected]
 system/dummy_infrastructure/** [email protected]
+system/duplicated_node_checker/** [email protected] [email protected]
 system/emergency_handler/** [email protected]
 system/mrm_comfortable_stop_operator/** [email protected]
 system/mrm_emergency_stop_operator/** [email protected]
 system/system_diagnostic_graph/** [email protected]
-system/system_diagnostic_monitor/** [email protected]
 system/system_error_monitor/** [email protected]
 system/system_monitor/** [email protected] [email protected]
 system/topic_state_monitor/** [email protected]

common/kalman_filter/CMakeLists.txt

@@ -19,4 +19,11 @@ ament_auto_add_library(kalman_filter SHARED
   include/kalman_filter/time_delay_kalman_filter.hpp
 )
 
+if(BUILD_TESTING)
+  file(GLOB_RECURSE test_files test/*.cpp)
+  ament_add_ros_isolated_gtest(test_kalman_filter ${test_files})
+
+  target_link_libraries(test_kalman_filter kalman_filter)
+endif()
+
 ament_auto_package()

common/kalman_filter/include/kalman_filter/kalman_filter.hpp

@@ -109,20 +109,20 @@ class KalmanFilter
    * @brief get current kalman filter state
    * @param x kalman filter state
    */
-  void getX(Eigen::MatrixXd & x);
+  void getX(Eigen::MatrixXd & x) const;
 
   /**
    * @brief get current kalman filter covariance
    * @param P kalman filter covariance
    */
-  void getP(Eigen::MatrixXd & P);
+  void getP(Eigen::MatrixXd & P) const;
 
   /**
    * @brief get component of current kalman filter state
    * @param i index of kalman filter state
    * @return value of i's component of the kalman filter state x[i]
    */
-  double getXelement(unsigned int i);
+  double getXelement(unsigned int i) const;
 
   /**
    * @brief calculate kalman filter state and covariance by prediction model with A, B, Q matrix.

common/kalman_filter/package.xml

@@ -18,7 +18,9 @@
   <build_depend>eigen3_cmake_module</build_depend>
 
   <test_depend>ament_cmake_cppcheck</test_depend>
+  <test_depend>ament_cmake_ros</test_depend>
   <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

common/kalman_filter/src/kalman_filter.cpp

@@ -77,15 +77,15 @@ void KalmanFilter::setR(const Eigen::MatrixXd & R)
 {
   R_ = R;
 }
-void KalmanFilter::getX(Eigen::MatrixXd & x)
+void KalmanFilter::getX(Eigen::MatrixXd & x) const
 {
   x = x_;
 }
-void KalmanFilter::getP(Eigen::MatrixXd & P)
+void KalmanFilter::getP(Eigen::MatrixXd & P) const
 {
   P = P_;
 }
-double KalmanFilter::getXelement(unsigned int i)
+double KalmanFilter::getXelement(unsigned int i) const
 {
   return x_(i);
 }

common/kalman_filter/test/test_kalman_filter.cpp

@@ -0,0 +1,94 @@
+// Copyright 2023 The 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 "kalman_filter/kalman_filter.hpp"
+
+#include <gtest/gtest.h>
+
+TEST(kalman_filter, kf)
+{
+  KalmanFilter kf_;
+
+  Eigen::MatrixXd x_t(2, 1);
+  x_t << 1, 2;
+
+  Eigen::MatrixXd P_t(2, 2);
+  P_t << 1, 0, 0, 1;
+
+  Eigen::MatrixXd Q_t(2, 2);
+  Q_t << 0.01, 0, 0, 0.01;
+
+  Eigen::MatrixXd R_t(2, 2);
+  R_t << 0.09, 0, 0, 0.09;
+
+  Eigen::MatrixXd C_t(2, 2);
+  C_t << 1, 0, 0, 1;
+
+  Eigen::MatrixXd A_t(2, 2);
+  A_t << 1, 0, 0, 1;
+
+  Eigen::MatrixXd B_t(2, 2);
+  B_t << 1, 0, 0, 1;
+
+  // Initialize the filter and check if initialization was successful
+  EXPECT_TRUE(kf_.init(x_t, A_t, B_t, C_t, Q_t, R_t, P_t));
+
+  // Perform prediction
+  Eigen::MatrixXd u_t(2, 1);
+  u_t << 0.1, 0.1;
+  EXPECT_TRUE(kf_.predict(u_t));
+
+  // Check the updated state and covariance matrix
+  Eigen::MatrixXd x_predict_expected = A_t * x_t + B_t * u_t;
+  Eigen::MatrixXd P_predict_expected = A_t * P_t * A_t.transpose() + Q_t;
+
+  Eigen::MatrixXd x_predict;
+  kf_.getX(x_predict);
+  Eigen::MatrixXd P_predict;
+  kf_.getP(P_predict);
+
+  EXPECT_NEAR(x_predict(0, 0), x_predict_expected(0, 0), 1e-5);
+  EXPECT_NEAR(x_predict(1, 0), x_predict_expected(1, 0), 1e-5);
+  EXPECT_NEAR(P_predict(0, 0), P_predict_expected(0, 0), 1e-5);
+  EXPECT_NEAR(P_predict(1, 1), P_predict_expected(1, 1), 1e-5);
+
+  // Perform update
+  Eigen::MatrixXd y_t(2, 1);
+  y_t << 1.05, 2.05;
+  EXPECT_TRUE(kf_.update(y_t));
+
+  // Check the updated state and covariance matrix
+  const Eigen::MatrixXd PCT_t = P_predict_expected * C_t.transpose();
+  const Eigen::MatrixXd K_t = PCT_t * ((R_t + C_t * PCT_t).inverse());
+  const Eigen::MatrixXd y_pred = C_t * x_predict_expected;
+  Eigen::MatrixXd x_update_expected = x_predict_expected + K_t * (y_t - y_pred);
+  Eigen::MatrixXd P_update_expected = P_predict_expected - K_t * (C_t * P_predict_expected);
+
+  Eigen::MatrixXd x_update;
+  kf_.getX(x_update);
+  Eigen::MatrixXd P_update;
+  kf_.getP(P_update);
+
+  EXPECT_NEAR(x_update(0, 0), x_update_expected(0, 0), 1e-5);
+  EXPECT_NEAR(x_update(1, 0), x_update_expected(1, 0), 1e-5);
+  EXPECT_NEAR(P_update(0, 0), P_update_expected(0, 0), 1e-5);
+  EXPECT_NEAR(P_update(1, 1), P_update_expected(1, 1), 1e-5);
+}
+
+int main(int argc, char * argv[])
+{
+  testing::InitGoogleTest(&argc, argv);
+  bool result = RUN_ALL_TESTS();
+  return result;
+}

common/kalman_filter/test/test_time_delay_kalman_filter.cpp

@@ -0,0 +1,121 @@
+// Copyright 2023 The 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 "kalman_filter/time_delay_kalman_filter.hpp"
+
+#include <gtest/gtest.h>
+
+TEST(time_delay_kalman_filter, td_kf)
+{
+  TimeDelayKalmanFilter td_kf_;
+
+  Eigen::MatrixXd x_t(3, 1);
+  x_t << 1.0, 2.0, 3.0;
+  Eigen::MatrixXd P_t(3, 3);
+  P_t << 0.1, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.3;
+  const int max_delay_step = 5;
+  const int dim_x = x_t.rows();
+  const int dim_x_ex = dim_x * max_delay_step;
+  // Initialize the filter
+  td_kf_.init(x_t, P_t, max_delay_step);
+
+  // Check if initialization was successful
+  Eigen::MatrixXd x_init = td_kf_.getLatestX();
+  Eigen::MatrixXd P_init = td_kf_.getLatestP();
+  Eigen::MatrixXd x_ex_t = Eigen::MatrixXd::Zero(dim_x_ex, 1);
+  Eigen::MatrixXd P_ex_t = Eigen::MatrixXd::Zero(dim_x_ex, dim_x_ex);
+  for (int i = 0; i < max_delay_step; ++i) {
+    x_ex_t.block(i * dim_x, 0, dim_x, 1) = x_t;
+    P_ex_t.block(i * dim_x, i * dim_x, dim_x, dim_x) = P_t;
+  }
+
+  EXPECT_EQ(x_init.rows(), 3);
+  EXPECT_EQ(x_init.cols(), 1);
+  EXPECT_EQ(P_init.rows(), 3);
+  EXPECT_EQ(P_init.cols(), 3);
+  EXPECT_NEAR(x_init(0, 0), x_t(0, 0), 1e-5);
+  EXPECT_NEAR(x_init(1, 0), x_t(1, 0), 1e-5);
+  EXPECT_NEAR(x_init(2, 0), x_t(2, 0), 1e-5);
+  EXPECT_NEAR(P_init(0, 0), P_t(0, 0), 1e-5);
+  EXPECT_NEAR(P_init(1, 1), P_t(1, 1), 1e-5);
+  EXPECT_NEAR(P_init(2, 2), P_t(2, 2), 1e-5);
+
+  // Define prediction parameters
+  Eigen::MatrixXd A_t(3, 3);
+  A_t << 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 2.0;
+  Eigen::MatrixXd Q_t(3, 3);
+  Q_t << 0.01, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, 0.03;
+  Eigen::MatrixXd x_next(3, 1);
+  x_next << 2.0, 4.0, 6.0;
+
+  // Perform prediction
+  EXPECT_TRUE(td_kf_.predictWithDelay(x_next, A_t, Q_t));
+
+  // Check the prediction state and covariance matrix
+  Eigen::MatrixXd x_predict = td_kf_.getLatestX();
+  Eigen::MatrixXd P_predict = td_kf_.getLatestP();
+  Eigen::MatrixXd x_tmp = Eigen::MatrixXd::Zero(dim_x_ex, 1);
+  x_tmp.block(0, 0, dim_x, 1) = A_t * x_t;
+  x_tmp.block(dim_x, 0, dim_x_ex - dim_x, 1) = x_ex_t.block(0, 0, dim_x_ex - dim_x, 1);
+  x_ex_t = x_tmp;
+  Eigen::MatrixXd x_predict_expected = x_ex_t.block(0, 0, dim_x, 1);
+  Eigen::MatrixXd P_tmp = Eigen::MatrixXd::Zero(dim_x_ex, dim_x_ex);
+  P_tmp.block(0, 0, dim_x, dim_x) = A_t * P_ex_t.block(0, 0, dim_x, dim_x) * A_t.transpose() + Q_t;
+  P_tmp.block(0, dim_x, dim_x, dim_x_ex - dim_x) =
+    A_t * P_ex_t.block(0, 0, dim_x, dim_x_ex - dim_x);
+  P_tmp.block(dim_x, 0, dim_x_ex - dim_x, dim_x) =
+    P_ex_t.block(0, 0, dim_x_ex - dim_x, dim_x) * A_t.transpose();
+  P_tmp.block(dim_x, dim_x, dim_x_ex - dim_x, dim_x_ex - dim_x) =
+    P_ex_t.block(0, 0, dim_x_ex - dim_x, dim_x_ex - dim_x);
+  P_ex_t = P_tmp;
+  Eigen::MatrixXd P_predict_expected = P_ex_t.block(0, 0, dim_x, dim_x);
+  EXPECT_NEAR(x_predict(0, 0), x_predict_expected(0, 0), 1e-5);
+  EXPECT_NEAR(x_predict(1, 0), x_predict_expected(1, 0), 1e-5);
+  EXPECT_NEAR(x_predict(2, 0), x_predict_expected(2, 0), 1e-5);
+  EXPECT_NEAR(P_predict(0, 0), P_predict_expected(0, 0), 1e-5);
+  EXPECT_NEAR(P_predict(1, 1), P_predict_expected(1, 1), 1e-5);
+  EXPECT_NEAR(P_predict(2, 2), P_predict_expected(2, 2), 1e-5);
+
+  // Define update parameters
+  Eigen::MatrixXd C_t(3, 3);
+  C_t << 0.5, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.5;
+  Eigen::MatrixXd R_t(3, 3);
+  R_t << 0.001, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.003;
+  Eigen::MatrixXd y_t(3, 1);
+  y_t << 1.05, 2.05, 3.05;
+  const int delay_step = 2;  // Choose an appropriate delay step
+  const int dim_y = y_t.rows();
+
+  // Perform update
+  EXPECT_TRUE(td_kf_.updateWithDelay(y_t, C_t, R_t, delay_step));
+
+  // Check the updated state and covariance matrix
+  Eigen::MatrixXd x_update = td_kf_.getLatestX();
+  Eigen::MatrixXd P_update = td_kf_.getLatestP();
+
+  Eigen::MatrixXd C_ex_t = Eigen::MatrixXd::Zero(dim_y, dim_x_ex);
+  const Eigen::MatrixXd PCT_t = P_ex_t * C_ex_t.transpose();
+  const Eigen::MatrixXd K_t = PCT_t * ((R_t + C_ex_t * PCT_t).inverse());
+  const Eigen::MatrixXd y_pred = C_ex_t * x_ex_t;
+  x_ex_t = x_ex_t + K_t * (y_t - y_pred);
+  P_ex_t = P_ex_t - K_t * (C_ex_t * P_ex_t);
+  Eigen::MatrixXd x_update_expected = x_ex_t.block(0, 0, dim_x, 1);
+  Eigen::MatrixXd P_update_expected = P_ex_t.block(0, 0, dim_x, dim_x);
+  EXPECT_NEAR(x_update(0, 0), x_update_expected(0, 0), 1e-5);
+  EXPECT_NEAR(x_update(1, 0), x_update_expected(1, 0), 1e-5);
+  EXPECT_NEAR(x_update(2, 0), x_update_expected(2, 0), 1e-5);
+  EXPECT_NEAR(P_update(0, 0), P_update_expected(0, 0), 1e-5);
+  EXPECT_NEAR(P_update(1, 1), P_update_expected(1, 1), 1e-5);
+  EXPECT_NEAR(P_update(2, 2), P_update_expected(2, 2), 1e-5);
+}

common/motion_utils/include/motion_utils/trajectory/trajectory.hpp

@@ -177,8 +177,7 @@ T removeOverlapPoints(const T & points, const size_t start_idx = 0)
   for (size_t i = start_idx + 1; i < points.size(); ++i) {
     const auto prev_p = tier4_autoware_utils::getPoint(dst.back());
     const auto curr_p = tier4_autoware_utils::getPoint(points.at(i));
-    const double dist = tier4_autoware_utils::calcDistance2d(prev_p, curr_p);
-    if (dist < eps) {
+    if (std::abs(prev_p.x - curr_p.x) < eps && std::abs(prev_p.y - curr_p.y) < eps) {
       continue;
     }
     dst.push_back(points.at(i));