Skip to content

Commit

Permalink
minor refactor
Browse files Browse the repository at this point in the history
Signed-off-by: kyoichi-sugahara <[email protected]>
  • Loading branch information
kyoichi-sugahara committed Oct 4, 2023
1 parent 2eb497f commit 1ddc01f
Show file tree
Hide file tree
Showing 2 changed files with 13 additions and 27 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@
# For the case where the stop position is determined according to the object position.
stop_distance_from_object: 2.0 # [m] the vehicle decelerates to be able to stop in front of object with margin
# If the ahead stop position is already inserted, use the same position for the new stop point.
max_ahead_longitudinal_margin: 3.0 # [m] specifies the margin between the current position and the stop point
max_ahead_longitudinal_margin: 5.0 # [m] specifies the margin between the current position and the stop point

# param for ego's slow down velocity
slow_down:
Expand Down
38 changes: 12 additions & 26 deletions planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -331,7 +331,7 @@ std::vector<Point> CrosswalkModule::searchAheadInsertedStopPoint(
break;
}
if (std::abs(ego_path.points.at(idx).point.longitudinal_velocity_mps) < epsilon) {
RCLCPP_INFO_EXPRESSION(
RCLCPP_INFO(
logger_, "Stop point is found and distance from inserted stop point: %f module_id: %ld",
calcSignedArcLength(ego_path.points, candidate_stop_point_idx, idx), module_id_);
const auto stop_point = createPoint(
Expand Down Expand Up @@ -415,27 +415,22 @@ std::optional<StopFactor> CrosswalkModule::checkStopForCrosswalkUsers(
if (!nearest_stop_info) {
return {};
}

std::vector<Point> ahead_inserted_stop_point{};
bool stop_point_inserted = false;
bool inserted_stop_point_is_front_of_crosswalk = false;

// TBD
if (default_stop_pose.has_value()) {
const double ahead_margin = planner_param_.max_ahead_longitudinal_margin;
const Point default_stop_point = createPoint(
default_stop_pose->position.x, default_stop_pose->position.y, default_stop_pose->position.z);
ahead_inserted_stop_point =
searchAheadInsertedStopPoint(ego_path, default_stop_point, ahead_margin);

stop_point_inserted = !ahead_inserted_stop_point.empty();
// don't insert stop point if the stop point is inserted within ***[m] ahead
// NOTE: Make sure that the stop point is located before the crosswalk.
if (stop_point_inserted) {
if (!ahead_inserted_stop_point.empty()) {
const double dist_inserted_stop_point2crosswalk = calcSignedArcLength(
ego_path.points, path_intersects.front(), ahead_inserted_stop_point.front());
inserted_stop_point_is_front_of_crosswalk = dist_inserted_stop_point2crosswalk + base_link2front < 0.0;
RCLCPP_INFO_EXPRESSION(logger_, "Distance to crosswalk: %f", dist_inserted_stop_point2crosswalk);
inserted_stop_point_is_front_of_crosswalk =
dist_inserted_stop_point2crosswalk + base_link2front < 0.0;
RCLCPP_INFO(logger_, "Distance to crosswalk: %f", dist_inserted_stop_point2crosswalk);
}
}

Expand All @@ -444,23 +439,14 @@ std::optional<StopFactor> CrosswalkModule::checkStopForCrosswalkUsers(
dist_ego_to_stop < nearest_stop_info->second &&
nearest_stop_info->second < dist_ego_to_stop + planner_param_.far_object_threshold;

// std::cerr << "stop_at_stop_line:" << stop_at_stop_line << std::endl;
// std::cerr << "default_stop_pose:" << default_stop_pose.has_value() << std::endl;
// std::cerr << "stop_line_found:" << stop_line_found << std::endl;
// std::cerr << "stop_point_inserted:" << stop_point_inserted << std::endl;
// std::cerr << "inserted_stop_point_is_front_of_crosswalk:"
// << inserted_stop_point_is_front_of_crosswalk << std::endl;
if (stop_at_stop_line) {
// Stop at the stop line
if (default_stop_pose) {
if (inserted_stop_point_is_front_of_crosswalk) {
RCLCPP_INFO_EXPRESSION(logger_, "Insert stop point ahead");
const auto inserted_stop_pose = calcLongitudinalOffsetPose(
ego_path.points, ahead_inserted_stop_point.front(), 0.0);
return createStopFactor(*inserted_stop_pose, ahead_inserted_stop_point);
}
return createStopFactor(*default_stop_pose, stop_factor_points);
if (stop_at_stop_line && default_stop_pose) {
if (inserted_stop_point_is_front_of_crosswalk) {
RCLCPP_INFO(logger_, "change stop point forward");
const auto inserted_stop_pose =
calcLongitudinalOffsetPose(ego_path.points, ahead_inserted_stop_point.front(), 0.0);
return createStopFactor(*inserted_stop_pose, ahead_inserted_stop_point);
}
return createStopFactor(*default_stop_pose, stop_factor_points);
} else {
// Stop beyond the stop line
const auto stop_pose = calcLongitudinalOffsetPose(
Expand Down

0 comments on commit 1ddc01f

Please sign in to comment.