chore: sync upstream

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

launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml

@@ -6,7 +6,7 @@
       keep_detection_line_margin: 1.0 # distance (toward path end) from generated stop line. keep detection if ego is before this line and ego.vel < keep_detection_vel_thr
       keep_detection_vel_thr: 0.833 # == 3.0km/h
       stuck_vehicle_detect_dist: 3.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length.
-      stuck_vehicle_ignore_dist: 7.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-)
+      stuck_vehicle_ignore_dist: 10.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-)
       stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h
       intersection_velocity: 2.778 # 2.778m/s = 10.0km/h
       intersection_max_accel: 0.5 # m/ss

perception/traffic_light_classifier/utils/trt_common.cpp

@@ -75,8 +75,16 @@ void TrtCommon::setup()
   }
 
   context_ = UniquePtr<nvinfer1::IExecutionContext>(engine_->createExecutionContext());
-  input_dims_ = engine_->getBindingDimensions(getInputBindingIndex());
-  output_dims_ = engine_->getBindingDimensions(getOutputBindingIndex());
+
+#if (NV_TENSORRT_MAJOR * 10000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 80500
+  input_dims_ = engine_->getTensorShape(input_name_.c_str());
+  output_dims_ = engine_->getTensorShape(output_name_.c_str());
+#else
+  // Deprecated since 8.5
+  input_dims_ = engine_->getBindingDimensions(engine_->getBindingIndex(input_name_.c_str()));
+  output_dims_ = engine_->getBindingDimensions(engine_->getBindingIndex(output_name_.c_str()));
+#endif
+
   is_initialized_ = true;
 }
 
@@ -155,8 +163,4 @@ int TrtCommon::getNumOutput()
     output_dims_.d, output_dims_.d + output_dims_.nbDims, 1, std::multiplies<int>());
 }
 
-int TrtCommon::getInputBindingIndex() { return engine_->getBindingIndex(input_name_.c_str()); }
-
-int TrtCommon::getOutputBindingIndex() { return engine_->getBindingIndex(output_name_.c_str()); }
-
 }  // namespace Tn

perception/traffic_light_classifier/utils/trt_common.hpp

@@ -118,8 +118,6 @@ class TrtCommon
   bool isInitialized();
   int getNumInput();
   int getNumOutput();
-  int getInputBindingIndex();
-  int getOutputBindingIndex();
 
   UniquePtr<nvinfer1::IExecutionContext> context_;
 

planning/behavior_path_planner/README.md

@@ -54,28 +54,6 @@ The following modules are currently supported:
 
 ## Inner-workings / Algorithms
 
-### Drivable Area Generation
-
-Drivable lanes are quantized and drawn on an image as a drivable area, whose resolution is `drivable_area_resolution`.
-To prevent the quantization from causing instability to the planning modules, drivable area's pose follows the rules below.
-
-- Drivable area is generated in the map coordinate.
-- Its position is quantized with `drivable_area_resolution`.
-- Its orientation is 0.
-
-The size of the drivable area changes dynamically to realize both decreasing the computation cost and covering enough lanes to follow.
-For the second purpose, the drivable area covers a certain length forward and backward lanes with some margins defined by parameters.
-
-#### Parameters for drivable area generation
-
-| Name                          | Unit | Type   | Description                                                                | Default value |
-| :---------------------------- | :--- | :----- | :------------------------------------------------------------------------- | :------------ |
-| drivable_area_resolution      | [m]  | double | resolution of the image of the drivable area                               | 0.1           |
-| drivable_lane_forward_length  | [m]  | double | length of the forward lane from the ego covered by the drivable area       | 50.0          |
-| drivable_lane_backward_length | [m]  | double | length of the backward lane from the ego covered by the drivable area      | 5.0           |
-| drivable_lane_margin          | [m]  | double | forward and backward lane margin from the ego covered by the drivable area | 3.0           |
-| drivable_area_margin          | [m]  | double | margin of width and height of the drivable area                            | 6.0           |
-
 ### Behavior Tree
 
 In the behavior path planner, the behavior tree mechanism is used to manage which modules are activated in which situations. In general, this "behavior manager" like function is expected to become bigger as more and more modules are added in the future. To improve maintainability, we adopted the behavior tree. The behavior tree has the following advantages: easy visualization, easy configuration management (behaviors can be changed by replacing configuration files), and high scalability compared to the state machine.

planning/behavior_path_planner/behavior_path_planner_avoidance-design.md

