From f67d7337d4975a10925d53e7967a042587b84315 Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Tue, 20 Dec 2022 11:25:04 +0900 Subject: [PATCH 1/8] feat(behavior_path_planner): enable generating backward drivable area (#2512) Signed-off-by: yutaka Signed-off-by: yutaka --- .../behavior_path_planner/utilities.hpp | 2 +- .../behavior_path_planner/src/utilities.cpp | 142 ++++++++---------- 2 files changed, 65 insertions(+), 79 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp index 3697ad5576b1c..6b684998141c5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -294,7 +294,7 @@ std::vector cutOverlappedLanes( void generateDrivableArea( PathWithLaneId & path, const std::vector & lanes, const double vehicle_length, - const std::shared_ptr planner_data); + const std::shared_ptr planner_data, const bool is_driving_forward = true); lanelet::ConstLineStrings3d getDrivableAreaForAllSharedLinestringLanelets( const std::shared_ptr & planner_data); diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index 500eb5f90c774..d335c6c13b582 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -76,15 +76,15 @@ size_t findNearestSegmentIndex( template size_t findNearestSegmentIndexFromLateralDistance( - const std::vector & points, const geometry_msgs::msg::Pose & pose) + const std::vector & points, const geometry_msgs::msg::Point & target_point) { - size_t closest_idx = motion_utils::findNearestSegmentIndex(points, pose.position); + size_t closest_idx = motion_utils::findNearestSegmentIndex(points, target_point); double min_lateral_dist = - std::fabs(motion_utils::calcLateralOffset(points, pose.position, closest_idx)); + std::fabs(motion_utils::calcLateralOffset(points, target_point, closest_idx)); for (size_t seg_idx = 0; seg_idx < points.size() - 1; ++seg_idx) { const double lon_dist = - motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, pose.position); + motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, target_point); const double segment_length = tier4_autoware_utils::calcDistance2d(points.at(seg_idx), points.at(seg_idx + 1)); if (lon_dist < 0.0 || segment_length < lon_dist) { @@ -92,7 +92,7 @@ size_t findNearestSegmentIndexFromLateralDistance( } const double lat_dist = - std::fabs(motion_utils::calcLateralOffset(points, pose.position, seg_idx)); + std::fabs(motion_utils::calcLateralOffset(points, target_point, seg_idx)); if (lat_dist < min_lateral_dist) { closest_idx = seg_idx; min_lateral_dist = lat_dist; @@ -1103,36 +1103,36 @@ std::vector cutOverlappedLanes( return shorten_lanes; } -geometry_msgs::msg::Pose calcLongitudinalOffsetStartPose( - const std::vector & points, const geometry_msgs::msg::Pose & pose, +geometry_msgs::msg::Point calcLongitudinalOffsetStartPoint( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const size_t nearest_segment_idx, const double offset) { const double offset_length = motion_utils::calcLongitudinalOffsetToSegment(points, nearest_segment_idx, pose.position); - const auto offset_pose = - motion_utils::calcLongitudinalOffsetPose(points, nearest_segment_idx, offset_length + offset); + const auto offset_point = + motion_utils::calcLongitudinalOffsetPoint(points, nearest_segment_idx, offset_length + offset); - return offset_pose ? offset_pose.get() : points.at(nearest_segment_idx); + return offset_point ? offset_point.get() : points.at(nearest_segment_idx); } -geometry_msgs::msg::Pose calcLongitudinalOffsetGoalPose( - const std::vector & points, const geometry_msgs::msg::Pose & pose, +geometry_msgs::msg::Point calcLongitudinalOffsetGoalPoint( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const size_t nearest_segment_idx, const double offset) { const double offset_length = motion_utils::calcLongitudinalOffsetToSegment(points, nearest_segment_idx, pose.position); - const auto offset_pose = - motion_utils::calcLongitudinalOffsetPose(points, nearest_segment_idx, offset_length + offset); + const auto offset_point = + motion_utils::calcLongitudinalOffsetPoint(points, nearest_segment_idx, offset_length + offset); - return offset_pose ? offset_pose.get() : points.at(nearest_segment_idx + 1); + return offset_point ? offset_point.get() : points.at(nearest_segment_idx + 1); } void generateDrivableArea( PathWithLaneId & path, const std::vector & lanes, const double vehicle_length, - const std::shared_ptr planner_data) + const std::shared_ptr planner_data, const bool is_driving_forward) { - std::vector left_bound; - std::vector right_bound; + std::vector left_bound; + std::vector right_bound; // extract data const auto transformed_lanes = util::transformToLanelets(lanes); @@ -1140,38 +1140,17 @@ void generateDrivableArea( const auto route_handler = planner_data->route_handler; constexpr double overlap_threshold = 0.01; - auto addLeftBoundPoints = [&left_bound](const lanelet::ConstLanelet & lane) { - for (const auto & lp : lane.leftBound()) { - geometry_msgs::msg::Pose current_pose; - current_pose.position = lanelet::utils::conversion::toGeomMsgPt(lp); - if (left_bound.empty()) { - left_bound.push_back(current_pose); - } else if ( - tier4_autoware_utils::calcDistance2d(current_pose, left_bound.back()) > overlap_threshold) { - left_bound.push_back(current_pose); - } - } - }; - - auto addRightBoundPoints = [&right_bound](const lanelet::ConstLanelet & lane) { - for (const auto & rp : lane.rightBound()) { - geometry_msgs::msg::Pose current_pose; - current_pose.position = lanelet::utils::conversion::toGeomMsgPt(rp); - if (right_bound.empty()) { - right_bound.push_back(current_pose); - } else if ( - tier4_autoware_utils::calcDistance2d(current_pose, right_bound.back()) > - overlap_threshold) { - right_bound.push_back(current_pose); + auto addPoints = + [](const lanelet::ConstLineString3d & points, std::vector & bound) { + for (const auto & bound_p : points) { + const auto cp = lanelet::utils::conversion::toGeomMsgPt(bound_p); + if (bound.empty()) { + bound.push_back(cp); + } else if (tier4_autoware_utils::calcDistance2d(cp, bound.back()) > overlap_threshold) { + bound.push_back(cp); + } } - } - }; - - // Insert Position - for (const auto & lane : lanes) { - addLeftBoundPoints(lane.left_lane); - addRightBoundPoints(lane.right_lane); - } + }; const auto has_overlap = [&](const lanelet::ConstLanelet & lane, const lanelet::Id & ignore_lane_id = lanelet::InvalId) { @@ -1187,6 +1166,12 @@ void generateDrivableArea( return false; }; + // Insert Position + for (const auto & lane : lanes) { + addPoints(lane.left_lane.leftBound3d(), left_bound); + addPoints(lane.right_lane.rightBound3d(), right_bound); + } + // Insert points after goal lanelet::ConstLanelet goal_lanelet; if ( @@ -1208,64 +1193,65 @@ void generateDrivableArea( continue; } - addLeftBoundPoints(lane); - addRightBoundPoints(lane); + addPoints(lane.leftBound3d(), left_bound); + addPoints(lane.rightBound3d(), right_bound); } } - // Give Orientation - motion_utils::insertOrientation(left_bound, true); - motion_utils::insertOrientation(right_bound, true); + if (!is_driving_forward) { + std::reverse(left_bound.begin(), left_bound.end()); + std::reverse(right_bound.begin(), right_bound.end()); + } // Get Closest segment for the start point constexpr double front_length = 0.5; const auto front_pose = path.points.empty() ? current_pose->pose : path.points.front().point.pose; const size_t front_left_start_idx = - findNearestSegmentIndexFromLateralDistance(left_bound, front_pose); + findNearestSegmentIndexFromLateralDistance(left_bound, front_pose.position); const size_t front_right_start_idx = - findNearestSegmentIndexFromLateralDistance(right_bound, front_pose); - const auto left_start_pose = - calcLongitudinalOffsetStartPose(left_bound, front_pose, front_left_start_idx, -front_length); - const auto right_start_pose = - calcLongitudinalOffsetStartPose(right_bound, front_pose, front_right_start_idx, -front_length); + findNearestSegmentIndexFromLateralDistance(right_bound, front_pose.position); + const auto left_start_point = + calcLongitudinalOffsetStartPoint(left_bound, front_pose, front_left_start_idx, -front_length); + const auto right_start_point = + calcLongitudinalOffsetStartPoint(right_bound, front_pose, front_right_start_idx, -front_length); const size_t left_start_idx = - findNearestSegmentIndexFromLateralDistance(left_bound, left_start_pose); + findNearestSegmentIndexFromLateralDistance(left_bound, left_start_point); const size_t right_start_idx = - findNearestSegmentIndexFromLateralDistance(right_bound, right_start_pose); + findNearestSegmentIndexFromLateralDistance(right_bound, right_start_point); // Get Closest segment for the goal point const auto goal_pose = path.points.empty() ? current_pose->pose : path.points.back().point.pose; const size_t goal_left_start_idx = - findNearestSegmentIndexFromLateralDistance(left_bound, goal_pose); + findNearestSegmentIndexFromLateralDistance(left_bound, goal_pose.position); const size_t goal_right_start_idx = - findNearestSegmentIndexFromLateralDistance(right_bound, goal_pose); - const auto left_goal_pose = - calcLongitudinalOffsetGoalPose(left_bound, goal_pose, goal_left_start_idx, vehicle_length); - const auto right_goal_pose = - calcLongitudinalOffsetGoalPose(right_bound, goal_pose, goal_right_start_idx, vehicle_length); + findNearestSegmentIndexFromLateralDistance(right_bound, goal_pose.position); + const auto left_goal_point = + calcLongitudinalOffsetGoalPoint(left_bound, goal_pose, goal_left_start_idx, vehicle_length); + const auto right_goal_point = + calcLongitudinalOffsetGoalPoint(right_bound, goal_pose, goal_right_start_idx, vehicle_length); const size_t left_goal_idx = - findNearestSegmentIndexFromLateralDistance(left_bound, left_goal_pose); + findNearestSegmentIndexFromLateralDistance(left_bound, left_goal_point); const size_t right_goal_idx = - findNearestSegmentIndexFromLateralDistance(right_bound, right_goal_pose); + findNearestSegmentIndexFromLateralDistance(right_bound, right_goal_point); // Store Data path.left_bound.clear(); path.right_bound.clear(); // Insert a start point - path.left_bound.push_back(left_start_pose.position); - path.right_bound.push_back(right_start_pose.position); + path.left_bound.push_back(left_start_point); + path.right_bound.push_back(right_start_point); // Insert middle points for (size_t i = left_start_idx + 1; i <= left_goal_idx; ++i) { - const auto & next_point = left_bound.at(i).position; + const auto & next_point = left_bound.at(i); const double dist = tier4_autoware_utils::calcDistance2d(path.left_bound.back(), next_point); if (dist > overlap_threshold) { path.left_bound.push_back(next_point); } } for (size_t i = right_start_idx + 1; i <= right_goal_idx; ++i) { - const auto & next_point = right_bound.at(i).position; + const auto & next_point = right_bound.at(i); const double dist = tier4_autoware_utils::calcDistance2d(path.right_bound.back(), next_point); if (dist > overlap_threshold) { path.right_bound.push_back(next_point); @@ -1274,14 +1260,14 @@ void generateDrivableArea( // Insert a goal point if ( - tier4_autoware_utils::calcDistance2d(path.left_bound.back(), left_goal_pose.position) > + tier4_autoware_utils::calcDistance2d(path.left_bound.back(), left_goal_point) > overlap_threshold) { - path.left_bound.push_back(left_goal_pose.position); + path.left_bound.push_back(left_goal_point); } if ( - tier4_autoware_utils::calcDistance2d(path.right_bound.back(), right_goal_pose.position) > + tier4_autoware_utils::calcDistance2d(path.right_bound.back(), right_goal_point) > overlap_threshold) { - path.right_bound.push_back(right_goal_pose.position); + path.right_bound.push_back(right_goal_point); } } From b970720117c3f0b272148d5ff1ce79337b947871 Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Tue, 20 Dec 2022 15:31:58 +0900 Subject: [PATCH 2/8] feat(behavior_path_planner): add a new document for the drivable area generation (#2532) * feat(behavior_path_planner): add drivable area document Signed-off-by: yutaka * update Signed-off-by: yutaka * update Signed-off-by: yutaka * add expanded lanes explanation Signed-off-by: yutaka * fix format Signed-off-by: yutaka * update Signed-off-by: yutaka * Update planning/behavior_path_planner/behavior_path_planner_drivable_area.md Co-authored-by: Fumiya Watanabe * Update planning/behavior_path_planner/behavior_path_planner_avoidance-design.md Co-authored-by: Fumiya Watanabe * Update planning/behavior_path_planner/behavior_path_planner_drivable_area.md Co-authored-by: Fumiya Watanabe * Update planning/behavior_path_planner/behavior_path_planner_drivable_area.md Co-authored-by: Fumiya Watanabe Signed-off-by: yutaka Co-authored-by: Fumiya Watanabe --- planning/behavior_path_planner/README.md | 22 - .../behavior_path_planner_avoidance-design.md | 1 + .../behavior_path_planner_drivable_area.md | 78 ++++ .../drivable_area/drivable_lines.drawio.svg | 241 +++++++++++ .../drivable_area/expanded_lanes.drawio.svg | 115 +++++ .../drivable_area/sorted_lanes.drawio.svg | 409 ++++++++++++++++++ 6 files changed, 844 insertions(+), 22 deletions(-) create mode 100644 planning/behavior_path_planner/behavior_path_planner_drivable_area.md create mode 100644 planning/behavior_path_planner/image/drivable_area/drivable_lines.drawio.svg create mode 100644 planning/behavior_path_planner/image/drivable_area/expanded_lanes.drawio.svg create mode 100644 planning/behavior_path_planner/image/drivable_area/sorted_lanes.drawio.svg diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index d0e4a49605dd9..d6bf2067c9cb7 100644 --- a/planning/behavior_path_planner/README.md +++ b/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. diff --git a/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md b/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md index cefc482be4896..821190b62956b 100644 --- a/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md +++ b/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. diff --git a/planning/behavior_path_planner/behavior_path_planner_drivable_area.md b/planning/behavior_path_planner/behavior_path_planner_drivable_area.md new file mode 100644 index 0000000000000..88dcf30299a59 --- /dev/null +++ b/planning/behavior_path_planner/behavior_path_planner_drivable_area.md @@ -0,0 +1,78 @@ +# 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 left_bound; +std::vector 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) diff --git a/planning/behavior_path_planner/image/drivable_area/drivable_lines.drawio.svg b/planning/behavior_path_planner/image/drivable_area/drivable_lines.drawio.svg new file mode 100644 index 0000000000000..fd1729013c1ba --- /dev/null +++ b/planning/behavior_path_planner/image/drivable_area/drivable_lines.drawio.svg @@ -0,0 +1,241 @@ + + + + + + + + + + +
+
+
+ Lane 3 +
+
+
+ + Lane 3 + + + + + + +
+
+
+ Lane 7 +
+
+
+ + Lane 7 + + + + + + +
+
+
+ Lane 2 +
+
+
+ + Lane 2 + + + + + + +
+
+
+ Lane 6 +
+
+
+ + Lane 6 + + + + + + + +
+
+
+ Lane 4 +
+
+
+ + Lane 4 + + + + + + +
+
+
+ Lane 8 +
+
+
+ + Lane 8 + + + + + + +
+
+
+ Lane 10 +
+
+
+ + Lane 10 + + + + + + +
+
+
+ Lane 9 +
+
+
+ + Lane 9 + + + + + + +
+
+
+ Lane 11 +
+
+
+ + Lane 11 + + + + + + +
+
+
+ + Lane 5 + +
+
+
+ + Lane 5 + + + + + + + + + + +
+
+
+ Lane 1 +
+
+
+ + Lane 1 + + + + + + + + + + + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/drivable_area/expanded_lanes.drawio.svg b/planning/behavior_path_planner/image/drivable_area/expanded_lanes.drawio.svg new file mode 100644 index 0000000000000..c1bd0ccdd0e09 --- /dev/null +++ b/planning/behavior_path_planner/image/drivable_area/expanded_lanes.drawio.svg @@ -0,0 +1,115 @@ + + + + + + + + + + +
+
+
+ + Lane1 +
+ +
+
+
+ + Lane1 + + + + + + + + + +
+
+
+ expanded lanes +
+
+
+ + expanded lanes + + + + + + + + + + +
+
+
+ + Lane2 +
+ +
+
+
+ + Lane2 + + + + + + +
+
+
+ + Lane3 +
+ +
+
+
+ + Lane3 + + + + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/drivable_area/sorted_lanes.drawio.svg b/planning/behavior_path_planner/image/drivable_area/sorted_lanes.drawio.svg new file mode 100644 index 0000000000000..3c6bc512ff328 --- /dev/null +++ b/planning/behavior_path_planner/image/drivable_area/sorted_lanes.drawio.svg @@ -0,0 +1,409 @@ + + + + + + + + + + +
+
+
+ Lane 3 +
+
+
+ + Lane 3 + + + + + + +
+
+
+ Lane 7 +
+
+
+ + Lane 7 + + + + + + +
+
+
+ Lane 2 +
+
+
+ + Lane 2 + + + + + + +
+
+
+ Lane 6 +
+
+
+ + Lane 6 + + + + + + + + + +
+
+
+ Lane 4 +
+
+
+ + Lane 4 + + + + + + +
+
+
+ Lane 8 +
+
+
+ + Lane 8 + + + + + + +
+
+
+ Lane 10 +
+
+
+ + Lane 10 + + + + + + +
+
+
+ Lane 9 +
+
+
+ + Lane 9 + + + + + + +
+
+
+ Lane 11 +
+
+
+ + Lane 11 + + + + + + +
+
+
+ + Lane 5 + +
+
+
+ + Lane 5 + + + + + + + + + + +
+
+
+ Lane 1 +
+
+
+ + Lane 1 + + + + + + +
+
+
+
+ DrivableLanes1 +
+
+ { +
+
+ right_lane = 1 +
+
+ left_lane = 1 +
+
+ middle_lanes = {} +
+
+ } +
+
+
+
+ + DrivableLanes1... + + + + + + +
+
+
+
+ DrivableLanes2 +
+
+ { +
+
+ right_lane = 4 +
+
+ left_lane = 2 +
+
+ middle_lanes = {3} +
+
+ } +
+
+
+
+ + DrivableLanes2... + + + + + + + + + + +
+
+
+
+ DrivableLanes3 +
+
+ { +
+
+ right_lane = 8 +
+
+ left_lane = 5 +
+
+ middle_lanes = {6,7} +
+
+ } +
+
+
+
+ + DrivableLanes3... + + + + + + + + +
+
+
+
+ DrivableLanes4 +
+
+ { +
+
+ right_lane = 10 +
+
+ left_lane = 9 +
+
+ middle_lanes = {} +
+
+ } +
+
+
+
+ + DrivableLanes4... + + + + + + +
+
+
+
+ DrivableLanes5 +
+
+ { +
+
+ right_lane = 11 +
+
+ left_lane = 11 +
+
+ middle_lanes = {} +
+
+ } +
+
+
+
+ + DrivableLanes5... + + + + + + + + + Text is not SVG - cannot display + + + From 2be41d7bdc658aa2810f9903f938bbb65c2d01f5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=2E=20Fatih=20C=C4=B1r=C4=B1t?= Date: Tue, 20 Dec 2022 10:50:55 +0300 Subject: [PATCH 3/8] fix(tensorrt): update tensorrt code of traffic_light_classifier (#2325) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: M. Fatih Cırıt --- .../utils/trt_common.cpp | 16 ++++++++++------ .../utils/trt_common.hpp | 2 -- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/perception/traffic_light_classifier/utils/trt_common.cpp b/perception/traffic_light_classifier/utils/trt_common.cpp index 4aae3e0ece5e1..adb2fbe037a31 100644 --- a/perception/traffic_light_classifier/utils/trt_common.cpp +++ b/perception/traffic_light_classifier/utils/trt_common.cpp @@ -75,8 +75,16 @@ void TrtCommon::setup() } context_ = UniquePtr(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 TrtCommon::getInputBindingIndex() { return engine_->getBindingIndex(input_name_.c_str()); } - -int TrtCommon::getOutputBindingIndex() { return engine_->getBindingIndex(output_name_.c_str()); } - } // namespace Tn diff --git a/perception/traffic_light_classifier/utils/trt_common.hpp b/perception/traffic_light_classifier/utils/trt_common.hpp index 9577dfd2262a6..7fc3d3b3e46d9 100644 --- a/perception/traffic_light_classifier/utils/trt_common.hpp +++ b/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 context_; From ecf39cef52ad8b295916ebe59584bd6f60cbf436 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 20 Dec 2022 17:34:14 +0900 Subject: [PATCH 4/8] fix(intersection): always set RTC distance to default stop line (#2529) always set RTC distance to default stop line Signed-off-by: Mamoru Sobue Signed-off-by: Mamoru Sobue --- .../scene_module/intersection/scene_intersection.cpp | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index d3c20427f118e..6072745b7fe07 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -236,13 +236,9 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * logger_.get_child("state_machine"), *clock_); setSafe(state_machine_.getState() == StateMachine::State::GO); - if (is_entry_prohibited) { - setDistance(motion_utils::calcSignedArcLength( - path->points, planner_data_->current_pose.pose.position, - path->points.at(stop_line_idx_final).point.pose.position)); - } else { - setDistance(std::numeric_limits::lowest()); - } + setDistance(motion_utils::calcSignedArcLength( + path->points, planner_data_->current_pose.pose.position, + path->points.at(stop_line_idx_final).point.pose.position)); if (!isActivated()) { // if RTC says intersection entry is 'dangerous', insert stop_line(v == 0.0) in this block From 067a4ba5ec94d75b2680fd9f1598d1c2267bdc2b Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 20 Dec 2022 18:10:08 +0900 Subject: [PATCH 5/8] fix(intersection): fixed stuck vehicle detection area (#2463) Signed-off-by: Mamoru Sobue --- .../intersection.param.yaml | 2 +- .../config/intersection.param.yaml | 2 +- .../intersection/scene_intersection.hpp | 2 +- .../intersection/scene_intersection.cpp | 146 +++++++++--------- .../src/scene_module/intersection/util.cpp | 9 +- 5 files changed, 76 insertions(+), 85 deletions(-) diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml index 79eb257de6889..220c087d18de0 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml +++ b/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 diff --git a/planning/behavior_velocity_planner/config/intersection.param.yaml b/planning/behavior_velocity_planner/config/intersection.param.yaml index 79eb257de6889..220c087d18de0 100644 --- a/planning/behavior_velocity_planner/config/intersection.param.yaml +++ b/planning/behavior_velocity_planner/config/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 diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index 8decae844adb7..c1097f39bbf01 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -166,7 +166,7 @@ class IntersectionModule : public SceneModuleInterface Polygon2d generateEgoIntersectionLanePolygon( lanelet::LaneletMapConstPtr lanelet_map_ptr, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, - const int start_idx, const double extra_dist, const double ignore_dist) const; + const double extra_dist, const double ignore_dist) const; /** * @brief Modify objects predicted path. remove path point if the time exceeds timer_thr. diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index 6072745b7fe07..01a38f42f4d49 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -150,87 +150,83 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * } const size_t closest_idx = closest_idx_opt.get(); - if (stop_lines_idx_opt.has_value()) { - const auto & stop_lines = stop_lines_idx_opt.value(); - const size_t stop_line_idx = stop_lines.stop_line; - const size_t pass_judge_line_idx = stop_lines.pass_judge_line; - const size_t keep_detection_line_idx = stop_lines.keep_detection_line; - - const bool is_over_pass_judge_line = - util::isOverTargetIndex(*path, closest_idx, current_pose.pose, pass_judge_line_idx); - const bool is_before_keep_detection_line = - util::isBeforeTargetIndex(*path, closest_idx, current_pose.pose, keep_detection_line_idx); - const bool keep_detection = is_before_keep_detection_line && - std::fabs(current_vel) < planner_param_.keep_detection_vel_thr; - - if (is_over_pass_judge_line && keep_detection) { - // in case ego could not stop exactly before the stop line, but with some overshoot, - // keep detection within some margin under low velocity threshold - RCLCPP_DEBUG( - logger_, - "over the pass judge line, but before keep detection line and low speed, " - "continue planning"); - // no return here, keep planning - } else if (is_over_pass_judge_line && is_go_out_ && !external_stop) { - RCLCPP_DEBUG(logger_, "over the keep_detection line and not low speed. no plan needed."); - RCLCPP_DEBUG(logger_, "===== plan end ====="); - setSafe(true); - setDistance(motion_utils::calcSignedArcLength( - path->points, planner_data_->current_pose.pose.position, - path->points.at(stop_line_idx).point.pose.position)); - // no plan needed. - return true; - } - } + /* collision checking */ + bool is_entry_prohibited = false; /* get dynamic object */ const auto objects_ptr = planner_data_->predicted_objects; - /* calculate final stop lines */ - bool is_entry_prohibited = false; - const double detect_length = + /* check stuck vehicle */ + const double ignore_length = + planner_param_.stuck_vehicle_ignore_dist + planner_data_->vehicle_info_.vehicle_length_m; + const double detect_dist = planner_param_.stuck_vehicle_detect_dist + planner_data_->vehicle_info_.vehicle_length_m; const auto stuck_vehicle_detect_area = generateEgoIntersectionLanePolygon( - lanelet_map_ptr, *path, closest_idx, stuck_line_idx, detect_length, - planner_param_.stuck_vehicle_detect_dist); + lanelet_map_ptr, *path, closest_idx, detect_dist, ignore_length); const bool is_stuck = checkStuckVehicleInIntersection(objects_ptr, stuck_vehicle_detect_area); - int stop_line_idx_final = stuck_line_idx; - int pass_judge_line_idx_final = stuck_line_idx; + + /* calculate dynamic collision around detection area */ + const bool has_collision = checkCollision( + lanelet_map_ptr, *path, detection_lanelets, adjacent_lanelets, intersection_area, objects_ptr, + closest_idx, stuck_vehicle_detect_area); + + /* calculate final stop lines */ + int stop_line_idx_final = -1; + int pass_judge_line_idx_final = -1; if (external_go) { is_entry_prohibited = false; } else if (external_stop) { is_entry_prohibited = true; - } else if (is_stuck) { + } else if (is_stuck || has_collision) { is_entry_prohibited = true; - stop_line_idx_final = stuck_line_idx; - pass_judge_line_idx_final = stuck_line_idx; - } else { - /* calculate dynamic collision around detection area */ - const bool has_collision = checkCollision( - lanelet_map_ptr, *path, detection_lanelets, adjacent_lanelets, intersection_area, objects_ptr, - closest_idx, stuck_vehicle_detect_area); - is_entry_prohibited = has_collision; - if (stop_lines_idx_opt.has_value()) { - const auto & stop_lines_idx = stop_lines_idx_opt.value(); - stop_line_idx_final = stop_lines_idx.stop_line; - pass_judge_line_idx_final = stop_lines_idx.pass_judge_line; - } else { - if (has_collision) { - RCLCPP_ERROR(logger_, "generateStopLine() failed for detected objects"); - RCLCPP_DEBUG(logger_, "===== plan end ====="); - setSafe(true); - setDistance(std::numeric_limits::lowest()); - return false; - } else { - RCLCPP_DEBUG(logger_, "no need to stop"); - RCLCPP_DEBUG(logger_, "===== plan end ====="); - setSafe(true); - setDistance(std::numeric_limits::lowest()); - return true; - } + const double dist_stuck_stopline = motion_utils::calcSignedArcLength( + path->points, path->points.at(stuck_line_idx).point.pose.position, + path->points.at(closest_idx).point.pose.position); + const double eps = 1e-1; // NOTE: check if sufficiently over the stuck stopline + const bool is_over_stuck_stopline = + util::isOverTargetIndex(*path, closest_idx, current_pose.pose, stuck_line_idx) && + dist_stuck_stopline > eps; + if (is_stuck && !is_over_stuck_stopline) { + stop_line_idx_final = stuck_line_idx; + pass_judge_line_idx_final = stuck_line_idx; + } else if ( + ((is_stuck && is_over_stuck_stopline) || has_collision) && stop_lines_idx_opt.has_value()) { + stop_line_idx_final = stop_lines_idx_opt.value().stop_line; + pass_judge_line_idx_final = stop_lines_idx_opt.value().pass_judge_line; } } + if (stop_line_idx_final == -1) { + RCLCPP_DEBUG(logger_, "detection_area is empty, no plan needed"); + RCLCPP_DEBUG(logger_, "===== plan end ====="); + setSafe(true); + setDistance(std::numeric_limits::lowest()); + return false; + } + + const bool is_over_pass_judge_line = + util::isOverTargetIndex(*path, closest_idx, current_pose.pose, pass_judge_line_idx_final); + const bool is_before_keep_detection_line = + stop_lines_idx_opt.has_value() + ? util::isBeforeTargetIndex( + *path, closest_idx, current_pose.pose, stop_lines_idx_opt.value().keep_detection_line) + : false; + const bool keep_detection = + is_before_keep_detection_line && std::fabs(current_vel) < planner_param_.keep_detection_vel_thr; + + if (is_over_pass_judge_line && keep_detection) { + // in case ego could not stop exactly before the stop line, but with some overshoot, + // keep detection within some margin under low velocity threshold + } else if (is_over_pass_judge_line && is_go_out_ && !external_stop) { + RCLCPP_DEBUG(logger_, "over the keep_detection line and not low speed. no plan needed."); + RCLCPP_DEBUG(logger_, "===== plan end ====="); + setSafe(true); + setDistance(motion_utils::calcSignedArcLength( + path->points, planner_data_->current_pose.pose.position, + path->points.at(stop_line_idx_final).point.pose.position)); + return true; + } + state_machine_.setStateWithMarginTime( is_entry_prohibited ? StateMachine::State::STOP : StateMachine::State::GO, logger_.get_child("state_machine"), *clock_); @@ -314,8 +310,11 @@ bool IntersectionModule::checkCollision( using lanelet::utils::getPolygonFromArcLength; /* generate ego-lane polygon */ - const auto ego_poly = - generateEgoIntersectionLanePolygon(lanelet_map_ptr, path, closest_idx, closest_idx, 0.0, 0.0); + const auto ego_lane_poly = lanelet_map_ptr->laneletLayer.get(module_id_).polygon2d(); + Polygon2d ego_poly{}; + for (const auto & p : ego_lane_poly) { + ego_poly.outer().emplace_back(p.x(), p.y()); + } lanelet::ConstLanelets ego_lane_with_next_lane = getEgoLaneWithNextLane(lanelet_map_ptr, path); lanelet::ConstLanelet closest_lanelet; lanelet::utils::query::getClosestLanelet( @@ -475,7 +474,7 @@ bool IntersectionModule::checkCollision( Polygon2d IntersectionModule::generateEgoIntersectionLanePolygon( lanelet::LaneletMapConstPtr lanelet_map_ptr, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, - const int start_idx, const double extra_dist, const double ignore_dist) const + const double extra_dist, const double ignore_dist) const { using lanelet::utils::getArcCoordinates; using lanelet::utils::getLaneletLength3d; @@ -484,15 +483,14 @@ Polygon2d IntersectionModule::generateEgoIntersectionLanePolygon( lanelet::ConstLanelets ego_lane_with_next_lane = getEgoLaneWithNextLane(lanelet_map_ptr, path); - const auto start_arc_coords = getArcCoordinates( - ego_lane_with_next_lane, tier4_autoware_utils::getPose(path.points.at(start_idx).point)); + const double intersection_exit_length = getLaneletLength3d(ego_lane_with_next_lane.front()); const auto closest_arc_coords = getArcCoordinates( ego_lane_with_next_lane, tier4_autoware_utils::getPose(path.points.at(closest_idx).point)); - const double start_arc_length = start_arc_coords.length + ignore_dist < closest_arc_coords.length - ? closest_arc_coords.length - : start_arc_coords.length + ignore_dist; + const double start_arc_length = intersection_exit_length - ignore_dist > closest_arc_coords.length + ? intersection_exit_length - ignore_dist + : closest_arc_coords.length; const double end_arc_length = getLaneletLength3d(ego_lane_with_next_lane.front()) + extra_dist; diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp index a7e1739c38119..1f4cba618fe28 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp @@ -148,7 +148,7 @@ std::pair, std::optional> generateStopLine( const double keep_detection_line_margin, const bool use_stuck_stopline, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock) + [[maybe_unused]] const rclcpp::Clock::SharedPtr clock) { /* set judge line dist */ const double current_vel = planner_data->current_velocity->twist.linear.x; @@ -196,13 +196,6 @@ std::pair, std::optional> generateStopLine( if (use_stuck_stopline) { // the first point in intersection lane stuck_stop_line_idx_ip = lane_interval_ip_start; - if (stuck_stop_line_idx_ip == 0) { - RCLCPP_WARN_SKIPFIRST_THROTTLE( - logger, *clock, 1000 /* ms */, - "use_stuck_stopline, but ego is already in the intersection, not generating stuck stop " - "line"); - return {std::nullopt, std::nullopt}; - } } else { const auto stuck_stop_line_idx_ip_opt = util::getFirstPointInsidePolygons( path_ip, lane_interval_ip_start, lane_interval_ip_end, lane_id, conflicting_areas); From fb4164f1d6a74e96603c657d764b4a5587d08c84 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 20 Dec 2022 18:11:29 +0900 Subject: [PATCH 6/8] fix(behavior_path_planner): candidate path header (#2536) Signed-off-by: kosuke55 Signed-off-by: kosuke55 --- .../behavior_path_planner/src/behavior_path_planner_node.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 58afd67a77f14..f9d98b0241629 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -788,6 +788,9 @@ Path BehaviorPathPlannerNode::convertToPath( } output = util::toPath(*path_candidate_ptr); + // header is replaced by the input one, so it is substituted again + output.header = planner_data_->route_handler->getRouteHeader(); + output.header.stamp = this->now(); if (!is_ready) { for (auto & point : output.points) { From 9085f2f31f0d8cbba6cbe3ffc3b8ca447631ad32 Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Tue, 20 Dec 2022 20:28:57 +0900 Subject: [PATCH 7/8] fix(tier4_control_launch): add parameter about nearest search (#2542) Signed-off-by: tomoya.kimura Signed-off-by: tomoya.kimura --- .../operation_mode_transition_manager.param.yaml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/launch/tier4_control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml b/launch/tier4_control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml index a0ddef6e363cc..a86443f5cabdb 100644 --- a/launch/tier4_control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml +++ b/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 From 078857d5e3dda8028f7a0cddd08cb3613f83d644 Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Tue, 20 Dec 2022 22:49:53 +0900 Subject: [PATCH 8/8] feat(behavior_path_planner): rename drivable area name (#2535) * feat(behavior_path_planner): rename drivable area name Signed-off-by: yutaka * update document Signed-off-by: yutaka * update Signed-off-by: yutaka * update Signed-off-by: yutaka Signed-off-by: yutaka --- .../behavior_path_planner_avoidance-design.md | 22 ------------------ .../behavior_path_planner_drivable_area.md | 14 +++++++++++ .../config/behavior_path_planner.param.yaml | 2 +- ...drivable_area_boundary_marker_example1.png | Bin ...drivable_area_boundary_marker_example2.png | Bin .../behavior_path_planner_node.hpp | 2 +- .../behavior_path_planner/parameters.hpp | 4 ++-- .../behavior_path_planner/utilities.hpp | 2 +- .../src/behavior_path_planner_node.cpp | 17 +++++++------- .../behavior_path_planner/src/utilities.cpp | 2 +- 10 files changed, 28 insertions(+), 37 deletions(-) rename planning/behavior_path_planner/image/{avoidance_design => drivable_area}/drivable_area_boundary_marker_example1.png (100%) rename planning/behavior_path_planner/image/{avoidance_design => drivable_area}/drivable_area_boundary_marker_example2.png (100%) diff --git a/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md b/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md index 821190b62956b..3a2bc1a0cd1c0 100644 --- a/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md +++ b/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md @@ -513,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. diff --git a/planning/behavior_path_planner/behavior_path_planner_drivable_area.md b/planning/behavior_path_planner/behavior_path_planner_drivable_area.md index 88dcf30299a59..0d17c8c9fd509 100644 --- a/planning/behavior_path_planner/behavior_path_planner_drivable_area.md +++ b/planning/behavior_path_planner/behavior_path_planner_drivable_area.md @@ -76,3 +76,17 @@ std::vector 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` diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml index da2342f6782d4..6dce3ff06206b 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/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 diff --git a/planning/behavior_path_planner/image/avoidance_design/drivable_area_boundary_marker_example1.png b/planning/behavior_path_planner/image/drivable_area/drivable_area_boundary_marker_example1.png similarity index 100% rename from planning/behavior_path_planner/image/avoidance_design/drivable_area_boundary_marker_example1.png rename to planning/behavior_path_planner/image/drivable_area/drivable_area_boundary_marker_example1.png diff --git a/planning/behavior_path_planner/image/avoidance_design/drivable_area_boundary_marker_example2.png b/planning/behavior_path_planner/image/drivable_area/drivable_area_boundary_marker_example2.png similarity index 100% rename from planning/behavior_path_planner/image/avoidance_design/drivable_area_boundary_marker_example2.png rename to planning/behavior_path_planner/image/drivable_area/drivable_area_boundary_marker_example2.png diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 75d1f798d8b65..d4bc06a33d349 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -178,7 +178,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node const BehaviorModuleOutput & output); // debug - rclcpp::Publisher::SharedPtr debug_drivable_area_lanelets_publisher_; + rclcpp::Publisher::SharedPtr debug_maximum_drivable_area_publisher_; rclcpp::Publisher::SharedPtr debug_avoidance_msg_array_publisher_; rclcpp::Publisher::SharedPtr debug_lane_change_msg_array_publisher_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index 8103092e23cdc..7caa25a9b4cf6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -58,8 +58,8 @@ struct BehaviorPathPlannerParameters double base_link2front; double base_link2rear; - // drivable area visualization - bool visualize_drivable_area_for_shared_linestrings_lanelet; + // maximum drivable area visualization + bool visualize_maximum_drivable_area; // collision check double lateral_distance_max_threshold; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp index 6b684998141c5..2de16855391ff 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -296,7 +296,7 @@ void generateDrivableArea( PathWithLaneId & path, const std::vector & lanes, const double vehicle_length, const std::shared_ptr planner_data, const bool is_driving_forward = true); -lanelet::ConstLineStrings3d getDrivableAreaForAllSharedLinestringLanelets( +lanelet::ConstLineStrings3d getMaximumDrivableArea( const std::shared_ptr & planner_data); /** diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index f9d98b0241629..5f5de72b26160 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -69,9 +69,9 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod debug_lane_change_msg_array_publisher_ = create_publisher("~/debug/lane_change_debug_message_array", 1); - if (planner_data_->parameters.visualize_drivable_area_for_shared_linestrings_lanelet) { - debug_drivable_area_lanelets_publisher_ = - create_publisher("~/drivable_area_boundary", 1); + if (planner_data_->parameters.visualize_maximum_drivable_area) { + debug_maximum_drivable_area_publisher_ = + create_publisher("~/maximum_drivable_area", 1); } bound_publisher_ = create_publisher("~/debug/bound", 1); @@ -232,8 +232,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.turn_signal_on_swerving = declare_parameter("turn_signal_on_swerving", true); p.path_interval = declare_parameter("path_interval"); - p.visualize_drivable_area_for_shared_linestrings_lanelet = - declare_parameter("visualize_drivable_area_for_shared_linestrings_lanelet", true); + p.visualize_maximum_drivable_area = declare_parameter("visualize_maximum_drivable_area", true); p.ego_nearest_dist_threshold = declare_parameter("ego_nearest_dist_threshold"); p.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); @@ -655,10 +654,10 @@ void BehaviorPathPlannerNode::run() publishSceneModuleDebugMsg(); - if (planner_data->parameters.visualize_drivable_area_for_shared_linestrings_lanelet) { - const auto drivable_area_lines = marker_utils::createFurthestLineStringMarkerArray( - util::getDrivableAreaForAllSharedLinestringLanelets(planner_data)); - debug_drivable_area_lanelets_publisher_->publish(drivable_area_lines); + if (planner_data->parameters.visualize_maximum_drivable_area) { + const auto maximum_drivable_area = + marker_utils::createFurthestLineStringMarkerArray(util::getMaximumDrivableArea(planner_data)); + debug_maximum_drivable_area_publisher_->publish(maximum_drivable_area); } mutex_bt_.unlock(); diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index d335c6c13b582..c38b54086b3fd 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -1665,7 +1665,7 @@ std::shared_ptr generateCenterLinePath( // TODO(Azu) Some parts of is the same with generateCenterLinePath. Therefore it might be better if // we can refactor some of the code for better readability -lanelet::ConstLineStrings3d getDrivableAreaForAllSharedLinestringLanelets( +lanelet::ConstLineStrings3d getMaximumDrivableArea( const std::shared_ptr & planner_data) { const auto & p = planner_data->parameters;