Skip to content

Commit

Permalink
perf(crosswalk): reduce processing time (autowarefoundation#2137) (au…
Browse files Browse the repository at this point in the history 
…towarefoundation#175)

* perf(crosswalk): use sparse resampled path

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* perf(crosswalk): create attention area all at once

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>
Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>
  • Loading branch information
@tkimura4 @satoshi-ota
tkimura4 and satoshi-ota authored Nov 11, 2022
1 parent f933658 commit e1a9339
Show file tree
Hide file tree
Showing 2 changed files with 54 additions and 39 deletions.
2 changes: 2 additions & 0 deletions planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -117,6 +117,8 @@ class CrosswalkModule : public SceneModuleInterface

std::vector<CollisionPoint> getCollisionPoints(
const PathWithLaneId & ego_path, const PredictedObject & object,
const boost::geometry::model::polygon<boost::geometry::model::d2::point_xy<double>> &
attention_area,
const std::pair<double, double> & crosswalk_attention_range);

std::pair<double, double> getAttentionRange(const PathWithLaneId & ego_path);
Expand Down
91 changes: 52 additions & 39 deletions planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
#include <rclcpp/rclcpp.hpp>
#include <scene_module/crosswalk/scene_crosswalk.hpp>
#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
#include <utilization/path_utilization.hpp>
#include <utilization/util.hpp>

#include <cmath>
Expand Down Expand Up @@ -464,13 +465,19 @@ boost::optional<std::pair<size_t, PathPointWithLaneId>> CrosswalkModule::findNea
bool found_pedestrians = false;
bool found_stuck_vehicle = false;

const auto crosswalk_attention_range = getAttentionRange(ego_path);
PathWithLaneId sparse_resample_path{};
constexpr double RESAMPLE_INTERVAL = 4.0;
if (!splineInterpolate(ego_path, RESAMPLE_INTERVAL, sparse_resample_path, logger_)) {
return {};
}

const auto crosswalk_attention_range = getAttentionRange(sparse_resample_path);
const auto & ego_pos = planner_data_->current_pose.pose.position;
const auto & objects_ptr = planner_data_->predicted_objects;
const auto & base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;

bool exist_stopline_in_map;
const auto p_stop_line = getStopLine(ego_path, exist_stopline_in_map);
const auto p_stop_line = getStopLine(sparse_resample_path, exist_stopline_in_map);
if (!p_stop_line) {
return {};
}
Expand All @@ -482,13 +489,44 @@ boost::optional<std::pair<size_t, PathPointWithLaneId>> CrosswalkModule::findNea

const auto ignore_crosswalk = debug_data_.ignore_crosswalk = isRedSignalForPedestrians();

const auto ego_polygon = createVehiclePolygon(planner_data_->vehicle_info_);

Polygon attention_area;
for (size_t j = 0; j < sparse_resample_path.points.size() - 1; ++j) {
const auto & p_ego_front = sparse_resample_path.points.at(j).point.pose;
const auto & p_ego_back = sparse_resample_path.points.at(j + 1).point.pose;
const auto front_length =
calcSignedArcLength(sparse_resample_path.points, ego_pos, p_ego_front.position);
const auto back_length =
calcSignedArcLength(sparse_resample_path.points, ego_pos, p_ego_back.position);

if (back_length < crosswalk_attention_range.first) {
continue;
}

if (crosswalk_attention_range.second < front_length) {
break;
}

const auto ego_one_step_polygon = createOneStepPolygon(p_ego_front, p_ego_back, ego_polygon);

debug_data_.ego_polygons.push_back(toMsg(ego_one_step_polygon, ego_pos.z));

std::vector<Polygon> unions;
bg::union_(attention_area, ego_one_step_polygon, unions);
if (!unions.empty()) {
attention_area = unions.front();
bg::correct(attention_area);
}
}

for (const auto & object : objects_ptr->objects) {
const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position;
const auto & obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear;
const auto obj_uuid = toHexString(object.object_id);

if (isVehicle(object)) {
found_stuck_vehicle = found_stuck_vehicle || isStuckVehicle(ego_path, object);
found_stuck_vehicle = found_stuck_vehicle || isStuckVehicle(sparse_resample_path, object);
}

if (!isTargetType(object)) {
Expand All @@ -499,7 +537,8 @@ boost::optional<std::pair<size_t, PathPointWithLaneId>> CrosswalkModule::findNea
continue;
}

for (auto & cp : getCollisionPoints(ego_path, object, crosswalk_attention_range)) {
for (auto & cp : getCollisionPoints(
sparse_resample_path, object, attention_area, crosswalk_attention_range)) {
const auto is_ignore_object = ignore_objects_.count(obj_uuid) != 0;
if (is_ignore_object) {
cp.state = CollisionPointState::IGNORE;
Expand All @@ -521,8 +560,9 @@ boost::optional<std::pair<size_t, PathPointWithLaneId>> CrosswalkModule::findNea
found_pedestrians = true;
stop_factor.stop_factor_points.push_back(obj_pos);

const auto dist_ego2cp = calcSignedArcLength(ego_path.points, ego_pos, cp.collision_point) -
planner_param_.stop_margin;
const auto dist_ego2cp =
calcSignedArcLength(sparse_resample_path.points, ego_pos, cp.collision_point) -
planner_param_.stop_margin;

if (dist_ego2cp < minimum_stop_dist) {
first_stop_point = cp.collision_point;
Expand Down Expand Up @@ -577,12 +617,13 @@ boost::optional<std::pair<size_t, PathPointWithLaneId>> CrosswalkModule::findNea
const auto & p_stop = stop_at_stop_line ? p_stop_line.get().second : first_stop_point;
const auto stop_line_distance = exist_stopline_in_map ? 0.0 : planner_param_.stop_line_distance;
const auto margin = stop_at_stop_line ? stop_line_distance : planner_param_.stop_margin;

const size_t base_idx = findNearestSegmentIndex(ego_path.points, p_stop);
const auto residual_length = calcLongitudinalOffsetToSegment(ego_path.points, base_idx, p_stop);
const size_t base_idx = findNearestSegmentIndex(sparse_resample_path.points, p_stop);
const auto residual_length =
calcLongitudinalOffsetToSegment(sparse_resample_path.points, base_idx, p_stop);
const auto update_margin = margin - residual_length + base_link2front;

const auto stop_point = getBackwardInsertPointFromBasePoint(base_idx, ego_path, update_margin);
const auto stop_point =
getBackwardInsertPointFromBasePoint(base_idx, sparse_resample_path, update_margin);
stop_factor.stop_pose = stop_point.get().second.point.pose;

return stop_point;
Expand Down Expand Up @@ -802,7 +843,7 @@ void CrosswalkModule::clampAttentionRangeByNeighborCrosswalks(
}

std::vector<CollisionPoint> CrosswalkModule::getCollisionPoints(
const PathWithLaneId & ego_path, const PredictedObject & object,
const PathWithLaneId & ego_path, const PredictedObject & object, const Polygon & attention_area,
const std::pair<double, double> & crosswalk_attention_range)
{
stop_watch_.tic(__func__);
Expand All @@ -814,37 +855,9 @@ std::vector<CollisionPoint> CrosswalkModule::getCollisionPoints(
const auto & ego_pos = planner_data_->current_pose.pose.position;
const auto & ego_vel = planner_data_->current_velocity->twist.linear;

const auto ego_polygon = createVehiclePolygon(planner_data_->vehicle_info_);
const auto obj_polygon =
createObjectPolygon(object.shape.dimensions.x, object.shape.dimensions.y);

Polygon attention_area;
for (size_t j = 0; j < ego_path.points.size() - 1; ++j) {
const auto & p_ego_front = ego_path.points.at(j).point.pose;
const auto & p_ego_back = ego_path.points.at(j + 1).point.pose;
const auto front_length = calcSignedArcLength(ego_path.points, ego_pos, p_ego_front.position);
const auto back_length = calcSignedArcLength(ego_path.points, ego_pos, p_ego_back.position);

if (back_length < crosswalk_attention_range.first) {
continue;
}

if (crosswalk_attention_range.second < front_length) {
break;
}

const auto ego_one_step_polygon = createOneStepPolygon(p_ego_front, p_ego_back, ego_polygon);

debug_data_.ego_polygons.push_back(toMsg(ego_one_step_polygon, ego_pos.z));

std::vector<Polygon> unions;
bg::union_(attention_area, ego_one_step_polygon, unions);
if (!unions.empty()) {
attention_area = unions.front();
bg::correct(attention_area);
}
}

for (const auto & obj_path : object.kinematics.predicted_paths) {
for (size_t i = 0; i < obj_path.path.size() - 1; ++i) {
const auto p_obj_front = obj_path.path.at(i);
Expand Down

0 comments on commit e1a9339

Please sign in to comment.