Skip to content

Commit 9c7de00

Browse files
fix name, add doc
Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>
1 parent 40ca4f1 commit 9c7de00

File tree

2 files changed

+23
-7
lines changed

2 files changed

+23
-7
lines changed

planning/behavior_path_dynamic_avoidance_module/README.md

+19-3
Original file line numberDiff line numberDiff line change
@@ -32,7 +32,7 @@ The other, _can be avoided_ denotes whether it can be avoided without risk to th
3232
For this purpose, the module judges whether the obstacle can be avoided with satisfying the constraints of lateral acceleration and lateral jerk.
3333
For example, the module decides not to avoid an object that is too close or fast in the lateral direction.
3434

35-
### Cuts off the drivable area against the selected obstacles
35+
### Cuts off the drivable area against the selected vehicless
3636
For the selected obstacles to be avoided, the module cuts off the drivable area.
3737
As inputs to decide the shapes of cut-off polygons, poses of the obstacles are mainly used, assuming they move in parallel to the ego's path, instead of its predicted path.
3838
This design arises from that the predicted path of objects is not accurate enough to use the path modifications (at least currently).
@@ -46,7 +46,7 @@ We can limit the lateral shift length by `drivable_area_generation.max_lat_offse
4646
![drivable_area_extraction_width](./image/drivable_area_extraction_width.drawio.svg)
4747

4848
#### Determination of longitudinal dimension
49-
Then, extracting the same directional and opposite directional obstacles from the drivable area will work as follows, considering TTC (time to collision).
49+
Then, extracting the same directional and opposite directional obstacles from the drivable area will work as follows considering TTC (time to collision).
5050

5151
Regarding the same directional obstacles, obstacles whose TTC is negative will be ignored (e.g., The obstacle is in front of the ego, and the obstacle's velocity is larger than the ego's velocity.).
5252

@@ -57,7 +57,22 @@ Opposite directional obstacles (Parameter names may differ from implementation)
5757
![opposite_directional_object](./image/opposite_directional_object.svg)
5858

5959
### Cuts off the drivable area against the selected pedestrians
60-
while the
60+
Then, we describe the logic to generate the drivable areas against pedestrians to be avoided.
61+
Objects of this type are considered to have priority right of way over the ego's vehicle while ensuring a minimum safety of the ego's vehicle.
62+
In other words, the module assigns a drivable area to an obstacle with a specific margin based on the predicted paths with specific confidences for a specific time interval, as shown in the following figure.
63+
<figure>
64+
<img src="./image/2024-04-18_15-13-01.png" width="600">
65+
<figcaption> Drivable areas are generated from each pedestrian's predicted paths</figcaption>
66+
</figure>
67+
68+
Apart from polygons for objects, the module also generates another polygon to ensure the ego's safety, i.e., to avoid abrupt steering or significant changes from the path.
69+
This is similar to avoidance against the vehicles and takes precedence over keeping a safe distance from the object to be avoided.
70+
As a result, as shown in the figure below, the polygons around the objects reduced by the secured polygon of the ego are subtracted from the ego's drivable area.
71+
<figure>
72+
<img src="./image/2024-04-18_15-32-03.png" width="600">
73+
<figcaption> Drivable areas are generated from each pedestrian's predicted paths</figcaption>
74+
</figure>
75+
6176

6277
## Example
6378

@@ -82,6 +97,7 @@ while the
8297
</figure>
8398

8499
## Future works
100+
85101
Currently, the path shifting length is limited to 0.5 meters or less by `drivable_area_generation.max_lat_offset_to_avoid`.
86102
This is caused by the lack of functionality to work with other modules and the structure of the planning component.
87103
Due to this issue, this module can only handle situations where a small avoidance width is sufficient.

planning/behavior_path_dynamic_avoidance_module/src/manager.cpp

+4-4
Original file line numberDiff line numberDiff line change
@@ -99,8 +99,8 @@ void DynamicAvoidanceModuleManager::init(rclcpp::Node * node)
9999
node->declare_parameter<double>(ns + "crossing_object.min_oncoming_object_vel");
100100
p.max_oncoming_crossing_object_angle =
101101
node->declare_parameter<double>(ns + "crossing_object.max_oncoming_object_angle");
102-
p.max_pedestrians_crossing_vel =
103-
node->declare_parameter<double>(ns + "crossing_object.max_pedestrians_crossing_vel");
102+
p.max_pedestrian_crossing_vel =
103+
node->declare_parameter<double>(ns + "crossing_object.max_pedestrian_crossing_vel");
104104

105105
p.max_stopped_object_vel =
106106
node->declare_parameter<double>(ns + "stopped_object.max_object_vel");
@@ -223,8 +223,8 @@ void DynamicAvoidanceModuleManager::updateModuleParams(
223223
parameters, ns + "crossing_object.max_oncoming_object_angle",
224224
p->max_oncoming_crossing_object_angle);
225225
updateParam<double>(
226-
parameters, ns + "crossing_object.max_pedestrians_crossing_vel",
227-
p->max_pedestrians_crossing_vel);
226+
parameters, ns + "crossing_object.max_pedestrian_crossing_vel",
227+
p->max_pedestrian_crossing_vel);
228228

229229
updateParam<double>(
230230
parameters, ns + "stopped_object.max_object_vel", p->max_stopped_object_vel);

0 commit comments

Comments
 (0)