From 7386bb85d7049be5c011a36708fe26f37d827292 Mon Sep 17 00:00:00 2001
From: satoshi-ota <satoshi.ota928@gmail.com>
Date: Mon, 5 Feb 2024 11:06:12 +0900
Subject: [PATCH 1/4] refactor(path_shifter): add id to ShiftLine
Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
---
.../behavior_path_avoidance_module/data_structs.hpp | 3 ---
.../utils/path_shifter/path_shifter.hpp | 9 +++++++++
2 files changed, 9 insertions(+), 3 deletions(-)
diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp
index a536fa1b568cb..223db73e72a4c 100644
--- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp
+++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp
@@ -443,9 +443,6 @@ struct AvoidLine : public ShiftLine
// Distance from ego to end point in Frenet
double end_longitudinal = 0.0;
- // for unique_id
- UUID id{};
-
// for the case the point is created by merge other points
std::vector<UUID> parent_ids{};
diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp
index 165a11ebb54b4..67f581123057e 100644
--- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp
+++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp
@@ -17,9 +17,11 @@
#include <rclcpp/clock.hpp>
#include <rclcpp/logging.hpp>
+#include <tier4_autoware_utils/ros/uuid_helper.hpp>
#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
#include <geometry_msgs/msg/point.hpp>
+#include <unique_identifier_msgs/msg/uuid.hpp>
#include <optional>
#include <string>
@@ -31,9 +33,13 @@ using autoware_auto_planning_msgs::msg::PathPointWithLaneId;
using autoware_auto_planning_msgs::msg::PathWithLaneId;
using geometry_msgs::msg::Point;
using geometry_msgs::msg::Pose;
+using tier4_autoware_utils::generateUUID;
+using unique_identifier_msgs::msg::UUID;
struct ShiftLine
{
+ ShiftLine() : id(generateUUID()) {}
+
Pose start{}; // shift start point in absolute coordinate
Pose end{}; // shift start point in absolute coordinate
@@ -45,6 +51,9 @@ struct ShiftLine
size_t start_idx{}; // associated start-point index for the reference path
size_t end_idx{}; // associated end-point index for the reference path
+
+ // for unique_id
+ UUID id{};
};
using ShiftLineArray = std::vector<ShiftLine>;
From 92584ddc636f364d2cc0ef7acbe80a6ecd104ffc Mon Sep 17 00:00:00 2001
From: satoshi-ota <satoshi.ota928@gmail.com>
Date: Fri, 2 Feb 2024 19:19:13 +0900
Subject: [PATCH 2/4] fix(avoidance): turn off blinker when the ego return
original lane
Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
---
.../behavior_path_avoidance_module/scene.hpp | 2 +
.../behavior_path_avoidance_module/utils.hpp | 2 +-
.../src/scene.cpp | 17 +++-
.../src/utils.cpp | 94 ++++++++++++++-----
4 files changed, 89 insertions(+), 26 deletions(-)
diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp
index a74c3a69d7bce..82e2c7af32f8d 100644
--- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp
+++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp
@@ -404,6 +404,8 @@ class AvoidanceModule : public SceneModuleInterface
bool safe_{true};
+ std::optional<UUID> ignore_signal_{std::nullopt};
+
std::shared_ptr<AvoidanceHelper> helper_;
std::shared_ptr<AvoidanceParameters> parameters_;
diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp
index 02d8d68cf7a56..2d61571392eef 100644
--- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp
+++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp
@@ -176,7 +176,7 @@ double calcDistanceToAvoidStartLine(
const std::shared_ptr<const PlannerData> & planner_data,
const std::shared_ptr<AvoidanceParameters> & parameters);
-TurnSignalInfo calcTurnSignalInfo(
+std::pair<TurnSignalInfo, bool> calcTurnSignalInfo(
const ShiftedPath & path, const ShiftLine & shift_line, const double current_shift_length,
const AvoidancePlanningData & data, const std::shared_ptr<const PlannerData> & planner_data);
} // namespace behavior_path_planner::utils::avoidance
diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp
index 75529c35c2af5..27cd15cc525df 100644
--- a/planning/behavior_path_avoidance_module/src/scene.cpp
+++ b/planning/behavior_path_avoidance_module/src/scene.cpp
@@ -918,12 +918,26 @@ BehaviorModuleOutput AvoidanceModule::plan()
BehaviorModuleOutput output;
+ const auto is_ignore_signal = [this](const UUID & uuid) {
+ if (!ignore_signal_.has_value()) {
+ return false;
+ }
+
+ return ignore_signal_.value() == uuid;
+ };
+
+ const auto update_ignore_signal = [this](const UUID & uuid, const bool is_ignore) {
+ ignore_signal_ = is_ignore ? std::make_optional(uuid) : std::nullopt;
+ };
+
// turn signal info
if (path_shifter_.getShiftLines().empty()) {
output.turn_signal_info = getPreviousModuleOutput().turn_signal_info;
+ } else if (is_ignore_signal(path_shifter_.getShiftLines().front().id)) {
+ output.turn_signal_info = getPreviousModuleOutput().turn_signal_info;
} else {
const auto original_signal = getPreviousModuleOutput().turn_signal_info;
- const auto new_signal = utils::avoidance::calcTurnSignalInfo(
+ const auto [new_signal, is_ignore] = utils::avoidance::calcTurnSignalInfo(
linear_shift_path, path_shifter_.getShiftLines().front(), helper_->getEgoShift(), avoid_data_,
planner_data_);
const auto current_seg_idx = planner_data_->findEgoSegmentIndex(spline_shift_path.path.points);
@@ -931,6 +945,7 @@ BehaviorModuleOutput AvoidanceModule::plan()
spline_shift_path.path, getEgoPose(), current_seg_idx, original_signal, new_signal,
planner_data_->parameters.ego_nearest_dist_threshold,
planner_data_->parameters.ego_nearest_yaw_threshold);
+ update_ignore_signal(path_shifter_.getShiftLines().front().id, is_ignore);
}
// sparse resampling for computational cost
diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp
index 6a44350dd48c6..cca6e6f2ab351 100644
--- a/planning/behavior_path_avoidance_module/src/utils.cpp
+++ b/planning/behavior_path_avoidance_module/src/utils.cpp
@@ -250,16 +250,37 @@ void pushUniqueVector(T & base_vector, const T & additional_vector)
base_vector.insert(base_vector.end(), additional_vector.begin(), additional_vector.end());
}
+bool isAvoidShift(const double start_shift_length, const double end_shift_length)
+{
+ constexpr double THRESHOLD = 0.1;
+ return std::abs(start_shift_length) < THRESHOLD && std::abs(end_shift_length) > THRESHOLD;
+}
+
+bool isReturnShift(const double start_shift_length, const double end_shift_length)
+{
+ constexpr double THRESHOLD = 0.1;
+ return std::abs(start_shift_length) > THRESHOLD && std::abs(end_shift_length) < THRESHOLD;
+}
+
+bool isLeftMiddleShift(const double start_shift_length, const double end_shift_length)
+{
+ constexpr double THRESHOLD = 0.1;
+ return start_shift_length > THRESHOLD && end_shift_length > THRESHOLD;
+}
+
+bool isRightMiddleShift(const double start_shift_length, const double end_shift_length)
+{
+ constexpr double THRESHOLD = 0.1;
+ return start_shift_length < THRESHOLD && end_shift_length < THRESHOLD;
+}
+
bool existShiftSideLane(
const double start_shift_length, const double end_shift_length, const bool no_left_lanes,
const bool no_right_lanes)
{
- constexpr double THRESHOLD = 0.1;
const auto relative_shift_length = end_shift_length - start_shift_length;
- const auto avoid_shift =
- std::abs(start_shift_length) < THRESHOLD && std::abs(end_shift_length) > THRESHOLD;
- if (avoid_shift) {
+ if (isAvoidShift(start_shift_length, end_shift_length)) {
// Left avoid. But there is no adjacent lane. No need blinker.
if (relative_shift_length > 0.0 && no_left_lanes) {
return false;
@@ -271,9 +292,7 @@ bool existShiftSideLane(
}
}
- const auto return_shift =
- std::abs(start_shift_length) > THRESHOLD && std::abs(end_shift_length) < THRESHOLD;
- if (return_shift) {
+ if (isReturnShift(start_shift_length, end_shift_length)) {
// Right return. But there is no adjacent lane. No need blinker.
if (relative_shift_length > 0.0 && no_right_lanes) {
return false;
@@ -285,8 +304,7 @@ bool existShiftSideLane(
}
}
- const auto left_middle_shift = start_shift_length > THRESHOLD && end_shift_length > THRESHOLD;
- if (left_middle_shift) {
+ if (isLeftMiddleShift(start_shift_length, end_shift_length)) {
// Left avoid. But there is no adjacent lane. No need blinker.
if (relative_shift_length > 0.0 && no_left_lanes) {
return false;
@@ -298,8 +316,7 @@ bool existShiftSideLane(
}
}
- const auto right_middle_shift = start_shift_length < THRESHOLD && end_shift_length < THRESHOLD;
- if (right_middle_shift) {
+ if (isRightMiddleShift(start_shift_length, end_shift_length)) {
// Right avoid. But there is no adjacent lane. No need blinker.
if (relative_shift_length < 0.0 && no_right_lanes) {
return false;
@@ -314,6 +331,27 @@ bool existShiftSideLane(
return true;
}
+bool isNearEndOfShift(
+ const double start_shift_length, const double end_shift_length, const Point & ego_pos,
+ const lanelet::ConstLanelets & original_lanes)
+{
+ using boost::geometry::within;
+ using lanelet::utils::to2D;
+ using lanelet::utils::conversion::toLaneletPoint;
+
+ if (!isReturnShift(start_shift_length, end_shift_length)) {
+ return false;
+ }
+
+ for (const auto & lane : original_lanes) {
+ if (within(to2D(toLaneletPoint(ego_pos)), lane.polygon2d().basicPolygon())) {
+ return true;
+ }
+ }
+
+ return false;
+}
+
bool straddleRoadBound(
const ShiftedPath & path, const ShiftLine & shift_line, const lanelet::ConstLanelets & lanes,
const vehicle_info_util::VehicleInfo & vehicle_info)
@@ -2323,7 +2361,7 @@ double calcDistanceToReturnDeadLine(
return distance_to_return_dead_line;
}
-TurnSignalInfo calcTurnSignalInfo(
+std::pair<TurnSignalInfo, bool> calcTurnSignalInfo(
const ShiftedPath & path, const ShiftLine & shift_line, const double current_shift_length,
const AvoidancePlanningData & data, const std::shared_ptr<const PlannerData> & planner_data)
{
@@ -2335,22 +2373,22 @@ TurnSignalInfo calcTurnSignalInfo(
if (shift_line.start_idx + 1 > path.shift_length.size()) {
RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency.");
- return {};
+ return std::make_pair(TurnSignalInfo{}, true);
}
if (shift_line.start_idx + 1 > path.path.points.size()) {
RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency.");
- return {};
+ return std::make_pair(TurnSignalInfo{}, true);
}
if (shift_line.end_idx + 1 > path.shift_length.size()) {
RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency.");
- return {};
+ return std::make_pair(TurnSignalInfo{}, true);
}
if (shift_line.end_idx + 1 > path.path.points.size()) {
RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency.");
- return {};
+ return std::make_pair(TurnSignalInfo{}, true);
}
const auto start_shift_length = path.shift_length.at(shift_line.start_idx);
@@ -2359,12 +2397,12 @@ TurnSignalInfo calcTurnSignalInfo(
// If shift length is shorter than the threshold, it does not need to turn on blinkers
if (std::fabs(relative_shift_length) < p.turn_signal_shift_length_threshold) {
- return {};
+ return std::make_pair(TurnSignalInfo{}, true);
}
// If the vehicle does not shift anymore, we turn off the blinker
if (std::fabs(path.shift_length.at(shift_line.end_idx) - current_shift_length) < THRESHOLD) {
- return {};
+ return std::make_pair(TurnSignalInfo{}, true);
}
const auto get_command = [](const auto & shift_length) {
@@ -2379,7 +2417,7 @@ TurnSignalInfo calcTurnSignalInfo(
p.vehicle_info.max_longitudinal_offset_m;
if (signal_prepare_distance < ego_front_to_shift_start) {
- return {};
+ return std::make_pair(TurnSignalInfo{}, false);
}
const auto blinker_start_pose = path.path.points.at(shift_line.start_idx).point.pose;
@@ -2396,12 +2434,12 @@ TurnSignalInfo calcTurnSignalInfo(
turn_signal_info.turn_signal.command = get_command(relative_shift_length);
if (!p.turn_signal_on_swerving) {
- return turn_signal_info;
+ return std::make_pair(turn_signal_info, false);
}
lanelet::ConstLanelet lanelet;
if (!rh->getClosestLaneletWithinRoute(shift_line.end, &lanelet)) {
- return {};
+ return std::make_pair(TurnSignalInfo{}, true);
}
const auto left_same_direction_lane = rh->getLeftLanelet(lanelet, true, true);
@@ -2413,13 +2451,21 @@ TurnSignalInfo calcTurnSignalInfo(
right_same_direction_lane.has_value() || !right_opposite_lanes.empty();
if (!existShiftSideLane(start_shift_length, end_shift_length, !has_left_lane, !has_right_lane)) {
- return {};
+ return std::make_pair(TurnSignalInfo{}, true);
}
if (!straddleRoadBound(path, shift_line, data.current_lanelets, p.vehicle_info)) {
- return {};
+ return std::make_pair(TurnSignalInfo{}, true);
+ }
+
+ constexpr double STOPPED_THRESHOLD = 0.1; // [m/s]
+ if (ego_speed < STOPPED_THRESHOLD) {
+ if (isNearEndOfShift(
+ start_shift_length, end_shift_length, ego_pose.position, data.current_lanelets)) {
+ return std::make_pair(TurnSignalInfo{}, true);
+ }
}
- return turn_signal_info;
+ return std::make_pair(turn_signal_info, false);
}
} // namespace behavior_path_planner::utils::avoidance
From 6066b98a7a16a7f72290b4d16804238e02ca4dfc Mon Sep 17 00:00:00 2001
From: satoshi-ota <satoshi.ota928@gmail.com>
Date: Mon, 26 Feb 2024 17:45:33 +0900
Subject: [PATCH 3/4] fix(avoidance): use threshold param
Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
---
.../src/utils.cpp | 45 ++++++++++---------
1 file changed, 24 insertions(+), 21 deletions(-)
diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp
index cca6e6f2ab351..15abd67c76a09 100644
--- a/planning/behavior_path_avoidance_module/src/utils.cpp
+++ b/planning/behavior_path_avoidance_module/src/utils.cpp
@@ -250,37 +250,37 @@ void pushUniqueVector(T & base_vector, const T & additional_vector)
base_vector.insert(base_vector.end(), additional_vector.begin(), additional_vector.end());
}
-bool isAvoidShift(const double start_shift_length, const double end_shift_length)
+bool isAvoidShift(
+ const double start_shift_length, const double end_shift_length, const double threshold)
{
- constexpr double THRESHOLD = 0.1;
- return std::abs(start_shift_length) < THRESHOLD && std::abs(end_shift_length) > THRESHOLD;
+ return std::abs(start_shift_length) < threshold && std::abs(end_shift_length) > threshold;
}
-bool isReturnShift(const double start_shift_length, const double end_shift_length)
+bool isReturnShift(
+ const double start_shift_length, const double end_shift_length, const double threshold)
{
- constexpr double THRESHOLD = 0.1;
- return std::abs(start_shift_length) > THRESHOLD && std::abs(end_shift_length) < THRESHOLD;
+ return std::abs(start_shift_length) > threshold && std::abs(end_shift_length) < threshold;
}
-bool isLeftMiddleShift(const double start_shift_length, const double end_shift_length)
+bool isLeftMiddleShift(
+ const double start_shift_length, const double end_shift_length, const double threshold)
{
- constexpr double THRESHOLD = 0.1;
- return start_shift_length > THRESHOLD && end_shift_length > THRESHOLD;
+ return start_shift_length > threshold && end_shift_length > threshold;
}
-bool isRightMiddleShift(const double start_shift_length, const double end_shift_length)
+bool isRightMiddleShift(
+ const double start_shift_length, const double end_shift_length, const double threshold)
{
- constexpr double THRESHOLD = 0.1;
- return start_shift_length < THRESHOLD && end_shift_length < THRESHOLD;
+ return start_shift_length < threshold && end_shift_length < threshold;
}
bool existShiftSideLane(
const double start_shift_length, const double end_shift_length, const bool no_left_lanes,
- const bool no_right_lanes)
+ const bool no_right_lanes, const double threshold)
{
const auto relative_shift_length = end_shift_length - start_shift_length;
- if (isAvoidShift(start_shift_length, end_shift_length)) {
+ if (isAvoidShift(start_shift_length, end_shift_length, threshold)) {
// Left avoid. But there is no adjacent lane. No need blinker.
if (relative_shift_length > 0.0 && no_left_lanes) {
return false;
@@ -292,7 +292,7 @@ bool existShiftSideLane(
}
}
- if (isReturnShift(start_shift_length, end_shift_length)) {
+ if (isReturnShift(start_shift_length, end_shift_length, threshold)) {
// Right return. But there is no adjacent lane. No need blinker.
if (relative_shift_length > 0.0 && no_right_lanes) {
return false;
@@ -304,7 +304,7 @@ bool existShiftSideLane(
}
}
- if (isLeftMiddleShift(start_shift_length, end_shift_length)) {
+ if (isLeftMiddleShift(start_shift_length, end_shift_length, threshold)) {
// Left avoid. But there is no adjacent lane. No need blinker.
if (relative_shift_length > 0.0 && no_left_lanes) {
return false;
@@ -316,7 +316,7 @@ bool existShiftSideLane(
}
}
- if (isRightMiddleShift(start_shift_length, end_shift_length)) {
+ if (isRightMiddleShift(start_shift_length, end_shift_length, threshold)) {
// Right avoid. But there is no adjacent lane. No need blinker.
if (relative_shift_length < 0.0 && no_right_lanes) {
return false;
@@ -333,13 +333,13 @@ bool existShiftSideLane(
bool isNearEndOfShift(
const double start_shift_length, const double end_shift_length, const Point & ego_pos,
- const lanelet::ConstLanelets & original_lanes)
+ const lanelet::ConstLanelets & original_lanes, const double threshold)
{
using boost::geometry::within;
using lanelet::utils::to2D;
using lanelet::utils::conversion::toLaneletPoint;
- if (!isReturnShift(start_shift_length, end_shift_length)) {
+ if (!isReturnShift(start_shift_length, end_shift_length, threshold)) {
return false;
}
@@ -2450,7 +2450,9 @@ std::pair<TurnSignalInfo, bool> calcTurnSignalInfo(
const auto has_right_lane =
right_same_direction_lane.has_value() || !right_opposite_lanes.empty();
- if (!existShiftSideLane(start_shift_length, end_shift_length, !has_left_lane, !has_right_lane)) {
+ if (!existShiftSideLane(
+ start_shift_length, end_shift_length, !has_left_lane, !has_right_lane,
+ p.turn_signal_shift_length_threshold)) {
return std::make_pair(TurnSignalInfo{}, true);
}
@@ -2461,7 +2463,8 @@ std::pair<TurnSignalInfo, bool> calcTurnSignalInfo(
constexpr double STOPPED_THRESHOLD = 0.1; // [m/s]
if (ego_speed < STOPPED_THRESHOLD) {
if (isNearEndOfShift(
- start_shift_length, end_shift_length, ego_pose.position, data.current_lanelets)) {
+ start_shift_length, end_shift_length, ego_pose.position, data.current_lanelets,
+ p.turn_signal_shift_length_threshold)) {
return std::make_pair(TurnSignalInfo{}, true);
}
}
From 2114b0e8da2e240417a7d208bc24fb8c637cf13e Mon Sep 17 00:00:00 2001
From: satoshi-ota <satoshi.ota928@gmail.com>
Date: Mon, 26 Feb 2024 17:54:41 +0900
Subject: [PATCH 4/4] fix(avoidance): use lambda
Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
---
planning/behavior_path_avoidance_module/src/utils.cpp | 10 +++-------
1 file changed, 3 insertions(+), 7 deletions(-)
diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp
index 15abd67c76a09..df775ea6b2c0a 100644
--- a/planning/behavior_path_avoidance_module/src/utils.cpp
+++ b/planning/behavior_path_avoidance_module/src/utils.cpp
@@ -343,13 +343,9 @@ bool isNearEndOfShift(
return false;
}
- for (const auto & lane : original_lanes) {
- if (within(to2D(toLaneletPoint(ego_pos)), lane.polygon2d().basicPolygon())) {
- return true;
- }
- }
-
- return false;
+ return std::any_of(original_lanes.begin(), original_lanes.end(), [&ego_pos](const auto & lane) {
+ return within(to2D(toLaneletPoint(ego_pos)), lane.polygon2d().basicPolygon());
+ });
}
bool straddleRoadBound(