diff --git a/planning/behavior_path_dynamic_avoidance_module/README.md b/planning/behavior_path_dynamic_avoidance_module/README.md
index 89a661d66735e..b8af767afd9a5 100644
--- a/planning/behavior_path_dynamic_avoidance_module/README.md
+++ b/planning/behavior_path_dynamic_avoidance_module/README.md
@@ -12,7 +12,7 @@ Obstacle Avoidance module modifies the path to be followed so that it fits withi
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.
The Avoidance module performs avoidance through the outside of own lanes but cannot avoid the moving objects.
On the other hand, this module can avoid moving objects.
-For this reason, the word "dynamic" is used in the modules's name.
+For this reason, the word "dynamic" is used in the module's name.
The table below lists the avoidance modules that can handle each situation.
| | avoid within the own lane | avoid through the outside of own lanes |
@@ -23,7 +23,7 @@ The table below lists the avoidance modules that can handle each situation.
## Policy of algorithms
Here, we describe the policy of inner algorithms.
-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.
+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.
### Select obstacles to avoid
@@ -36,7 +36,7 @@ The other, _can be avoided_ denotes whether it can be avoided without risk to th
For this purpose, the module judges whether the obstacle can be avoided with satisfying the constraints of lateral acceleration and lateral jerk.
For example, the module decides not to avoid an object that is too close or fast in the lateral direction.
-### Cuts off the drivable area against the selected obstacles
+### Cuts off the drivable area against the selected vehicles
For the selected obstacles to be avoided, the module cuts off the drivable area.
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.
@@ -45,7 +45,7 @@ Furthermore, the output drivable area shape is designed as a rectangular cutout
#### Determination of lateral dimension
-Lateral dimensions of the polygon is calculated as follows.
+The lateral dimensions of the polygon are calculated as follows.
The polygon's width to extract from the drivable area is the obstacle width and `drivable_area_generation.lat_offset_from_obstacle`.
We can limit the lateral shift length by `drivable_area_generation.max_lat_offset_to_avoid`.
@@ -55,7 +55,7 @@ We can limit the lateral shift length by `drivable_area_generation.max_lat_offse
Then, extracting the same directional and opposite directional obstacles from the drivable area will work as follows considering TTC (time to collision).
-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.).
+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.).
Same directional obstacles (Parameter names may differ from implementation)
![same_directional_object](./image/same_directional_object.svg)
@@ -63,6 +63,26 @@ Same directional obstacles (Parameter names may differ from implementation)
Opposite directional obstacles (Parameter names may differ from implementation)
![opposite_directional_object](./image/opposite_directional_object.svg)
+### Cuts off the drivable area against the selected pedestrians
+
+Then, we describe the logic to generate the drivable areas against pedestrians to be avoided.
+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.
+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.
+
+
+
+ Restriction areas are generated from each pedestrian's predicted paths
+
+
+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.
+This is similar to avoidance against the vehicles and takes precedence over keeping a safe distance from the object to be avoided.
+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.
+
+
+
+ Ego's minimum requirements are prioritized against object margin
+
+
## Example
diff --git a/planning/behavior_path_dynamic_avoidance_module/config/dynamic_avoidance.param.yaml b/planning/behavior_path_dynamic_avoidance_module/config/dynamic_avoidance.param.yaml
index 371f7da2fadf5..c1f4646ef7736 100644
--- a/planning/behavior_path_dynamic_avoidance_module/config/dynamic_avoidance.param.yaml
+++ b/planning/behavior_path_dynamic_avoidance_module/config/dynamic_avoidance.param.yaml
@@ -12,9 +12,9 @@
bus: true
trailer: true
unknown: false
- bicycle: false
+ bicycle: true
motorcycle: true
- pedestrian: false
+ pedestrian: true
max_obstacle_vel: 100.0 # [m/s]
min_obstacle_vel: 0.0 # [m/s]
@@ -38,8 +38,9 @@
crossing_object:
min_overtaking_object_vel: 1.0
max_overtaking_object_angle: 1.05
- min_oncoming_object_vel: 0.0
+ min_oncoming_object_vel: 1.0
max_oncoming_object_angle: 0.523
+ max_pedestrian_crossing_vel: 0.8
front_object:
max_object_angle: 0.785
@@ -54,7 +55,11 @@
object_path_base:
min_longitudinal_polygon_margin: 3.0 # [m]
- lat_offset_from_obstacle: 0.8 # [m]
+ lat_offset_from_obstacle: 1.0 # [m]
+ margin_distance_around_pedestrian: 2.0 # [m]
+ predicted_path:
+ end_time_to_consider: 3.0 # [s]
+ threshold_confidence: 0.0 # [] not probability
max_lat_offset_to_avoid: 0.5 # [m]
max_time_for_object_lat_shift: 0.0 # [s]
lpf_gain_for_lat_avoid_to_offset: 0.9 # [-]
@@ -66,12 +71,12 @@
# for same directional object
overtaking_object:
max_time_to_collision: 40.0 # [s]
- start_duration_to_avoid: 2.0 # [s]
- end_duration_to_avoid: 4.0 # [s]
+ start_duration_to_avoid: 1.0 # [s]
+ end_duration_to_avoid: 1.0 # [s]
duration_to_hold_avoidance: 3.0 # [s]
# for opposite directional object
oncoming_object:
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
- start_duration_to_avoid: 12.0 # [s]
+ start_duration_to_avoid: 1.0 # [s]
end_duration_to_avoid: 0.0 # [s]
diff --git a/planning/behavior_path_dynamic_avoidance_module/image/2024-04-18_15-13-01.png b/planning/behavior_path_dynamic_avoidance_module/image/2024-04-18_15-13-01.png
new file mode 100644
index 0000000000000..719636ed295c0
Binary files /dev/null and b/planning/behavior_path_dynamic_avoidance_module/image/2024-04-18_15-13-01.png differ
diff --git a/planning/behavior_path_dynamic_avoidance_module/image/2024-04-18_15-32-03.png b/planning/behavior_path_dynamic_avoidance_module/image/2024-04-18_15-32-03.png
new file mode 100644
index 0000000000000..934c495bbb9f1
Binary files /dev/null and b/planning/behavior_path_dynamic_avoidance_module/image/2024-04-18_15-32-03.png differ
diff --git a/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp b/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp
index bdcd1d7424d37..353a5fab92032 100644
--- a/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp
+++ b/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp
@@ -16,6 +16,7 @@
#define BEHAVIOR_PATH_DYNAMIC_AVOIDANCE_MODULE__SCENE_HPP_
#include "behavior_path_planner_common/interface/scene_module_interface.hpp"
+#include "tier4_autoware_utils/system/stop_watch.hpp"
#include
#include
@@ -54,12 +55,21 @@ std::vector getAllKeys(const std::unordered_map & map)
namespace behavior_path_planner
{
using autoware_auto_perception_msgs::msg::PredictedPath;
+using autoware_auto_planning_msgs::msg::PathWithLaneId;
using tier4_autoware_utils::Polygon2d;
struct MinMaxValue
{
double min_value{0.0};
double max_value{0.0};
+ MinMaxValue operator+(const double scalar) const
+ {
+ MinMaxValue ret;
+ ret.min_value = min_value + scalar;
+ ret.max_value = max_value + scalar;
+ return ret;
+ };
+ void swap() { std::swap(min_value, max_value); }
};
enum class PolygonGenerationMethod {
@@ -67,6 +77,16 @@ enum class PolygonGenerationMethod {
OBJECT_PATH_BASE,
};
+enum class ObjectType {
+ OUT_OF_SCOPE = 0, // The module do not care about this type of objects.
+ REGULATED, // The module assumes this type of objects move in parallel against lanes. Drivable
+ // areas are divided proportionately with the ego. Typically, cars, bus and trucks
+ // are classified to this type.
+ UNREGULATED, // The module does not assume the objects move in parallel against lanes and
+ // assigns drivable area with priority to ego. Typically, pedestrians should be
+ // classified to this type.
+};
+
struct DynamicAvoidanceParameters
{
// common
@@ -103,12 +123,18 @@ struct DynamicAvoidanceParameters
double max_overtaking_crossing_object_angle{0.0};
double min_oncoming_crossing_object_vel{0.0};
double max_oncoming_crossing_object_angle{0.0};
+ double max_pedestrian_crossing_vel{0.0};
double max_stopped_object_vel{0.0};
// drivable area generation
PolygonGenerationMethod polygon_generation_method{};
double min_obj_path_based_lon_polygon_margin{0.0};
double lat_offset_from_obstacle{0.0};
+ double margin_distance_around_pedestrian{0.0};
+
+ double end_time_to_consider{0.0};
+ double threshold_confidence{0.0};
+
double max_lat_offset_to_avoid{0.0};
double max_time_for_lat_shift{0.0};
double lpf_gain_for_lat_avoid_to_offset{0.0};
@@ -148,6 +174,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface
const bool arg_is_object_on_ego_path,
const std::optional & arg_latest_time_inside_ego_path)
: uuid(tier4_autoware_utils::toHexString(predicted_object.object_id)),
+ label(predicted_object.classification.front().label),
pose(predicted_object.kinematics.initial_pose_with_covariance.pose),
shape(predicted_object.shape),
vel(arg_vel),
@@ -161,6 +188,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface
}
std::string uuid{};
+ uint8_t label{};
geometry_msgs::msg::Pose pose{};
autoware_auto_perception_msgs::msg::Shape shape;
double vel{0.0};
@@ -178,6 +206,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface
std::vector ref_path_points_for_obj_poly;
LatFeasiblePaths ego_lat_feasible_paths;
+ // add additional information (not update to the latest data)
void update(
const MinMaxValue & arg_lon_offset_to_avoid, const MinMaxValue & arg_lat_offset_to_avoid,
const bool arg_is_collision_left, const bool arg_should_be_avoided,
@@ -216,7 +245,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface
// increase counter
if (counter_map_.count(uuid) != 0) {
- counter_map_.at(uuid) = std::min(max_count_ + 1, std::max(1, counter_map_.at(uuid) + 1));
+ counter_map_.at(uuid) = std::min(max_count_, std::max(1, counter_map_.at(uuid) + 1));
} else {
counter_map_.emplace(uuid, 1);
}
@@ -236,7 +265,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface
}
for (const auto & uuid : not_updated_uuids) {
if (counter_map_.count(uuid) != 0) {
- counter_map_.at(uuid) = std::max(min_count_ - 1, std::min(-1, counter_map_.at(uuid) - 1));
+ counter_map_.at(uuid) = std::max(0, counter_map_.at(uuid) - 1);
} else {
counter_map_.emplace(uuid, -1);
}
@@ -253,7 +282,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface
std::remove_if(
valid_object_uuids_.begin(), valid_object_uuids_.end(),
[&](const auto & uuid) {
- return counter_map_.count(uuid) == 0 || counter_map_.at(uuid) < max_count_;
+ return counter_map_.count(uuid) == 0 || counter_map_.at(uuid) < min_count_;
}),
valid_object_uuids_.end());
@@ -340,13 +369,23 @@ class DynamicAvoidanceModule : public SceneModuleInterface
const double max_lon_offset;
const double min_lon_offset;
};
+ struct EgoPathReservePoly
+ {
+ const tier4_autoware_utils::Polygon2d left_avoid;
+ const tier4_autoware_utils::Polygon2d right_avoid;
+ };
bool canTransitSuccessState() override;
bool canTransitFailureState() override { return false; }
- bool isLabelTargetObstacle(const uint8_t label) const;
- void updateTargetObjects();
+ ObjectType getObjectType(const uint8_t label) const;
+ void registerRegulatedObjects(const std::vector & prev_objects);
+ void registerUnregulatedObjects(const std::vector & prev_objects);
+ void determineWhetherToAvoidAgainstRegulatedObjects(
+ const std::vector & prev_objects);
+ void determineWhetherToAvoidAgainstUnregulatedObjects(
+ const std::vector & prev_objects);
LatFeasiblePaths generateLateralFeasiblePaths(
const geometry_msgs::msg::Pose & ego_pose, const double ego_vel) const;
void updateRefPathBeforeLaneChange(const std::vector & ego_ref_path_points);
@@ -378,18 +417,24 @@ class DynamicAvoidanceModule : public SceneModuleInterface
const geometry_msgs::msg::Pose & obj_pose, const Polygon2d & obj_points, const double obj_vel,
const PredictedPath & obj_path, const autoware_auto_perception_msgs::msg::Shape & obj_shape,
const TimeWhileCollision & time_while_collision) const;
- std::optional calcMinMaxLateralOffsetToAvoid(
+ std::optional calcMinMaxLateralOffsetToAvoidRegulatedObject(
const std::vector & ref_path_points_for_obj_poly,
const Polygon2d & obj_points, const geometry_msgs::msg::Point & obj_pos, const double obj_vel,
const bool is_collision_left, const double obj_normal_vel,
const std::optional & prev_object) const;
-
+ std::optional calcMinMaxLateralOffsetToAvoidUnregulatedObject(
+ const std::vector & ref_path_points_for_obj_poly,
+ const std::optional & prev_object,
+ const DynamicAvoidanceObject & object) const;
std::pair getAdjacentLanes(
const double forward_distance, const double backward_distance) const;
std::optional calcEgoPathBasedDynamicObstaclePolygon(
const DynamicAvoidanceObject & object) const;
std::optional calcObjectPathBasedDynamicObstaclePolygon(
const DynamicAvoidanceObject & object) const;
+ std::optional calcPredictedPathBasedDynamicObstaclePolygon(
+ const DynamicAvoidanceObject & object, const EgoPathReservePoly & ego_path_poly) const;
+ EgoPathReservePoly calcEgoPathReservePoly(const PathWithLaneId & ego_path) const;
void printIgnoreReason(const std::string & obj_uuid, const std::string & reason)
{
@@ -406,6 +451,10 @@ class DynamicAvoidanceModule : public SceneModuleInterface
std::shared_ptr parameters_;
TargetObjectsManager target_objects_manager_;
+
+ mutable tier4_autoware_utils::StopWatch<
+ std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock>
+ stop_watch_;
};
} // namespace behavior_path_planner
diff --git a/planning/behavior_path_dynamic_avoidance_module/src/manager.cpp b/planning/behavior_path_dynamic_avoidance_module/src/manager.cpp
index 569c6a17b5b32..baa1c631df8da 100644
--- a/planning/behavior_path_dynamic_avoidance_module/src/manager.cpp
+++ b/planning/behavior_path_dynamic_avoidance_module/src/manager.cpp
@@ -99,6 +99,8 @@ void DynamicAvoidanceModuleManager::init(rclcpp::Node * node)
node->declare_parameter(ns + "crossing_object.min_oncoming_object_vel");
p.max_oncoming_crossing_object_angle =
node->declare_parameter(ns + "crossing_object.max_oncoming_object_angle");
+ p.max_pedestrian_crossing_vel =
+ node->declare_parameter(ns + "crossing_object.max_pedestrian_crossing_vel");
p.max_stopped_object_vel =
node->declare_parameter(ns + "stopped_object.max_object_vel");
@@ -111,6 +113,12 @@ void DynamicAvoidanceModuleManager::init(rclcpp::Node * node)
p.min_obj_path_based_lon_polygon_margin =
node->declare_parameter(ns + "object_path_base.min_longitudinal_polygon_margin");
p.lat_offset_from_obstacle = node->declare_parameter(ns + "lat_offset_from_obstacle");
+ p.margin_distance_around_pedestrian =
+ node->declare_parameter(ns + "margin_distance_around_pedestrian");
+ p.end_time_to_consider =
+ node->declare_parameter(ns + "predicted_path.end_time_to_consider");
+ p.threshold_confidence =
+ node->declare_parameter(ns + "predicted_path.threshold_confidence");
p.max_lat_offset_to_avoid = node->declare_parameter(ns + "max_lat_offset_to_avoid");
p.max_time_for_lat_shift =
node->declare_parameter(ns + "max_time_for_object_lat_shift");
@@ -214,6 +222,9 @@ void DynamicAvoidanceModuleManager::updateModuleParams(
updateParam(
parameters, ns + "crossing_object.max_oncoming_object_angle",
p->max_oncoming_crossing_object_angle);
+ updateParam(
+ parameters, ns + "crossing_object.max_pedestrian_crossing_vel",
+ p->max_pedestrian_crossing_vel);
updateParam(
parameters, ns + "stopped_object.max_object_vel", p->max_stopped_object_vel);
@@ -231,6 +242,12 @@ void DynamicAvoidanceModuleManager::updateModuleParams(
parameters, ns + "object_path_base.min_longitudinal_polygon_margin",
p->min_obj_path_based_lon_polygon_margin);
updateParam(parameters, ns + "lat_offset_from_obstacle", p->lat_offset_from_obstacle);
+ updateParam(
+ parameters, ns + "margin_distance_around_pedestrian", p->margin_distance_around_pedestrian);
+ updateParam(
+ parameters, ns + "predicted_path.end_time_to_consider", p->end_time_to_consider);
+ updateParam(
+ parameters, ns + "predicted_path.threshold_confidence", p->threshold_confidence);
updateParam(parameters, ns + "max_lat_offset_to_avoid", p->max_lat_offset_to_avoid);
updateParam(
parameters, ns + "max_time_for_object_lat_shift", p->max_time_for_lat_shift);
diff --git a/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp b/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp
index c6b7b2e0e8031..84871ed756e81 100644
--- a/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp
+++ b/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp
@@ -18,12 +18,15 @@
#include "behavior_path_planner_common/utils/utils.hpp"
#include "object_recognition_utils/predicted_path_utils.hpp"
#include "signal_processing/lowpass_filter_1d.hpp"
+#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp"
-#include
#include
-#include
+#include
+#include
#include
+#include
+#include
#include
#include
@@ -55,6 +58,11 @@ MinMaxValue getMinMaxValues(const std::vector & vec)
return MinMaxValue{vec.at(min_idx), vec.at(max_idx)};
}
+MinMaxValue combineMinMaxValues(const MinMaxValue & r1, const MinMaxValue & r2)
+{
+ return MinMaxValue{std::min(r1.min_value, r2.min_value), std::max(r1.max_value, r2.max_value)};
+}
+
void appendObjectMarker(MarkerArray & marker_array, const geometry_msgs::msg::Pose & obj_pose)
{
auto marker = tier4_autoware_utils::createDefaultMarker(
@@ -339,11 +347,24 @@ bool DynamicAvoidanceModule::isExecutionReady() const
void DynamicAvoidanceModule::updateData()
{
+ // stop_watch_.tic(std::string(__func__));
+
info_marker_.markers.clear();
debug_marker_.markers.clear();
- // calculate target objects
- updateTargetObjects();
+ const auto prev_objects = target_objects_manager_.getValidObjects();
+ target_objects_manager_.initialize();
+
+ // 1. Rough filtering of target objects with small computing cost
+ registerRegulatedObjects(prev_objects);
+ registerUnregulatedObjects(prev_objects);
+
+ const auto & ego_lat_feasible_paths = generateLateralFeasiblePaths(getEgoPose(), getEgoSpeed());
+ target_objects_manager_.finalize(ego_lat_feasible_paths);
+
+ // 2. Precise filtering of target objects and check if they should be avoided
+ determineWhetherToAvoidAgainstRegulatedObjects(prev_objects);
+ determineWhetherToAvoidAgainstUnregulatedObjects(prev_objects);
const auto target_objects_candidate = target_objects_manager_.getValidObjects();
target_objects_.clear();
@@ -352,6 +373,11 @@ void DynamicAvoidanceModule::updateData()
target_objects_.push_back(target_object_candidate);
}
}
+
+ // const double calculation_time = stop_watch_.toc(std::string(__func__));
+ // RCLCPP_INFO_STREAM_EXPRESSION(
+ // getLogger(), parameters_->enable_debug_info, __func__ << ":=" << calculation_time << "
+ // [ms]");
}
bool DynamicAvoidanceModule::canTransitSuccessState()
@@ -361,12 +387,23 @@ bool DynamicAvoidanceModule::canTransitSuccessState()
BehaviorModuleOutput DynamicAvoidanceModule::plan()
{
+ // stop_watch_.tic(std::string(__func__));
+
const auto & input_path = getPreviousModuleOutput().path;
+ if (input_path.points.empty()) {
+ throw std::runtime_error("input path is empty");
+ }
+
+ const auto ego_path_reserve_poly = calcEgoPathReservePoly(input_path);
// create obstacles to avoid (= extract from the drivable area)
std::vector obstacles_for_drivable_area;
for (const auto & object : target_objects_) {
const auto obstacle_poly = [&]() {
+ if (getObjectType(object.label) == ObjectType::UNREGULATED) {
+ return calcPredictedPathBasedDynamicObstaclePolygon(object, ego_path_reserve_poly);
+ }
+
if (parameters_->polygon_generation_method == PolygonGenerationMethod::EGO_PATH_BASE) {
return calcEgoPathBasedDynamicObstaclePolygon(object);
}
@@ -399,6 +436,11 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan()
output.turn_signal_info = getPreviousModuleOutput().turn_signal_info;
output.modified_goal = getPreviousModuleOutput().modified_goal;
+ // const double calculation_time = stop_watch_.toc(std::string(__func__));
+ // RCLCPP_INFO_STREAM_EXPRESSION(
+ // getLogger(), parameters_->enable_debug_info, __func__ << ":=" << calculation_time << "
+ // [ms]");
+
return output;
}
@@ -414,53 +456,43 @@ BehaviorModuleOutput DynamicAvoidanceModule::planWaitingApproval()
return out;
}
-bool DynamicAvoidanceModule::isLabelTargetObstacle(const uint8_t label) const
+ObjectType DynamicAvoidanceModule::getObjectType(const uint8_t label) const
{
using autoware_auto_perception_msgs::msg::ObjectClassification;
if (label == ObjectClassification::CAR && parameters_->avoid_car) {
- return true;
+ return ObjectType::REGULATED;
}
if (label == ObjectClassification::TRUCK && parameters_->avoid_truck) {
- return true;
+ return ObjectType::REGULATED;
}
if (label == ObjectClassification::BUS && parameters_->avoid_bus) {
- return true;
+ return ObjectType::REGULATED;
}
if (label == ObjectClassification::TRAILER && parameters_->avoid_trailer) {
- return true;
+ return ObjectType::REGULATED;
}
if (label == ObjectClassification::UNKNOWN && parameters_->avoid_unknown) {
- return true;
+ return ObjectType::UNREGULATED;
}
if (label == ObjectClassification::BICYCLE && parameters_->avoid_bicycle) {
- return true;
+ return ObjectType::UNREGULATED;
}
if (label == ObjectClassification::MOTORCYCLE && parameters_->avoid_motorcycle) {
- return true;
+ return ObjectType::REGULATED;
}
if (label == ObjectClassification::PEDESTRIAN && parameters_->avoid_pedestrian) {
- return true;
+ return ObjectType::UNREGULATED;
}
- return false;
+ return ObjectType::OUT_OF_SCOPE;
}
-void DynamicAvoidanceModule::updateTargetObjects()
+void DynamicAvoidanceModule::registerRegulatedObjects(
+ const std::vector & prev_objects)
{
const auto input_path = getPreviousModuleOutput().path;
const auto & predicted_objects = planner_data_->dynamic_object->objects;
- const auto input_ref_path_points = getPreviousModuleOutput().reference_path.points;
- const auto prev_objects = target_objects_manager_.getValidObjects();
-
- updateRefPathBeforeLaneChange(input_ref_path_points);
-
- const auto & ego_pose = getEgoPose();
- const double ego_vel = getEgoSpeed();
- const auto ego_lat_feasible_paths = generateLateralFeasiblePaths(ego_pose, ego_vel);
-
- // 1. Rough filtering of target objects
- target_objects_manager_.initialize();
for (const auto & predicted_object : predicted_objects) {
const auto obj_uuid = tier4_autoware_utils::toHexString(predicted_object.object_id);
const auto & obj_pose = predicted_object.kinematics.initial_pose_with_covariance.pose;
@@ -474,9 +506,7 @@ void DynamicAvoidanceModule::updateTargetObjects()
[](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; });
// 1.a. check label
- const bool is_label_target_obstacle =
- isLabelTargetObstacle(predicted_object.classification.front().label);
- if (!is_label_target_obstacle) {
+ if (getObjectType(predicted_object.classification.front().label) != ObjectType::REGULATED) {
continue;
}
@@ -543,10 +573,92 @@ void DynamicAvoidanceModule::updateTargetObjects()
latest_time_inside_ego_path);
target_objects_manager_.updateObject(obj_uuid, target_object);
}
- target_objects_manager_.finalize(ego_lat_feasible_paths);
+}
+
+void DynamicAvoidanceModule::registerUnregulatedObjects(
+ const std::vector & prev_objects)
+{
+ const auto input_path = getPreviousModuleOutput().path;
+ const auto & predicted_objects = planner_data_->dynamic_object->objects;
+
+ for (const auto & predicted_object : predicted_objects) {
+ const auto obj_uuid = tier4_autoware_utils::toHexString(predicted_object.object_id);
+ const auto & obj_pose = predicted_object.kinematics.initial_pose_with_covariance.pose;
+ const double obj_vel_norm = std::hypot(
+ predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x,
+ predicted_object.kinematics.initial_twist_with_covariance.twist.linear.y);
+ const auto prev_object = getObstacleFromUuid(prev_objects, obj_uuid);
+ const auto obj_path = *std::max_element(
+ predicted_object.kinematics.predicted_paths.begin(),
+ predicted_object.kinematics.predicted_paths.end(),
+ [](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; });
+
+ // 1.a. Check if the obstacle is labeled as pedestrians, bicycle or similar.
+ if (getObjectType(predicted_object.classification.front().label) != ObjectType::UNREGULATED) {
+ continue;
+ }
+
+ // 1.b. Check if the object's velocity is within the module's coverage range.
+ const auto [obj_tangent_vel, obj_normal_vel] =
+ projectObstacleVelocityToTrajectory(input_path.points, predicted_object);
+ if (
+ obj_vel_norm < parameters_->min_obstacle_vel ||
+ obj_vel_norm > parameters_->max_obstacle_vel) {
+ continue;
+ }
+
+ // 1.c. Check if object' lateral velocity is small enough to be avoid.
+ if (std::abs(obj_normal_vel) > parameters_->max_pedestrian_crossing_vel) {
+ RCLCPP_INFO_EXPRESSION(
+ getLogger(), parameters_->enable_debug_info,
+ "[DynamicAvoidance] Ignore obstacle (%s) since it crosses the ego's path with its normal "
+ "vel (%5.2f) m/s.",
+ obj_uuid.c_str(), obj_normal_vel);
+ continue;
+ }
+
+ // Blocks for compatibility with existing code
+ // 1.e. check if object lateral distance to ego's path is small enough
+ // 1.f. calculate the object is on ego's path or not
+
+ const double dist_obj_center_to_path =
+ std::abs(motion_utils::calcLateralOffset(input_path.points, obj_pose.position));
+ const bool is_object_on_ego_path =
+ dist_obj_center_to_path <
+ planner_data_->parameters.vehicle_width / 2.0 + parameters_->min_obj_lat_offset_to_ego_path;
+
+ // 1.g. calculate last time inside ego's path
+ const auto latest_time_inside_ego_path = [&]() -> std::optional {
+ if (!prev_object || !prev_object->latest_time_inside_ego_path) {
+ if (is_object_on_ego_path) {
+ return clock_->now();
+ }
+ return std::nullopt;
+ }
+ if (is_object_on_ego_path) {
+ return clock_->now();
+ }
+ return *prev_object->latest_time_inside_ego_path;
+ }();
+
+ // register the object
+ const auto target_object = DynamicAvoidanceObject(
+ predicted_object, obj_tangent_vel, obj_normal_vel, is_object_on_ego_path,
+ latest_time_inside_ego_path);
+ target_objects_manager_.updateObject(obj_uuid, target_object);
+ }
+}
+
+void DynamicAvoidanceModule::determineWhetherToAvoidAgainstRegulatedObjects(
+ const std::vector & prev_objects)
+{
+ const auto & input_path = getPreviousModuleOutput().path;
- // 2. Precise filtering of target objects and check if they should be avoided
for (const auto & object : target_objects_manager_.getValidObjects()) {
+ if (getObjectType(object.label) != ObjectType::REGULATED) {
+ continue;
+ }
+
const auto obj_uuid = object.uuid;
const auto prev_object = getObstacleFromUuid(prev_objects, obj_uuid);
const auto obj_path = *std::max_element(
@@ -651,7 +763,7 @@ void DynamicAvoidanceModule::updateTargetObjects()
const double signed_dist_ego_to_obj = [&]() {
const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(input_path.points);
const double lon_offset_ego_to_obj = motion_utils::calcSignedArcLength(
- input_path.points, ego_pose.position, ego_seg_idx, lat_lon_offset.nearest_idx);
+ input_path.points, getEgoPose().position, ego_seg_idx, lat_lon_offset.nearest_idx);
if (0 < lon_offset_ego_to_obj) {
return std::max(
0.0, lon_offset_ego_to_obj - planner_data_->parameters.front_overhang +
@@ -676,9 +788,10 @@ void DynamicAvoidanceModule::updateTargetObjects()
const auto lon_offset_to_avoid = calcMinMaxLongitudinalOffsetToAvoid(
ref_path_points_for_obj_poly, object.pose, obj_points, object.vel, obj_path, object.shape,
time_while_collision);
- const auto lat_offset_to_avoid = calcMinMaxLateralOffsetToAvoid(
+ const auto lat_offset_to_avoid = calcMinMaxLateralOffsetToAvoidRegulatedObject(
ref_path_points_for_obj_poly, obj_points, object.pose.position, object.vel, is_collision_left,
object.lat_vel, prev_object);
+
if (!lat_offset_to_avoid) {
RCLCPP_INFO_EXPRESSION(
getLogger(), parameters_->enable_debug_info,
@@ -693,8 +806,68 @@ void DynamicAvoidanceModule::updateTargetObjects()
obj_uuid, lon_offset_to_avoid, *lat_offset_to_avoid, is_collision_left, should_be_avoided,
ref_path_points_for_obj_poly);
}
+ // prev_input_ref_path_points_ = input_ref_path_points;
+}
+
+void DynamicAvoidanceModule::determineWhetherToAvoidAgainstUnregulatedObjects(
+ const std::vector & prev_objects)
+{
+ const auto & input_path = getPreviousModuleOutput().path;
+
+ for (const auto & object : target_objects_manager_.getValidObjects()) {
+ if (getObjectType(object.label) != ObjectType::UNREGULATED) {
+ continue;
+ }
+
+ const auto obj_uuid = object.uuid;
+ const auto & ref_path_points_for_obj_poly = input_path.points;
+
+ // 2.g. check if the ego is not ahead of the object.
+ const auto lat_lon_offset =
+ getLateralLongitudinalOffset(input_path.points, object.pose, object.shape);
+ const double signed_dist_ego_to_obj = [&]() {
+ const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(input_path.points);
+ const double lon_offset_ego_to_obj = motion_utils::calcSignedArcLength(
+ input_path.points, getEgoPose().position, ego_seg_idx, lat_lon_offset.nearest_idx);
+ if (0 < lon_offset_ego_to_obj) {
+ return std::max(
+ 0.0, lon_offset_ego_to_obj - planner_data_->parameters.front_overhang +
+ lat_lon_offset.min_lon_offset);
+ }
+ return std::min(
+ 0.0, lon_offset_ego_to_obj + planner_data_->parameters.rear_overhang +
+ lat_lon_offset.max_lon_offset);
+ }();
+ if (signed_dist_ego_to_obj < 0) {
+ RCLCPP_INFO_EXPRESSION(
+ getLogger(), parameters_->enable_debug_info,
+ "[DynamicAvoidance] Ignore obstacle (%s) since distance from ego to object (%f) is less "
+ "than 0.",
+ obj_uuid.c_str(), signed_dist_ego_to_obj);
+ continue;
+ }
+
+ // 2.h. calculate longitudinal and lateral offset to avoid to generate object polygon by
+ // "ego_path_base"
+ const auto lat_offset_to_avoid = calcMinMaxLateralOffsetToAvoidUnregulatedObject(
+ ref_path_points_for_obj_poly, getObstacleFromUuid(prev_objects, obj_uuid), object);
+ if (!lat_offset_to_avoid) {
+ RCLCPP_INFO_EXPRESSION(
+ getLogger(), parameters_->enable_debug_info,
+ "[DynamicAvoidance] Ignore obstacle (%s) since the object will intersects the ego's path "
+ "enough",
+ obj_uuid.c_str());
+ continue;
+ }
- prev_input_ref_path_points_ = input_ref_path_points;
+ const bool is_collision_left = (lat_offset_to_avoid.value().max_value > 0.0);
+ const auto lon_offset_to_avoid = MinMaxValue{0.0, 1.0}; // not used. dummy value
+
+ const bool should_be_avoided = true;
+ target_objects_manager_.updateObjectVariables(
+ obj_uuid, lon_offset_to_avoid, *lat_offset_to_avoid, is_collision_left, should_be_avoided,
+ ref_path_points_for_obj_poly);
+ }
}
LatFeasiblePaths DynamicAvoidanceModule::generateLateralFeasiblePaths(
@@ -706,6 +879,7 @@ LatFeasiblePaths DynamicAvoidanceModule::generateLateralFeasiblePaths(
LatFeasiblePaths ego_lat_feasible_paths;
for (double t = 0; t < 10.0; t += 1.0) {
+ // maybe this equation does not have physical meaning (takagi)
const double feasible_lat_offset = lat_acc * std::pow(std::max(t - delay_time, 0.0), 2) / 2.0 +
lat_jerk * std::pow(std::max(t - delay_time, 0.0), 3) / 6.0 -
planner_data_->parameters.vehicle_width / 2.0;
@@ -733,7 +907,7 @@ LatFeasiblePaths DynamicAvoidanceModule::generateLateralFeasiblePaths(
return ego_lat_feasible_paths;
}
-void DynamicAvoidanceModule::updateRefPathBeforeLaneChange(
+[[maybe_unused]] void DynamicAvoidanceModule::updateRefPathBeforeLaneChange(
const std::vector & ego_ref_path_points)
{
if (ref_path_before_lane_change_) {
@@ -1204,7 +1378,8 @@ double DynamicAvoidanceModule::calcValidLengthToAvoid(
return motion_utils::calcSignedArcLength(obj_path.path, 0, valid_obj_path_end_idx);
}
-std::optional DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoid(
+// min value denotes near side, max value denotes far side
+std::optional DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoidRegulatedObject(
const std::vector & ref_path_points_for_obj_poly,
const Polygon2d & obj_points, const geometry_msgs::msg::Point & obj_pos, const double obj_vel,
const bool is_collision_left, const double obj_normal_vel,
@@ -1297,6 +1472,89 @@ std::optional DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoi
return MinMaxValue{filtered_min_bound_lat_offset, max_bound_lat_offset};
}
+// min value denotes near side, max value denotes far side
+std::optional DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoidUnregulatedObject(
+ const std::vector & ref_path_points_for_obj_poly,
+ const std::optional & prev_object,
+ const DynamicAvoidanceObject & object) const
+{
+ const bool enable_lowpass_filter = [&]() {
+ if (
+ !prev_object || prev_object->ref_path_points_for_obj_poly.size() < 2 ||
+ ref_path_points_for_obj_poly.size() < 2) {
+ return true;
+ }
+ const size_t obj_point_idx =
+ motion_utils::findNearestIndex(ref_path_points_for_obj_poly, object.pose.position);
+ const double paths_lat_diff = std::abs(motion_utils::calcLateralOffset(
+ prev_object->ref_path_points_for_obj_poly,
+ ref_path_points_for_obj_poly.at(obj_point_idx).point.pose.position));
+
+ constexpr double min_paths_lat_diff = 0.3;
+ if (paths_lat_diff < min_paths_lat_diff) {
+ return true;
+ }
+ // NOTE: When the input reference path laterally changes, the low-pass filter is disabled not to
+ // shift the obstacle polygon suddenly.
+ return false;
+ }();
+
+ const auto obj_occupancy_region = [&]() {
+ const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape);
+ std::vector lat_pos_vec;
+ for (size_t i = 0; i < obj_points.outer().size(); ++i) {
+ const auto geom_obj_point = toGeometryPoint(obj_points.outer().at(i));
+ const double obj_point_lat_offset = motion_utils::calcLateralOffset(
+ ref_path_points_for_obj_poly, geom_obj_point,
+ motion_utils::findNearestSegmentIndex(ref_path_points_for_obj_poly, geom_obj_point));
+ lat_pos_vec.push_back(obj_point_lat_offset);
+ }
+ const auto current_pos_area = getMinMaxValues(lat_pos_vec);
+ return combineMinMaxValues(current_pos_area, current_pos_area + object.lat_vel * 3.0);
+ }();
+
+ if (
+ obj_occupancy_region.min_value * obj_occupancy_region.max_value < 0.0 ||
+ obj_occupancy_region.max_value - obj_occupancy_region.min_value < 1e-3) {
+ return std::nullopt;
+ }
+
+ // calculate bound pos
+ const auto bound_pos = [&]() {
+ auto temp_bound_pos = obj_occupancy_region;
+ temp_bound_pos.max_value += parameters_->lat_offset_from_obstacle;
+ temp_bound_pos.min_value -= parameters_->lat_offset_from_obstacle;
+ if (std::abs(temp_bound_pos.max_value) < std::abs(temp_bound_pos.min_value)) {
+ temp_bound_pos.swap(); // From here, min denotes near bound, max denotes far bound.
+ }
+
+ const double near_bound_limit =
+ planner_data_->parameters.vehicle_width / 2.0 - parameters_->max_lat_offset_to_avoid;
+ if (temp_bound_pos.max_value > 0.0) {
+ temp_bound_pos.min_value = std::max(temp_bound_pos.min_value, near_bound_limit);
+ } else {
+ temp_bound_pos.min_value = std::min(temp_bound_pos.min_value, -near_bound_limit);
+ }
+ return temp_bound_pos;
+ }();
+
+ // low pass filter for min_bound
+ const auto prev_min_lat_avoid_to_offset = [&]() -> std::optional {
+ if (!prev_object || !prev_object->lat_offset_to_avoid) {
+ return std::nullopt;
+ }
+ return prev_object->lat_offset_to_avoid->min_value;
+ }();
+ const double filtered_min_bound_pos =
+ (prev_min_lat_avoid_to_offset.has_value() & enable_lowpass_filter)
+ ? signal_processing::lowpassFilter(
+ bound_pos.min_value, *prev_min_lat_avoid_to_offset,
+ parameters_->lpf_gain_for_lat_avoid_to_offset)
+ : bound_pos.min_value;
+
+ return MinMaxValue{filtered_min_bound_pos, bound_pos.max_value};
+}
+
// NOTE: object does not have const only to update min_bound_lat_offset.
std::optional
DynamicAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon(
@@ -1310,7 +1568,7 @@ DynamicAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon(
const size_t obj_seg_idx =
motion_utils::findNearestSegmentIndex(ref_path_points_for_obj_poly, object.pose.position);
- const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape);
+ // const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape);
const auto lon_bound_start_idx_opt = motion_utils::insertTargetPoint(
obj_seg_idx, object.lon_offset_to_avoid->min_value, ref_path_points_for_obj_poly);
@@ -1403,6 +1661,7 @@ DynamicAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon(
return obj_poly;
}
+// should be replace by the function calcPredictedPathBasedDynamicObstaclePolygon() (takagi)
std::optional
DynamicAvoidanceModule::calcObjectPathBasedDynamicObstaclePolygon(
const DynamicAvoidanceObject & object) const
@@ -1458,4 +1717,145 @@ DynamicAvoidanceModule::calcObjectPathBasedDynamicObstaclePolygon(
boost::geometry::correct(obj_poly);
return obj_poly;
}
+
+// Calculate polygons according to predicted_path with certain confidence,
+// except for the area required for ego safety.
+// input: an object, the minimum area required for ego safety, and some global params
+std::optional
+DynamicAvoidanceModule::calcPredictedPathBasedDynamicObstaclePolygon(
+ const DynamicAvoidanceObject & object, const EgoPathReservePoly & ego_path_poly) const
+{
+ std::vector obj_poses;
+ for (const auto & predicted_path : object.predicted_paths) {
+ if (predicted_path.confidence < parameters_->threshold_confidence) continue;
+
+ double time_count = 0.0;
+ for (const auto & pose : predicted_path.path) {
+ obj_poses.push_back(pose);
+ time_count += predicted_path.time_step.sec + predicted_path.time_step.nanosec * 1e-9;
+ if (time_count > parameters_->end_time_to_consider + 1e-3) break;
+ }
+ }
+
+ tier4_autoware_utils::Polygon2d obj_points_as_poly;
+ for (const auto pose : obj_poses) {
+ boost::geometry::append(
+ obj_points_as_poly, tier4_autoware_utils::toFootprint(
+ pose, object.shape.dimensions.x * 0.5, object.shape.dimensions.x * 0.5,
+ object.shape.dimensions.y * 0.5)
+ .outer());
+ }
+ boost::geometry::correct(obj_points_as_poly);
+ Polygon2d obj_poly;
+ boost::geometry::convex_hull(obj_points_as_poly, obj_poly);
+
+ tier4_autoware_utils::MultiPolygon2d expanded_poly;
+ namespace strategy = boost::geometry::strategy::buffer;
+ boost::geometry::buffer(
+ obj_poly, expanded_poly,
+ strategy::distance_symmetric(parameters_->margin_distance_around_pedestrian),
+ strategy::side_straight(), strategy::join_round(), strategy::end_flat(),
+ strategy::point_circle());
+ if (expanded_poly.empty()) return {};
+
+ tier4_autoware_utils::MultiPolygon2d output_poly;
+ boost::geometry::difference(
+ expanded_poly[0],
+ object.is_collision_left ? ego_path_poly.right_avoid : ego_path_poly.left_avoid, output_poly);
+
+ if (output_poly.empty()) {
+ RCLCPP_INFO_EXPRESSION(
+ getLogger(), parameters_->enable_debug_info,
+ "[DynamicAvoidance] Ignore obstacle (%s) because it stay inside the ego's path.",
+ object.uuid.c_str());
+ return {};
+ } else if (output_poly.size() >= 2) {
+ RCLCPP_INFO_EXPRESSION(
+ getLogger(), parameters_->enable_debug_info,
+ "[DynamicAvoidance] Ignore obstacle (%s) because it covers the ego's path.",
+ object.uuid.c_str());
+ return {};
+ }
+
+ return output_poly[0];
+}
+
+// Calculate the driving area required to ensure the safety of the own vehicle.
+// It is assumed that this area will not be clipped.
+// input: ego's reference path, ego's pose, ego's speed, and some global params
+DynamicAvoidanceModule::EgoPathReservePoly DynamicAvoidanceModule::calcEgoPathReservePoly(
+ const PathWithLaneId & ego_path) const
+{
+ // This function require almost 0.5 ms. Should be refactored in the future
+ // double calculation_time;
+ // stop_watch_.tic(std::string(__func__));
+
+ namespace strategy = boost::geometry::strategy::buffer;
+
+ assert(!ego_path.points.empty());
+
+ tier4_autoware_utils::LineString2d ego_path_lines;
+ for (const auto & path_point : ego_path.points) {
+ ego_path_lines.push_back(tier4_autoware_utils::fromMsg(path_point.point.pose.position).to_2d());
+ }
+
+ auto calcReservePoly =
+ [&ego_path_lines](
+ strategy::distance_asymmetric path_expand_strategy,
+ strategy::distance_asymmetric steer_expand_strategy,
+ std::vector outer_body_path) -> tier4_autoware_utils::Polygon2d {
+ // reserve area based on the reference path
+ tier4_autoware_utils::MultiPolygon2d path_poly;
+ boost::geometry::buffer(
+ ego_path_lines, path_poly, path_expand_strategy, strategy::side_straight(),
+ strategy::join_round(), strategy::end_flat(), strategy::point_circle());
+
+ // reserve area steer to the avoidance path
+ tier4_autoware_utils::LineString2d steer_lines;
+ for (const auto & point : outer_body_path) {
+ const auto bg_point = tier4_autoware_utils::fromMsg(point).to_2d();
+ if (boost::geometry::within(bg_point, path_poly)) {
+ if (steer_lines.size() != 0) {
+ ;
+ // boost::geometry::append(steer_lines, bg_point);
+ }
+ break;
+ }
+ // boost::geometry::append(steer_lines, bg_point);
+ }
+ tier4_autoware_utils::MultiPolygon2d steer_poly;
+ boost::geometry::buffer(
+ steer_lines, steer_poly, steer_expand_strategy, strategy::side_straight(),
+ strategy::join_round(), strategy::end_flat(), strategy::point_circle());
+
+ tier4_autoware_utils::MultiPolygon2d output_poly;
+ boost::geometry::union_(path_poly, steer_poly, output_poly);
+ if (output_poly.size() != 1) {
+ assert(false);
+ }
+ return output_poly[0];
+ };
+
+ const auto motion_saturated_outer_paths =
+ generateLateralFeasiblePaths(getEgoPose(), getEgoSpeed());
+
+ const double vehicle_half_width = planner_data_->parameters.vehicle_width * 0.5;
+ const double reserve_width_obj_side = vehicle_half_width - parameters_->max_lat_offset_to_avoid;
+
+ const tier4_autoware_utils::Polygon2d left_avoid_poly = calcReservePoly(
+ strategy::distance_asymmetric(vehicle_half_width, reserve_width_obj_side),
+ strategy::distance_asymmetric(vehicle_half_width, 0.0),
+ motion_saturated_outer_paths.right_path);
+ const tier4_autoware_utils::Polygon2d right_avoid_poly = calcReservePoly(
+ strategy::distance_asymmetric(reserve_width_obj_side, vehicle_half_width),
+ strategy::distance_asymmetric(0.0, vehicle_half_width),
+ motion_saturated_outer_paths.left_path);
+
+ // calculation_time = stop_watch_.toc(std::string(__func__));
+ // RCLCPP_INFO_STREAM_EXPRESSION(
+ // getLogger(), parameters_->enable_debug_info, __func__ << ":=" << calculation_time << "[ms]");
+
+ return {left_avoid_poly, right_avoid_poly};
+}
+
} // namespace behavior_path_planner