From d6124fa3ea47060ec9bdb001136d280963960e09 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Tue, 12 Mar 2024 17:11:06 +0900
Subject: [PATCH 01/13] Move avoidance calc turn signal to common
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../behavior_path_avoidance_module/utils.hpp | 3 -
.../src/scene.cpp | 8 +-
.../src/utils.cpp | 237 ------------------
...ehavior_path_planner_turn_signal_design.md | 64 ++---
.../data_manager.hpp | 9 +
.../turn_signal_decider.hpp | 144 +++++++++++
.../src/turn_signal_decider.cpp | 122 +++++++++
7 files changed, 312 insertions(+), 275 deletions(-)
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 2d61571392eef..9dc23ea4ec80e 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,9 +176,6 @@ double calcDistanceToAvoidStartLine(
const std::shared_ptr<const PlannerData> & planner_data,
const std::shared_ptr<AvoidanceParameters> & parameters);
-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
#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__UTILS_HPP_
diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp
index 384fd4a0d2fa3..2b10c8e94ebc5 100644
--- a/planning/behavior_path_avoidance_module/src/scene.cpp
+++ b/planning/behavior_path_avoidance_module/src/scene.cpp
@@ -947,9 +947,11 @@ BehaviorModuleOutput AvoidanceModule::plan()
output.turn_signal_info = getPreviousModuleOutput().turn_signal_info;
} else {
const auto original_signal = getPreviousModuleOutput().turn_signal_info;
- const auto [new_signal, is_ignore] = utils::avoidance::calcTurnSignalInfo(
- linear_shift_path, path_shifter_.getShiftLines().front(), helper_->getEgoShift(), avoid_data_,
- planner_data_);
+
+ const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo(
+ linear_shift_path, path_shifter_.getShiftLines().front(), avoid_data_.current_lanelets,
+ helper_->getEgoShift());
+
const auto current_seg_idx = planner_data_->findEgoSegmentIndex(spline_shift_path.path.points);
output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal(
spline_shift_path.path, getEgoPose(), current_seg_idx, original_signal, new_signal,
diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp
index 943ff8bfe5c4d..0fd649131bdcd 100644
--- a/planning/behavior_path_avoidance_module/src/utils.cpp
+++ b/planning/behavior_path_avoidance_module/src/utils.cpp
@@ -250,132 +250,6 @@ 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, const double 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, const double 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, const double threshold)
-{
- return start_shift_length > threshold && end_shift_length > threshold;
-}
-
-bool isRightMiddleShift(
- const double start_shift_length, const double end_shift_length, const double 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 double threshold)
-{
- const auto relative_shift_length = end_shift_length - start_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;
- }
-
- // Right avoid. But there is no adjacent lane. No need blinker.
- if (relative_shift_length < 0.0 && no_right_lanes) {
- return false;
- }
- }
-
- 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;
- }
-
- // Left return. But there is no adjacent lane. No need blinker.
- if (relative_shift_length < 0.0 && no_left_lanes) {
- return false;
- }
- }
-
- 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;
- }
-
- // Left return. But there is no adjacent lane. No need blinker.
- if (relative_shift_length < 0.0 && no_left_lanes) {
- return false;
- }
- }
-
- 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;
- }
-
- // Left avoid. But there is no adjacent lane. No need blinker.
- if (relative_shift_length > 0.0 && no_right_lanes) {
- return false;
- }
- }
-
- return true;
-}
-
-bool isNearEndOfShift(
- const double start_shift_length, const double end_shift_length, const Point & ego_pos,
- 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, threshold)) {
- 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(
- const ShiftedPath & path, const ShiftLine & shift_line, const lanelet::ConstLanelets & lanes,
- const vehicle_info_util::VehicleInfo & vehicle_info)
-{
- using boost::geometry::intersects;
- using tier4_autoware_utils::pose2transform;
- using tier4_autoware_utils::transformVector;
-
- const auto footprint = vehicle_info.createFootprint();
-
- for (const auto & lane : lanes) {
- for (size_t i = shift_line.start_idx; i < shift_line.end_idx; ++i) {
- const auto transform = pose2transform(path.path.points.at(i).point.pose);
- const auto shifted_vehicle_footprint = transformVector(footprint, transform);
-
- if (intersects(lane.leftBound2d().basicLineString(), shifted_vehicle_footprint)) {
- return true;
- }
-
- if (intersects(lane.rightBound2d().basicLineString(), shifted_vehicle_footprint)) {
- return true;
- }
- }
- }
-
- return false;
-}
-
} // namespace
namespace filtering_utils
@@ -2360,115 +2234,4 @@ double calcDistanceToReturnDeadLine(
return distance_to_return_dead_line;
}
-
-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)
-{
- constexpr double THRESHOLD = 0.1;
- const auto & p = planner_data->parameters;
- const auto & rh = planner_data->route_handler;
- const auto & ego_pose = planner_data->self_odometry->pose.pose;
- const auto & ego_speed = planner_data->self_odometry->twist.twist.linear.x;
-
- if (shift_line.start_idx + 1 > path.shift_length.size()) {
- RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency.");
- 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 std::make_pair(TurnSignalInfo{}, true);
- }
-
- if (shift_line.end_idx + 1 > path.shift_length.size()) {
- RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency.");
- 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 std::make_pair(TurnSignalInfo{}, true);
- }
-
- const auto start_shift_length = path.shift_length.at(shift_line.start_idx);
- const auto end_shift_length = path.shift_length.at(shift_line.end_idx);
- const auto relative_shift_length = end_shift_length - start_shift_length;
-
- // 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 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 std::make_pair(TurnSignalInfo{}, true);
- }
-
- const auto get_command = [](const auto & shift_length) {
- return shift_length > 0.0 ? TurnIndicatorsCommand::ENABLE_LEFT
- : TurnIndicatorsCommand::ENABLE_RIGHT;
- };
-
- const auto signal_prepare_distance =
- std::max(ego_speed * p.turn_signal_search_time, p.turn_signal_minimum_search_distance);
- const auto ego_front_to_shift_start =
- calcSignedArcLength(path.path.points, ego_pose.position, shift_line.start_idx) -
- p.vehicle_info.max_longitudinal_offset_m;
-
- if (signal_prepare_distance < ego_front_to_shift_start) {
- return std::make_pair(TurnSignalInfo{}, false);
- }
-
- const auto blinker_start_pose = path.path.points.at(shift_line.start_idx).point.pose;
- const auto blinker_end_pose = path.path.points.at(shift_line.end_idx).point.pose;
- const auto get_start_pose = [&](const auto & ego_to_shift_start) {
- return ego_to_shift_start ? ego_pose : blinker_start_pose;
- };
-
- TurnSignalInfo turn_signal_info{};
- turn_signal_info.desired_start_point = get_start_pose(ego_front_to_shift_start);
- turn_signal_info.desired_end_point = blinker_end_pose;
- turn_signal_info.required_start_point = blinker_start_pose;
- turn_signal_info.required_end_point = blinker_end_pose;
- turn_signal_info.turn_signal.command = get_command(relative_shift_length);
-
- if (!p.turn_signal_on_swerving) {
- return std::make_pair(turn_signal_info, false);
- }
-
- lanelet::ConstLanelet lanelet;
- if (!rh->getClosestLaneletWithinRoute(shift_line.end, &lanelet)) {
- return std::make_pair(TurnSignalInfo{}, true);
- }
-
- const auto left_same_direction_lane = rh->getLeftLanelet(lanelet, true, true);
- const auto left_opposite_lanes = rh->getLeftOppositeLanelets(lanelet);
- const auto right_same_direction_lane = rh->getRightLanelet(lanelet, true, true);
- const auto right_opposite_lanes = rh->getRightOppositeLanelets(lanelet);
- const auto has_left_lane = left_same_direction_lane.has_value() || !left_opposite_lanes.empty();
- 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,
- p.turn_signal_shift_length_threshold)) {
- return std::make_pair(TurnSignalInfo{}, true);
- }
-
- if (!straddleRoadBound(path, shift_line, data.current_lanelets, p.vehicle_info)) {
- 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,
- p.turn_signal_shift_length_threshold)) {
- return std::make_pair(TurnSignalInfo{}, true);
- }
- }
-
- return std::make_pair(turn_signal_info, false);
-}
} // namespace behavior_path_planner::utils::avoidance
diff --git a/planning/behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md b/planning/behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md
index c183e0627a674..bfc22bacc8ede 100644
--- a/planning/behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md
+++ b/planning/behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md
@@ -4,7 +4,7 @@ Turn Signal decider determines necessary blinkers.
## Purpose / Role
-This module is responsible for activating a necessary blinker during driving. It uses rule-based algorithm to determine blinkers, and the details of this algorithm are described in the following sections. Note that this algorithm is strictly based on the Japanese Road Traffic Raw.
+This module is responsible for activating a necessary blinker during driving. It uses rule-based algorithm to determine blinkers, and the details of this algorithm are described in the following sections. Note that this algorithm is strictly based on the Japanese Road Traffic Law.
### Assumptions
@@ -16,7 +16,7 @@ Autoware has following order of priorities for turn signals.
### Limitations
-Currently, this algorithm can sometimes give unnatural (not wrong) blinkers in a complicated situations. This is because it tries to follow the road traffic raw and cannot solve `blinker conflicts` clearly in that environment.
+Currently, this algorithm can sometimes give unnatural (not wrong) blinkers in complicated situations. This is because it tries to follow the road traffic law and cannot solve `blinker conflicts` clearly in that environment.
## Parameters for turn signal decider
@@ -59,16 +59,16 @@ For left turn, right turn, avoidance, lane change, goal planner and pull out, we
Turn signal decider checks each lanelet on the map if it has `turn_direction` information. If a lanelet has this information, it activates necessary blinker based on this information.
-- desired start point
- The `search_distance` for blinkers at intersections is `v * turn_signal_search_time + turn_signal_intersection_search_distance`. Then the start point becomes `search_distance` meters before the start point of the intersection lanelet(depicted in gree in the following picture), where `v` is the velocity of the ego vehicle. However, if we set `turn_signal_distance` in the lanelet, we use that length as search distance.
+- desired start point
+ The `search_distance` for blinkers at intersections is `v * turn_signal_search_time + turn_signal_intersection_search_distance`. Then the start point becomes `search_distance` meters before the start point of the intersection lanelet(depicted in green in the following picture), where `v` is the velocity of the ego vehicle. However, if we set `turn_signal_distance` in the lanelet, we use that length as search distance.
-- desired end point
+- desired end point
Terminal point of the intersection lanelet.
-- required start point
+- required start point
Initial point of the intersection lanelet.
-- required end point
+- required end point
The earliest point that satisfies the following condition. $\theta - \theta_{\textrm{end}} < \delta$, where $\theta_{\textrm{end}}$ is yaw angle of the terminal point of the lanelet, $\theta$ is the angle of a required end point and $\delta$ is the threshold defined by the user.
@@ -79,99 +79,99 @@ Avoidance can be separated into two sections, first section and second section.
First section
-- desired start point
+- desired start point
`v * turn_signal_search_time` meters before the start point of the avoidance shift path.
-- desired end point
+- desired end point
Shift complete point where the path shift is completed.
-- required start point
+- required start point
Avoidance start point.
-- required end point
+- required end point
Shift complete point same as the desired end point.
Second section
-- desired start point
+- desired start point
Shift complete point.
-- desired end point
+- desired end point
Avoidance terminal point
-- required start point
+- required start point
Shift complete point.
-- required end point
+- required end point
Avoidance terminal point.
#### 3. Lane Change
-- desired start point
+- desired start point
`v * turn_signal_search_time` meters before the start point of the lane change path.
-- desired end point
+- desired end point
Terminal point of the lane change path.
-- required start point
+- required start point
Initial point of the lane change path.
-- required end point
+- required end point
Cross point with lane change path and boundary line of the adjacent lane.
#### 4. Pull out
-- desired start point
+- desired start point
Start point of the path of pull out.
-- desired end point
+- desired end point
Terminal point of the path of pull out.
-- required start point
+- required start point
Start point of the path pull out.
-- required end point
+- required end point
Terminal point of the path of pull out.
#### 5. Goal Planner
-- desired start point
+- desired start point
`v * turn_signal_search_time` meters before the start point of the pull over path.
-- desired end point
+- desired end point
Terminal point of the path of pull over.
-- required start point
+- required start point
Start point of the path of pull over.
-- required end point
+- required end point
Terminal point of the path of pull over.
When it comes to handle several blinkers, it gives priority to the first blinker that comes first. However, this rule sometimes activate unnatural blinkers, so turn signal decider uses the following five rules to decide the necessary turn signal.
-- pattern1
+- pattern1
-- pattern2
+- pattern2
-- pattern3
+- pattern3
-- pattern4
+- pattern4
-- pattern5
+- pattern5
Based on these five rules, turn signal decider can solve `blinker conflicts`. The following pictures show some examples of this kind of conflicts.
diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp
index 28943b5724fe8..b59b9450f1b3b 100644
--- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp
+++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp
@@ -165,6 +165,15 @@ struct PlannerData
mutable std::vector<double> drivable_area_expansion_prev_curvatures{};
mutable TurnSignalDecider turn_signal_decider;
+ std::pair<TurnSignalInfo, bool> getBehaviorTurnSignalInfo(
+ const ShiftedPath & path, const ShiftLine & shift_line,
+ const lanelet::ConstLanelets & current_lanelets, const double current_shift_length) const
+ {
+ return turn_signal_decider.getBehaviorTurnSignalInfo(
+ path, shift_line, current_lanelets, route_handler, parameters, self_odometry,
+ current_shift_length);
+ }
+
TurnIndicatorsCommand getTurnSignal(
const PathWithLaneId & path, const TurnSignalInfo & turn_signal_info,
TurnSignalDebugData & debug_data)
diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp
index cd639b5e81092..6dddd7041118b 100644
--- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp
+++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp
@@ -15,14 +15,24 @@
#ifndef BEHAVIOR_PATH_PLANNER_COMMON__TURN_SIGNAL_DECIDER_HPP_
#define BEHAVIOR_PATH_PLANNER_COMMON__TURN_SIGNAL_DECIDER_HPP_
+#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp"
+
#include <behavior_path_planner_common/parameters.hpp>
+#include <behavior_path_planner_common/utils/path_shifter/path_shifter.hpp>
+#include <lanelet2_extension/utility/message_conversion.hpp>
#include <route_handler/route_handler.hpp>
+#include <tier4_autoware_utils/geometry/boost_geometry.hpp>
+#include <tier4_autoware_utils/geometry/geometry.hpp>
#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
#include <autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp>
#include <autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+
+#include <boost/geometry/algorithms/intersects.hpp>
#include <lanelet2_core/Forward.h>
+#include <lanelet2_routing/RoutingGraphContainer.h>
#include <limits>
#include <map>
@@ -37,6 +47,7 @@ using autoware_auto_vehicle_msgs::msg::HazardLightsCommand;
using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand;
using geometry_msgs::msg::Point;
using geometry_msgs::msg::Pose;
+using nav_msgs::msg::Odometry;
using route_handler::RouteHandler;
const std::map<std::string, uint8_t> g_signal_map = {
@@ -100,6 +111,13 @@ class TurnSignalDecider
std::pair<bool, bool> getIntersectionTurnSignalFlag();
std::pair<Pose, double> getIntersectionPoseAndDistance();
+ std::pair<TurnSignalInfo, bool> getBehaviorTurnSignalInfo(
+ const ShiftedPath & path, const ShiftLine & shift_line,
+ const lanelet::ConstLanelets & current_lanelets,
+ const std::shared_ptr<RouteHandler> route_handler,
+ const BehaviorPathPlannerParameters & parameters, const Odometry::ConstSharedPtr self_odometry,
+ const double current_shift_length) const;
+
private:
std::optional<TurnSignalInfo> getIntersectionTurnSignalInfo(
const PathWithLaneId & path, const Pose & current_pose, const double current_vel,
@@ -118,6 +136,132 @@ class TurnSignalDecider
const double nearest_yaw_threshold);
void initialize_intersection_info();
+ inline bool isAvoidShift(
+ const double start_shift_length, const double end_shift_length, const double threshold) const
+ {
+ return std::abs(start_shift_length) < threshold && std::abs(end_shift_length) > threshold;
+ };
+
+ inline bool isReturnShift(
+ const double start_shift_length, const double end_shift_length, const double threshold) const
+ {
+ return std::abs(start_shift_length) > threshold && std::abs(end_shift_length) < threshold;
+ };
+
+ inline bool isLeftMiddleShift(
+ const double start_shift_length, const double end_shift_length, const double threshold) const
+ {
+ return start_shift_length > threshold && end_shift_length > threshold;
+ };
+
+ inline bool isRightMiddleShift(
+ const double start_shift_length, const double end_shift_length, const double threshold) const
+ {
+ return start_shift_length < threshold && end_shift_length < threshold;
+ };
+
+ inline bool existShiftSideLane(
+ const double start_shift_length, const double end_shift_length, const bool no_left_lanes,
+ const bool no_right_lanes, const double threshold) const
+ {
+ const auto relative_shift_length = end_shift_length - start_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;
+ }
+
+ // Right avoid. But there is no adjacent lane. No need blinker.
+ if (relative_shift_length < 0.0 && no_right_lanes) {
+ return false;
+ }
+ }
+
+ 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;
+ }
+
+ // Left return. But there is no adjacent lane. No need blinker.
+ if (relative_shift_length < 0.0 && no_left_lanes) {
+ return false;
+ }
+ }
+
+ 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;
+ }
+
+ // Left return. But there is no adjacent lane. No need blinker.
+ if (relative_shift_length < 0.0 && no_left_lanes) {
+ return false;
+ }
+ }
+
+ 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;
+ }
+
+ // Left avoid. But there is no adjacent lane. No need blinker.
+ if (relative_shift_length > 0.0 && no_right_lanes) {
+ return false;
+ }
+ }
+
+ return true;
+ };
+
+ inline bool straddleRoadBound(
+ const ShiftedPath & path, const ShiftLine & shift_line, const lanelet::ConstLanelets & lanes,
+ const vehicle_info_util::VehicleInfo & vehicle_info) const
+ {
+ using boost::geometry::intersects;
+ using tier4_autoware_utils::pose2transform;
+ using tier4_autoware_utils::transformVector;
+
+ const auto footprint = vehicle_info.createFootprint();
+
+ for (const auto & lane : lanes) {
+ for (size_t i = shift_line.start_idx; i < shift_line.end_idx; ++i) {
+ const auto transform = pose2transform(path.path.points.at(i).point.pose);
+ const auto shifted_vehicle_footprint = transformVector(footprint, transform);
+
+ if (intersects(lane.leftBound2d().basicLineString(), shifted_vehicle_footprint)) {
+ return true;
+ }
+
+ if (intersects(lane.rightBound2d().basicLineString(), shifted_vehicle_footprint)) {
+ return true;
+ }
+ }
+ }
+
+ return false;
+ };
+
+ inline bool isNearEndOfShift(
+ const double start_shift_length, const double end_shift_length, const Point & ego_pos,
+ const lanelet::ConstLanelets & original_lanes, const double threshold) const
+ {
+ using boost::geometry::within;
+ using lanelet::utils::to2D;
+ using lanelet::utils::conversion::toLaneletPoint;
+
+ if (!isReturnShift(start_shift_length, end_shift_length, threshold)) {
+ 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());
+ });
+ };
+
geometry_msgs::msg::Quaternion calc_orientation(const Point & src_point, const Point & dst_point);
rclcpp::Logger logger_{
diff --git a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp
index 6ddc0df4eebf4..b3176d0e279bc 100644
--- a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp
+++ b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp
@@ -33,6 +33,8 @@
namespace behavior_path_planner
{
+using motion_utils::calcSignedArcLength;
+
double calc_distance(
const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx,
const Pose & input_point, const double nearest_dist_threshold, const double nearest_yaw_threshold)
@@ -43,6 +45,11 @@ double calc_distance(
path.points, current_pose.position, current_seg_idx, input_point.position, nearest_seg_idx);
}
+/***
+ * @brief:
+ * Gets the turn signal info after comparing the turn signal info output from the behavior path
+ * module and comparing it to turn signal info obtained from intersections.
+ */
TurnIndicatorsCommand TurnSignalDecider::getTurnSignal(
const std::shared_ptr<RouteHandler> & route_handler, const PathWithLaneId & path,
const TurnSignalInfo & turn_signal_info, const Pose & current_pose, const double current_vel,
@@ -606,4 +613,119 @@ geometry_msgs::msg::Quaternion TurnSignalDecider::calc_orientation(
const double yaw = tier4_autoware_utils::calcAzimuthAngle(src_point, dst_point);
return tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw);
}
+
+std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(
+ const ShiftedPath & path, const ShiftLine & shift_line,
+ const lanelet::ConstLanelets & current_lanelets,
+ const std::shared_ptr<RouteHandler> route_handler,
+ const BehaviorPathPlannerParameters & parameters, const Odometry::ConstSharedPtr self_odometry,
+ const double current_shift_length) const
+{
+ constexpr double THRESHOLD = 0.1;
+ const auto & p = parameters;
+ const auto & rh = route_handler;
+ const auto & ego_pose = self_odometry->pose.pose;
+ const auto & ego_speed = self_odometry->twist.twist.linear.x;
+
+ if (shift_line.start_idx + 1 > path.shift_length.size()) {
+ RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency.");
+ 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 std::make_pair(TurnSignalInfo{}, true);
+ }
+
+ if (shift_line.end_idx + 1 > path.shift_length.size()) {
+ RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency.");
+ 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 std::make_pair(TurnSignalInfo{}, true);
+ }
+
+ const auto start_shift_length = path.shift_length.at(shift_line.start_idx);
+ const auto end_shift_length = path.shift_length.at(shift_line.end_idx);
+ const auto relative_shift_length = end_shift_length - start_shift_length;
+
+ // 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 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 std::make_pair(TurnSignalInfo{}, true);
+ }
+
+ const auto get_command = [](const auto & shift_length) {
+ return shift_length > 0.0 ? TurnIndicatorsCommand::ENABLE_LEFT
+ : TurnIndicatorsCommand::ENABLE_RIGHT;
+ };
+
+ const auto signal_prepare_distance =
+ std::max(ego_speed * p.turn_signal_search_time, p.turn_signal_minimum_search_distance);
+ const auto ego_front_to_shift_start =
+ calcSignedArcLength(path.path.points, ego_pose.position, shift_line.start_idx) -
+ p.vehicle_info.max_longitudinal_offset_m;
+
+ if (signal_prepare_distance < ego_front_to_shift_start) {
+ return std::make_pair(TurnSignalInfo{}, false);
+ }
+
+ const auto blinker_start_pose = path.path.points.at(shift_line.start_idx).point.pose;
+ const auto blinker_end_pose = path.path.points.at(shift_line.end_idx).point.pose;
+ const auto get_start_pose = [&](const auto & ego_to_shift_start) {
+ return ego_to_shift_start ? ego_pose : blinker_start_pose;
+ };
+
+ TurnSignalInfo turn_signal_info{};
+ turn_signal_info.desired_start_point = get_start_pose(ego_front_to_shift_start);
+ turn_signal_info.desired_end_point = blinker_end_pose;
+ turn_signal_info.required_start_point = blinker_start_pose;
+ turn_signal_info.required_end_point = blinker_end_pose;
+ turn_signal_info.turn_signal.command = get_command(relative_shift_length);
+
+ if (!p.turn_signal_on_swerving) {
+ return std::make_pair(turn_signal_info, false);
+ }
+
+ lanelet::ConstLanelet lanelet;
+ if (!rh->getClosestLaneletWithinRoute(shift_line.end, &lanelet)) {
+ return std::make_pair(TurnSignalInfo{}, true);
+ }
+
+ const auto left_same_direction_lane = rh->getLeftLanelet(lanelet, true, true);
+ const auto left_opposite_lanes = rh->getLeftOppositeLanelets(lanelet);
+ const auto right_same_direction_lane = rh->getRightLanelet(lanelet, true, true);
+ const auto right_opposite_lanes = rh->getRightOppositeLanelets(lanelet);
+ const auto has_left_lane = left_same_direction_lane.has_value() || !left_opposite_lanes.empty();
+ 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,
+ p.turn_signal_shift_length_threshold)) {
+ return std::make_pair(TurnSignalInfo{}, true);
+ }
+
+ if (!straddleRoadBound(path, shift_line, current_lanelets, p.vehicle_info)) {
+ 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, current_lanelets,
+ p.turn_signal_shift_length_threshold)) {
+ return std::make_pair(TurnSignalInfo{}, true);
+ }
+ }
+
+ return std::make_pair(turn_signal_info, false);
+}
+
} // namespace behavior_path_planner
From c21847a19ebb63caceda0f74efd03100232a256f Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Tue, 12 Mar 2024 18:02:25 +0900
Subject: [PATCH 02/13] Add backwards drive support
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../behavior_path_avoidance_module/src/scene.cpp | 2 +-
.../data_manager.hpp | 5 +++--
.../turn_signal_decider.hpp | 2 +-
.../src/turn_signal_decider.cpp | 16 +++++++++++++++-
4 files changed, 20 insertions(+), 5 deletions(-)
diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp
index 2b10c8e94ebc5..ee8089017bd57 100644
--- a/planning/behavior_path_avoidance_module/src/scene.cpp
+++ b/planning/behavior_path_avoidance_module/src/scene.cpp
@@ -950,7 +950,7 @@ BehaviorModuleOutput AvoidanceModule::plan()
const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo(
linear_shift_path, path_shifter_.getShiftLines().front(), avoid_data_.current_lanelets,
- helper_->getEgoShift());
+ helper_->getEgoShift(), true);
const auto current_seg_idx = planner_data_->findEgoSegmentIndex(spline_shift_path.path.points);
output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal(
diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp
index b59b9450f1b3b..33a662255823d 100644
--- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp
+++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp
@@ -167,11 +167,12 @@ struct PlannerData
std::pair<TurnSignalInfo, bool> getBehaviorTurnSignalInfo(
const ShiftedPath & path, const ShiftLine & shift_line,
- const lanelet::ConstLanelets & current_lanelets, const double current_shift_length) const
+ const lanelet::ConstLanelets & current_lanelets, const double current_shift_length,
+ const bool is_driving_forward) const
{
return turn_signal_decider.getBehaviorTurnSignalInfo(
path, shift_line, current_lanelets, route_handler, parameters, self_odometry,
- current_shift_length);
+ current_shift_length, is_driving_forward);
}
TurnIndicatorsCommand getTurnSignal(
diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp
index 6dddd7041118b..2baa7740d0ccf 100644
--- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp
+++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp
@@ -116,7 +116,7 @@ class TurnSignalDecider
const lanelet::ConstLanelets & current_lanelets,
const std::shared_ptr<RouteHandler> route_handler,
const BehaviorPathPlannerParameters & parameters, const Odometry::ConstSharedPtr self_odometry,
- const double current_shift_length) const;
+ const double current_shift_length, const bool is_driving_forward) const;
private:
std::optional<TurnSignalInfo> getIntersectionTurnSignalInfo(
diff --git a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp
index b3176d0e279bc..10089b7baf9f8 100644
--- a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp
+++ b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp
@@ -619,7 +619,7 @@ std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(
const lanelet::ConstLanelets & current_lanelets,
const std::shared_ptr<RouteHandler> route_handler,
const BehaviorPathPlannerParameters & parameters, const Odometry::ConstSharedPtr self_odometry,
- const double current_shift_length) const
+ const double current_shift_length, const bool is_driving_forward) const
{
constexpr double THRESHOLD = 0.1;
const auto & p = parameters;
@@ -627,6 +627,20 @@ std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(
const auto & ego_pose = self_odometry->pose.pose;
const auto & ego_speed = self_odometry->twist.twist.linear.x;
+ if (!is_driving_forward) {
+ TurnSignalInfo turn_signal_info{};
+ turn_signal_info.hazard_signal.command = HazardLightsCommand::ENABLE;
+ const auto back_start_pose = rh->getOriginalStartPose();
+ const Pose & start_pose = self_odometry->pose.pose;
+
+ turn_signal_info.desired_start_point = back_start_pose;
+ turn_signal_info.required_start_point = back_start_pose;
+ // pull_out start_pose is same to backward driving end_pose
+ turn_signal_info.required_end_point = start_pose;
+ turn_signal_info.desired_end_point = start_pose;
+ return std::make_pair(turn_signal_info, false);
+ }
+
if (shift_line.start_idx + 1 > path.shift_length.size()) {
RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency.");
return std::make_pair(TurnSignalInfo{}, true);
From e4a6bf91851a70bf5e52dd17e4160b22e055fed7 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Wed, 13 Mar 2024 09:23:25 +0900
Subject: [PATCH 03/13] Add general turn signal to lane change
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../src/scene.cpp | 24 +++++++++++++------
1 file changed, 17 insertions(+), 7 deletions(-)
diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp
index 1e97017b24856..7944f08b9a8c0 100644
--- a/planning/behavior_path_lane_change_module/src/scene.cpp
+++ b/planning/behavior_path_lane_change_module/src/scene.cpp
@@ -480,14 +480,24 @@ void NormalLaneChange::resetParameters()
TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() const
{
- TurnSignalInfo turn_signal_info = calcTurnSignalInfo();
- const auto [turn_signal_command, distance_to_vehicle_front] = utils::getPathTurnSignal(
- status_.current_lanes, status_.lane_change_path.shifted_path,
- status_.lane_change_path.info.shift_line, getEgoPose(), getEgoTwist().linear.x,
- planner_data_->parameters);
- turn_signal_info.turn_signal.command = turn_signal_command.command;
+ const auto & pose = getEgoPose();
+ const auto & current_lanes = status_.current_lanes;
+ const auto & shift_line = status_.lane_change_path.info.shift_line;
+ const auto & shift_path = status_.lane_change_path.shifted_path;
+ const auto arc_position_current_pose = lanelet::utils::getArcCoordinates(current_lanes, pose);
+
+ const double distance_to_shift_start =
+ std::invoke([¤t_lanes, &shift_line, &arc_position_current_pose]() {
+ const auto arc_position_shift_start =
+ lanelet::utils::getArcCoordinates(current_lanes, shift_line.start);
+ return arc_position_shift_start.length - arc_position_current_pose.length;
+ });
- return turn_signal_info;
+ // return turn_signal_info;
+
+ const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo(
+ shift_path, shift_line, current_lanes, distance_to_shift_start, true);
+ return new_signal;
}
lanelet::ConstLanelets NormalLaneChange::getCurrentLanes() const
From e50bb9d8dee9a495f62f5a3218d16be8b308a6dc Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Wed, 13 Mar 2024 18:29:38 +0900
Subject: [PATCH 04/13] WIP return avoid not working
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../src/scene.cpp | 13 ++-----
.../turn_signal_decider.hpp | 8 ++++-
.../src/turn_signal_decider.cpp | 35 ++++++++++++++++---
3 files changed, 40 insertions(+), 16 deletions(-)
diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp
index 7944f08b9a8c0..34c3b74118d8d 100644
--- a/planning/behavior_path_lane_change_module/src/scene.cpp
+++ b/planning/behavior_path_lane_change_module/src/scene.cpp
@@ -484,19 +484,10 @@ TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() const
const auto & current_lanes = status_.current_lanes;
const auto & shift_line = status_.lane_change_path.info.shift_line;
const auto & shift_path = status_.lane_change_path.shifted_path;
- const auto arc_position_current_pose = lanelet::utils::getArcCoordinates(current_lanes, pose);
-
- const double distance_to_shift_start =
- std::invoke([¤t_lanes, &shift_line, &arc_position_current_pose]() {
- const auto arc_position_shift_start =
- lanelet::utils::getArcCoordinates(current_lanes, shift_line.start);
- return arc_position_shift_start.length - arc_position_current_pose.length;
- });
-
- // return turn_signal_info;
+ const auto current_shift_length = lanelet::utils::getArcCoordinates(current_lanes, pose).distance;
const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo(
- shift_path, shift_line, current_lanes, distance_to_shift_start, true);
+ shift_path, shift_line, current_lanes, current_shift_length, true);
return new_signal;
}
diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp
index 2baa7740d0ccf..a3adac557c185 100644
--- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp
+++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp
@@ -165,8 +165,9 @@ class TurnSignalDecider
const bool no_right_lanes, const double threshold) const
{
const auto relative_shift_length = end_shift_length - start_shift_length;
-
+ std::cout << "relative_shift_length " << relative_shift_length << "\n";
if (isAvoidShift(start_shift_length, end_shift_length, threshold)) {
+ std::cerr << "Enters here 0 ? \n";
// Left avoid. But there is no adjacent lane. No need blinker.
if (relative_shift_length > 0.0 && no_left_lanes) {
return false;
@@ -179,6 +180,7 @@ class TurnSignalDecider
}
if (isReturnShift(start_shift_length, end_shift_length, threshold)) {
+ std::cerr << "Enters here 1 ? \n";
// Right return. But there is no adjacent lane. No need blinker.
if (relative_shift_length > 0.0 && no_right_lanes) {
return false;
@@ -192,6 +194,8 @@ class TurnSignalDecider
if (isLeftMiddleShift(start_shift_length, end_shift_length, threshold)) {
// Left avoid. But there is no adjacent lane. No need blinker.
+ std::cerr << "Enters here 2 ? \n";
+
if (relative_shift_length > 0.0 && no_left_lanes) {
return false;
}
@@ -204,6 +208,8 @@ class TurnSignalDecider
if (isRightMiddleShift(start_shift_length, end_shift_length, threshold)) {
// Right avoid. But there is no adjacent lane. No need blinker.
+ std::cerr << "Enters here 3 ? \n";
+
if (relative_shift_length < 0.0 && no_right_lanes) {
return false;
}
diff --git a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp
index 10089b7baf9f8..c4a67f91b1a30 100644
--- a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp
+++ b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp
@@ -621,6 +621,7 @@ std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(
const BehaviorPathPlannerParameters & parameters, const Odometry::ConstSharedPtr self_odometry,
const double current_shift_length, const bool is_driving_forward) const
{
+ std::cerr << "-------------TurnSignalDecider::getBehaviorTurnSignalInfo-----------\n";
constexpr double THRESHOLD = 0.1;
const auto & p = parameters;
const auto & rh = route_handler;
@@ -661,17 +662,29 @@ std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(
return std::make_pair(TurnSignalInfo{}, true);
}
- const auto start_shift_length = path.shift_length.at(shift_line.start_idx);
- const auto end_shift_length = path.shift_length.at(shift_line.end_idx);
+ const auto [start_shift_length, end_shift_length] =
+ std::invoke([&path, &shift_line]() -> std::pair<double, double> {
+ const auto temp_start_shift_length = path.shift_length.at(shift_line.start_idx);
+ const auto temp_end_shift_length = path.shift_length.at(shift_line.end_idx);
+ // Shift is done using the target lane and not current lane
+ if (std::fabs(temp_start_shift_length) > std::fabs(temp_end_shift_length)) {
+ return std::make_pair(temp_end_shift_length, -temp_start_shift_length);
+ }
+ return std::make_pair(temp_start_shift_length, temp_end_shift_length);
+ });
+
const auto relative_shift_length = end_shift_length - start_shift_length;
// 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) {
+ std::cerr << "No signal 0\n";
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) {
+ if (std::fabs(end_shift_length - current_shift_length) < THRESHOLD) {
+ std::cerr << "No signal 1\n";
+
return std::make_pair(TurnSignalInfo{}, true);
}
@@ -686,14 +699,19 @@ std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(
calcSignedArcLength(path.path.points, ego_pose.position, shift_line.start_idx) -
p.vehicle_info.max_longitudinal_offset_m;
+ std::cerr << "ego_front_to_shift_start " << ego_front_to_shift_start << "\n";
+ std::cerr << "signal_prepare_distance " << signal_prepare_distance << "\n";
+
if (signal_prepare_distance < ego_front_to_shift_start) {
+ std::cerr << "No signal 2\n";
+
return std::make_pair(TurnSignalInfo{}, false);
}
const auto blinker_start_pose = path.path.points.at(shift_line.start_idx).point.pose;
const auto blinker_end_pose = path.path.points.at(shift_line.end_idx).point.pose;
const auto get_start_pose = [&](const auto & ego_to_shift_start) {
- return ego_to_shift_start ? ego_pose : blinker_start_pose;
+ return ego_to_shift_start > 0.0 ? ego_pose : blinker_start_pose;
};
TurnSignalInfo turn_signal_info{};
@@ -704,11 +722,13 @@ std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(
turn_signal_info.turn_signal.command = get_command(relative_shift_length);
if (!p.turn_signal_on_swerving) {
+ std::cerr << "No signal 3\n";
return std::make_pair(turn_signal_info, false);
}
lanelet::ConstLanelet lanelet;
if (!rh->getClosestLaneletWithinRoute(shift_line.end, &lanelet)) {
+ std::cerr << "No signal 4\n";
return std::make_pair(TurnSignalInfo{}, true);
}
@@ -723,10 +743,14 @@ std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(
if (!existShiftSideLane(
start_shift_length, end_shift_length, !has_left_lane, !has_right_lane,
p.turn_signal_shift_length_threshold)) {
+ std::cerr << "No signal 5\n";
+
return std::make_pair(TurnSignalInfo{}, true);
}
if (!straddleRoadBound(path, shift_line, current_lanelets, p.vehicle_info)) {
+ std::cerr << "No signal 6\n";
+
return std::make_pair(TurnSignalInfo{}, true);
}
@@ -735,9 +759,12 @@ std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(
if (isNearEndOfShift(
start_shift_length, end_shift_length, ego_pose.position, current_lanelets,
p.turn_signal_shift_length_threshold)) {
+ std::cerr << "No signal 7\n";
+
return std::make_pair(TurnSignalInfo{}, true);
}
}
+ std::cerr << "No signal 8\n";
return std::make_pair(turn_signal_info, false);
}
From ada03b7ef67bdbc7e3854e9889c5626c3bc3a4de Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Thu, 14 Mar 2024 09:50:20 +0900
Subject: [PATCH 05/13] add option for modules that shift the target lane and
not current lane
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
planning/behavior_path_avoidance_module/src/scene.cpp | 4 +++-
.../behavior_path_lane_change_module/src/scene.cpp | 5 ++++-
.../behavior_path_planner_common/data_manager.hpp | 4 ++--
.../turn_signal_decider.hpp | 3 ++-
.../src/turn_signal_decider.cpp | 10 ++++++----
5 files changed, 17 insertions(+), 9 deletions(-)
diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp
index ee8089017bd57..d1a55f5dc0b05 100644
--- a/planning/behavior_path_avoidance_module/src/scene.cpp
+++ b/planning/behavior_path_avoidance_module/src/scene.cpp
@@ -948,9 +948,11 @@ BehaviorModuleOutput AvoidanceModule::plan()
} else {
const auto original_signal = getPreviousModuleOutput().turn_signal_info;
+ constexpr bool is_driving_forward = true;
+ constexpr bool egos_lane_is_shifted = true;
const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo(
linear_shift_path, path_shifter_.getShiftLines().front(), avoid_data_.current_lanelets,
- helper_->getEgoShift(), true);
+ helper_->getEgoShift(), is_driving_forward, egos_lane_is_shifted);
const auto current_seg_idx = planner_data_->findEgoSegmentIndex(spline_shift_path.path.points);
output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal(
diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp
index 34c3b74118d8d..d71b7d9b11397 100644
--- a/planning/behavior_path_lane_change_module/src/scene.cpp
+++ b/planning/behavior_path_lane_change_module/src/scene.cpp
@@ -485,9 +485,12 @@ TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() const
const auto & shift_line = status_.lane_change_path.info.shift_line;
const auto & shift_path = status_.lane_change_path.shifted_path;
const auto current_shift_length = lanelet::utils::getArcCoordinates(current_lanes, pose).distance;
+ constexpr bool is_driving_forward = true;
+ constexpr bool egos_lane_is_shifted = false;
const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo(
- shift_path, shift_line, current_lanes, current_shift_length, true);
+ shift_path, shift_line, current_lanes, current_shift_length, is_driving_forward,
+ egos_lane_is_shifted);
return new_signal;
}
diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp
index 33a662255823d..164f69a0f099d 100644
--- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp
+++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp
@@ -168,11 +168,11 @@ struct PlannerData
std::pair<TurnSignalInfo, bool> getBehaviorTurnSignalInfo(
const ShiftedPath & path, const ShiftLine & shift_line,
const lanelet::ConstLanelets & current_lanelets, const double current_shift_length,
- const bool is_driving_forward) const
+ const bool is_driving_forward, const bool egos_lane_is_shifted) const
{
return turn_signal_decider.getBehaviorTurnSignalInfo(
path, shift_line, current_lanelets, route_handler, parameters, self_odometry,
- current_shift_length, is_driving_forward);
+ current_shift_length, is_driving_forward, egos_lane_is_shifted);
}
TurnIndicatorsCommand getTurnSignal(
diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp
index a3adac557c185..5cf79ec0f593b 100644
--- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp
+++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp
@@ -116,7 +116,8 @@ class TurnSignalDecider
const lanelet::ConstLanelets & current_lanelets,
const std::shared_ptr<RouteHandler> route_handler,
const BehaviorPathPlannerParameters & parameters, const Odometry::ConstSharedPtr self_odometry,
- const double current_shift_length, const bool is_driving_forward) const;
+ const double current_shift_length, const bool is_driving_forward,
+ const bool egos_lane_is_shifted) const;
private:
std::optional<TurnSignalInfo> getIntersectionTurnSignalInfo(
diff --git a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp
index c4a67f91b1a30..eaf2fbd554fa2 100644
--- a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp
+++ b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp
@@ -619,7 +619,8 @@ std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(
const lanelet::ConstLanelets & current_lanelets,
const std::shared_ptr<RouteHandler> route_handler,
const BehaviorPathPlannerParameters & parameters, const Odometry::ConstSharedPtr self_odometry,
- const double current_shift_length, const bool is_driving_forward) const
+ const double current_shift_length, const bool is_driving_forward,
+ const bool egos_lane_is_shifted) const
{
std::cerr << "-------------TurnSignalDecider::getBehaviorTurnSignalInfo-----------\n";
constexpr double THRESHOLD = 0.1;
@@ -663,11 +664,11 @@ std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(
}
const auto [start_shift_length, end_shift_length] =
- std::invoke([&path, &shift_line]() -> std::pair<double, double> {
+ std::invoke([&path, &shift_line, &egos_lane_is_shifted]() -> std::pair<double, double> {
const auto temp_start_shift_length = path.shift_length.at(shift_line.start_idx);
const auto temp_end_shift_length = path.shift_length.at(shift_line.end_idx);
// Shift is done using the target lane and not current lane
- if (std::fabs(temp_start_shift_length) > std::fabs(temp_end_shift_length)) {
+ if (!egos_lane_is_shifted) {
return std::make_pair(temp_end_shift_length, -temp_start_shift_length);
}
return std::make_pair(temp_start_shift_length, temp_end_shift_length);
@@ -727,7 +728,8 @@ std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(
}
lanelet::ConstLanelet lanelet;
- if (!rh->getClosestLaneletWithinRoute(shift_line.end, &lanelet)) {
+ const auto query_pose = (egos_lane_is_shifted) ? shift_line.end : shift_line.start;
+ if (!rh->getClosestLaneletWithinRoute(query_pose, &lanelet)) {
std::cerr << "No signal 4\n";
return std::make_pair(TurnSignalInfo{}, true);
}
From 24fe4aaa2d6d58e9a178e7e97d3cfa102ff94c72 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Thu, 14 Mar 2024 17:34:34 +0900
Subject: [PATCH 06/13] add options for start planner getTurnSignalInfo
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../data_manager.hpp | 51 ++++-
.../turn_signal_decider.hpp | 8 +-
.../src/turn_signal_decider.cpp | 18 +-
.../src/start_planner_module.cpp | 211 ++++++++++--------
4 files changed, 188 insertions(+), 100 deletions(-)
diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp
index 164f69a0f099d..d196f2bc551bb 100644
--- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp
+++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp
@@ -21,6 +21,7 @@
#include "motion_utils/trajectory/trajectory.hpp"
#include <lanelet2_extension/regulatory_elements/Forward.hpp>
+#include <lanelet2_extension/utility/utilities.hpp>
#include <rclcpp/rclcpp/clock.hpp>
#include <rclcpp/rclcpp/time.hpp>
#include <route_handler/route_handler.hpp>
@@ -165,14 +166,60 @@ struct PlannerData
mutable std::vector<double> drivable_area_expansion_prev_curvatures{};
mutable TurnSignalDecider turn_signal_decider;
+ std::pair<TurnSignalInfo, bool> getBehaviorTurnSignalInfo(
+ const PathWithLaneId & path, const size_t shift_start_idx, const size_t shift_end_idx,
+ const lanelet::ConstLanelets & current_lanelets, const double current_shift_length,
+ const bool is_driving_forward, const bool egos_lane_is_shifted,
+ const bool override_ego_stopped_check = false, const bool is_pull_out = false) const
+ {
+ if (shift_start_idx + 1 > path.points.size()) {
+ RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency.");
+ return std::make_pair(TurnSignalInfo{}, true);
+ }
+
+ if (shift_end_idx + 1 > path.points.size()) {
+ RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency.");
+ return std::make_pair(TurnSignalInfo{}, true);
+ }
+
+ std::vector<double> lengths(path.points.size(), 0.0);
+ ShiftedPath shifted_path{path, lengths};
+ ShiftLine shift_line;
+
+ {
+ const auto start_pose = path.points.at(shift_start_idx).point.pose;
+ const auto start_shift_length =
+ lanelet::utils::getArcCoordinates(current_lanelets, start_pose).distance;
+ const auto end_pose = path.points.at(shift_end_idx).point.pose;
+ const auto end_shift_length =
+ lanelet::utils::getArcCoordinates(current_lanelets, end_pose).distance;
+ shifted_path.shift_length.at(shift_start_idx) = start_shift_length;
+ shifted_path.shift_length.at(shift_end_idx) = end_shift_length;
+
+ shift_line.start = start_pose;
+ shift_line.end = end_pose;
+ shift_line.start_shift_length = start_shift_length;
+ shift_line.end_shift_length = end_shift_length;
+ shift_line.start_idx = shift_start_idx;
+ shift_line.end_idx = shift_end_idx;
+ }
+
+ return turn_signal_decider.getBehaviorTurnSignalInfo(
+ shifted_path, shift_line, current_lanelets, route_handler, parameters, self_odometry,
+ current_shift_length, is_driving_forward, egos_lane_is_shifted, override_ego_stopped_check,
+ is_pull_out);
+ }
+
std::pair<TurnSignalInfo, bool> getBehaviorTurnSignalInfo(
const ShiftedPath & path, const ShiftLine & shift_line,
const lanelet::ConstLanelets & current_lanelets, const double current_shift_length,
- const bool is_driving_forward, const bool egos_lane_is_shifted) const
+ const bool is_driving_forward, const bool egos_lane_is_shifted,
+ const bool override_ego_stopped_check = false, const bool is_pull_out = false) const
{
return turn_signal_decider.getBehaviorTurnSignalInfo(
path, shift_line, current_lanelets, route_handler, parameters, self_odometry,
- current_shift_length, is_driving_forward, egos_lane_is_shifted);
+ current_shift_length, is_driving_forward, egos_lane_is_shifted, override_ego_stopped_check,
+ is_pull_out);
}
TurnIndicatorsCommand getTurnSignal(
diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp
index 5cf79ec0f593b..30ef0d60b2ddc 100644
--- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp
+++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp
@@ -117,7 +117,8 @@ class TurnSignalDecider
const std::shared_ptr<RouteHandler> route_handler,
const BehaviorPathPlannerParameters & parameters, const Odometry::ConstSharedPtr self_odometry,
const double current_shift_length, const bool is_driving_forward,
- const bool egos_lane_is_shifted) const;
+ const bool egos_lane_is_shifted, const bool override_ego_stopped_check = false,
+ const bool is_pull_out = false) const;
private:
std::optional<TurnSignalInfo> getIntersectionTurnSignalInfo(
@@ -169,6 +170,9 @@ class TurnSignalDecider
std::cout << "relative_shift_length " << relative_shift_length << "\n";
if (isAvoidShift(start_shift_length, end_shift_length, threshold)) {
std::cerr << "Enters here 0 ? \n";
+ std::cerr << "no_left_lanes " << no_left_lanes << "\n";
+ std::cerr << "no_right_lanes " << no_right_lanes << "\n";
+
// Left avoid. But there is no adjacent lane. No need blinker.
if (relative_shift_length > 0.0 && no_left_lanes) {
return false;
@@ -220,7 +224,7 @@ class TurnSignalDecider
return false;
}
}
-
+ std::cerr << "Returns True\n";
return true;
};
diff --git a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp
index eaf2fbd554fa2..fd4c31095c44a 100644
--- a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp
+++ b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp
@@ -619,8 +619,8 @@ std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(
const lanelet::ConstLanelets & current_lanelets,
const std::shared_ptr<RouteHandler> route_handler,
const BehaviorPathPlannerParameters & parameters, const Odometry::ConstSharedPtr self_odometry,
- const double current_shift_length, const bool is_driving_forward,
- const bool egos_lane_is_shifted) const
+ const double current_shift_length, const bool is_driving_forward, const bool egos_lane_is_shifted,
+ const bool override_ego_stopped_check, const bool is_pull_out) const
{
std::cerr << "-------------TurnSignalDecider::getBehaviorTurnSignalInfo-----------\n";
constexpr double THRESHOLD = 0.1;
@@ -667,7 +667,7 @@ std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(
std::invoke([&path, &shift_line, &egos_lane_is_shifted]() -> std::pair<double, double> {
const auto temp_start_shift_length = path.shift_length.at(shift_line.start_idx);
const auto temp_end_shift_length = path.shift_length.at(shift_line.end_idx);
- // Shift is done using the target lane and not current lane
+ // Shift is done using the target lane and not current ego's lane
if (!egos_lane_is_shifted) {
return std::make_pair(temp_end_shift_length, -temp_start_shift_length);
}
@@ -742,9 +742,12 @@ std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(
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,
- p.turn_signal_shift_length_threshold)) {
+ std::cerr << "has_right_lane " << has_right_lane << "\n";
+ std::cerr << "has_left_lane " << has_left_lane << "\n";
+ if (
+ !is_pull_out && !existShiftSideLane(
+ start_shift_length, end_shift_length, !has_left_lane, !has_right_lane,
+ p.turn_signal_shift_length_threshold)) {
std::cerr << "No signal 5\n";
return std::make_pair(TurnSignalInfo{}, true);
@@ -757,12 +760,11 @@ std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(
}
constexpr double STOPPED_THRESHOLD = 0.1; // [m/s]
- if (ego_speed < STOPPED_THRESHOLD) {
+ if (ego_speed < STOPPED_THRESHOLD && !override_ego_stopped_check) {
if (isNearEndOfShift(
start_shift_length, end_shift_length, ego_pose.position, current_lanelets,
p.turn_signal_shift_length_threshold)) {
std::cerr << "No signal 7\n";
-
return std::make_pair(TurnSignalInfo{}, true);
}
}
diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp
index bb47182d953cc..ad41fe28697c4 100644
--- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp
+++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp
@@ -1217,101 +1217,136 @@ bool StartPlannerModule::hasFinishedCurrentPath()
TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const
{
- TurnSignalInfo turn_signal{}; // output
-
- const Pose & current_pose = planner_data_->self_odometry->pose.pose;
- const Pose & start_pose = status_.pull_out_path.start_pose;
- const Pose & end_pose = status_.pull_out_path.end_pose;
-
- // turn on hazard light when backward driving
- if (!status_.driving_forward) {
- turn_signal.hazard_signal.command = HazardLightsCommand::ENABLE;
- const auto back_start_pose = planner_data_->route_handler->getOriginalStartPose();
- turn_signal.desired_start_point = back_start_pose;
- turn_signal.required_start_point = back_start_pose;
- // pull_out start_pose is same to backward driving end_pose
- turn_signal.required_end_point = start_pose;
- turn_signal.desired_end_point = start_pose;
- return turn_signal;
- }
-
- // turn on right signal until passing pull_out end point
const auto path = getFullPath();
- // pull out path does not overlap
- const double distance_from_end =
- motion_utils::calcSignedArcLength(path.points, end_pose.position, current_pose.position);
- if (path.points.empty()) {
- return {};
- }
-
- // calculate lateral offset from pull out target lane center line
- lanelet::ConstLanelet closest_road_lane;
- const double backward_path_length =
- planner_data_->parameters.backward_path_length + parameters_->max_back_distance;
- const auto road_lanes = utils::getExtendedCurrentLanes(
- planner_data_, backward_path_length, std::numeric_limits<double>::max(),
- /*forward_only_in_route*/ true);
- lanelet::utils::query::getClosestLanelet(road_lanes, start_pose, &closest_road_lane);
- const double lateral_offset =
- lanelet::utils::getLateralDistanceToCenterline(closest_road_lane, start_pose);
-
- turn_signal.turn_signal.command = std::invoke([&]() {
- if (distance_from_end >= 0.0) return TurnIndicatorsCommand::DISABLE;
- if (lateral_offset > parameters_->th_turn_signal_on_lateral_offset)
- return TurnIndicatorsCommand::ENABLE_RIGHT;
- if (lateral_offset < -parameters_->th_turn_signal_on_lateral_offset)
- return TurnIndicatorsCommand::ENABLE_LEFT;
- return TurnIndicatorsCommand::DISABLE;
- });
+ const Pose & current_pose = planner_data_->self_odometry->pose.pose;
+ const auto shift_start_idx =
+ motion_utils::findNearestIndex(path.points, status_.pull_out_path.start_pose.position);
+ const auto shift_end_idx =
+ motion_utils::findNearestIndex(path.points, status_.pull_out_path.end_pose.position);
- turn_signal.desired_start_point = start_pose;
- turn_signal.required_start_point = start_pose;
- turn_signal.desired_end_point = end_pose;
-
- // check if intersection exists within search length
- const bool is_near_intersection = std::invoke([&]() {
- const double check_length = parameters_->intersection_search_length;
- double accumulated_length = 0.0;
- const size_t current_idx = motion_utils::findNearestIndex(path.points, current_pose.position);
- for (size_t i = current_idx; i < path.points.size() - 1; ++i) {
- const auto & p = path.points.at(i);
- for (const auto & lane : planner_data_->route_handler->getLaneletsFromIds(p.lane_ids)) {
- const std::string turn_direction = lane.attributeOr("turn_direction", "else");
- if (turn_direction == "right" || turn_direction == "left" || turn_direction == "straight") {
- return true;
- }
- }
- accumulated_length += tier4_autoware_utils::calcDistance2d(p, path.points.at(i + 1));
- if (accumulated_length > check_length) {
- return false;
- }
- }
- return false;
- });
+ const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_);
+ const double current_shift_length =
+ lanelet::utils::getArcCoordinates(current_lanes, current_pose).distance;
- turn_signal.required_end_point = std::invoke([&]() {
- if (is_near_intersection) return end_pose;
- const double length_start_to_end =
- motion_utils::calcSignedArcLength(path.points, start_pose.position, end_pose.position);
- const auto ratio = std::clamp(
- parameters_->length_ratio_for_turn_signal_deactivation_near_intersection, 0.0, 1.0);
-
- const double required_end_length = length_start_to_end * ratio;
- double accumulated_length = 0.0;
- const size_t start_idx = motion_utils::findNearestIndex(path.points, start_pose.position);
- for (size_t i = start_idx; i < path.points.size() - 1; ++i) {
- accumulated_length +=
- tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1));
- if (accumulated_length > required_end_length) {
- return path.points.at(i).point.pose;
- }
+ constexpr bool egos_lane_is_shifted = true;
+ constexpr bool is_pull_out = true;
+ const bool override_ego_stopped_check = std::invoke([&]() {
+ if (status_.planner_type != PlannerType::GEOMETRIC) {
+ return false;
}
- // not found required end point
- return end_pose;
+ constexpr double distance_threshold = 1.0;
+ const auto stop_point = status_.pull_out_path.partial_paths.front().points.back();
+ const double distance_from_ego_to_stop_point = std::abs(motion_utils::calcSignedArcLength(
+ path.points, stop_point.point.pose.position, current_pose.position));
+ return distance_from_ego_to_stop_point < distance_threshold;
});
- return turn_signal;
+ // std::map<PlannerType, double> collision_check_distances = {
+ // {PlannerType::SHIFT, parameters_->shift_collision_check_distance_from_end},
+ // {PlannerType::GEOMETRIC, parameters_->geometric_collision_check_distance_from_end}};
+
+ // double collision_check_distance_from_end = collision_check_distances[status_.planner_type];
+ // const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose(
+ // getFullPath().points, status_.pull_out_path.end_pose.position,
+ // collision_check_distance_from_end);
+
+ const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo(
+ path, shift_start_idx, shift_end_idx, current_lanes, current_shift_length,
+ status_.driving_forward, egos_lane_is_shifted, override_ego_stopped_check, is_pull_out);
+
+ // const Pose & start_pose = status_.pull_out_path.start_pose;
+ // const Pose & end_pose = status_.pull_out_path.end_pose;
+
+ // // turn on hazard light when backward driving
+ // if (!status_.driving_forward) {
+ // turn_signal.hazard_signal.command = HazardLightsCommand::ENABLE;
+ // const auto back_start_pose = planner_data_->route_handler->getOriginalStartPose();
+ // turn_signal.desired_start_point = back_start_pose;
+ // turn_signal.required_start_point = back_start_pose;
+ // // pull_out start_pose is same to backward driving end_pose
+ // turn_signal.required_end_point = start_pose;
+ // turn_signal.desired_end_point = start_pose;
+ // return turn_signal;
+ // }
+
+ // // turn on right signal until passing pull_out end point
+ // // pull out path does not overlap
+ // const double distance_from_end =
+ // motion_utils::calcSignedArcLength(path.points, end_pose.position, current_pose.position);
+
+ // if (path.points.empty()) {
+ // return {};
+ // }
+
+ // // calculate lateral offset from pull out target lane center line
+ // lanelet::ConstLanelet closest_road_lane;
+ // const double backward_path_length =
+ // planner_data_->parameters.backward_path_length + parameters_->max_back_distance;
+ // const auto road_lanes = utils::getExtendedCurrentLanes(
+ // planner_data_, backward_path_length, std::numeric_limits<double>::max(),
+ // /*forward_only_in_route*/ true);
+ // lanelet::utils::query::getClosestLanelet(road_lanes, start_pose, &closest_road_lane);
+ // const double lateral_offset =
+ // lanelet::utils::getLateralDistanceToCenterline(closest_road_lane, start_pose);
+
+ // turn_signal.turn_signal.command = std::invoke([&]() {
+ // if (distance_from_end >= 0.0) return TurnIndicatorsCommand::DISABLE;
+ // if (lateral_offset > parameters_->th_turn_signal_on_lateral_offset)
+ // return TurnIndicatorsCommand::ENABLE_RIGHT;
+ // if (lateral_offset < -parameters_->th_turn_signal_on_lateral_offset)
+ // return TurnIndicatorsCommand::ENABLE_LEFT;
+ // return TurnIndicatorsCommand::DISABLE;
+ // });
+
+ // turn_signal.desired_start_point = start_pose;
+ // turn_signal.required_start_point = start_pose;
+ // turn_signal.desired_end_point = end_pose;
+
+ // // check if intersection exists within search length
+ // const bool is_near_intersection = std::invoke([&]() {
+ // const double check_length = parameters_->intersection_search_length;
+ // double accumulated_length = 0.0;
+ // const size_t current_idx = motion_utils::findNearestIndex(path.points,
+ // current_pose.position); for (size_t i = current_idx; i < path.points.size() - 1; ++i) {
+ // const auto & p = path.points.at(i);
+ // for (const auto & lane : planner_data_->route_handler->getLaneletsFromIds(p.lane_ids)) {
+ // const std::string turn_direction = lane.attributeOr("turn_direction", "else");
+ // if (turn_direction == "right" || turn_direction == "left" || turn_direction ==
+ // "straight") {
+ // return true;
+ // }
+ // }
+ // accumulated_length += tier4_autoware_utils::calcDistance2d(p, path.points.at(i + 1));
+ // if (accumulated_length > check_length) {
+ // return false;
+ // }
+ // }
+ // return false;
+ // });
+
+ // turn_signal.required_end_point = std::invoke([&]() {
+ // if (is_near_intersection) return end_pose;
+ // const double length_start_to_end =
+ // motion_utils::calcSignedArcLength(path.points, start_pose.position, end_pose.position);
+ // const auto ratio = std::clamp(
+ // parameters_->length_ratio_for_turn_signal_deactivation_near_intersection, 0.0, 1.0);
+
+ // const double required_end_length = length_start_to_end * ratio;
+ // double accumulated_length = 0.0;
+ // const size_t start_idx = motion_utils::findNearestIndex(path.points, start_pose.position);
+ // for (size_t i = start_idx; i < path.points.size() - 1; ++i) {
+ // accumulated_length +=
+ // tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1));
+ // if (accumulated_length > required_end_length) {
+ // return path.points.at(i).point.pose;
+ // }
+ // }
+ // // not found required end point
+ // return end_pose;
+ // });
+
+ return new_signal;
}
bool StartPlannerModule::isSafePath() const
From 7502947a111717fd7d34e01c24df980560f66724 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Thu, 14 Mar 2024 18:32:42 +0900
Subject: [PATCH 07/13] add ignore id func to ignore prev explored lanes
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../start_planner_module.hpp | 4 +-
.../src/start_planner_module.cpp | 122 +++---------------
2 files changed, 24 insertions(+), 102 deletions(-)
diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp
index 14846a05a0929..89faafe95727c 100644
--- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp
+++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp
@@ -33,6 +33,7 @@
#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
+#include <lanelet2_core/Forward.h>
#include <tf2/utils.h>
#include <atomic>
@@ -239,6 +240,7 @@ class StartPlannerModule : public SceneModuleInterface
std::vector<std::shared_ptr<PullOutPlannerBase>> start_planners_;
PullOutStatus status_;
mutable StartPlannerDebugData debug_data_;
+ std::optional<lanelet::Id> ignore_signal_{std::nullopt};
std::deque<nav_msgs::msg::Odometry::ConstSharedPtr> odometry_buffer_;
@@ -264,7 +266,7 @@ class StartPlannerModule : public SceneModuleInterface
std::shared_ptr<LaneDepartureChecker> lane_departure_checker_;
// turn signal
- TurnSignalInfo calcTurnSignalInfo() const;
+ TurnSignalInfo calcTurnSignalInfo();
void incrementPathIndex();
PathWithLaneId getCurrentPath() const;
diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp
index ad41fe28697c4..c8a4d9348241a 100644
--- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp
+++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp
@@ -1215,7 +1215,7 @@ bool StartPlannerModule::hasFinishedCurrentPath()
return is_near_target && isStopped();
}
-TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const
+TurnSignalInfo StartPlannerModule::calcTurnSignalInfo()
{
const auto path = getFullPath();
@@ -1226,6 +1226,25 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const
motion_utils::findNearestIndex(path.points, status_.pull_out_path.end_pose.position);
const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_);
+
+ const auto is_ignore_signal = [this](const lanelet::Id & id) {
+ if (!ignore_signal_.has_value()) {
+ return false;
+ }
+ return ignore_signal_.value() == id;
+ };
+
+ const auto update_ignore_signal = [this](const lanelet::Id & id, const bool is_ignore) {
+ return is_ignore ? std::make_optional(id) : std::nullopt;
+ };
+
+ lanelet::Lanelet closest_lanelet;
+ lanelet::utils::query::getClosestLanelet(current_lanes, current_pose, &closest_lanelet);
+
+ if (is_ignore_signal(closest_lanelet.id())) {
+ return getPreviousModuleOutput().turn_signal_info;
+ }
+
const double current_shift_length =
lanelet::utils::getArcCoordinates(current_lanes, current_pose).distance;
@@ -1242,110 +1261,11 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const
return distance_from_ego_to_stop_point < distance_threshold;
});
- // std::map<PlannerType, double> collision_check_distances = {
- // {PlannerType::SHIFT, parameters_->shift_collision_check_distance_from_end},
- // {PlannerType::GEOMETRIC, parameters_->geometric_collision_check_distance_from_end}};
-
- // double collision_check_distance_from_end = collision_check_distances[status_.planner_type];
- // const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose(
- // getFullPath().points, status_.pull_out_path.end_pose.position,
- // collision_check_distance_from_end);
-
const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo(
path, shift_start_idx, shift_end_idx, current_lanes, current_shift_length,
status_.driving_forward, egos_lane_is_shifted, override_ego_stopped_check, is_pull_out);
- // const Pose & start_pose = status_.pull_out_path.start_pose;
- // const Pose & end_pose = status_.pull_out_path.end_pose;
-
- // // turn on hazard light when backward driving
- // if (!status_.driving_forward) {
- // turn_signal.hazard_signal.command = HazardLightsCommand::ENABLE;
- // const auto back_start_pose = planner_data_->route_handler->getOriginalStartPose();
- // turn_signal.desired_start_point = back_start_pose;
- // turn_signal.required_start_point = back_start_pose;
- // // pull_out start_pose is same to backward driving end_pose
- // turn_signal.required_end_point = start_pose;
- // turn_signal.desired_end_point = start_pose;
- // return turn_signal;
- // }
-
- // // turn on right signal until passing pull_out end point
- // // pull out path does not overlap
- // const double distance_from_end =
- // motion_utils::calcSignedArcLength(path.points, end_pose.position, current_pose.position);
-
- // if (path.points.empty()) {
- // return {};
- // }
-
- // // calculate lateral offset from pull out target lane center line
- // lanelet::ConstLanelet closest_road_lane;
- // const double backward_path_length =
- // planner_data_->parameters.backward_path_length + parameters_->max_back_distance;
- // const auto road_lanes = utils::getExtendedCurrentLanes(
- // planner_data_, backward_path_length, std::numeric_limits<double>::max(),
- // /*forward_only_in_route*/ true);
- // lanelet::utils::query::getClosestLanelet(road_lanes, start_pose, &closest_road_lane);
- // const double lateral_offset =
- // lanelet::utils::getLateralDistanceToCenterline(closest_road_lane, start_pose);
-
- // turn_signal.turn_signal.command = std::invoke([&]() {
- // if (distance_from_end >= 0.0) return TurnIndicatorsCommand::DISABLE;
- // if (lateral_offset > parameters_->th_turn_signal_on_lateral_offset)
- // return TurnIndicatorsCommand::ENABLE_RIGHT;
- // if (lateral_offset < -parameters_->th_turn_signal_on_lateral_offset)
- // return TurnIndicatorsCommand::ENABLE_LEFT;
- // return TurnIndicatorsCommand::DISABLE;
- // });
-
- // turn_signal.desired_start_point = start_pose;
- // turn_signal.required_start_point = start_pose;
- // turn_signal.desired_end_point = end_pose;
-
- // // check if intersection exists within search length
- // const bool is_near_intersection = std::invoke([&]() {
- // const double check_length = parameters_->intersection_search_length;
- // double accumulated_length = 0.0;
- // const size_t current_idx = motion_utils::findNearestIndex(path.points,
- // current_pose.position); for (size_t i = current_idx; i < path.points.size() - 1; ++i) {
- // const auto & p = path.points.at(i);
- // for (const auto & lane : planner_data_->route_handler->getLaneletsFromIds(p.lane_ids)) {
- // const std::string turn_direction = lane.attributeOr("turn_direction", "else");
- // if (turn_direction == "right" || turn_direction == "left" || turn_direction ==
- // "straight") {
- // return true;
- // }
- // }
- // accumulated_length += tier4_autoware_utils::calcDistance2d(p, path.points.at(i + 1));
- // if (accumulated_length > check_length) {
- // return false;
- // }
- // }
- // return false;
- // });
-
- // turn_signal.required_end_point = std::invoke([&]() {
- // if (is_near_intersection) return end_pose;
- // const double length_start_to_end =
- // motion_utils::calcSignedArcLength(path.points, start_pose.position, end_pose.position);
- // const auto ratio = std::clamp(
- // parameters_->length_ratio_for_turn_signal_deactivation_near_intersection, 0.0, 1.0);
-
- // const double required_end_length = length_start_to_end * ratio;
- // double accumulated_length = 0.0;
- // const size_t start_idx = motion_utils::findNearestIndex(path.points, start_pose.position);
- // for (size_t i = start_idx; i < path.points.size() - 1; ++i) {
- // accumulated_length +=
- // tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1));
- // if (accumulated_length > required_end_length) {
- // return path.points.at(i).point.pose;
- // }
- // }
- // // not found required end point
- // return end_pose;
- // });
-
+ ignore_signal_ = update_ignore_signal(closest_lanelet.id(), is_ignore);
return new_signal;
}
From 8378472211bb795dc8a38080e70408d869a383f1 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Fri, 15 Mar 2024 09:05:52 +0900
Subject: [PATCH 08/13] add use prior turn signal
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../src/start_planner_module.cpp | 12 ++++++++++--
1 file changed, 10 insertions(+), 2 deletions(-)
diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp
index c8a4d9348241a..30203d721967c 100644
--- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp
+++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp
@@ -1218,6 +1218,7 @@ bool StartPlannerModule::hasFinishedCurrentPath()
TurnSignalInfo StartPlannerModule::calcTurnSignalInfo()
{
const auto path = getFullPath();
+ if (path.points.empty()) return getPreviousModuleOutput().turn_signal_info;
const Pose & current_pose = planner_data_->self_odometry->pose.pose;
const auto shift_start_idx =
@@ -1264,9 +1265,16 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo()
const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo(
path, shift_start_idx, shift_end_idx, current_lanes, current_shift_length,
status_.driving_forward, egos_lane_is_shifted, override_ego_stopped_check, is_pull_out);
-
ignore_signal_ = update_ignore_signal(closest_lanelet.id(), is_ignore);
- return new_signal;
+
+ const auto original_signal = getPreviousModuleOutput().turn_signal_info;
+ const auto current_seg_idx = planner_data_->findEgoSegmentIndex(path.points);
+ const auto output_turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal(
+ path, current_pose, current_seg_idx, original_signal, new_signal,
+ planner_data_->parameters.ego_nearest_dist_threshold,
+ planner_data_->parameters.ego_nearest_yaw_threshold);
+
+ return output_turn_signal_info;
}
bool StartPlannerModule::isSafePath() const
From 9b119ffb742529a8ba3df7c6f74da69453af9c7b Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Fri, 15 Mar 2024 11:46:01 +0900
Subject: [PATCH 09/13] implement general behaviorturnsignal on goal planner
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../goal_planner_module.hpp | 9 +-
.../src/goal_planner_module.cpp | 111 +++++++++++++-----
.../src/start_planner_module.cpp | 1 -
3 files changed, 89 insertions(+), 32 deletions(-)
diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp
index 1cb0a3b98c8f2..21ea06531c2f4 100644
--- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp
+++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp
@@ -37,6 +37,8 @@
#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
#include <autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp>
+#include <lanelet2_core/Forward.h>
+
#include <atomic>
#include <deque>
#include <limits>
@@ -525,14 +527,15 @@ class GoalPlannerModule : public SceneModuleInterface
void setDrivableAreaInfo(BehaviorModuleOutput & output) const;
// output setter
- void setOutput(BehaviorModuleOutput & output) const;
+ void setOutput(BehaviorModuleOutput & output);
void updatePreviousData();
void setModifiedGoal(BehaviorModuleOutput & output) const;
- void setTurnSignalInfo(BehaviorModuleOutput & output) const;
+ void setTurnSignalInfo(BehaviorModuleOutput & output);
// new turn signal
- TurnSignalInfo calcTurnSignalInfo() const;
+ TurnSignalInfo calcTurnSignalInfo();
+ std::optional<lanelet::Id> ignore_signal_{std::nullopt};
std::optional<BehaviorModuleOutput> last_previous_module_output_{};
bool hasPreviousModulePathShapeChanged() const;
diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp
index ce0fc8cfbece1..93686f2911193 100644
--- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp
+++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp
@@ -893,7 +893,7 @@ std::vector<DrivableLanes> GoalPlannerModule::generateDrivableLanes() const
return utils::generateDrivableLanesWithShoulderLanes(current_lanes, pull_over_lanes);
}
-void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) const
+void GoalPlannerModule::setOutput(BehaviorModuleOutput & output)
{
output.reference_path = getPreviousModuleOutput().reference_path;
@@ -966,7 +966,7 @@ void GoalPlannerModule::setModifiedGoal(BehaviorModuleOutput & output) const
}
}
-void GoalPlannerModule::setTurnSignalInfo(BehaviorModuleOutput & output) const
+void GoalPlannerModule::setTurnSignalInfo(BehaviorModuleOutput & output)
{
const auto original_signal = getPreviousModuleOutput().turn_signal_info;
const auto new_signal = calcTurnSignalInfo();
@@ -1573,43 +1573,98 @@ bool GoalPlannerModule::isOnModifiedGoal() const
parameters_->th_arrived_distance;
}
-TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() const
+TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo()
{
- TurnSignalInfo turn_signal{}; // output
+ const auto path = thread_safe_data_.get_pull_over_path()->getFullPath();
+ if (path.points.empty()) return getPreviousModuleOutput().turn_signal_info;
const auto & current_pose = planner_data_->self_odometry->pose.pose;
const auto & start_pose = thread_safe_data_.get_pull_over_path()->start_pose;
const auto & end_pose = thread_safe_data_.get_pull_over_path()->end_pose;
- const auto full_path = thread_safe_data_.get_pull_over_path()->getFullPath();
- // calc TurnIndicatorsCommand
- {
- const double distance_to_end =
- calcSignedArcLength(full_path.points, current_pose.position, end_pose.position);
- const bool is_before_end_pose = distance_to_end >= 0.0;
- if (is_before_end_pose) {
- if (left_side_parking_) {
- turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT;
- } else {
- turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT;
- }
- } else {
- turn_signal.turn_signal.command = TurnIndicatorsCommand::NO_COMMAND;
+ const auto shift_start_idx = motion_utils::findNearestIndex(path.points, start_pose.position);
+ const auto shift_end_idx = motion_utils::findNearestIndex(path.points, end_pose.position);
+
+ const auto is_ignore_signal = [this](const lanelet::Id & id) {
+ if (!ignore_signal_.has_value()) {
+ return false;
}
+ return ignore_signal_.value() == id;
+ };
+
+ const auto update_ignore_signal = [this](const lanelet::Id & id, const bool is_ignore) {
+ return is_ignore ? std::make_optional(id) : std::nullopt;
+ };
+
+ const lanelet::ConstLanelets current_lanes = utils::getExtendedCurrentLanes(
+ planner_data_, parameters_->backward_goal_search_length,
+ parameters_->forward_goal_search_length,
+ /*forward_only_in_route*/ false);
+
+ if (current_lanes.empty()) {
+ return {};
}
- // calc desired/required start/end point
- {
- // ego decelerates so that current pose is the point `turn_light_on_threshold_time` seconds
- // before starting pull_over
- turn_signal.desired_start_point =
- last_approval_data_ && hasDecidedPath() ? last_approval_data_->pose : current_pose;
- turn_signal.desired_end_point = end_pose;
- turn_signal.required_start_point = start_pose;
- turn_signal.required_end_point = end_pose;
+ lanelet::Lanelet closest_lanelet;
+ lanelet::utils::query::getClosestLanelet(current_lanes, current_pose, &closest_lanelet);
+
+ if (is_ignore_signal(closest_lanelet.id())) {
+ return getPreviousModuleOutput().turn_signal_info;
}
- return turn_signal;
+ const double current_shift_length =
+ lanelet::utils::getArcCoordinates(current_lanes, current_pose).distance;
+
+ constexpr bool egos_lane_is_shifted = true;
+ constexpr bool is_driving_forward = true;
+
+ constexpr bool is_pull_out = false;
+ const bool override_ego_stopped_check = std::invoke([&]() {
+ if (thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::SHIFT) {
+ return false;
+ }
+ constexpr double distance_threshold = 1.0;
+ const auto stop_point =
+ thread_safe_data_.get_pull_over_path()->partial_paths.front().points.back();
+ const double distance_from_ego_to_stop_point = std::abs(motion_utils::calcSignedArcLength(
+ path.points, stop_point.point.pose.position, current_pose.position));
+ return distance_from_ego_to_stop_point < distance_threshold;
+ });
+
+ const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo(
+ path, shift_start_idx, shift_end_idx, current_lanes, current_shift_length, is_driving_forward,
+ egos_lane_is_shifted, override_ego_stopped_check, is_pull_out);
+ ignore_signal_ = update_ignore_signal(closest_lanelet.id(), is_ignore);
+
+ return new_signal;
+ // // calc TurnIndicatorsCommand
+ // {
+ // const double distance_to_end =
+ // calcSignedArcLength(full_path.points, current_pose.position, end_pose.position);
+ // const bool is_before_end_pose = distance_to_end >= 0.0;
+ // if (is_before_end_pose) {
+ // if (left_side_parking_) {
+ // turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT;
+ // } else {
+ // turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT;
+ // }
+ // } else {
+ // turn_signal.turn_signal.command = TurnIndicatorsCommand::NO_COMMAND;
+ // }
+ // }
+
+ // // calc desired/required start/end point
+ // {
+ // // ego decelerates so that current pose is the point `turn_light_on_threshold_time` seconds
+ // // before starting pull_over
+ // turn_signal.desired_start_point =
+ // last_approval_data_ && hasDecidedPath() ? last_approval_data_->pose : current_pose;
+ // turn_signal.desired_end_point = end_pose;
+ // turn_signal.required_start_point = start_pose;
+ // turn_signal.required_end_point = end_pose;
+ // }
+
+ // return turn_signal;
}
bool GoalPlannerModule::checkOccupancyGridCollision(const PathWithLaneId & path) const
diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp
index 30203d721967c..c2450daea3728 100644
--- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp
+++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp
@@ -1225,7 +1225,6 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo()
motion_utils::findNearestIndex(path.points, status_.pull_out_path.start_pose.position);
const auto shift_end_idx =
motion_utils::findNearestIndex(path.points, status_.pull_out_path.end_pose.position);
-
const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_);
const auto is_ignore_signal = [this](const lanelet::Id & id) {
From 2fcaaccfe392a7edbbe8d13e7aa440d2daaf00ad Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Fri, 15 Mar 2024 14:01:42 +0900
Subject: [PATCH 10/13] delete comments
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../src/turn_signal_decider.cpp | 19 -------------------
1 file changed, 19 deletions(-)
diff --git a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp
index fd4c31095c44a..7bb2bab20bdcd 100644
--- a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp
+++ b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp
@@ -622,7 +622,6 @@ std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(
const double current_shift_length, const bool is_driving_forward, const bool egos_lane_is_shifted,
const bool override_ego_stopped_check, const bool is_pull_out) const
{
- std::cerr << "-------------TurnSignalDecider::getBehaviorTurnSignalInfo-----------\n";
constexpr double THRESHOLD = 0.1;
const auto & p = parameters;
const auto & rh = route_handler;
@@ -678,14 +677,11 @@ std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(
// 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) {
- std::cerr << "No signal 0\n";
return std::make_pair(TurnSignalInfo{}, true);
}
// If the vehicle does not shift anymore, we turn off the blinker
if (std::fabs(end_shift_length - current_shift_length) < THRESHOLD) {
- std::cerr << "No signal 1\n";
-
return std::make_pair(TurnSignalInfo{}, true);
}
@@ -700,12 +696,7 @@ std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(
calcSignedArcLength(path.path.points, ego_pose.position, shift_line.start_idx) -
p.vehicle_info.max_longitudinal_offset_m;
- std::cerr << "ego_front_to_shift_start " << ego_front_to_shift_start << "\n";
- std::cerr << "signal_prepare_distance " << signal_prepare_distance << "\n";
-
if (signal_prepare_distance < ego_front_to_shift_start) {
- std::cerr << "No signal 2\n";
-
return std::make_pair(TurnSignalInfo{}, false);
}
@@ -723,14 +714,12 @@ std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(
turn_signal_info.turn_signal.command = get_command(relative_shift_length);
if (!p.turn_signal_on_swerving) {
- std::cerr << "No signal 3\n";
return std::make_pair(turn_signal_info, false);
}
lanelet::ConstLanelet lanelet;
const auto query_pose = (egos_lane_is_shifted) ? shift_line.end : shift_line.start;
if (!rh->getClosestLaneletWithinRoute(query_pose, &lanelet)) {
- std::cerr << "No signal 4\n";
return std::make_pair(TurnSignalInfo{}, true);
}
@@ -742,20 +731,14 @@ std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(
const auto has_right_lane =
right_same_direction_lane.has_value() || !right_opposite_lanes.empty();
- std::cerr << "has_right_lane " << has_right_lane << "\n";
- std::cerr << "has_left_lane " << has_left_lane << "\n";
if (
!is_pull_out && !existShiftSideLane(
start_shift_length, end_shift_length, !has_left_lane, !has_right_lane,
p.turn_signal_shift_length_threshold)) {
- std::cerr << "No signal 5\n";
-
return std::make_pair(TurnSignalInfo{}, true);
}
if (!straddleRoadBound(path, shift_line, current_lanelets, p.vehicle_info)) {
- std::cerr << "No signal 6\n";
-
return std::make_pair(TurnSignalInfo{}, true);
}
@@ -764,11 +747,9 @@ std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(
if (isNearEndOfShift(
start_shift_length, end_shift_length, ego_pose.position, current_lanelets,
p.turn_signal_shift_length_threshold)) {
- std::cerr << "No signal 7\n";
return std::make_pair(TurnSignalInfo{}, true);
}
}
- std::cerr << "No signal 8\n";
return std::make_pair(turn_signal_info, false);
}
From e2e8e18a7c982954b29152f89e87378e81390775 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Fri, 15 Mar 2024 14:11:38 +0900
Subject: [PATCH 11/13] delete prints
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../behavior_path_planner_common/turn_signal_decider.hpp | 9 ---------
1 file changed, 9 deletions(-)
diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp
index 30ef0d60b2ddc..fe187567cd893 100644
--- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp
+++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp
@@ -167,12 +167,7 @@ class TurnSignalDecider
const bool no_right_lanes, const double threshold) const
{
const auto relative_shift_length = end_shift_length - start_shift_length;
- std::cout << "relative_shift_length " << relative_shift_length << "\n";
if (isAvoidShift(start_shift_length, end_shift_length, threshold)) {
- std::cerr << "Enters here 0 ? \n";
- std::cerr << "no_left_lanes " << no_left_lanes << "\n";
- std::cerr << "no_right_lanes " << no_right_lanes << "\n";
-
// Left avoid. But there is no adjacent lane. No need blinker.
if (relative_shift_length > 0.0 && no_left_lanes) {
return false;
@@ -185,7 +180,6 @@ class TurnSignalDecider
}
if (isReturnShift(start_shift_length, end_shift_length, threshold)) {
- std::cerr << "Enters here 1 ? \n";
// Right return. But there is no adjacent lane. No need blinker.
if (relative_shift_length > 0.0 && no_right_lanes) {
return false;
@@ -199,7 +193,6 @@ class TurnSignalDecider
if (isLeftMiddleShift(start_shift_length, end_shift_length, threshold)) {
// Left avoid. But there is no adjacent lane. No need blinker.
- std::cerr << "Enters here 2 ? \n";
if (relative_shift_length > 0.0 && no_left_lanes) {
return false;
@@ -213,7 +206,6 @@ class TurnSignalDecider
if (isRightMiddleShift(start_shift_length, end_shift_length, threshold)) {
// Right avoid. But there is no adjacent lane. No need blinker.
- std::cerr << "Enters here 3 ? \n";
if (relative_shift_length < 0.0 && no_right_lanes) {
return false;
@@ -224,7 +216,6 @@ class TurnSignalDecider
return false;
}
}
- std::cerr << "Returns True\n";
return true;
};
From d1baad8d8856e6dee72e076ca6d88f72d7632c04 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Fri, 15 Mar 2024 14:19:57 +0900
Subject: [PATCH 12/13] add comment on override
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../src/start_planner_module.cpp | 5 +++++
1 file changed, 5 insertions(+)
diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp
index c2450daea3728..70568742aaa22 100644
--- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp
+++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp
@@ -1250,6 +1250,11 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo()
constexpr bool egos_lane_is_shifted = true;
constexpr bool is_pull_out = true;
+
+ // In Geometric pull out, the ego stops once and then steers the wheels to the opposite direction.
+ // This sometimes causes the getBehaviorTurnSignalInfo method to detect the ego as stopped and
+ // close to complete its shift, so it wrongly turns off the blinkers, this override helps avoid
+ // this issue.
const bool override_ego_stopped_check = std::invoke([&]() {
if (status_.planner_type != PlannerType::GEOMETRIC) {
return false;
From 168409a188d2e859e0f0ce12d139c743a4ced901 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Fri, 15 Mar 2024 14:22:28 +0900
Subject: [PATCH 13/13] comments
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
planning/behavior_path_lane_change_module/src/scene.cpp | 2 ++
1 file changed, 2 insertions(+)
diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp
index d71b7d9b11397..8d870002e96ce 100644
--- a/planning/behavior_path_lane_change_module/src/scene.cpp
+++ b/planning/behavior_path_lane_change_module/src/scene.cpp
@@ -486,6 +486,8 @@ TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() const
const auto & shift_path = status_.lane_change_path.shifted_path;
const auto current_shift_length = lanelet::utils::getArcCoordinates(current_lanes, pose).distance;
constexpr bool is_driving_forward = true;
+ // The getBehaviorTurnSignalInfo method expects the shifted line to be generated off of the ego's
+ // current lane, lane change is different, so we set this flag to false.
constexpr bool egos_lane_is_shifted = false;
const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo(