Merge pull request #413 from tier4/sync-upstream

chore: sync upstream
tier4 · May 9, 2023 · 6dafaf4 · 6dafaf4
2 parents c5985e5 + ce1255a
commit 6dafaf4
Show file tree

Hide file tree

Showing 77 changed files with 5,314 additions and 2,174 deletions.
diff --git a/.pages b/.pages
@@ -0,0 +1,2 @@
+nav:
+  - ... |  regex=^(?!.*(README.md|CONTRIBUTING.md|CODE_OF_CONDUCT.md|DISCLAIMER.md)).*$
diff --git a/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp b/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp
@@ -60,8 +60,14 @@ bool QPSolverOSQP::solve(
 
   const int status_val = std::get<3>(result);
   if (status_val != 1) {
-    // TODO(Horibe): Should return false and the failure must be handled in an appropriate way.
     RCLCPP_WARN(logger_, "optimization failed : %s", osqpsolver_.getStatusMessage().c_str());
+    return false;
+  }
+  const auto has_nan =
+    std::any_of(U_osqp.begin(), U_osqp.end(), [](const auto v) { return std::isnan(v); });
+  if (has_nan) {
+    RCLCPP_WARN(logger_, "optimization failed: result contains NaN values");
+    return false;
   }
 
   // polish status: successful (1), unperformed (0), (-1) unsuccessful

diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt
@@ -14,14 +14,14 @@ set(common_src
   src/behavior_path_planner_node.cpp
   src/scene_module/scene_module_visitor.cpp
   src/scene_module/avoidance/avoidance_module.cpp
-  src/scene_module/avoidance_by_lc/module.cpp
   src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp
   src/scene_module/pull_out/pull_out_module.cpp
   src/scene_module/goal_planner/goal_planner_module.cpp
   src/scene_module/side_shift/side_shift_module.cpp
   src/scene_module/lane_change/interface.cpp
   src/scene_module/lane_change/normal.cpp
   src/scene_module/lane_change/external_request.cpp
+  src/scene_module/lane_change/avoidance_by_lane_change.cpp
   src/turn_signal_decider.cpp
   src/utils/utils.cpp
   src/utils/path_utils.cpp
@@ -66,7 +66,6 @@ else()
   ament_auto_add_library(behavior_path_planner_node SHARED
     src/planner_manager.cpp
     src/scene_module/avoidance/manager.cpp
-    src/scene_module/avoidance_by_lc/manager.cpp
     src/scene_module/dynamic_avoidance/manager.cpp
     src/scene_module/pull_out/manager.cpp
     src/scene_module/goal_planner/manager.cpp

diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml
@@ -143,3 +143,11 @@
         matrix:
           [2.78, 13.9,                                  # velocity [m/s]
            0.50, 1.00]                                  # margin [m]
+
+      shift_line_pipeline:
+        trim:
+          quantize_filter_threshold: 0.2
+          same_grad_filter_1_threshold: 0.1
+          same_grad_filter_2_threshold: 0.2
+          same_grad_filter_3_threshold: 0.5
+          sharp_shift_filter_threshold: 0.2
diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_interface_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_interface_design.md
@@ -1 +1,5 @@
 # Interface design
+
+!!! warning
+
+    Under Construction
diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_manager_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_manager_design.md
diff --git a/planning/behavior_path_planner/image/manager/avoidance.svg b/planning/behavior_path_planner/image/manager/avoidance.svg
Original file line number	Diff line number	Diff line change
		@@ -0,0 +1,2 @@
		nav:
		- ... \| regex=^(?!.(README.md\|CONTRIBUTING.md\|CODE_OF_CONDUCT.md\|DISCLAIMER.md)).$