Skip to content

Commit 1ad6c07

Browse files
authored
Merge pull request #1266 from tier4/beta/v0.26.1+2b48d25+e5b3f60
feat(dynamic_avoidance): avoid pedestrians autowarefoundation#6553
2 parents 637a5cb + eae100f commit 1ad6c07

File tree

7 files changed

+557
-67
lines changed

7 files changed

+557
-67
lines changed

planning/behavior_path_dynamic_avoidance_module/README.md

+35-16
Original file line numberDiff line numberDiff line change
@@ -4,15 +4,16 @@ This module is under development.
44

55
## Purpose / Role
66

7-
This module provides avoidance functions for vehicles, pedestrians, and obstacles in the vicinity of the ego's path in combination with the obstacle_avoidance module.
7+
This module provides avoidance functions for vehicles, pedestrians, and obstacles in the vicinity of the ego's path in combination with the [obstacle_avoidance_planner](https://autowarefoundation.github.io/autoware.universe/main/planning/obstacle_avoidance_planner/).
88
Each module performs the following roles.
9-
Dynamic Avoidance module: This module cuts off the drivable area according to the position and velocity of the target to be avoided.
10-
Obstacle Avoidance module: This module modifies the path to be followed so that it fits within the drivable area received.
9+
Dynamic Avoidance module cuts off the drivable area according to the position and velocity of the target to be avoided.
10+
Obstacle Avoidance module modifies the path to be followed so that it fits within the received drivable area.
1111

