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

chore: sync upstream #809

Merged
merged 9 commits into from
Sep 8, 2023
Merged
33 changes: 24 additions & 9 deletions control/lane_departure_checker/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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\<std::string\> | line_string types to detect with boundary_departure_checker | [road_border] |

### Core Parameters

Expand Down
Original file line number Diff line number Diff line change
@@ -1,13 +1,19 @@
/**:
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
include_right_lanes: false
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
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -72,14 +72,15 @@ struct Input
lanelet::ConstLanelets shoulder_lanelets{};
Trajectory::ConstSharedPtr reference_trajectory{};
Trajectory::ConstSharedPtr predicted_trajectory{};
std::vector<std::string> boundary_types_to_detect{};
};

struct Output
{
std::map<std::string, double> 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{};
Expand Down Expand Up @@ -136,9 +137,10 @@ class LaneDepartureChecker
const lanelet::ConstLanelets & candidate_lanelets,
const std::vector<LinearRing2d> & vehicle_footprints);

static bool willCrossRoadBorder(
static bool willCrossBoundary(
const lanelet::ConstLanelets & candidate_lanelets,
const std::vector<LinearRing2d> & vehicle_footprints);
const std::vector<LinearRing2d> & vehicle_footprints,
const std::vector<std::string> & boundary_types_to_detects);

static bool isCrossingRoadBorder(
const lanelet::BasicLineString2d & road_border, const std::vector<LinearRing2d> & footprints);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,7 @@

#include <map>
#include <memory>
#include <string>
#include <vector>

namespace lane_departure_checker
Expand All @@ -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<std::string> boundary_types_to_detect;
};

class LaneDepartureCheckerNode : public rclcpp::Node
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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<LinearRing2d> & footprints)
bool isCrossingWithBoundary(
const lanelet::BasicLineString2d & boundary, const std::vector<LinearRing2d> & 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;
}
}
Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -334,15 +334,18 @@ bool LaneDepartureChecker::isOutOfLane(
return false;
}

bool LaneDepartureChecker::willCrossRoadBorder(
bool LaneDepartureChecker::willCrossBoundary(
const lanelet::ConstLanelets & candidate_lanelets,
const std::vector<LinearRing2d> & vehicle_footprints)
const std::vector<LinearRing2d> & vehicle_footprints,
const std::vector<std::string> & 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;
Expand All @@ -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;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -125,15 +125,22 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o
{
using std::placeholders::_1;

// Enable feature
node_param_.will_out_of_lane_checker = declare_parameter<bool>("will_out_of_lane_checker");
node_param_.out_of_lane_checker = declare_parameter<bool>("out_of_lane_checker");
node_param_.boundary_departure_checker = declare_parameter<bool>("boundary_departure_checker");

// Node Parameter
node_param_.update_rate = declare_parameter<double>("update_rate");
node_param_.visualize_lanelet = declare_parameter<bool>("visualize_lanelet");
node_param_.include_right_lanes = declare_parameter<bool>("include_right_lanes");
node_param_.include_left_lanes = declare_parameter<bool>("include_left_lanes");
node_param_.include_opposite_lanes = declare_parameter<bool>("include_opposite_lanes");
node_param_.include_conflicting_lanes = declare_parameter<bool>("include_conflicting_lanes");
node_param_.road_border_departure_checker =
declare_parameter<bool>("road_border_departure_checker");

// Boundary_departure_checker
node_param_.boundary_types_to_detect =
declare_parameter<std::vector<std::string>>("boundary_types_to_detect");

// Vehicle Info
const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo();
Expand Down Expand Up @@ -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_);
Expand Down Expand Up @@ -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);
Expand All @@ -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);
Expand Down
Loading