@@ -422,6 +422,7 @@ The avoidance specific parameter configuration file can be located at `src/autow
 | enable_avoidance_over_same_direction       | [-]    | bool   | Extend avoidance trajectory to adjacent lanes that has same direction. If false, avoidance only happen in current lane.                          | true          |
 | enable_avoidance_over_opposite_direction   | [-]    | bool   | Extend avoidance trajectory to adjacent lanes that has opposite direction. `enable_avoidance_over_same_direction` must be `true` to take effects | true          |
 | enable_update_path_when_object_is_gone     | [-]    | bool   | Reset trajectory when avoided objects are gone. If false, shifted path points remain same even though the avoided objects are gone.              | false         |
+| enable_bound_clipping                      | `true` | bool   | Enable clipping left and right bound of drivable area when obstacles are in the drivable area                                                    | false         |
 
 (\*2) If there are multiple vehicles in a row to be avoided, no new avoidance path will be generated unless their lateral margin difference exceeds this value.
 
@@ -512,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

@@ -0,0 +1,92 @@
+# Drivable Area
+
+Drivable Area represents the area where ego vehicle can pass.
+
+## Purpose / Role
+
+In order to defined the area that ego vehicle can travel safely, we generate drivable area in behavior path planner module. Our drivable area is represented by two line strings, which are `left_bound` line and `right_bound` line respectively. Both `left_bound` and `right_bound` are created from left and right boundaries of lanelets. Note that `left_bound` and `right bound` are generated by `generateDrivableArea` function.
+
+## Assumption
+
+Our drivable area has several assumptions.
+
+- Drivable Area should have all of the necessary area but should not represent unnecessary area for current behaviors. For example, when ego vehicle is in `follow lane` mode, drivable area should not contain adjacent lanes.
+
+- When generating a drivable area, lanes need to be arranged in the order in which cars pass by (More details can be found in following sections).
+
+- Both left and right bounds should cover the front of the path and the end of the path.
+
+## Limitations
+
+Currently, when clipping left bound or right bound, it can clip the bound more than necessary and the generated path might be conservative.
+
+## Parameters for drivable area generation
+
+| Name                             | Unit | Type   | Description                                 | Default value |
+| :------------------------------- | :--- | :----- | :------------------------------------------ | :------------ |
+| drivable_area_right_bound_offset | [m]  | double | right offset length to expand drivable area | 5.0           |
+| drivable_area_left_bound_offset  | [m]  | double | left offset length to expand drivable area  | 5.0           |
+
+Note that default values can be varied by the module. Please refer to the `config/drivable_area_expansion.yaml` file.
+
+## Inner-workings / Algorithms
+
+This section gives details of the generation of the drivable area (`left_bound` and `right_bound`).
+
+### Drivable Lanes Generation
+
+Before generating drivable areas, drivable lanes need to be sorted. Drivable Lanes are selected in each module (`Lane Follow`, `Avoidance`, `Lane Change`, `Pull Over`, `Pull Out` and etc.), so more details about selection of drivable lanes can be found in each module's document. We use the following structure to define the drivable lanes.
+
+```plantuml
+struct DrivalbleLanes
+{
+    lanelet::ConstLanelet right_lanelet; // right most lane
+    lanelet::ConstLanelet left_lanelet; // left most lane
+    lanelet::ConstLanelets middle_lanelets; // middle lanes
+};
+```
+
+The image of the sorted drivable lanes is depicted in the following picture.
+
+![sorted_lanes](./image/drivable_area/sorted_lanes.drawio.svg)
+
+Note that, the order of drivable lanes become
+
+```plantuml
+drivable_lanes = {DrivableLane1, DrivableLanes2, DrivableLanes3, DrivableLanes4, DrivableLanes5}
+```
+
+### Drivable Area Expansion
+
+Each module can expand the drivable area based on parameters. It expands right bound and left bound of target lanes. This enables large vehicles to pass narrow curve. The image of this process can be described as
+
+![expanded_lanes](./image/drivable_area/expanded_lanes.drawio.svg)
+
+Note that we only expand right bound of the rightmost lane and left bound of the leftmost lane.
+
+### Drivable Area Generation
+
+In this section, a drivable area is created using drivable lanes arranged in the order in which vehicles pass by. We created `left_bound` from left boundary of the leftmost lanelet and `right_bound` from right boundary of the rightmost lanelet. The image of the created drivable area will be the following blue lines. Note that the drivable area is defined in the `Path` and `PathWithLaneId` messages as
+
+```plantuml
+std::vector<geometry_msgs::msg::Point> left_bound;
+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