Skip to content

Commit

Permalink
Merge pull request #1095 from tier4/feat/update_bpp_v0.20.1
Browse files Browse the repository at this point in the history 
feat(behavior_path_planner): update behavior path planner
  • Loading branch information
@kosuke55
kosuke55 authored Jan 18, 2024
2 parents b1f7188 + 8ac97a0 commit a2908c0
Show file tree
Hide file tree
Showing 34 changed files with 855 additions and 394 deletions.
5 changes: 4 additions & 1 deletion common/motion_utils/include/motion_utils/trajectory/trajectory.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -952,7 +952,10 @@ calcArcLength<std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>>(
template <class T>
std::vector<double> calcCurvature(const T & points)
{
std::vector<double> curvature_vec(points.size());
std::vector<double> curvature_vec(points.size(), 0.0);
if (points.size() < 3) {
return curvature_vec;
}

for (size_t i = 1; i < points.size() - 1; ++i) {
const auto p1 = tier4_autoware_utils::getPoint(points.at(i - 1));
Expand Down
6 changes: 4 additions & 2 deletions planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -187,7 +187,7 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData(
}

void AvoidanceByLaneChange::fillAvoidanceTargetObjects(
AvoidancePlanningData & data, DebugData & debug) const
AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const
{
const auto p = std::dynamic_pointer_cast<AvoidanceParameters>(avoidance_parameters_);

Expand Down Expand Up @@ -227,7 +227,9 @@ void AvoidanceByLaneChange::fillAvoidanceTargetObjects(
[&](const auto & object) { return createObjectData(data, object); });
}

utils::avoidance::filterTargetObjects(target_lane_objects, data, debug, planner_data_, p);
utils::avoidance::filterTargetObjects(
target_lane_objects, data, avoidance_parameters_->object_check_max_forward_distance,
planner_data_, p);
}

ObjectData AvoidanceByLaneChange::createObjectData(
Expand Down
3 changes: 2 additions & 1 deletion planning/behavior_path_avoidance_module/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -833,7 +833,8 @@ namespace: `avoidance.target_filtering.`
| object_ignore_section_crosswalk_behind_distance | [m] | double | If the back distance between crosswalk and vehicle is less than this parameter, this module will ignore it. | 30.0 |
| object_check_forward_distance | [m] | double | Forward distance to search the avoidance target. | 150.0 |
| object_check_backward_distance | [m] | double | Backward distance to search the avoidance target. | 2.0 |
| object_check_goal_distance | [m] | double | Backward distance to search the avoidance target. | 20.0 |
| object_check_goal_distance | [m] | double | If the distance between object and goal position is less than this parameter, the module do not return center line. | 20.0 |
| object_check_return_pose_distance | [m] | double | If the distance between object and return position is less than this parameter, the module do not return center line. | 20.0 |
| object_check_shiftable_ratio | [m] | double | Vehicles around the center line within this distance will be excluded from avoidance target. | 0.6 |
| object_check_min_road_shoulder_width | [m] | double | Width considered as a road shoulder if the lane does not have a road shoulder target. | 0.5 |
| object_last_seen_threshold | [s] | double | For the compensation of the detection lost. The object is registered once it is observed as an avoidance target. When the detection loses, the timer will start and the object will be un-registered when the time exceeds this limit. | 2.0 |
Expand Down
3 changes: 2 additions & 1 deletion planning/behavior_path_avoidance_module/config/avoidance.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -122,7 +122,8 @@
motorcycle: true # [-]
pedestrian: true # [-]
# detection range
object_check_goal_distance: 20.0 # [m]
object_check_goal_distance: 20.0 # [m]
object_check_return_pose_distance: 20.0 # [m]
# filtering parking objects
threshold_distance_object_is_on_center: 1.0 # [m]
object_check_shiftable_ratio: 0.6 # [-]
Expand Down
14 changes: 9 additions & 5 deletions ...ng/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -164,10 +164,14 @@ struct AvoidanceParameters
double object_check_backward_distance{0.0};
double object_check_yaw_deviation{0.0};

// if the distance between object and goal position is less than this parameter, the module ignore
// the object.
// if the distance between object and goal position is less than this parameter, the module do not
// return center line.
double object_check_goal_distance{0.0};

// if the distance between object and return position is less than this parameter, the module do
// not return center line.
double object_check_return_pose_distance{0.0};

// use in judge whether the vehicle is parking object on road shoulder
double object_check_shiftable_ratio{0.0};

Expand Down Expand Up @@ -462,14 +466,14 @@ using AvoidLineArray = std::vector<AvoidLine>;

struct AvoidOutline
{
AvoidOutline(AvoidLine avoid_line, AvoidLine return_line)
AvoidOutline(AvoidLine avoid_line, const std::optional<AvoidLine> return_line)
: avoid_line{std::move(avoid_line)}, return_line{std::move(return_line)}
{
}

AvoidLine avoid_line{};

AvoidLine return_line{};
std::optional<AvoidLine> return_line{};

AvoidLineArray middle_lines{};
};
Expand Down Expand Up @@ -588,7 +592,7 @@ struct ShiftLineData
*/
struct DebugData
{
geometry_msgs::msg::Polygon detection_area;
std::vector<geometry_msgs::msg::Polygon> detection_areas;

lanelet::ConstLineStrings3d bounds;

Expand Down
2 changes: 2 additions & 0 deletions ...ehavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -122,6 +122,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node)

p.object_check_goal_distance =
getOrDeclareParameter<double>(*node, ns + "object_check_goal_distance");
p.object_check_return_pose_distance =
getOrDeclareParameter<double>(*node, ns + "object_check_return_pose_distance");
p.threshold_distance_object_is_on_center =
getOrDeclareParameter<double>(*node, ns + "threshold_distance_object_is_on_center");
p.object_check_shiftable_ratio =
Expand Down
13 changes: 6 additions & 7 deletions planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -112,13 +112,12 @@ class AvoidanceModule : public SceneModuleInterface
*/
void updateRegisteredRTCStatus(const PathWithLaneId & path)
{
const Point ego_position = planner_data_->self_odometry->pose.pose.position;
const auto ego_idx = planner_data_->findEgoIndex(path.points);

for (const auto & left_shift : left_shift_array_) {
const double start_distance =
calcSignedArcLength(path.points, ego_position, left_shift.start_pose.position);
const double finish_distance =
calcSignedArcLength(path.points, ego_position, left_shift.finish_pose.position);
calcSignedArcLength(path.points, ego_idx, left_shift.start_pose.position);
const double finish_distance = start_distance + left_shift.relative_longitudinal;
rtc_interface_ptr_map_.at("left")->updateCooperateStatus(
left_shift.uuid, true, start_distance, finish_distance, clock_->now());
if (finish_distance > -1.0e-03) {
Expand All @@ -130,9 +129,8 @@ class AvoidanceModule : public SceneModuleInterface

for (const auto & right_shift : right_shift_array_) {
const double start_distance =
calcSignedArcLength(path.points, ego_position, right_shift.start_pose.position);
const double finish_distance =
calcSignedArcLength(path.points, ego_position, right_shift.finish_pose.position);
calcSignedArcLength(path.points, ego_idx, right_shift.start_pose.position);
const double finish_distance = start_distance + right_shift.relative_longitudinal;
rtc_interface_ptr_map_.at("right")->updateCooperateStatus(
right_shift.uuid, true, start_distance, finish_distance, clock_->now());
if (finish_distance > -1.0e-03) {
Expand Down Expand Up @@ -399,6 +397,7 @@ class AvoidanceModule : public SceneModuleInterface
UUID uuid;
Pose start_pose;
Pose finish_pose;
double relative_longitudinal{0.0};
};

using RegisteredShiftLineArray = std::vector<RegisteredShiftLine>;
Expand Down
19 changes: 5 additions & 14 deletions planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -77,11 +77,6 @@ std::vector<DrivableAreaInfo::Obstacle> generateObstaclePolygonsForDrivableArea(
const ObjectDataArray & objects, const std::shared_ptr<AvoidanceParameters> & parameters,
const double vehicle_width);

double getLongitudinalVelocity(const Pose & p_ref, const Pose & p_target, const double v);

bool isCentroidWithinLanelets(
const PredictedObject & object, const lanelet::ConstLanelets & target_lanelets);

lanelet::ConstLanelets getAdjacentLane(
const std::shared_ptr<const PlannerData> & planner_data,
const std::shared_ptr<AvoidanceParameters> & parameters, const bool is_right_shift);
Expand Down Expand Up @@ -128,12 +123,7 @@ void compensateDetectionLost(
ObjectDataArray & other_objects);

void filterTargetObjects(
ObjectDataArray & objects, AvoidancePlanningData & data, DebugData & debug,
const std::shared_ptr<const PlannerData> & planner_data,
const std::shared_ptr<AvoidanceParameters> & parameters);

double getRoadShoulderDistance(
ObjectData & object, const AvoidancePlanningData & data,
ObjectDataArray & objects, AvoidancePlanningData & data, const double forward_detection_range,
const std::shared_ptr<const PlannerData> & planner_data,
const std::shared_ptr<AvoidanceParameters> & parameters);

Expand All @@ -157,9 +147,10 @@ std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects(
DebugData & debug);

std::pair<PredictedObjects, PredictedObjects> separateObjectsByPath(
const PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data,
const AvoidancePlanningData & data, const std::shared_ptr<AvoidanceParameters> & parameters,
const double object_check_forward_distance, const bool is_running, DebugData & debug);
const PathWithLaneId & reference_path, const PathWithLaneId & spline_path,
const std::shared_ptr<const PlannerData> & planner_data, const AvoidancePlanningData & data,
const std::shared_ptr<AvoidanceParameters> & parameters,
const double object_check_forward_distance, DebugData & debug);

DrivableLanes generateExpandDrivableLanes(
const lanelet::ConstLanelet & lanelet, const std::shared_ptr<const PlannerData> & planner_data,
Expand Down
7 changes: 6 additions & 1 deletion planning/behavior_path_avoidance_module/src/debug.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -608,10 +608,15 @@ MarkerArray createDebugMarkerArray(
addShiftGrad(debug.total_backward_grad, debug.total_shift, "grad_backward", 0.4, 0.2, 0.9);
}

// detection area
size_t i = 0;
for (const auto & detection_area : debug.detection_areas) {
add(createPolygonMarkerArray(detection_area, "detection_area", i++, 0.16, 1.0, 0.69, 0.1));
}

// misc
{
add(createPathMarkerArray(path, "centerline_resampled", 0, 0.0, 0.9, 0.5));
add(createPolygonMarkerArray(debug.detection_area, "detection_area", 0L, 0.16, 1.0, 0.69, 0.1));
add(createDrivableBounds(data, "drivable_bound", 1.0, 0.0, 0.42));
add(laneletsAsTriangleMarkerArray(
"drivable_lanes", transformToLanelets(data.drivable_lanes),
Expand Down
Loading

0 comments on commit a2908c0

Please sign in to comment.