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.

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

fix(crosswalk): don't stop in front of the crosswalk if vehicle stuck in intersection #5722

Merged
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,7 @@

# param for stuck vehicles
stuck_vehicle:
enable_stuck_check_in_intersection: false # [-] flag to enable stuck vehicle checking for crosswalk which is in intersection
stuck_vehicle_velocity: 1.0 # [m/s] maximum velocity threshold whether the vehicle is stuck
max_stuck_vehicle_lateral_offset: 2.0 # [m] maximum lateral offset for stuck vehicle position should be looked
stuck_vehicle_attention_range: 10.0 # [m] the detection area is defined as X meters behind the crosswalk
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -82,7 +82,7 @@ struct DebugData
std::vector<std::vector<geometry_msgs::msg::Point>> obj_polygons;
};

std::vector<lanelet::ConstLanelet> getCrosswalksOnPath(
std::vector<std::pair<int64_t, lanelet::ConstLanelet>> getCrosswalksOnPath(
const geometry_msgs::msg::Pose & current_pose,
const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
const lanelet::LaneletMapPtr lanelet_map,
Expand Down
19 changes: 12 additions & 7 deletions planning/behavior_velocity_crosswalk_module/src/manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -63,6 +63,8 @@
cp.no_relax_velocity = getOrDeclareParameter<double>(node, ns + ".slow_down.no_relax_velocity");

// param for stuck vehicle
cp.enable_stuck_check_in_intersection =
getOrDeclareParameter<bool>(node, ns + ".stuck_vehicle.enable_stuck_check_in_intersection");
cp.stuck_vehicle_velocity =
getOrDeclareParameter<double>(node, ns + ".stuck_vehicle.stuck_vehicle_velocity");
cp.max_stuck_vehicle_lateral_offset =
Expand Down Expand Up @@ -129,8 +131,9 @@
const auto rh = planner_data_->route_handler_;

const auto launch = [this, &path](
const auto lane_id, const std::optional<int64_t> & reg_elem_id) {
if (isModuleRegistered(lane_id)) {
const auto road_lanelet_id, const auto crosswalk_lanelet_id,
const std::optional<int64_t> & reg_elem_id) {

Check warning on line 135 in planning/behavior_velocity_crosswalk_module/src/manager.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_velocity_crosswalk_module/src/manager.cpp#L135

Added line #L135 was not covered by tests
if (isModuleRegistered(crosswalk_lanelet_id)) {
return;
}

Expand All @@ -141,25 +144,27 @@
// NOTE: module_id is always a lane id so that isModuleRegistered works correctly in the case
// where both regulatory element and non-regulatory element crosswalks exist.
registerModule(std::make_shared<CrosswalkModule>(
node_, lane_id, reg_elem_id, lanelet_map_ptr, p, logger, clock_));
generateUUID(lane_id);
node_, road_lanelet_id, crosswalk_lanelet_id, reg_elem_id, lanelet_map_ptr, p, logger,
clock_));
generateUUID(crosswalk_lanelet_id);
updateRTCStatus(
getUUID(lane_id), true, std::numeric_limits<double>::lowest(), path.header.stamp);
getUUID(crosswalk_lanelet_id), true, std::numeric_limits<double>::lowest(),
path.header.stamp);
};

const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath<Crosswalk>(
path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose);

for (const auto & crosswalk : crosswalk_leg_elem_map) {
// NOTE: The former id is a lane id, and the latter one is a regulatory element's id.
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@satoshi-ota
question for my understanging
crosswalk.second.id(): id of the lanelet on the path
crosswalk.first->crosswalkLanelet().id(): id of the crosswalk lanelet
crosswalk.first->id(): id of the crosswalk regulatory element

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes.

launch(crosswalk.first->crosswalkLanelet().id(), crosswalk.first->id());
launch(crosswalk.second.id(), crosswalk.first->crosswalkLanelet().id(), crosswalk.first->id());
}

const auto crosswalk_lanelets = getCrosswalksOnPath(
planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr());

for (const auto & crosswalk : crosswalk_lanelets) {
launch(crosswalk.id(), std::nullopt);
launch(crosswalk.first, crosswalk.second.id(), std::nullopt);
}
}

Expand Down
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2020 Tier IV, Inc.

Check notice on line 1 in planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Overall Code Complexity

The mean cyclomatic complexity increases from 5.34 to 5.50, threshold = 4. This file has many conditional statements (e.g. if, for, while) across its implementation, leading to lower code health. Avoid adding more conditionals.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -176,9 +176,10 @@
} // namespace

CrosswalkModule::CrosswalkModule(
rclcpp::Node & node, const int64_t module_id, const std::optional<int64_t> & reg_elem_id,
const lanelet::LaneletMapPtr & lanelet_map_ptr, const PlannerParam & planner_param,
const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock)
rclcpp::Node & node, const int64_t lane_id, const int64_t module_id,
const std::optional<int64_t> & reg_elem_id, const lanelet::LaneletMapPtr & lanelet_map_ptr,
const PlannerParam & planner_param, const rclcpp::Logger & logger,
const rclcpp::Clock::SharedPtr clock)

Check warning on line 182 in planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp#L182

Added line #L182 was not covered by tests
: SceneModuleInterface(module_id, logger, clock),
module_id_(module_id),
planner_param_(planner_param),
Expand All @@ -200,6 +201,8 @@
crosswalk_ = lanelet_map_ptr->laneletLayer.get(module_id);
}

road_ = lanelet_map_ptr->laneletLayer.get(lane_id);

collision_info_pub_ =
node.create_publisher<tier4_debug_msgs::msg::StringStamped>("~/debug/collision_info", 1);
}
Expand Down Expand Up @@ -803,6 +806,16 @@
return {};
}

