Merge pull request #993 from tier4/beta-to-tier4-main-sync

chore: sync beta branch beta/v0.15.0 with tier4/main

.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]
@@ -140,6 +141,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]
@@ -181,7 +183,6 @@ planning/planning_debug_tools/** [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,6 +216,7 @@ 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]

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/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/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));

common/motion_utils/include/motion_utils/vehicle/vehicle_state_checker.hpp

@@ -55,11 +55,11 @@ class VehicleStopChecker : public VehicleStopCheckerBase
 
 protected:
   rclcpp::Subscription<Odometry>::SharedPtr sub_odom_;
-  Odometry::SharedPtr odometry_ptr_;
+  Odometry::ConstSharedPtr odometry_ptr_;
 
 private:
   static constexpr double velocity_buffer_time_sec = 10.0;
-  void onOdom(const Odometry::SharedPtr msg);
+  void onOdom(const Odometry::ConstSharedPtr msg);
 };
 
 class VehicleArrivalChecker : public VehicleStopChecker
@@ -74,9 +74,9 @@ class VehicleArrivalChecker : public VehicleStopChecker
 
   rclcpp::Subscription<Trajectory>::SharedPtr sub_trajectory_;
 
-  Trajectory::SharedPtr trajectory_ptr_;
+  Trajectory::ConstSharedPtr trajectory_ptr_;
 
-  void onTrajectory(const Trajectory::SharedPtr msg);
+  void onTrajectory(const Trajectory::ConstSharedPtr msg);
 };
 }  // namespace motion_utils
 

common/motion_utils/src/vehicle/vehicle_state_checker.cpp

@@ -88,7 +88,7 @@ VehicleStopChecker::VehicleStopChecker(rclcpp::Node * node)
     std::bind(&VehicleStopChecker::onOdom, this, _1));
 }
 
-void VehicleStopChecker::onOdom(const Odometry::SharedPtr msg)
+void VehicleStopChecker::onOdom(const Odometry::ConstSharedPtr msg)
 {
   odometry_ptr_ = msg;
 
@@ -128,7 +128,7 @@ bool VehicleArrivalChecker::isVehicleStoppedAtStopPoint(const double stop_durati
          th_arrived_distance_m;
 }
 
-void VehicleArrivalChecker::onTrajectory(const Trajectory::SharedPtr msg)
+void VehicleArrivalChecker::onTrajectory(const Trajectory::ConstSharedPtr msg)
 {
   trajectory_ptr_ = msg;
 }

common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp

@@ -261,29 +261,29 @@ inline void setOrientation(const geometry_msgs::msg::Quaternion & orientation, T
 }
 
 template <class T>
-void setLongitudinalVelocity([[maybe_unused]] const double velocity, [[maybe_unused]] T & p)
+void setLongitudinalVelocity([[maybe_unused]] const float velocity, [[maybe_unused]] T & p)
 {
   static_assert(sizeof(T) == 0, "Only specializations of getLongitudinalVelocity can be used.");
   throw std::logic_error("Only specializations of getLongitudinalVelocity can be used.");
 }
 
 template <>
 inline void setLongitudinalVelocity(
-  const double velocity, autoware_auto_planning_msgs::msg::TrajectoryPoint & p)
+  const float velocity, autoware_auto_planning_msgs::msg::TrajectoryPoint & p)
 {
   p.longitudinal_velocity_mps = velocity;
 }
 
 template <>
 inline void setLongitudinalVelocity(
-  const double velocity, autoware_auto_planning_msgs::msg::PathPoint & p)
+  const float velocity, autoware_auto_planning_msgs::msg::PathPoint & p)
 {
   p.longitudinal_velocity_mps = velocity;
 }
 
 template <>
 inline void setLongitudinalVelocity(
-  const double velocity, autoware_auto_planning_msgs::msg::PathPointWithLaneId & p)
+  const float velocity, autoware_auto_planning_msgs::msg::PathPointWithLaneId & p)
 {
   p.point.longitudinal_velocity_mps = velocity;
 }

common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml

@@ -14,6 +14,10 @@ behavior_path_planner_avoidance:
   - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner
     logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.avoidance
 
+behavior_path_planner_lane_change:
+  - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner
+    logger_name: lane_change
+
 behavior_velocity_planner:
   - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner
     logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner

common/tier4_system_rviz_plugin/README.md

@@ -1 +1,11 @@
 # tier4_system_rviz_plugin
+
+## Purpose
+
+This plugin display the Hazard information from Autoware; and output notices when emergencies are from initial localization and route setting.
+
+## Input
+
+| Name                              | Type                                                  | Description                                                  |
+| --------------------------------- | ----------------------------------------------------- | ------------------------------------------------------------ |
+| `/system/emergency/hazard_status` | `autoware_auto_system_msgs::msg::HazardStatusStamped` | The topic represents the emergency information from Autoware |