[pull] main from autowarefoundation:main

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

map/map_projection_loader/README.md

@@ -25,7 +25,7 @@ sample-map-rosbag
 
 ```yaml
 # map_projector_info.yaml
-type: "local"
+projector_type: local
 ```
 
 ### Using MGRS

planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp

@@ -242,6 +242,11 @@ class PlannerManager
    */
   void print() const;
 
+  /**
+   * @brief publish processing time of each module.
+   */
+  void publishProcessingTime() const;
+
   /**
    * @brief visit each module and get debug information.
    */

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -579,6 +579,7 @@ void BehaviorPathPlannerNode::run()
   lk_pd.unlock();  // release planner_data_
 
   planner_manager_->print();
+  planner_manager_->publishProcessingTime();
   planner_manager_->publishMarker();
   planner_manager_->publishVirtualWall();
   lk_manager.unlock();  // release planner_manager_

planning/behavior_path_planner/src/planner_manager.cpp

@@ -886,13 +886,19 @@ void PlannerManager::print() const
     string_stream << std::right << "[" << std::setw(max_string_num + 1) << std::left << t.first
                   << ":" << std::setw(4) << std::right << t.second << "ms]\n"
                   << std::setw(21);
-    std::string name = std::string("processing_time/") + t.first;
-    debug_publisher_ptr_->publish<DebugDoubleMsg>(name, t.second);
   }
 
   RCLCPP_INFO_STREAM(logger_, string_stream.str());
 }
 
+void PlannerManager::publishProcessingTime() const
+{
+  for (const auto & t : processing_time_) {
+    std::string name = std::string("processing_time/") + t.first;
+    debug_publisher_ptr_->publish<DebugDoubleMsg>(name, t.second);
+  }
+}
+
 std::shared_ptr<SceneModuleVisitor> PlannerManager::getDebugMsg()
 {
   debug_msg_ptr_ = std::make_shared<SceneModuleVisitor>();

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -25,6 +25,7 @@
 #include <tier4_autoware_utils/geometry/geometry.hpp>
 #include <tier4_autoware_utils/ros/uuid_helper.hpp>
 
+#include <boost/geometry/algorithms/convex_hull.hpp>
 #include <boost/geometry/algorithms/correct.hpp>
 #include <boost/geometry/algorithms/intersection.hpp>
 
@@ -43,6 +44,32 @@ namespace behavior_velocity_planner
 {
 namespace bg = boost::geometry;
 
+namespace
+{
+Polygon2d createOneStepPolygon(
+  const geometry_msgs::msg::Pose & prev_pose, const geometry_msgs::msg::Pose & next_pose,
+  const autoware_auto_perception_msgs::msg::Shape & shape)
+{
+  const auto prev_poly = tier4_autoware_utils::toPolygon2d(prev_pose, shape);
+  const auto next_poly = tier4_autoware_utils::toPolygon2d(next_pose, shape);
+
+  Polygon2d one_step_poly;
+  for (const auto & point : prev_poly.outer()) {
+    one_step_poly.outer().push_back(point);
+  }
+  for (const auto & point : next_poly.outer()) {
+    one_step_poly.outer().push_back(point);
+  }
+
+  bg::correct(one_step_poly);
+
+  Polygon2d convex_one_step_poly;
+  bg::convex_hull(one_step_poly, convex_one_step_poly);
+
+  return convex_one_step_poly;
+}
+}  // namespace
+
 static bool isTargetCollisionVehicleType(
   const autoware_auto_perception_msgs::msg::PredictedObject & object)
 {
@@ -877,7 +904,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
     util::getTrafficPrioritizedLevel(assigned_lanelet, planner_data_->traffic_light_id_map);
   const bool is_prioritized =
     traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED;
-  intersection_lanelets.update(is_prioritized, interpolated_path_info);
+  const auto footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0);
+  intersection_lanelets.update(is_prioritized, interpolated_path_info, footprint);
 
   // this is abnormal
   const auto & conflicting_lanelets = intersection_lanelets.conflicting();
@@ -1185,6 +1213,11 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
 bool IntersectionModule::checkStuckVehicle(
   const std::shared_ptr<const PlannerData> & planner_data, const util::PathLanelets & path_lanelets)
 {
+  // NOTE: No need to stop for stuck vehicle when the ego will turn left.
+  if (turn_direction_ == std::string("left")) {
+    return false;
+  }
+
   const auto & objects_ptr = planner_data->predicted_objects;
 
   // considering lane change in the intersection, these lanelets are generated from the path
@@ -1312,14 +1345,14 @@ bool IntersectionModule::checkCollision(
       // collision point
       const auto first_itr = std::adjacent_find(
         predicted_path.path.cbegin(), predicted_path.path.cend(),
-        [&ego_poly](const auto & a, const auto & b) {
-          return bg::intersects(ego_poly, LineString2d{to_bg2d(a), to_bg2d(b)});
+        [&ego_poly, &object](const auto & a, const auto & b) {
+          return bg::intersects(ego_poly, createOneStepPolygon(a, b, object.shape));
         });
       if (first_itr == predicted_path.path.cend()) continue;
       const auto last_itr = std::adjacent_find(
         predicted_path.path.crbegin(), predicted_path.path.crend(),
-        [&ego_poly](const auto & a, const auto & b) {
-          return bg::intersects(ego_poly, LineString2d{to_bg2d(a), to_bg2d(b)});
+        [&ego_poly, &object](const auto & a, const auto & b) {
+          return bg::intersects(ego_poly, createOneStepPolygon(a, b, object.shape));
         });
       if (last_itr == predicted_path.path.crend()) continue;
 

planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp

@@ -94,8 +94,10 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR
       planner_param_.attention_area_length, planner_param_.occlusion_attention_area_length,
       planner_param_.consider_wrong_direction_vehicle);
   }
-  intersection_lanelets_.value().update(false, interpolated_path_info);
-  const auto & first_conflicting_area = intersection_lanelets_.value().first_conflicting_area();
+  auto & intersection_lanelets = intersection_lanelets_.value();
+  const auto local_footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0);
+  intersection_lanelets.update(false, interpolated_path_info, local_footprint);
+  const auto & first_conflicting_area = intersection_lanelets.first_conflicting_area();
   if (!first_conflicting_area) {
     return false;
   }