// skip stuck vehicle checking for crosswalk which is in intersection.
if (!p.enable_stuck_check_in_intersection) {
std::string turn_direction = road_.attributeOr("turn_direction", "else");
if (turn_direction == "right" || turn_direction == "left" || turn_direction == "straight") {

Check warning on line 812 in planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Complex Conditional

CrosswalkModule::checkStopForStuckVehicles has 1 complex conditionals with 2 branches, threshold = 2. A complex conditional is an expression inside a branch (e.g. if, for, while) which consists of multiple, logical operators such as AND/OR. The more logical operators in an expression, the more severe the code smell.
if (!road_.regulatoryElementsAs<const lanelet::TrafficLight>().empty()) {
return {};
}
}
}

Check notice on line 818 in planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Complex Method

CrosswalkModule::checkStopForStuckVehicles increases in cyclomatic complexity from 16 to 21, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.

Check notice on line 818 in planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Bumpy Road Ahead

CrosswalkModule::checkStopForStuckVehicles increases from 2 to 3 logical blocks with deeply nested code, threshold is one single block per function. The Bumpy Road code smell is a function that contains multiple chunks of nested conditional logic. The deeper the nesting and the more bumps, the lower the code health.
for (const auto & object : objects) {
if (!isVehicle(object)) {
continue;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -121,6 +121,7 @@ class CrosswalkModule : public SceneModuleInterface
double max_slow_down_accel;
double no_relax_velocity;
// param for stuck vehicle
bool enable_stuck_check_in_intersection{false};
double stuck_vehicle_velocity;
double max_stuck_vehicle_lateral_offset;
double stuck_vehicle_attention_range;
Expand Down Expand Up @@ -299,9 +300,10 @@ class CrosswalkModule : public SceneModuleInterface
};

CrosswalkModule(
rclcpp::Node & node, const int64_t module_id, const std::optional<int64_t> & reg_elem_id,
const lanelet::LaneletMapPtr & lanelet_map_ptr, const PlannerParam & planner_param,
const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock);
rclcpp::Node & node, const int64_t lane_id, const int64_t module_id,
const std::optional<int64_t> & reg_elem_id, const lanelet::LaneletMapPtr & lanelet_map_ptr,
const PlannerParam & planner_param, const rclcpp::Logger & logger,
const rclcpp::Clock::SharedPtr clock);

bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override;

Expand Down Expand Up @@ -406,6 +408,8 @@ class CrosswalkModule : public SceneModuleInterface

lanelet::ConstLanelet crosswalk_;

lanelet::ConstLanelet road_;

lanelet::ConstLineStrings3d stop_lines_;

// Parameter
Expand Down
14 changes: 7 additions & 7 deletions planning/behavior_velocity_crosswalk_module/src/util.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -53,19 +53,19 @@ using tier4_autoware_utils::createPoint;
using tier4_autoware_utils::Line2d;
using tier4_autoware_utils::Point2d;

std::vector<lanelet::ConstLanelet> getCrosswalksOnPath(
std::vector<std::pair<int64_t, lanelet::ConstLanelet>> getCrosswalksOnPath(
const geometry_msgs::msg::Pose & current_pose,
const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
const lanelet::LaneletMapPtr lanelet_map,
const std::shared_ptr<const lanelet::routing::RoutingGraphContainer> & overall_graphs)
{
std::vector<lanelet::ConstLanelet> crosswalks;
std::vector<std::pair<lanelet::Id, lanelet::ConstLanelet>> crosswalks;

// Add current lane id
const auto nearest_lane_id =
behavior_velocity_planner::planning_utils::getNearestLaneId(path, lanelet_map, current_pose);

std::vector<int64_t> unique_lane_ids;
std::vector<lanelet::Id> unique_lane_ids;
if (nearest_lane_id) {
// Add subsequent lane_ids from nearest lane_id
unique_lane_ids = behavior_velocity_planner::planning_utils::getSubsequentLaneIdsSetOnPath(
Expand All @@ -81,24 +81,24 @@ std::vector<lanelet::ConstLanelet> getCrosswalksOnPath(
constexpr int PEDESTRIAN_GRAPH_ID = 1;
const auto conflicting_crosswalks = overall_graphs->conflictingInGraph(ll, PEDESTRIAN_GRAPH_ID);
for (const auto & crosswalk : conflicting_crosswalks) {
crosswalks.push_back(crosswalk);
crosswalks.emplace_back(lane_id, crosswalk);
}
}

return crosswalks;
}

std::set<int64_t> getCrosswalkIdSetOnPath(
std::set<lanelet::Id> getCrosswalkIdSetOnPath(
const geometry_msgs::msg::Pose & current_pose,
const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
const lanelet::LaneletMapPtr lanelet_map,
const std::shared_ptr<const lanelet::routing::RoutingGraphContainer> & overall_graphs)
{
std::set<int64_t> crosswalk_id_set;
std::set<lanelet::Id> crosswalk_id_set;

for (const auto & crosswalk :
getCrosswalksOnPath(current_pose, path, lanelet_map, overall_graphs)) {
crosswalk_id_set.insert(crosswalk.id());
crosswalk_id_set.insert(crosswalk.second.id());
}

return crosswalk_id_set;
Expand Down
2 changes: 1 addition & 1 deletion planning/behavior_velocity_walkway_module/src/manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -75,7 +75,7 @@ void WalkwayModuleManager::launchNewModules(const PathWithLaneId & path)
planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr());

for (const auto & crosswalk : crosswalk_lanelets) {
launch(crosswalk, false);
launch(crosswalk.second, false);
}
}

Expand Down
Loading