12-
Avoidance functions are also provided by the Avoidance module, which allows avoidance through the outside of own lanes but not against moving objects.
12+
Avoidance functions are also provided by the [Avoidance module](https://autowarefoundation.github.io/autoware.universe/main/planning/behavior_path_avoidance_module/), but these modules have different roles.
13+
The Avoidance module performs avoidance through the outside of own lanes but cannot avoid the moving objects.
1314
On the other hand, this module can avoid moving objects.
14-
For this reason, the word "dynamic" is used in its name.
15-
The table below lists the avoidance modules that can be used for each situation.
15+
For this reason, the word "dynamic" is used in the module's name.
16+
The table below lists the avoidance modules that can handle each situation.
1617

1718
| | avoid within the own lane | avoid through the outside of own lanes |
1819
| :----------------------- | :------------------------------------------------------------------------: | :------------------------------------: |
@@ -22,22 +23,20 @@ The table below lists the avoidance modules that can be used for each situation.
2223
## Policy of algorithms
2324

2425
Here, we describe the policy of inner algorithms.
25-
The inner algorithms can be separated into two parts: The first decide whether to avoid the obstacles and the second cuts off the drivable area against the corresponding obstacle.
26-
If you are interested in more details, please see the code itself.
26+
The inner algorithms can be separated into two parts: The first decides whether to avoid the obstacles and the second cuts off the drivable area against the corresponding obstacle.
2727

2828
### Select obstacles to avoid
2929

3030
To decide whether to avoid an object, both the predicted path and the state (pose and twist) of each object are used.
3131
The type of objects the user wants this module to avoid is also required.
3232
Using this information, the module decides to _avoid_ objects that _obstruct the ego's passage_ and _can be avoided_.
3333

34-
The definition of _obstruct own passage_ is implemented as the object that collides within seconds.
35-
This process wastes computational cost by doing it for all objects; thus, filtering by the relative position and speed of the object with respect to the ego's path is also done as an auxiliary process.
36-
The other, _can be avoided_ denotes whether it can be avoided without risk to passengers or other vehicles.
37-
For this purpose, it is judged whether the obstacle can be avoided by satisfying the constraints of lateral acceleration and lateral jerk.
38-
For example, the module decides not to avoid an object that is too close or fast in the lateral direction because it cannot be avoided.
34+
The definition of _obstruct the ego's passage_ is implemented as the object that collides within seconds.
35+
The other, _can be avoided_ denotes whether it can be avoided without risk to the passengers or the other vehicles.
36+
For this purpose, the module judges whether the obstacle can be avoided with satisfying the constraints of lateral acceleration and lateral jerk.
37+
For example, the module decides not to avoid an object that is too close or fast in the lateral direction.
3938

40-
### Cuts off the drivable area against the selected obstacles
39+
### Cuts off the drivable area against the selected vehicles
4140

4241
For the selected obstacles to be avoided, the module cuts off the drivable area.
4342
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.
@@ -46,7 +45,7 @@ Furthermore, the output drivable area shape is designed as a rectangular cutout
4645

4746
#### Determination of lateral dimension
4847

49-
Lateral dimensions of the polygon is calculated as follows.
48+
The lateral dimensions of the polygon are calculated as follows.
5049
The polygon's width to extract from the drivable area is the obstacle width and `drivable_area_generation.lat_offset_from_obstacle`.
5150
We can limit the lateral shift length by `drivable_area_generation.max_lat_offset_to_avoid`.
5251

@@ -56,14 +55,34 @@ We can limit the lateral shift length by `drivable_area_generation.max_lat_offse
5655

5756
Then, extracting the same directional and opposite directional obstacles from the drivable area will work as follows considering TTC (time to collision).
5857

59-
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.).
58+
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.).
6059

6160
Same directional obstacles (Parameter names may differ from implementation)
6261
![same_directional_object](./image/same_directional_object.svg)
6362

6463
Opposite directional obstacles (Parameter names may differ from implementation)
6564
![opposite_directional_object](./image/opposite_directional_object.svg)
6665

66+
### Cuts off the drivable area against the selected pedestrians
67+
68+
Then, we describe the logic to generate the drivable areas against pedestrians to be avoided.
69+
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.
70+
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.
71+
72+
<figure>
73+
<img src="./image/2024-04-18_15-13-01.png" width="600">
74+
<figcaption> Restriction areas are generated from each pedestrian's predicted paths</figcaption>
75+
</figure>
76+
77+
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.
78+
This is similar to avoidance against the vehicles and takes precedence over keeping a safe distance from the object to be avoided.
79+
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.
80+
81+
<figure>
82+
<img src="./image/2024-04-18_15-32-03.png" width="600">
83+
<figcaption> Ego's minimum requirements are prioritized against object margin</figcaption>
84+
</figure>
85+
6786
## Example
6887

6988
<figure>

planning/behavior_path_dynamic_avoidance_module/config/dynamic_avoidance.param.yaml

+12-7
Original file line numberDiff line numberDiff line change
@@ -12,9 +12,9 @@
1212
bus: true
1313
trailer: true
1414
unknown: false
15-
bicycle: false
15+
bicycle: true
1616
motorcycle: true
17-
pedestrian: false
17+
pedestrian: true
1818

1919
max_obstacle_vel: 100.0 # [m/s]
2020
min_obstacle_vel: 0.0 # [m/s]
@@ -38,8 +38,9 @@
3838
crossing_object:
3939
min_overtaking_object_vel: 1.0
4040
max_overtaking_object_angle: 1.05
41-
min_oncoming_object_vel: 0.0
41+
min_oncoming_object_vel: 1.0
4242
max_oncoming_object_angle: 0.523
43+
max_pedestrian_crossing_vel: 0.8
4344

4445
front_object:
4546
max_object_angle: 0.785
@@ -54,7 +55,11 @@
5455
object_path_base:
5556
min_longitudinal_polygon_margin: 3.0 # [m]
5657

57-
lat_offset_from_obstacle: 0.8 # [m]
58+
lat_offset_from_obstacle: 1.0 # [m]
59+
margin_distance_around_pedestrian: 2.0 # [m]
60+
predicted_path:
61+
end_time_to_consider: 3.0 # [s]
62+
threshold_confidence: 0.0 # [] not probability
5863
max_lat_offset_to_avoid: 0.5 # [m]
5964
max_time_for_object_lat_shift: 0.0 # [s]
6065
lpf_gain_for_lat_avoid_to_offset: 0.9 # [-]
@@ -66,12 +71,12 @@
6671
# for same directional object
6772
overtaking_object:
6873
max_time_to_collision: 40.0 # [s]
69-
start_duration_to_avoid: 2.0 # [s]
70-
end_duration_to_avoid: 4.0 # [s]
74+
start_duration_to_avoid: 1.0 # [s]
75+
end_duration_to_avoid: 1.0 # [s]
7176
duration_to_hold_avoidance: 3.0 # [s]
7277

7378
# for opposite directional object
7479
oncoming_object:
7580
max_time_to_collision: 40.0 # [s] This value should be the same as overtaking_object's one to suppress chattering of this value for parked vehicles
76-
start_duration_to_avoid: 12.0 # [s]
81+
start_duration_to_avoid: 1.0 # [s]
7782
end_duration_to_avoid: 0.0 # [s]
Loading
Loading

planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp

+56-7
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,7 @@
1616
#define BEHAVIOR_PATH_DYNAMIC_AVOIDANCE_MODULE__SCENE_HPP_
1717

1818
#include "behavior_path_planner_common/interface/scene_module_interface.hpp"
19+
#include "tier4_autoware_utils/system/stop_watch.hpp"
1920

2021
#include <rclcpp/rclcpp.hpp>
2122
#include <tier4_autoware_utils/geometry/boost_geometry.hpp>
@@ -54,19 +55,38 @@ std::vector<T> getAllKeys(const std::unordered_map<T, S> & map)
5455
namespace behavior_path_planner
5556
{
5657
using autoware_auto_perception_msgs::msg::PredictedPath;
58+
using autoware_auto_planning_msgs::msg::PathWithLaneId;
5759
using tier4_autoware_utils::Polygon2d;
5860

5961
struct MinMaxValue
6062
{
6163
double min_value{0.0};
6264
double max_value{0.0};
65+
MinMaxValue operator+(const double scalar) const
66+
{
67+
MinMaxValue ret;
68+
ret.min_value = min_value + scalar;
69+
ret.max_value = max_value + scalar;
70+
return ret;
71+
};
72+
void swap() { std::swap(min_value, max_value); }
6373
};
6474

6575
enum class PolygonGenerationMethod {
6676
EGO_PATH_BASE = 0,
6777
OBJECT_PATH_BASE,
6878
};
6979

80+
enum class ObjectType {
81+
OUT_OF_SCOPE = 0, // The module do not care about this type of objects.
82+
REGULATED, // The module assumes this type of objects move in parallel against lanes. Drivable
83+
// areas are divided proportionately with the ego. Typically, cars, bus and trucks
84+
// are classified to this type.
85+
UNREGULATED, // The module does not assume the objects move in parallel against lanes and
86+
// assigns drivable area with priority to ego. Typically, pedestrians should be
87+
// classified to this type.
88+
};
89+
7090
struct DynamicAvoidanceParameters
7191
{
7292
// common
@@ -103,12 +123,18 @@ struct DynamicAvoidanceParameters
103123
double max_overtaking_crossing_object_angle{0.0};
104124
double min_oncoming_crossing_object_vel{0.0};
105125
double max_oncoming_crossing_object_angle{0.0};
126+
double max_pedestrian_crossing_vel{0.0};
106127
double max_stopped_object_vel{0.0};
107128

108129
// drivable area generation
109130
PolygonGenerationMethod polygon_generation_method{};
110131
double min_obj_path_based_lon_polygon_margin{0.0};
111132
double lat_offset_from_obstacle{0.0};
133+
double margin_distance_around_pedestrian{0.0};
134+
135+
double end_time_to_consider{0.0};
136+
double threshold_confidence{0.0};
137+
112138
double max_lat_offset_to_avoid{0.0};
113139
double max_time_for_lat_shift{0.0};
114140
double lpf_gain_for_lat_avoid_to_offset{0.0};
@@ -148,6 +174,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface
148174
const bool arg_is_object_on_ego_path,
149175
const std::optional<rclcpp::Time> & arg_latest_time_inside_ego_path)
150176
: uuid(tier4_autoware_utils::toHexString(predicted_object.object_id)),
177+
label(predicted_object.classification.front().label),
151178
pose(predicted_object.kinematics.initial_pose_with_covariance.pose),
152179
shape(predicted_object.shape),
153180
vel(arg_vel),
@@ -161,6 +188,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface
161188
}
162189

163190
std::string uuid{};
191+
uint8_t label{};
164192
geometry_msgs::msg::Pose pose{};
165193
autoware_auto_perception_msgs::msg::Shape shape;
166194
double vel{0.0};
@@ -178,6 +206,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface
178206
std::vector<PathPointWithLaneId> ref_path_points_for_obj_poly;
179207
LatFeasiblePaths ego_lat_feasible_paths;
180208

209+
// add additional information (not update to the latest data)
181210
void update(
182211
const MinMaxValue & arg_lon_offset_to_avoid, const MinMaxValue & arg_lat_offset_to_avoid,
183212
const bool arg_is_collision_left, const bool arg_should_be_avoided,
@@ -216,7 +245,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface
216245

217246
// increase counter
218247
if (counter_map_.count(uuid) != 0) {
219-
counter_map_.at(uuid) = std::min(max_count_ + 1, std::max(1, counter_map_.at(uuid) + 1));
248+
counter_map_.at(uuid) = std::min(max_count_, std::max(1, counter_map_.at(uuid) + 1));
220249
} else {
221250
counter_map_.emplace(uuid, 1);
222251
}
@@ -236,7 +265,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface
236265
}
237266
for (const auto & uuid : not_updated_uuids) {
238267
if (counter_map_.count(uuid) != 0) {
239-
counter_map_.at(uuid) = std::max(min_count_ - 1, std::min(-1, counter_map_.at(uuid) - 1));
268+
counter_map_.at(uuid) = std::max(0, counter_map_.at(uuid) - 1);
240269
} else {
241270
counter_map_.emplace(uuid, -1);
242271
}
@@ -253,7 +282,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface
253282
std::remove_if(
254283
valid_object_uuids_.begin(), valid_object_uuids_.end(),
255284
[&](const auto & uuid) {
256-
return counter_map_.count(uuid) == 0 || counter_map_.at(uuid) < max_count_;
285+
return counter_map_.count(uuid) == 0 || counter_map_.at(uuid) < min_count_;
257286
}),
258287
valid_object_uuids_.end());
259288

@@ -340,13 +369,23 @@ class DynamicAvoidanceModule : public SceneModuleInterface
340369
const double max_lon_offset;
341370
const double min_lon_offset;
342371
};
372+
struct EgoPathReservePoly
373+
{
374+
const tier4_autoware_utils::Polygon2d left_avoid;
375+
const tier4_autoware_utils::Polygon2d right_avoid;
376+
};
343377

344378
bool canTransitSuccessState() override;
345379

346380
bool canTransitFailureState() override { return false; }
347381

348-
bool isLabelTargetObstacle(const uint8_t label) const;
349-
void updateTargetObjects();
382+
ObjectType getObjectType(const uint8_t label) const;
383+
void registerRegulatedObjects(const std::vector<DynamicAvoidanceObject> & prev_objects);
384+
void registerUnregulatedObjects(const std::vector<DynamicAvoidanceObject> & prev_objects);
385+
void determineWhetherToAvoidAgainstRegulatedObjects(
386+
const std::vector<DynamicAvoidanceObject> & prev_objects);
387+
void determineWhetherToAvoidAgainstUnregulatedObjects(
388+
const std::vector<DynamicAvoidanceObject> & prev_objects);
350389
LatFeasiblePaths generateLateralFeasiblePaths(
351390
const geometry_msgs::msg::Pose & ego_pose, const double ego_vel) const;
352391
void updateRefPathBeforeLaneChange(const std::vector<PathPointWithLaneId> & ego_ref_path_points);
@@ -378,18 +417,24 @@ class DynamicAvoidanceModule : public SceneModuleInterface
378417
const geometry_msgs::msg::Pose & obj_pose, const Polygon2d & obj_points, const double obj_vel,
379418
const PredictedPath & obj_path, const autoware_auto_perception_msgs::msg::Shape & obj_shape,
380419
const TimeWhileCollision & time_while_collision) const;
381-
std::optional<MinMaxValue> calcMinMaxLateralOffsetToAvoid(
420+
std::optional<MinMaxValue> calcMinMaxLateralOffsetToAvoidRegulatedObject(
382421
const std::vector<PathPointWithLaneId> & ref_path_points_for_obj_poly,
383422
const Polygon2d & obj_points, const geometry_msgs::msg::Point & obj_pos, const double obj_vel,
384423
const bool is_collision_left, const double obj_normal_vel,
385424
const std::optional<DynamicAvoidanceObject> & prev_object) const;
386-
425+
std::optional<MinMaxValue> calcMinMaxLateralOffsetToAvoidUnregulatedObject(
426+
const std::vector<PathPointWithLaneId> & ref_path_points_for_obj_poly,
427+
const std::optional<DynamicAvoidanceObject> & prev_object,
428+
const DynamicAvoidanceObject & object) const;
387429
std::pair<lanelet::ConstLanelets, lanelet::ConstLanelets> getAdjacentLanes(
388430
const double forward_distance, const double backward_distance) const;
389431
std::optional<tier4_autoware_utils::Polygon2d> calcEgoPathBasedDynamicObstaclePolygon(
390432
const DynamicAvoidanceObject & object) const;
391433
std::optional<tier4_autoware_utils::Polygon2d> calcObjectPathBasedDynamicObstaclePolygon(
392434
const DynamicAvoidanceObject & object) const;
435+
std::optional<tier4_autoware_utils::Polygon2d> calcPredictedPathBasedDynamicObstaclePolygon(
436+
const DynamicAvoidanceObject & object, const EgoPathReservePoly & ego_path_poly) const;
437+
EgoPathReservePoly calcEgoPathReservePoly(const PathWithLaneId & ego_path) const;
393438

394439
void printIgnoreReason(const std::string & obj_uuid, const std::string & reason)
395440
{
@@ -406,6 +451,10 @@ class DynamicAvoidanceModule : public SceneModuleInterface
406451
std::shared_ptr<DynamicAvoidanceParameters> parameters_;
407452

408453
TargetObjectsManager target_objects_manager_;
454+
455+
mutable tier4_autoware_utils::StopWatch<
456+
std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock>
457+
stop_watch_;
409458
};
410459
} // namespace behavior_path_planner
411460

planning/behavior_path_dynamic_avoidance_module/src/manager.cpp

+17
Original file line numberDiff line numberDiff line change
@@ -99,6 +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_pedestrian_crossing_vel =
103+
node->declare_parameter<double>(ns + "crossing_object.max_pedestrian_crossing_vel");
102104

103105
p.max_stopped_object_vel =
104106
node->declare_parameter<double>(ns + "stopped_object.max_object_vel");
@@ -111,6 +113,12 @@ void DynamicAvoidanceModuleManager::init(rclcpp::Node * node)
111113
p.min_obj_path_based_lon_polygon_margin =
112114
node->declare_parameter<double>(ns + "object_path_base.min_longitudinal_polygon_margin");
113115
p.lat_offset_from_obstacle = node->declare_parameter<double>(ns + "lat_offset_from_obstacle");
116+
p.margin_distance_around_pedestrian =
117+
node->declare_parameter<double>(ns + "margin_distance_around_pedestrian");
118+
p.end_time_to_consider =
119+
node->declare_parameter<double>(ns + "predicted_path.end_time_to_consider");
120+
p.threshold_confidence =
121+
node->declare_parameter<double>(ns + "predicted_path.threshold_confidence");
114122
p.max_lat_offset_to_avoid = node->declare_parameter<double>(ns + "max_lat_offset_to_avoid");
115123
p.max_time_for_lat_shift =
116124
node->declare_parameter<double>(ns + "max_time_for_object_lat_shift");
@@ -214,6 +222,9 @@ void DynamicAvoidanceModuleManager::updateModuleParams(
214222
updateParam<double>(
215223
parameters, ns + "crossing_object.max_oncoming_object_angle",
216224
p->max_oncoming_crossing_object_angle);
225+
updateParam<double>(
226+
parameters, ns + "crossing_object.max_pedestrian_crossing_vel",
227+
p->max_pedestrian_crossing_vel);
217228

218229
updateParam<double>(
219230
parameters, ns + "stopped_object.max_object_vel", p->max_stopped_object_vel);
@@ -231,6 +242,12 @@ void DynamicAvoidanceModuleManager::updateModuleParams(
231242
parameters, ns + "object_path_base.min_longitudinal_polygon_margin",
232243
p->min_obj_path_based_lon_polygon_margin);
233244
updateParam<double>(parameters, ns + "lat_offset_from_obstacle", p->lat_offset_from_obstacle);
245+
updateParam<double>(
246+
parameters, ns + "margin_distance_around_pedestrian", p->margin_distance_around_pedestrian);
247+
updateParam<double>(
248+
parameters, ns + "predicted_path.end_time_to_consider", p->end_time_to_consider);
249+
updateParam<double>(
250+
parameters, ns + "predicted_path.threshold_confidence", p->threshold_confidence);
234251
updateParam<double>(parameters, ns + "max_lat_offset_to_avoid", p->max_lat_offset_to_avoid);
235252
updateParam<double>(
236253
parameters, ns + "max_time_for_object_lat_shift", p->max_time_for_lat_shift);

0 commit comments

Comments
 (0)