Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

perf(intersection): reduce bg::within call #2190

Merged
merged 4 commits into from
Nov 5, 2022
Merged

perf(intersection): reduce bg::within call #2190

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
e68bcc6
perf(intersection): reduce bg::within call
soblin Nov 1, 2022
7749634
use lane interval for getStopLineFromMap too
soblin Nov 4, 2022
feb9f32
changed index type to size_t
soblin Nov 5, 2022
686ff38
replace unsigned to size_t
soblin Nov 5, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
13 changes: 10 additions & 3 deletions planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@
#include <optional>
#include <string>
#include <tuple>
#include <utility>
#include <vector>

namespace behavior_velocity_planner
Expand All @@ -42,6 +43,8 @@ int insertPoint(
autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path);

bool hasLaneId(const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p, const int id);
std::optional<std::pair<int, int>> findLaneIdInterval(
const autoware_auto_planning_msgs::msg::PathWithLaneId & p, const int lane_id);
bool getDuplicatedPointIdx(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
const geometry_msgs::msg::Point & point, int * duplicated_point_idx);
Expand Down Expand Up @@ -98,11 +101,14 @@ bool generateStopLineBeforeIntersection(
/**
* @brief Calculate first path index that is in the polygon.
* @param path target path
* @param lane_interval_start the start index of point on the lane
* @param lane_interval_end the last index of point on the lane
* @param polygons target polygon
* @return path point index
*/
int getFirstPointInsidePolygons(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
std::optional<int> getFirstPointInsidePolygons(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int lane_interval_start,
const int lane_interval_end, const int lane_id,
const std::vector<lanelet::CompoundPolygon3d> & polygons);

/**
Expand All @@ -111,7 +117,8 @@ int getFirstPointInsidePolygons(
* @return true when the stop point is defined on map.
*/
bool getStopLineIndexFromMap(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int lane_id,
const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int lane_interval_start,
const int lane_interval_end, const int lane_id,
const std::shared_ptr<const PlannerData> & planner_data, int * stop_idx_ip, int dist_thr,
const rclcpp::Logger logger);

Expand Down
56 changes: 44 additions & 12 deletions planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -74,6 +74,29 @@ bool hasLaneId(const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p,
return false;
}

std::optional<std::pair<int, int>> findLaneIdInterval(
tkimura4 marked this conversation as resolved.
Show resolved Hide resolved
const autoware_auto_planning_msgs::msg::PathWithLaneId & p, const int lane_id)
{
bool found = false;
int start = 0;
int end = p.points.size() - 1;
for (unsigned i = 0; i < p.points.size(); ++i) {
tkimura4 marked this conversation as resolved.
Show resolved Hide resolved
if (hasLaneId(p.points.at(i), lane_id)) {
if (!found) {
// found interval for the first time
found = true;
start = static_cast<int>(i);
tkimura4 marked this conversation as resolved.
Show resolved Hide resolved
}
} else if (found) {
// prior point was in the interval. interval ended
end = i;
break;
}
}
start = std::max(0, start - 1); // the idx of last point before the interval
return found ? std::make_optional(std::make_pair(start, end)) : std::nullopt;
}

bool getDuplicatedPointIdx(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
const geometry_msgs::msg::Point & point, int * duplicated_point_idx)
Expand All @@ -91,13 +114,13 @@ bool getDuplicatedPointIdx(
return false;
}

// TODO(Mamoru Sobue): return optional, not -1
int getFirstPointInsidePolygons(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
std::optional<int> getFirstPointInsidePolygons(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int lane_interval_start,
const int lane_interval_end, [[maybe_unused]] const int lane_id,
const std::vector<lanelet::CompoundPolygon3d> & polygons)
{
int first_idx_inside_lanelet = -1;
for (size_t i = 0; i < path.points.size(); ++i) {
std::optional<int> first_idx_inside_lanelet = std::nullopt;
for (int i = lane_interval_start; i <= lane_interval_end; ++i) {
bool is_in_lanelet = false;
auto p = path.points.at(i).point.pose.position;
for (const auto & polygon : polygons) {
Expand Down Expand Up @@ -135,7 +158,6 @@ bool generateStopLine(
/* spline interpolation */
constexpr double interval = 0.2;
autoware_auto_planning_msgs::msg::PathWithLaneId path_ip;
// TODO(Mamoru Sobue): crop only intersection part of path for computation cost
if (!splineInterpolate(target_path, interval, path_ip, logger)) {
return false;
}
Expand All @@ -157,17 +179,27 @@ bool generateStopLine(
// stop_line_margin).
// stop point index for interpolated(ip) path.
int stop_idx_ip;
if (getStopLineIndexFromMap(path_ip, lane_id, planner_data, &stop_idx_ip, 10.0, logger)) {
const auto lane_interval_opt = util::findLaneIdInterval(path_ip, lane_id);
if (!lane_interval_opt.has_value()) {
RCLCPP_INFO(logger, "Path has no interval on intersection lane %d", lane_id);
return false;
}
const auto [lane_interval_start, lane_interval_end] = lane_interval_opt.value();
if (getStopLineIndexFromMap(
path_ip, lane_interval_start, lane_interval_end, lane_id, planner_data, &stop_idx_ip, 10.0,
logger)) {
stop_idx_ip = std::max(stop_idx_ip - base2front_idx_dist, 0);
} else {
// find the index of the first point that intersects with detection_areas
const int first_idx_ip_inside_lane = getFirstPointInsidePolygons(path_ip, detection_areas);
const auto first_idx_ip_inside_lane_opt = getFirstPointInsidePolygons(
path_ip, lane_interval_start, lane_interval_end, lane_id, detection_areas);
// if path is not intersecting with detection_area, skip
if (first_idx_ip_inside_lane == -1) {
if (!first_idx_ip_inside_lane_opt.has_value()) {
RCLCPP_DEBUG(
logger, "Path is not intersecting with detection_area, not generating stop_line");
return false;
}
const auto first_idx_ip_inside_lane = first_idx_ip_inside_lane_opt.value();
const auto & first_inside_point = path_ip.points.at(first_idx_ip_inside_lane).point.pose;
const auto first_idx_inside_lane_opt =
motion_utils::findNearestIndex(original_path->points, first_inside_point, 10.0, M_PI_4);
Expand Down Expand Up @@ -233,7 +265,8 @@ bool generateStopLine(
}

bool getStopLineIndexFromMap(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int lane_id,
const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int lane_interval_start,
const int lane_interval_end, const int lane_id,
const std::shared_ptr<const PlannerData> & planner_data, int * stop_idx_ip, int dist_thr,
const rclcpp::Logger logger)
{
Expand All @@ -258,7 +291,7 @@ bool getStopLineIndexFromMap(
const LineString2d extended_stop_line =
planning_utils::extendLine(p_start, p_end, planner_data->stop_line_extend_length);

for (size_t i = 0; i < path.points.size() - 1; i++) {
for (int i = lane_interval_start; i <= lane_interval_end; i++) {
const auto & p_front = path.points.at(i).point.pose.position;
const auto & p_back = path.points.at(i + 1).point.pose.position;

Expand Down Expand Up @@ -295,7 +328,6 @@ bool getStopLineIndexFromMap(
return true;
}

// TODO(Mamoru Sobue): return std::tuple<bool, lanelet::ConstLanelets, lanelet::ConstLanelets>
std::tuple<lanelet::ConstLanelets, lanelet::ConstLanelets> getObjectiveLanelets(
lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr,
const int lane_id, const double detection_area_length, const bool tl_arrow_solid_on)
Expand Down