[pull] main from autowarefoundation:main

launch/tier4_control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml

@@ -3,6 +3,8 @@
     transition_timeout: 10.0
     frequency_hz: 10.0
     check_engage_condition: false # set false if you do not want to care about the engage condition.
+    nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index
+    nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index
     engage_acceptable_limits:
       allow_autonomous_in_stopped: true  # no check if the velocity is zero, always allowed.
       dist_threshold: 1.5

planning/behavior_path_planner/behavior_path_planner_avoidance-design.md

@@ -513,25 +513,3 @@ To print the debug message, just run the following
 ```bash
 ros2 topic echo /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/avoidance_debug_message_array
 ```
-
-### Visualizing drivable area boundary
-
-Sometimes, the developers might get a different result between two maps that may look identical during visual inspection.
-
-For example, in the same area, one can perform avoidance and another cannot. This might be related to the drivable area issues due to the non-compliance vector map design from the user.
-
-To debug the issue, the drivable area boundary can be visualized.
-
-![drivable_area_boundary_marker1](./image/avoidance_design/drivable_area_boundary_marker_example1.png)
-
-![drivable_area_boundary_marker2](./image/avoidance_design/drivable_area_boundary_marker_example2.png)
-
-The boundary can be visualize by adding the marker from `/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_boundary`
-
-### Visualizing drivable area
-
-The drivable area can be visualize by adding the drivable area plugin
-
-![drivable_area_plugin](./image/drivable_area_plugin.png)
-
-and then add `/planning/scenario_planning/lane_driving/behavior_planning/path` as the topic.

planning/behavior_path_planner/behavior_path_planner_drivable_area.md

@@ -76,3 +76,17 @@ std::vector<geometry_msgs::msg::Point> right_bound;
 and each point of right bound and left bound has a position in the absolute coordinate system.
 
 ![drivable_lines](./image/drivable_area/drivable_lines.drawio.svg)
+
+### Visualizing maximum drivable area (Debug)
+
+Sometimes, the developers might get a different result between two maps that may look identical during visual inspection.
+
+For example, in the same area, one can perform avoidance and another cannot. This might be related to the maximum drivable area issues due to the non-compliance vector map design from the user.
+
+To debug the issue, the maximum drivable area boundary can be visualized.
+
+![drivable_area_boundary_marker1](./image/drivable_area/drivable_area_boundary_marker_example1.png)
+
+![drivable_area_boundary_marker2](./image/drivable_area/drivable_area_boundary_marker_example2.png)
+
+The maximum drivable area can be visualize by adding the marker from `/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/maximum_drivable_area`

planning/behavior_path_planner/config/behavior_path_planner.param.yaml

@@ -21,7 +21,7 @@
 
     path_interval: 2.0
 
-    visualize_drivable_area_for_shared_linestrings_lanelet: true
+    visualize_maximum_drivable_area: true
 
     lateral_distance_max_threshold: 2.0
     longitudinal_distance_min_threshold: 3.0

planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

@@ -178,7 +178,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
     const BehaviorModuleOutput & output);
 
   // debug
-  rclcpp::Publisher<MarkerArray>::SharedPtr debug_drivable_area_lanelets_publisher_;
+  rclcpp::Publisher<MarkerArray>::SharedPtr debug_maximum_drivable_area_publisher_;
   rclcpp::Publisher<AvoidanceDebugMsgArray>::SharedPtr debug_avoidance_msg_array_publisher_;
   rclcpp::Publisher<LaneChangeDebugMsgArray>::SharedPtr debug_lane_change_msg_array_publisher_;
 

planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp

@@ -58,8 +58,8 @@ struct BehaviorPathPlannerParameters
   double base_link2front;
   double base_link2rear;
 
-  // drivable area visualization
-  bool visualize_drivable_area_for_shared_linestrings_lanelet;
+  // maximum drivable area visualization
+  bool visualize_maximum_drivable_area;
 
   // collision check
   double lateral_distance_max_threshold;

planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp

@@ -296,7 +296,7 @@ void generateDrivableArea(
   PathWithLaneId & path, const std::vector<DrivableLanes> & lanes, const double vehicle_length,
   const std::shared_ptr<const PlannerData> planner_data, const bool is_driving_forward = true);
 
-lanelet::ConstLineStrings3d getDrivableAreaForAllSharedLinestringLanelets(
+lanelet::ConstLineStrings3d getMaximumDrivableArea(
   const std::shared_ptr<const PlannerData> & planner_data);
 
 /**

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -69,9 +69,9 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
   debug_lane_change_msg_array_publisher_ =
     create_publisher<LaneChangeDebugMsgArray>("~/debug/lane_change_debug_message_array", 1);
 
-  if (planner_data_->parameters.visualize_drivable_area_for_shared_linestrings_lanelet) {
-    debug_drivable_area_lanelets_publisher_ =
-      create_publisher<MarkerArray>("~/drivable_area_boundary", 1);
+  if (planner_data_->parameters.visualize_maximum_drivable_area) {
+    debug_maximum_drivable_area_publisher_ =
+      create_publisher<MarkerArray>("~/maximum_drivable_area", 1);
   }
 
   bound_publisher_ = create_publisher<MarkerArray>("~/debug/bound", 1);
@@ -232,8 +232,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
   p.turn_signal_on_swerving = declare_parameter("turn_signal_on_swerving", true);
 
   p.path_interval = declare_parameter<double>("path_interval");
-  p.visualize_drivable_area_for_shared_linestrings_lanelet =
-    declare_parameter("visualize_drivable_area_for_shared_linestrings_lanelet", true);
+  p.visualize_maximum_drivable_area = declare_parameter("visualize_maximum_drivable_area", true);
   p.ego_nearest_dist_threshold = declare_parameter<double>("ego_nearest_dist_threshold");
   p.ego_nearest_yaw_threshold = declare_parameter<double>("ego_nearest_yaw_threshold");
 
@@ -655,10 +654,10 @@ void BehaviorPathPlannerNode::run()
 
   publishSceneModuleDebugMsg();
 
-  if (planner_data->parameters.visualize_drivable_area_for_shared_linestrings_lanelet) {
-    const auto drivable_area_lines = marker_utils::createFurthestLineStringMarkerArray(
-      util::getDrivableAreaForAllSharedLinestringLanelets(planner_data));
-    debug_drivable_area_lanelets_publisher_->publish(drivable_area_lines);
+  if (planner_data->parameters.visualize_maximum_drivable_area) {
+    const auto maximum_drivable_area =
+      marker_utils::createFurthestLineStringMarkerArray(util::getMaximumDrivableArea(planner_data));
+    debug_maximum_drivable_area_publisher_->publish(maximum_drivable_area);
   }
 
   mutex_bt_.unlock();

planning/behavior_path_planner/src/utilities.cpp

@@ -1665,7 +1665,7 @@ std::shared_ptr<PathWithLaneId> generateCenterLinePath(
 
 // TODO(Azu) Some parts of is the same with generateCenterLinePath. Therefore it might be better if
 // we can refactor some of the code for better readability
-lanelet::ConstLineStrings3d getDrivableAreaForAllSharedLinestringLanelets(
+lanelet::ConstLineStrings3d getMaximumDrivableArea(
   const std::shared_ptr<const PlannerData> & planner_data)
 {
   const auto & p = planner_data->parameters;