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

feat(out_of_lane): improve reuse of previous decision #1017

Merged
merged 3 commits into from
Dec 5, 2023
Merged

feat(out_of_lane): improve reuse of previous decision #1017

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
a4114d9
Do not directly reuse a prev stop point but project it on the new path
maxime-clem Oct 20, 2023
d6607d2
Improve reuse of the previously inserted stop point
maxime-clem Oct 20, 2023
441b876
Fix precision of inserted stop point
maxime-clem Oct 23, 2023
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
3 changes: 2 additions & 1 deletion planning/behavior_velocity_out_of_lane_module/src/overlapping_range.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,8 @@ OverlapRanges calculate_overlapping_ranges(
/// @param [in] path_lanelets path lanelets used to calculate arc length along the ego path
/// @param [in] lanelets lanelets used to calculate the overlaps
/// @param [in] params parameters
/// @return the overlapping ranges found between the footprints and the lanelets
/// @return the overlapping ranges found between the footprints and the lanelets, sorted by
/// increasing arc length along the path
OverlapRanges calculate_overlapping_ranges(
const std::vector<lanelet::BasicPolygon2d> & path_footprints,
const lanelet::ConstLanelets & path_lanelets, const lanelet::ConstLanelets & lanelets,
Expand Down
42 changes: 23 additions & 19 deletions planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -62,18 +62,6 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
ego_data.path.points = path->points;
ego_data.first_path_idx =
motion_utils::findNearestSegmentIndex(ego_data.path.points, ego_data.pose.position);
for (const auto & p : prev_overlapping_path_points_) {
const auto nearest_idx =
motion_utils::findNearestIndex(ego_data.path.points, p.point.pose, 1.0);
const auto insert_idx =
motion_utils::findNearestSegmentIndex(ego_data.path.points, p.point.pose, 1.0);
if (nearest_idx && insert_idx && *insert_idx > ego_data.first_path_idx) {
if (
tier4_autoware_utils::calcDistance2d(
ego_data.path.points[*nearest_idx].point.pose, p.point.pose) > 0.5)
ego_data.path.points.insert(std::next(ego_data.path.points.begin(), *insert_idx), p);
}
}
motion_utils::removeOverlapPoints(ego_data.path.points);
ego_data.velocity = planner_data_->current_velocity->twist.linear.x;
ego_data.max_decel = -planner_data_->max_stop_acceleration_threshold;
Expand Down Expand Up @@ -113,10 +101,6 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
stopwatch.tic("calculate_overlapping_ranges");
const auto ranges =
calculate_overlapping_ranges(path_footprints, path_lanelets, other_lanelets, params_);
prev_overlapping_path_points_.clear();
for (const auto & range : ranges)
prev_overlapping_path_points_.push_back(
ego_data.path.points[ego_data.first_path_idx + range.entering_path_idx]);
const auto calculate_overlapping_ranges_us = stopwatch.toc("calculate_overlapping_ranges");
// Calculate stop and slowdown points
stopwatch.tic("calculate_decisions");
Expand All @@ -143,7 +127,28 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
(!prev_inserted_point_ || prev_inserted_point_->slowdown.lane_to_avoid.id() ==
point_to_insert->slowdown.lane_to_avoid.id()))
prev_inserted_point_time_ = clock_->now();
if (!point_to_insert && prev_inserted_point_) point_to_insert = prev_inserted_point_;
// reuse previous stop point if there is no new one or if its velocity is not higher than the new
// one and its arc length is lower
const auto should_use_prev_inserted_point = [&]() {
if (
point_to_insert && prev_inserted_point_ &&
prev_inserted_point_->slowdown.velocity <= point_to_insert->slowdown.velocity) {
const auto arc_length = motion_utils::calcSignedArcLength(
path->points, 0LU, point_to_insert->point.point.pose.position);
const auto prev_arc_length = motion_utils::calcSignedArcLength(
path->points, 0LU, prev_inserted_point_->point.point.pose.position);
return prev_arc_length < arc_length;
}
return !point_to_insert && prev_inserted_point_;
}();
if (should_use_prev_inserted_point) {
// if the path changed the prev point is no longer on the path so we project it
const auto insert_arc_length = motion_utils::calcSignedArcLength(
path->points, 0LU, prev_inserted_point_->point.point.pose.position);
prev_inserted_point_->point.point.pose =
motion_utils::calcInterpolatedPose(path->points, insert_arc_length);
point_to_insert = prev_inserted_point_;
}
if (point_to_insert) {
prev_inserted_point_ = point_to_insert;
RCLCPP_INFO(logger_, "Avoiding lane %lu", point_to_insert->slowdown.lane_to_avoid.id());
Expand All @@ -152,8 +157,7 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
path->points, point_to_insert->point.point.pose.position) +
1;
planning_utils::insertVelocity(
*path, point_to_insert->point, point_to_insert->slowdown.velocity, path_idx,
params_.precision);
*path, point_to_insert->point, point_to_insert->slowdown.velocity, path_idx);
if (point_to_insert->slowdown.velocity == 0.0) {
tier4_planning_msgs::msg::StopFactor stop_factor;
stop_factor.stop_pose = point_to_insert->point.point.pose;
Expand Down
2 changes: 0 additions & 2 deletions planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50,8 +50,6 @@ class OutOfLaneModule : public SceneModuleInterface
private:
PlannerParam params_;

std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId>
prev_overlapping_path_points_{};
std::optional<SlowdownToInsert> prev_inserted_point_{};
rclcpp::Time prev_inserted_point_time_{};

Expand Down
Loading