From 28ec5a6753e940552733e078d345f014008c7fcb Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Thu, 7 Sep 2023 22:38:25 +0900 Subject: [PATCH] feat(lane_departure_checker): add border types to check (#4861) * update lane_departure_checker package Signed-off-by: kyoichi-sugahara * style(pre-commit): autofix * update doc Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- control/lane_departure_checker/README.md | 33 ++++++++++++----- .../config/lane_departure_checker.param.yaml | 8 ++++- .../lane_departure_checker.hpp | 8 +++-- .../lane_departure_checker_node.hpp | 7 +++- .../lane_departure_checker.cpp | 35 +++++++++++-------- .../lane_departure_checker_node.cpp | 27 ++++++++++---- 6 files changed, 83 insertions(+), 35 deletions(-) diff --git a/control/lane_departure_checker/README.md b/control/lane_departure_checker/README.md index 5148d6a998f50..67b770aefb3d2 100644 --- a/control/lane_departure_checker/README.md +++ b/control/lane_departure_checker/README.md @@ -63,15 +63,30 @@ This package includes the following features: ### Node Parameters -| Name | Type | Description | Default value | -| :---------------------------- | :----- | :------------------------------------------------------------ | :------------ | -| update_rate | double | Frequency for publishing [Hz] | 10.0 | -| visualize_lanelet | bool | Flag for visualizing lanelet | False | -| include_right_lanes | bool | Flag for including right lanelet in borders | False | -| include_left_lanes | bool | Flag for including left lanelet in borders | False | -| include_opposite_lanes | bool | Flag for including opposite lanelet in borders | False | -| include_conflicting_lanes | bool | Flag for including conflicting lanelet in borders | False | -| road_border_departure_checker | bool | Flag for checking if the vehicle will departs the road border | False | +#### General Parameters + +| Name | Type | Description | Default value | +| :------------------------- | :----- | :---------------------------------------------------------------------------------------------------------- | :------------ | +| will_out_of_lane_checker | bool | Enable checker whether ego vehicle footprint will depart from lane | True | +| out_of_lane_checker | bool | Enable checker whether ego vehicle footprint is out of lane | True | +| boundary_departure_checker | bool | Enable checker whether ego vehicle footprint wil depart from boundary specified by boundary_types_to_detect | False | +| update_rate | double | Frequency for publishing [Hz] | 10.0 | +| visualize_lanelet | bool | Flag for visualizing lanelet | False | + +#### Parameters For Lane Departure + +| Name | Type | Description | Default value | +| :------------------------ | :--- | :------------------------------------------------ | :------------ | +| include_right_lanes | bool | Flag for including right lanelet in borders | False | +| include_left_lanes | bool | Flag for including left lanelet in borders | False | +| include_opposite_lanes | bool | Flag for including opposite lanelet in borders | False | +| include_conflicting_lanes | bool | Flag for including conflicting lanelet in borders | False | + +#### Parameters For Road Border Departure + +| Name | Type | Description | Default value | +| :----------------------- | :------------------------- | :---------------------------------------------------------- | :------------ | +| boundary_types_to_detect | std::vector\ | line_string types to detect with boundary_departure_checker | [road_border] | ### Core Parameters diff --git a/control/lane_departure_checker/config/lane_departure_checker.param.yaml b/control/lane_departure_checker/config/lane_departure_checker.param.yaml index 008832b1cab21..f0a8df21c425b 100644 --- a/control/lane_departure_checker/config/lane_departure_checker.param.yaml +++ b/control/lane_departure_checker/config/lane_departure_checker.param.yaml @@ -1,5 +1,10 @@ /**: ros__parameters: + # Enable feature + will_out_of_lane_checker: true + out_of_lane_checker: true + boundary_departure_checker: false + # Node update_rate: 10.0 visualize_lanelet: false @@ -7,7 +12,8 @@ include_left_lanes: false include_opposite_lanes: false include_conflicting_lanes: false - road_border_departure_checker: false + boundary_types_to_detect: [road_border] + # Core footprint_margin_scale: 1.0 diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp index 9822fd820dc3c..1f3865bd2dd45 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp @@ -73,6 +73,7 @@ struct Input lanelet::ConstLanelets shoulder_lanelets{}; Trajectory::ConstSharedPtr reference_trajectory{}; Trajectory::ConstSharedPtr predicted_trajectory{}; + std::vector boundary_types_to_detect{}; }; struct Output @@ -80,7 +81,7 @@ struct Output std::map processing_time_map{}; bool will_leave_lane{}; bool is_out_of_lane{}; - bool will_cross_road_border{}; + bool will_cross_boundary{}; PoseDeviation trajectory_deviation{}; lanelet::ConstLanelets candidate_lanelets{}; TrajectoryPoints resampled_trajectory{}; @@ -137,9 +138,10 @@ class LaneDepartureChecker const lanelet::ConstLanelets & candidate_lanelets, const std::vector & vehicle_footprints); - static bool willCrossRoadBorder( + static bool willCrossBoundary( const lanelet::ConstLanelets & candidate_lanelets, - const std::vector & vehicle_footprints); + const std::vector & vehicle_footprints, + const std::vector & boundary_types_to_detects); static bool isCrossingRoadBorder( const lanelet::BasicLineString2d & road_border, const std::vector & footprints); diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp index f7f7bcfda7d51..c146957bbd314 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp @@ -37,6 +37,7 @@ #include #include +#include #include namespace lane_departure_checker @@ -45,13 +46,17 @@ using autoware_auto_mapping_msgs::msg::HADMapBin; struct NodeParam { + bool will_out_of_lane_checker; + bool out_of_lane_checker; + bool boundary_departure_checker; + double update_rate; bool visualize_lanelet; bool include_right_lanes; bool include_left_lanes; bool include_opposite_lanes; bool include_conflicting_lanes; - bool road_border_departure_checker; + std::vector boundary_types_to_detect; }; class LaneDepartureCheckerNode : public rclcpp::Node diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp index f1d8d75452df1..9d413b2bf0a70 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp @@ -67,19 +67,19 @@ bool isInAnyLane(const lanelet::ConstLanelets & candidate_lanelets, const Point2 return false; } -bool isCrossingWithRoadBorder( - const lanelet::BasicLineString2d & road_border, const std::vector & footprints) +bool isCrossingWithBoundary( + const lanelet::BasicLineString2d & boundary, const std::vector & footprints) { for (auto & footprint : footprints) { for (size_t i = 0; i < footprint.size() - 1; ++i) { auto footprint1 = footprint.at(i).to_3d(); auto footprint2 = footprint.at(i + 1).to_3d(); - for (size_t i = 0; i < road_border.size() - 1; ++i) { - auto road_border1 = road_border.at(i); - auto road_border2 = road_border.at(i + 1); + for (size_t i = 0; i < boundary.size() - 1; ++i) { + auto boundary1 = boundary.at(i); + auto boundary2 = boundary.at(i + 1); if (tier4_autoware_utils::intersect( tier4_autoware_utils::toMsg(footprint1), tier4_autoware_utils::toMsg(footprint2), - fromVector2dToMsg(road_border1), fromVector2dToMsg(road_border2))) { + fromVector2dToMsg(boundary1), fromVector2dToMsg(boundary2))) { return true; } } @@ -172,9 +172,9 @@ Output LaneDepartureChecker::update(const Input & input) output.is_out_of_lane = isOutOfLane(output.candidate_lanelets, output.vehicle_footprints.front()); output.processing_time_map["isOutOfLane"] = stop_watch.toc(true); - output.will_cross_road_border = - willCrossRoadBorder(output.candidate_lanelets, output.vehicle_footprints); - output.processing_time_map["willCrossRoadBorder"] = stop_watch.toc(true); + output.will_cross_boundary = willCrossBoundary( + output.candidate_lanelets, output.vehicle_footprints, input.boundary_types_to_detect); + output.processing_time_map["willCrossBoundary"] = stop_watch.toc(true); return output; } @@ -334,15 +334,18 @@ bool LaneDepartureChecker::isOutOfLane( return false; } -bool LaneDepartureChecker::willCrossRoadBorder( +bool LaneDepartureChecker::willCrossBoundary( const lanelet::ConstLanelets & candidate_lanelets, - const std::vector & vehicle_footprints) + const std::vector & vehicle_footprints, + const std::vector & boundary_types_to_detect) { for (const auto & candidate_lanelet : candidate_lanelets) { const std::string r_type = candidate_lanelet.rightBound().attributeOr(lanelet::AttributeName::Type, "none"); - if (r_type == "road_border") { - if (isCrossingWithRoadBorder( + if ( + std::find(boundary_types_to_detect.begin(), boundary_types_to_detect.end(), r_type) != + boundary_types_to_detect.end()) { + if (isCrossingWithBoundary( candidate_lanelet.rightBound2d().basicLineString(), vehicle_footprints)) { // std::cerr << "The crossed road_border's line string id: " // << candidate_lanelet.rightBound().id() << std::endl; @@ -351,8 +354,10 @@ bool LaneDepartureChecker::willCrossRoadBorder( } const std::string l_type = candidate_lanelet.leftBound().attributeOr(lanelet::AttributeName::Type, "none"); - if (l_type == "road_border") { - if (isCrossingWithRoadBorder( + if ( + std::find(boundary_types_to_detect.begin(), boundary_types_to_detect.end(), l_type) != + boundary_types_to_detect.end()) { + if (isCrossingWithBoundary( candidate_lanelet.leftBound2d().basicLineString(), vehicle_footprints)) { // std::cerr << "The crossed road_border's line string id: " // << candidate_lanelet.leftBound().id() << std::endl; diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp index 20d535a82bfa1..f788e64316171 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp @@ -125,6 +125,11 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o { using std::placeholders::_1; + // Enable feature + node_param_.will_out_of_lane_checker = declare_parameter("will_out_of_lane_checker"); + node_param_.out_of_lane_checker = declare_parameter("out_of_lane_checker"); + node_param_.boundary_departure_checker = declare_parameter("boundary_departure_checker"); + // Node Parameter node_param_.update_rate = declare_parameter("update_rate"); node_param_.visualize_lanelet = declare_parameter("visualize_lanelet"); @@ -132,8 +137,10 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o node_param_.include_left_lanes = declare_parameter("include_left_lanes"); node_param_.include_opposite_lanes = declare_parameter("include_opposite_lanes"); node_param_.include_conflicting_lanes = declare_parameter("include_conflicting_lanes"); - node_param_.road_border_departure_checker = - declare_parameter("road_border_departure_checker"); + + // Boundary_departure_checker + node_param_.boundary_types_to_detect = + declare_parameter>("boundary_types_to_detect"); // Vehicle Info const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); @@ -338,6 +345,7 @@ void LaneDepartureCheckerNode::onTimer() input_.shoulder_lanelets = shoulder_lanelets_; input_.reference_trajectory = reference_trajectory_; input_.predicted_trajectory = predicted_trajectory_; + input_.boundary_types_to_detect = node_param_.boundary_types_to_detect; processing_time_map["Node: setInputData"] = stop_watch.toc(true); output_ = lane_departure_checker_->update(input_); @@ -377,12 +385,19 @@ rcl_interfaces::msg::SetParametersResult LaneDepartureCheckerNode::onParameter( result.reason = "success"; try { + // Enable feature + update_param(parameters, "will_out_of_lane_checker", node_param_.will_out_of_lane_checker); + update_param(parameters, "out_of_lane_checker", node_param_.out_of_lane_checker); + update_param(parameters, "boundary_departure_checker", node_param_.boundary_departure_checker); + // Node update_param(parameters, "visualize_lanelet", node_param_.visualize_lanelet); update_param(parameters, "include_right_lanes", node_param_.include_right_lanes); update_param(parameters, "include_left_lanes", node_param_.include_left_lanes); update_param(parameters, "include_opposite_lanes", node_param_.include_opposite_lanes); update_param(parameters, "include_conflicting_lanes", node_param_.include_conflicting_lanes); + update_param(parameters, "boundary_departure_checker", node_param_.boundary_departure_checker); + update_param(parameters, "boundary_types_to_detect", node_param_.boundary_types_to_detect); // Core update_param(parameters, "footprint_margin_scale", param_.footprint_margin_scale); @@ -409,19 +424,19 @@ void LaneDepartureCheckerNode::checkLaneDeparture( int8_t level = DiagStatus::OK; std::string msg = "OK"; - if (output_.will_leave_lane) { + if (output_.will_leave_lane && node_param_.will_out_of_lane_checker) { level = DiagStatus::WARN; msg = "vehicle will leave lane"; } - if (output_.is_out_of_lane) { + if (output_.is_out_of_lane && node_param_.out_of_lane_checker) { level = DiagStatus::ERROR; msg = "vehicle is out of lane"; } - if (output_.will_cross_road_border && node_param_.road_border_departure_checker) { + if (output_.will_cross_boundary && node_param_.boundary_departure_checker) { level = DiagStatus::ERROR; - msg = "vehicle will cross road border"; + msg = "vehicle will cross boundary"; } stat.summary(level, msg);