From 57d6df8c03c9350f7ef8b380917cb04f957bca33 Mon Sep 17 00:00:00 2001
From: Mamoru Sobue <hilo.soblin@gmail.com>
Date: Wed, 29 May 2024 13:53:25 +0900
Subject: [PATCH] feat(behavior_velocity_blind_spot): use TTC for object in
detection_area (#7146)
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
---
.../config/blind_spot.param.yaml | 8 +-
.../src/debug.cpp | 15 +-
.../src/manager.cpp | 13 +-
.../src/scene.cpp | 287 ++++++++----------
.../src/scene.hpp | 61 ++--
5 files changed, 170 insertions(+), 214 deletions(-)
diff --git a/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml b/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml
index 3c22d1fe65db5..d7131d8c19245 100644
--- a/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml
+++ b/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml
@@ -3,10 +3,12 @@
blind_spot:
use_pass_judge_line: true
stop_line_margin: 1.0 # [m]
- backward_length: 50.0 # [m]
+ backward_detection_length: 100.0 # [m]
ignore_width_from_center_line: 0.7 # [m]
- max_future_movement_time: 10.0 # [second]
- threshold_yaw_diff: 0.523 # [rad]
adjacent_extend_width: 1.5 # [m]
opposite_adjacent_extend_width: 1.5 # [m]
+ max_future_movement_time: 10.0 # [second]
+ ttc_min: -5.0 # [s]
+ ttc_max: 5.0 # [s]
+ ttc_ego_minimal_velocity: 5.0 # [m/s]
enable_rtc: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval
diff --git a/planning/behavior_velocity_blind_spot_module/src/debug.cpp b/planning/behavior_velocity_blind_spot_module/src/debug.cpp
index b733a1ea1e79c..a30836ab9f6a9 100644
--- a/planning/behavior_velocity_blind_spot_module/src/debug.cpp
+++ b/planning/behavior_velocity_blind_spot_module/src/debug.cpp
@@ -93,15 +93,12 @@ visualization_msgs::msg::MarkerArray BlindSpotModule::createDebugMarkerArray()
const auto now = this->clock_->now();
- appendMarkerArray(
- createLaneletPolygonsMarkerArray(
- debug_data_.conflict_areas, "conflict_area", module_id_, 0.0, 0.5, 0.5),
- &debug_marker_array, now);
-
- appendMarkerArray(
- createLaneletPolygonsMarkerArray(
- debug_data_.detection_areas, "detection_area", module_id_, 0.5, 0.0, 0.0),
- &debug_marker_array, now);
+ if (debug_data_.detection_area) {
+ appendMarkerArray(
+ createLaneletPolygonsMarkerArray(
+ {debug_data_.detection_area.value()}, "detection_area", module_id_, 0.5, 0.0, 0.0),
+ &debug_marker_array, now);
+ }
appendMarkerArray(
debug::createObjectsMarkerArray(
diff --git a/planning/behavior_velocity_blind_spot_module/src/manager.cpp b/planning/behavior_velocity_blind_spot_module/src/manager.cpp
index 0843199cac089..07742a1217ab3 100644
--- a/planning/behavior_velocity_blind_spot_module/src/manager.cpp
+++ b/planning/behavior_velocity_blind_spot_module/src/manager.cpp
@@ -36,17 +36,20 @@ BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node)
planner_param_.use_pass_judge_line =
getOrDeclareParameter<bool>(node, ns + ".use_pass_judge_line");
planner_param_.stop_line_margin = getOrDeclareParameter<double>(node, ns + ".stop_line_margin");
- planner_param_.backward_length = getOrDeclareParameter<double>(node, ns + ".backward_length");
+ planner_param_.backward_detection_length =
+ getOrDeclareParameter<double>(node, ns + ".backward_detection_length");
planner_param_.ignore_width_from_center_line =
getOrDeclareParameter<double>(node, ns + ".ignore_width_from_center_line");
- planner_param_.max_future_movement_time =
- getOrDeclareParameter<double>(node, ns + ".max_future_movement_time");
- planner_param_.threshold_yaw_diff =
- getOrDeclareParameter<double>(node, ns + ".threshold_yaw_diff");
planner_param_.adjacent_extend_width =
getOrDeclareParameter<double>(node, ns + ".adjacent_extend_width");
planner_param_.opposite_adjacent_extend_width =
getOrDeclareParameter<double>(node, ns + ".opposite_adjacent_extend_width");
+ planner_param_.max_future_movement_time =
+ getOrDeclareParameter<double>(node, ns + ".max_future_movement_time");
+ planner_param_.ttc_min = getOrDeclareParameter<double>(node, ns + ".ttc_min");
+ planner_param_.ttc_max = getOrDeclareParameter<double>(node, ns + ".ttc_max");
+ planner_param_.ttc_ego_minimal_velocity =
+ getOrDeclareParameter<double>(node, ns + ".ttc_ego_minimal_velocity");
}
void BlindSpotModuleManager::launchNewModules(
diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_blind_spot_module/src/scene.cpp
index c591a240029ff..6c7c12b556581 100644
--- a/planning/behavior_velocity_blind_spot_module/src/scene.cpp
+++ b/planning/behavior_velocity_blind_spot_module/src/scene.cpp
@@ -67,6 +67,9 @@ void BlindSpotModule::initializeRTCStatus()
BlindSpotDecision BlindSpotModule::modifyPathVelocityDetail(
PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason)
{
+ if (planner_param_.use_pass_judge_line && is_over_pass_judge_line_) {
+ return OverPassJudge{"already over the pass judge line. no plan needed."};
+ }
const auto & input_path = *path;
/* set stop-line and stop-judgement-line for base_link */
@@ -98,16 +101,36 @@ BlindSpotDecision BlindSpotModule::modifyPathVelocityDetail(
const auto is_over_pass_judge = isOverPassJudge(input_path, stop_line_pose);
if (is_over_pass_judge) {
+ is_over_pass_judge_line_ = true;
return is_over_pass_judge.value();
}
- const auto areas_opt = generateBlindSpotPolygons(input_path, closest_idx, stop_line_pose);
- if (!areas_opt) {
+ if (!blind_spot_lanelets_) {
+ const auto blind_spot_lanelets = generateBlindSpotLanelets(input_path);
+ if (!blind_spot_lanelets.empty()) {
+ blind_spot_lanelets_ = blind_spot_lanelets;
+ }
+ }
+ if (!blind_spot_lanelets_) {
+ return InternalError{"There are not blind_spot_lanelets"};
+ }
+ const auto & blind_spot_lanelets = blind_spot_lanelets_.value();
+
+ const auto detection_area_opt = generateBlindSpotPolygons(
+ input_path, closest_idx, blind_spot_lanelets,
+ path->points.at(critical_stopline_idx).point.pose);
+ if (!detection_area_opt) {
return InternalError{"Failed to generate BlindSpotPolygons"};
}
+ const auto & detection_area = detection_area_opt.value();
+ debug_data_.detection_area = detection_area;
+ const auto ego_time_to_reach_stop_line = computeTimeToPassStopLine(
+ blind_spot_lanelets, path->points.at(critical_stopline_idx).point.pose);
/* calculate dynamic collision around detection area */
- const auto collision_obstacle = isCollisionDetected(areas_opt.value());
+ const auto collision_obstacle = isCollisionDetected(
+ blind_spot_lanelets, path->points.at(critical_stopline_idx).point.pose, detection_area,
+ ego_time_to_reach_stop_line);
state_machine_.setStateWithMarginTime(
collision_obstacle.has_value() ? StateMachine::State::STOP : StateMachine::State::GO,
logger_.get_child("state_machine"), *clock_);
@@ -365,27 +388,28 @@ std::optional<std::pair<size_t, size_t>> BlindSpotModule::generateStopLine(
return std::make_pair(stopline_idx_default_opt.value(), stopline_idx_critical_opt.value());
}
-void BlindSpotModule::cutPredictPathWithDuration(
- autoware_auto_perception_msgs::msg::PredictedObjects * objects_ptr, const double time_thr) const
+autoware_auto_perception_msgs::msg::PredictedObject BlindSpotModule::cutPredictPathWithDuration(
+ const std_msgs::msg::Header & header,
+ const autoware_auto_perception_msgs::msg::PredictedObject & object_original,
+ const double time_thr) const
{
+ auto object = object_original;
const rclcpp::Time current_time = clock_->now();
- for (auto & object : objects_ptr->objects) { // each objects
- for (auto & predicted_path : object.kinematics.predicted_paths) { // each predicted paths
- const auto origin_path = predicted_path;
- predicted_path.path.clear();
-
- for (size_t k = 0; k < origin_path.path.size(); ++k) { // each path points
- const auto & predicted_pose = origin_path.path.at(k);
- const auto predicted_time =
- rclcpp::Time(objects_ptr->header.stamp) +
- rclcpp::Duration(origin_path.time_step) * static_cast<double>(k);
- if ((predicted_time - current_time).seconds() < time_thr) {
- predicted_path.path.push_back(predicted_pose);
- }
+ for (auto & predicted_path : object.kinematics.predicted_paths) { // each predicted paths
+ const auto origin_path = predicted_path;
+ predicted_path.path.clear();
+
+ for (size_t k = 0; k < origin_path.path.size(); ++k) { // each path points
+ const auto & predicted_pose = origin_path.path.at(k);
+ const auto predicted_time = rclcpp::Time(header.stamp) +
+ rclcpp::Duration(origin_path.time_step) * static_cast<double>(k);
+ if ((predicted_time - current_time).seconds() < time_thr) {
+ predicted_path.path.push_back(predicted_pose);
}
}
}
+ return object;
}
std::optional<OverPassJudge> BlindSpotModule::isOverPassJudge(
@@ -419,86 +443,61 @@ std::optional<OverPassJudge> BlindSpotModule::isOverPassJudge(
return std::nullopt;
}
-std::optional<autoware_auto_perception_msgs::msg::PredictedObject>
-BlindSpotModule::isCollisionDetected(const BlindSpotPolygons & areas)
+double BlindSpotModule::computeTimeToPassStopLine(
+ const lanelet::ConstLanelets & blind_spot_lanelets,
+ const geometry_msgs::msg::Pose & stop_line_pose) const
{
- // TODO(Mamoru Sobue): only do this for target object
- autoware_auto_perception_msgs::msg::PredictedObjects objects =
- *(planner_data_->predicted_objects);
- cutPredictPathWithDuration(&objects, planner_param_.max_future_movement_time);
+ // egoが停止している時にそのまま速度を使うと衝突しなくなってしまうのでegoについては最低速度を使う
+ const auto & current_pose = planner_data_->current_odometry->pose;
+ const auto current_arc_ego =
+ lanelet::utils::getArcCoordinates(blind_spot_lanelets, current_pose).length;
+ const auto stopline_arc_ego =
+ lanelet::utils::getArcCoordinates(blind_spot_lanelets, stop_line_pose).length;
+ const auto remaining_distance = stopline_arc_ego - current_arc_ego;
+ return remaining_distance / std::max<double>(
+ planner_param_.ttc_ego_minimal_velocity,
+ planner_data_->current_velocity->twist.linear.x);
+}
+std::optional<autoware_auto_perception_msgs::msg::PredictedObject>
+BlindSpotModule::isCollisionDetected(
+ const lanelet::ConstLanelets & blind_spot_lanelets,
+ const geometry_msgs::msg::Pose & stop_line_pose, const lanelet::CompoundPolygon3d & area,
+ const double ego_time_to_reach_stop_line)
+{
// check objects in blind spot areas
- for (const auto & object : objects.objects) {
- if (!isTargetObjectType(object)) {
+ const auto stop_line_arc_ego =
+ lanelet::utils::getArcCoordinates(blind_spot_lanelets, stop_line_pose).length;
+ for (const auto & original_object : planner_data_->predicted_objects->objects) {
+ if (!isTargetObjectType(original_object)) {
continue;
}
-
- const auto & detection_areas = areas.detection_areas;
- const auto & conflict_areas = areas.conflict_areas;
- const auto & opposite_detection_areas = areas.opposite_detection_areas;
- const auto & opposite_conflict_areas = areas.opposite_conflict_areas;
-
+ const auto object = cutPredictPathWithDuration(
+ planner_data_->predicted_objects->header, original_object,
+ planner_param_.max_future_movement_time);
// right direction
- const bool exist_in_right_detection_area =
- std::any_of(detection_areas.begin(), detection_areas.end(), [&object](const auto & area) {
- return bg::within(
- to_bg2d(object.kinematics.initial_pose_with_covariance.pose.position),
- lanelet::utils::to2D(area));
- });
- // opposite direction
- const bool exist_in_opposite_detection_area = std::any_of(
- opposite_detection_areas.begin(), opposite_detection_areas.end(),
- [&object](const auto & area) {
- return bg::within(
- to_bg2d(object.kinematics.initial_pose_with_covariance.pose.position),
- lanelet::utils::to2D(area));
- });
- const bool exist_in_detection_area =
- exist_in_right_detection_area || exist_in_opposite_detection_area;
+ const bool exist_in_detection_area = bg::within(
+ to_bg2d(object.kinematics.initial_pose_with_covariance.pose.position),
+ lanelet::utils::to2D(area));
if (!exist_in_detection_area) {
continue;
}
- const bool exist_in_right_conflict_area =
- isPredictedPathInArea(object, conflict_areas, planner_data_->current_odometry->pose);
- const bool exist_in_opposite_conflict_area =
- isPredictedPathInArea(object, opposite_conflict_areas, planner_data_->current_odometry->pose);
- const bool exist_in_conflict_area =
- exist_in_right_conflict_area || exist_in_opposite_conflict_area;
- if (exist_in_detection_area && exist_in_conflict_area) {
- debug_data_.conflicting_targets.objects.push_back(object);
- setObjectsOfInterestData(
- object.kinematics.initial_pose_with_covariance.pose, object.shape, ColorName::RED);
+ const auto object_arc_length =
+ lanelet::utils::getArcCoordinates(
+ blind_spot_lanelets, object.kinematics.initial_pose_with_covariance.pose)
+ .length;
+ const auto object_time_to_reach_stop_line =
+ (object_arc_length - stop_line_arc_ego) /
+ (object.kinematics.initial_twist_with_covariance.twist.linear.x);
+ const auto ttc = ego_time_to_reach_stop_line - object_time_to_reach_stop_line;
+ RCLCPP_INFO(logger_, "object ttc is %f", ttc);
+ if (planner_param_.ttc_min < ttc && ttc < planner_param_.ttc_max) {
return object;
}
}
return std::nullopt;
}
-bool BlindSpotModule::isPredictedPathInArea(
- const autoware_auto_perception_msgs::msg::PredictedObject & object,
- const std::vector<lanelet::CompoundPolygon3d> & areas, geometry_msgs::msg::Pose ego_pose) const
-{
- const auto ego_yaw = tf2::getYaw(ego_pose.orientation);
- const auto threshold_yaw_diff = planner_param_.threshold_yaw_diff;
- // NOTE: iterating all paths including those of low confidence
- return std::any_of(
- areas.begin(), areas.end(), [&object, &ego_yaw, &threshold_yaw_diff](const auto & area) {
- const auto area_2d = lanelet::utils::to2D(area);
- return std::any_of(
- object.kinematics.predicted_paths.begin(), object.kinematics.predicted_paths.end(),
- [&area_2d, &ego_yaw, &threshold_yaw_diff](const auto & path) {
- return std::any_of(
- path.path.begin(), path.path.end(),
- [&area_2d, &ego_yaw, &threshold_yaw_diff](const auto & point) {
- const auto is_in_area = bg::within(to_bg2d(point.position), area_2d);
- const auto match_yaw =
- std::fabs(ego_yaw - tf2::getYaw(point.orientation)) < threshold_yaw_diff;
- return is_in_area && match_yaw;
- });
- });
- });
-}
-
lanelet::ConstLanelet BlindSpotModule::generateHalfLanelet(
const lanelet::ConstLanelet lanelet) const
{
@@ -523,8 +522,6 @@ lanelet::ConstLanelet BlindSpotModule::generateHalfLanelet(
const auto left_bound = lanelet::LineString3d(lanelet::InvalId, std::move(lefts));
const auto right_bound = lanelet::LineString3d(lanelet::InvalId, std::move(rights));
auto half_lanelet = lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound);
- const auto centerline = lanelet::utils::generateFineCenterline(half_lanelet, 5.0);
- half_lanelet.setCenterline(centerline);
return half_lanelet;
}
@@ -589,17 +586,23 @@ lanelet::ConstLanelet BlindSpotModule::generateExtendedOppositeAdjacentLanelet(
return new_lanelet;
}
-std::optional<BlindSpotPolygons> BlindSpotModule::generateBlindSpotPolygons(
- const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx,
- const geometry_msgs::msg::Pose & stop_line_pose) const
+static lanelet::LineString3d removeConst(lanelet::ConstLineString3d line)
+{
+ lanelet::Points3d pts;
+ for (const auto & pt : line) {
+ pts.push_back(lanelet::Point3d(pt));
+ }
+ return lanelet::LineString3d(lanelet::InvalId, pts);
+}
+
+lanelet::ConstLanelets BlindSpotModule::generateBlindSpotLanelets(
+ const autoware_auto_planning_msgs::msg::PathWithLaneId & path) const
{
/* get lanelet map */
const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr();
const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr();
std::vector<int64_t> lane_ids;
- lanelet::ConstLanelets blind_spot_lanelets;
- lanelet::ConstLanelets blind_spot_opposite_lanelets;
/* get lane ids until intersection */
for (const auto & point : path.points) {
bool found_intersection_lane = false;
@@ -617,17 +620,10 @@ std::optional<BlindSpotPolygons> BlindSpotModule::generateBlindSpotPolygons(
if (found_intersection_lane) break;
}
- for (size_t i = 0; i < lane_ids.size(); ++i) {
- const auto half_lanelet =
- generateHalfLanelet(lanelet_map_ptr->laneletLayer.get(lane_ids.at(i)));
- blind_spot_lanelets.push_back(half_lanelet);
- }
-
- // additional detection area on left/right side
- lanelet::ConstLanelets adjacent_lanelets;
- lanelet::ConstLanelets opposite_adjacent_lanelets;
+ lanelet::ConstLanelets blind_spot_lanelets;
for (const auto i : lane_ids) {
const auto lane = lanelet_map_ptr->laneletLayer.get(i);
+ const auto ego_half_lanelet = generateHalfLanelet(lane);
const auto adj =
turn_direction_ == TurnDirection::LEFT
? (routing_graph_ptr->adjacentLeft(lane))
@@ -659,75 +655,42 @@ std::optional<BlindSpotPolygons> BlindSpotModule::generateBlindSpotPolygons(
return std::nullopt;
}
}();
- if (adj) {
- const auto half_lanelet = generateExtendedAdjacentLanelet(adj.value(), turn_direction_);
- adjacent_lanelets.push_back(half_lanelet);
- }
- if (opposite_adj) {
- const auto half_lanelet =
+
+ if (!adj && !opposite_adj) {
+ blind_spot_lanelets.push_back(ego_half_lanelet);
+ } else if (!!adj) {
+ const auto adj_half_lanelet = generateExtendedAdjacentLanelet(adj.value(), turn_direction_);
+ const auto lefts = (turn_direction_ == TurnDirection::LEFT) ? adj_half_lanelet.leftBound()
+ : ego_half_lanelet.leftBound();
+ const auto rights = (turn_direction_ == TurnDirection::RIGHT) ? adj_half_lanelet.rightBound()
+ : ego_half_lanelet.rightBound();
+ blind_spot_lanelets.push_back(
+ lanelet::ConstLanelet(lanelet::InvalId, removeConst(lefts), removeConst(rights)));
+ } else if (opposite_adj) {
+ const auto adj_half_lanelet =
generateExtendedOppositeAdjacentLanelet(opposite_adj.value(), turn_direction_);
- opposite_adjacent_lanelets.push_back(half_lanelet);
+ const auto lefts = (turn_direction_ == TurnDirection::LEFT) ? adj_half_lanelet.leftBound()
+ : ego_half_lanelet.leftBound();
+ const auto rights = (turn_direction_ == TurnDirection::LEFT) ? ego_half_lanelet.rightBound()
+ : adj_half_lanelet.rightBound();
+ blind_spot_lanelets.push_back(
+ lanelet::ConstLanelet(lanelet::InvalId, removeConst(lefts), removeConst(rights)));
}
}
+ return blind_spot_lanelets;
+}
- const auto current_arc_ego =
- lanelet::utils::getArcCoordinates(blind_spot_lanelets, path.points[closest_idx].point.pose)
- .length;
+std::optional<lanelet::CompoundPolygon3d> BlindSpotModule::generateBlindSpotPolygons(
+ [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
+ [[maybe_unused]] const size_t closest_idx, const lanelet::ConstLanelets & blind_spot_lanelets,
+ const geometry_msgs::msg::Pose & stop_line_pose) const
+{
const auto stop_line_arc_ego =
lanelet::utils::getArcCoordinates(blind_spot_lanelets, stop_line_pose).length;
- const auto detection_area_start_length_ego = stop_line_arc_ego - planner_param_.backward_length;
- if (detection_area_start_length_ego < current_arc_ego && current_arc_ego < stop_line_arc_ego) {
- BlindSpotPolygons blind_spot_polygons;
- auto conflict_area_ego = lanelet::utils::getPolygonFromArcLength(
- blind_spot_lanelets, current_arc_ego, stop_line_arc_ego);
- auto detection_area_ego = lanelet::utils::getPolygonFromArcLength(
- blind_spot_lanelets, detection_area_start_length_ego, stop_line_arc_ego);
- blind_spot_polygons.conflict_areas.emplace_back(std::move(conflict_area_ego));
- blind_spot_polygons.detection_areas.emplace_back(std::move(detection_area_ego));
- // additional detection area on left/right side
- if (!adjacent_lanelets.empty()) {
- const auto stop_line_arc_adj = lanelet::utils::getLaneletLength3d(adjacent_lanelets);
- const auto current_arc_adj = stop_line_arc_adj - (stop_line_arc_ego - current_arc_ego);
- const auto detection_area_start_length_adj =
- stop_line_arc_adj - planner_param_.backward_length;
- auto conflicting_area_adj = lanelet::utils::getPolygonFromArcLength(
- adjacent_lanelets, current_arc_adj, stop_line_arc_adj);
- auto detection_area_adj = lanelet::utils::getPolygonFromArcLength(
- adjacent_lanelets, detection_area_start_length_adj, stop_line_arc_adj);
- blind_spot_polygons.conflict_areas.emplace_back(std::move(conflicting_area_adj));
- blind_spot_polygons.detection_areas.emplace_back(std::move(detection_area_adj));
- }
- // additional detection area on left/right opposite lane side
- if (!opposite_adjacent_lanelets.empty()) {
- const auto stop_line_arc_opposite_adj =
- lanelet::utils::getLaneletLength3d(opposite_adjacent_lanelets);
- const auto current_arc_opposite_adj =
- stop_line_arc_opposite_adj - (stop_line_arc_ego - current_arc_ego);
- const auto detection_area_start_length_opposite_adj =
- stop_line_arc_opposite_adj - planner_param_.backward_length;
- auto conflicting_area_opposite_adj = lanelet::utils::getPolygonFromArcLength(
- opposite_adjacent_lanelets, current_arc_opposite_adj, stop_line_arc_opposite_adj);
- auto detection_area_opposite_adj = lanelet::utils::getPolygonFromArcLength(
- opposite_adjacent_lanelets, detection_area_start_length_opposite_adj,
- stop_line_arc_opposite_adj);
- blind_spot_polygons.opposite_conflict_areas.emplace_back(
- std::move(conflicting_area_opposite_adj));
- blind_spot_polygons.opposite_detection_areas.emplace_back(
- std::move(detection_area_opposite_adj));
- }
- debug_data_.detection_areas = blind_spot_polygons.detection_areas;
- debug_data_.conflict_areas = blind_spot_polygons.conflict_areas;
- debug_data_.detection_areas.insert(
- debug_data_.detection_areas.end(), blind_spot_polygons.opposite_detection_areas.begin(),
- blind_spot_polygons.opposite_detection_areas.end());
- debug_data_.conflict_areas.insert(
- debug_data_.conflict_areas.end(), blind_spot_polygons.opposite_conflict_areas.begin(),
- blind_spot_polygons.opposite_conflict_areas.end());
-
- return blind_spot_polygons;
- } else {
- return std::nullopt;
- }
+ const auto detection_area_start_length_ego =
+ std::max<double>(stop_line_arc_ego - planner_param_.backward_detection_length, 0.0);
+ return lanelet::utils::getPolygonFromArcLength(
+ blind_spot_lanelets, detection_area_start_length_ego, stop_line_arc_ego);
}
bool BlindSpotModule::isTargetObjectType(
diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.hpp b/planning/behavior_velocity_blind_spot_module/src/scene.hpp
index 2f4ccc37ec51e..709942794ec1e 100644
--- a/planning/behavior_velocity_blind_spot_module/src/scene.hpp
+++ b/planning/behavior_velocity_blind_spot_module/src/scene.hpp
@@ -36,14 +36,6 @@
namespace behavior_velocity_planner
{
-struct BlindSpotPolygons
-{
- std::vector<lanelet::CompoundPolygon3d> conflict_areas;
- std::vector<lanelet::CompoundPolygon3d> detection_areas;
- std::vector<lanelet::CompoundPolygon3d> opposite_conflict_areas;
- std::vector<lanelet::CompoundPolygon3d> opposite_detection_areas;
-};
-
/**
* @brief wrapper class of interpolated path with lane id
*/
@@ -93,26 +85,23 @@ class BlindSpotModule : public SceneModuleInterface
struct DebugData
{
std::optional<geometry_msgs::msg::Pose> virtual_wall_pose{std::nullopt};
- std::vector<lanelet::CompoundPolygon3d> conflict_areas;
- std::vector<lanelet::CompoundPolygon3d> detection_areas;
- std::vector<lanelet::CompoundPolygon3d> opposite_conflict_areas;
- std::vector<lanelet::CompoundPolygon3d> opposite_detection_areas;
+ std::optional<lanelet::CompoundPolygon3d> detection_area;
autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets;
};
public:
struct PlannerParam
{
- bool use_pass_judge_line; //! distance which ego can stop with max brake
- double stop_line_margin; //! distance from auto-generated stopline to detection_area boundary
- double backward_length; //! distance[m] from closest path point to the edge of beginning point
- double ignore_width_from_center_line; //! ignore width from center line from detection_area
- double
- max_future_movement_time; //! maximum time[second] for considering future movement of object
- double threshold_yaw_diff; //! threshold of yaw difference between ego and target object
- double
- adjacent_extend_width; //! the width of extended detection/conflict area on adjacent lane
+ bool use_pass_judge_line;
+ double stop_line_margin;
+ double backward_detection_length;
+ double ignore_width_from_center_line;
+ double adjacent_extend_width;
double opposite_adjacent_extend_width;
+ double max_future_movement_time;
+ double ttc_min;
+ double ttc_max;
+ double ttc_ego_minimal_velocity;
};
BlindSpotModule(
@@ -135,6 +124,7 @@ class BlindSpotModule : public SceneModuleInterface
const PlannerParam planner_param_;
const TurnDirection turn_direction_;
std::optional<lanelet::ConstLanelet> sibling_straight_lanelet_{std::nullopt};
+ std::optional<lanelet::ConstLanelets> blind_spot_lanelets_{std::nullopt};
// state variables
bool is_over_pass_judge_line_{false};
@@ -181,6 +171,10 @@ class BlindSpotModule : public SceneModuleInterface
const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
const geometry_msgs::msg::Pose & stop_point_pose) const;
+ double computeTimeToPassStopLine(
+ const lanelet::ConstLanelets & blind_spot_lanelets,
+ const geometry_msgs::msg::Pose & stop_line_pose) const;
+
/**
* @brief Check obstacle is in blind spot areas.
* Condition1: Object's position is in broad blind spot area.
@@ -192,7 +186,9 @@ class BlindSpotModule : public SceneModuleInterface
* @return true when an object is detected in blind spot
*/
std::optional<autoware_auto_perception_msgs::msg::PredictedObject> isCollisionDetected(
- const BlindSpotPolygons & area);
+ const lanelet::ConstLanelets & blind_spot_lanelets,
+ const geometry_msgs::msg::Pose & stop_line_pose, const lanelet::CompoundPolygon3d & area,
+ const double ego_time_to_reach_stop_line);
/**
* @brief Create half lanelet
@@ -206,6 +202,9 @@ class BlindSpotModule : public SceneModuleInterface
lanelet::ConstLanelet generateExtendedOppositeAdjacentLanelet(
const lanelet::ConstLanelet lanelet, const TurnDirection direction) const;
+ lanelet::ConstLanelets generateBlindSpotLanelets(
+ const autoware_auto_planning_msgs::msg::PathWithLaneId & path) const;
+
/**
* @brief Make blind spot areas. Narrow area is made from closest path point to stop line index.
* Broad area is made from backward expanded point to stop line point
@@ -213,8 +212,9 @@ class BlindSpotModule : public SceneModuleInterface
* @param closest_idx closest path point index from ego car in path points
* @return Blind spot polygons
*/
- std::optional<BlindSpotPolygons> generateBlindSpotPolygons(
+ std::optional<lanelet::CompoundPolygon3d> generateBlindSpotPolygons(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx,
+ const lanelet::ConstLanelets & blind_spot_lanelets,
const geometry_msgs::msg::Pose & pose) const;
/**
@@ -224,23 +224,14 @@ class BlindSpotModule : public SceneModuleInterface
*/
bool isTargetObjectType(const autoware_auto_perception_msgs::msg::PredictedObject & object) const;
- /**
- * @brief Check if at least one of object's predicted position is in area
- * @param object Dynamic object
- * @param area Area defined by polygon
- * @return True when at least one of object's predicted position is in area
- */
- bool isPredictedPathInArea(
- const autoware_auto_perception_msgs::msg::PredictedObject & object,
- const std::vector<lanelet::CompoundPolygon3d> & areas, geometry_msgs::msg::Pose ego_pose) const;
-
/**
* @brief Modify objects predicted path. remove path point if the time exceeds timer_thr.
* @param objects_ptr target objects
* @param time_thr time threshold to cut path
*/
- void cutPredictPathWithDuration(
- autoware_auto_perception_msgs::msg::PredictedObjects * objects_ptr,
+ autoware_auto_perception_msgs::msg::PredictedObject cutPredictPathWithDuration(
+ const std_msgs::msg::Header & header,
+ const autoware_auto_perception_msgs::msg::PredictedObject & object,
const double time_thr) const;
StateMachine state_machine_; //! for state