Skip to content

Commit

Permalink
fix(static_obstacle_avoidance): improve turn signal output timing whe…
Browse files Browse the repository at this point in the history 
…n the ego returns original lane (#1781)
  • Loading branch information
@zhiwango
zhiwango authored Jan 29, 2025
1 parent 28225d7 commit 8a6c325
Show file tree
Hide file tree
Showing 4 changed files with 186 additions and 50 deletions.
3 changes: 2 additions & 1 deletion ...voidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/debug.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,8 @@ MarkerArray createAmbiguousObjectsMarkerArray(
MarkerArray createStopTargetObjectMarkerArray(const AvoidancePlanningData & data);

MarkerArray createDebugMarkerArray(
const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug,
const BehaviorModuleOutput & output, const AvoidancePlanningData & data,
const PathShifter & shifter, const DebugData & debug,
const std::shared_ptr<AvoidanceParameters> & parameters);
} // namespace autoware::behavior_path_planner::utils::static_obstacle_avoidance

Expand Down
6 changes: 5 additions & 1 deletion ...voidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -346,7 +346,8 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface
* @brief fill debug markers.
*/
void updateDebugMarker(
const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug) const;
const BehaviorModuleOutput & output, const AvoidancePlanningData & data,
const PathShifter & shifter, const DebugData & debug) const;

/**
* @brief fill information markers that are shown in Rviz by default.
Expand All @@ -368,6 +369,9 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface
*/
bool isSafePath(ShiftedPath & shifted_path, DebugData & debug) const;

auto getTurnSignal(const ShiftedPath & spline_shift_path, const ShiftedPath & linear_shift_path)
-> TurnSignalInfo;

// post process

/**
Expand Down
65 changes: 64 additions & 1 deletion ...havior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -229,6 +229,67 @@ MarkerArray unAvoidableObjectsMarkerArray(const ObjectDataArray & objects, std::
return msg;
}

MarkerArray createTurnSignalMarkerArray(const TurnSignalInfo & turn_signal_info, std::string && ns)
{
MarkerArray msg;

if (turn_signal_info.turn_signal.command == TurnIndicatorsCommand::NO_COMMAND) {
return msg;
}

const auto yaw_offset = [&turn_signal_info]() {
if (turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_RIGHT) {
return -1.0 * M_PI_2;
}

if (turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) {
return M_PI_2;
}

return 0.0;
}();

size_t i = 0;
{
auto marker = createDefaultMarker(
"map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, i++, Marker::ARROW,
createMarkerScale(0.6, 0.3, 0.3), createMarkerColor(0.0, 0.0, 1.0, 0.999));
marker.pose = turn_signal_info.desired_start_point;
marker.pose = calcOffsetPose(marker.pose, 0.0, 0.0, 0.0, yaw_offset);

msg.markers.push_back(marker);
}
{
auto marker = createDefaultMarker(
"map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, i++, Marker::ARROW,
createMarkerScale(0.6, 0.3, 0.3), createMarkerColor(0.0, 0.0, 1.0, 0.999));
marker.pose = turn_signal_info.desired_end_point;
marker.pose = calcOffsetPose(marker.pose, 0.0, 0.0, 0.0, yaw_offset);

msg.markers.push_back(marker);
}
{
auto marker = createDefaultMarker(
"map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, i++, Marker::ARROW,
createMarkerScale(0.8, 0.4, 0.4), createMarkerColor(1.0, 0.0, 0.0, 0.999));
marker.pose = turn_signal_info.required_start_point;
marker.pose = calcOffsetPose(marker.pose, 0.0, 0.0, 0.0, yaw_offset);

msg.markers.push_back(marker);
}
{
auto marker = createDefaultMarker(
"map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, i++, Marker::ARROW,
createMarkerScale(0.8, 0.4, 0.4), createMarkerColor(1.0, 0.0, 0.0, 0.999));
marker.pose = turn_signal_info.required_end_point;
marker.pose = calcOffsetPose(marker.pose, 0.0, 0.0, 0.0, yaw_offset);

msg.markers.push_back(marker);
}

return msg;
}

} // namespace

MarkerArray createEgoStatusMarkerArray(
Expand Down Expand Up @@ -591,7 +652,8 @@ MarkerArray createDrivableBounds(
}

MarkerArray createDebugMarkerArray(
const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug,
const BehaviorModuleOutput & output, const AvoidancePlanningData & data,
const PathShifter & shifter, const DebugData & debug,
const std::shared_ptr<AvoidanceParameters> & parameters)
{
using autoware::behavior_path_planner::utils::transformToLanelets;
Expand Down Expand Up @@ -737,6 +799,7 @@ MarkerArray createDebugMarkerArray(
// misc
if (parameters->enable_misc_marker) {
add(createPathMarkerArray(path, "centerline_resampled", 0, 0.0, 0.9, 0.5));
add(createTurnSignalMarkerArray(output.turn_signal_info, "turn_signal_info"));
}

return msg;
Expand Down
162 changes: 115 additions & 47 deletions ...havior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -92,7 +92,7 @@ bool StaticObstacleAvoidanceModule::isExecutionRequested() const

// Check ego is in preferred lane
updateInfoMarker(avoid_data_);
updateDebugMarker(avoid_data_, path_shifter_, debug_data_);
updateDebugMarker(BehaviorModuleOutput{}, avoid_data_, path_shifter_, debug_data_);

// there is object that should be avoid. return true.
if (!!avoid_data_.stop_target_object) {
Expand Down Expand Up @@ -947,6 +947,112 @@ PathWithLaneId StaticObstacleAvoidanceModule::extendBackwardLength(
return extended_path;
}

auto StaticObstacleAvoidanceModule::getTurnSignal(
const ShiftedPath & spline_shift_path, const ShiftedPath & linear_shift_path) -> TurnSignalInfo
{
using autoware::motion_utils::calcSignedArcLength;

const auto is_ignore_signal = [this](const UUID & uuid) {
if (!ignore_signal_.has_value()) {
return false;
}

return ignore_signal_.value() == uuid;
};

const auto update_ignore_signal = [this](const UUID & uuid, const bool is_ignore) {
ignore_signal_ = is_ignore ? std::make_optional(uuid) : std::nullopt;
};

const auto is_large_deviation = [this](const auto & path) {
constexpr double threshold = 1.0;
const auto current_seg_idx = planner_data_->findEgoSegmentIndex(path.points);
const auto lateral_deviation =
autoware::motion_utils::calcLateralOffset(path.points, getEgoPosition(), current_seg_idx);
return std::abs(lateral_deviation) > threshold;
};

auto shift_lines = path_shifter_.getShiftLines();
if (shift_lines.empty()) {
return getPreviousModuleOutput().turn_signal_info;
}

if (is_large_deviation(spline_shift_path.path)) {
return getPreviousModuleOutput().turn_signal_info;
}

const auto itr =
std::remove_if(shift_lines.begin(), shift_lines.end(), [&, this](const auto & s) {
const auto threshold = planner_data_->parameters.turn_signal_shift_length_threshold;
return std::abs(s.start_shift_length - s.end_shift_length) < threshold ||
is_ignore_signal(s.id);
});
shift_lines.erase(itr, shift_lines.end());

if (shift_lines.empty()) {
return getPreviousModuleOutput().turn_signal_info;
}

const auto target_shift_line = [&]() {
const auto & s1 = shift_lines.front();

for (size_t i = 1; i < shift_lines.size(); i++) {
const auto & s2 = shift_lines.at(i);

const auto s1_relative_length = s1.start_shift_length - s1.end_shift_length;
const auto s2_relative_length = s2.start_shift_length - s2.end_shift_length;

// same side shift
if (s1_relative_length > 0.0 && s2_relative_length > 0.0) {
continue;
}

// same side shift
if (s1_relative_length < 0.0 && s2_relative_length < 0.0) {
continue;
}

// different side shift
const auto & points = path_shifter_.getReferencePath().points;
const size_t idx = planner_data_->findEgoIndex(points);

// output turn signal for near shift line.
if (calcSignedArcLength(points, idx, s1.start_idx) > 0.0) {
return s1;
}

// output turn signal for far shift line.
if (
calcSignedArcLength(points, idx, s2.start_idx) <
getEgoSpeed() * parameters_->max_prepare_time) {
return s2;
}

// output turn signal for near shift line.
return s1;
}

return s1;
}();

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, target_shift_line, avoid_data_.current_lanelets, helper_->getEgoShift(),
is_driving_forward, egos_lane_is_shifted);

update_ignore_signal(target_shift_line.id, is_ignore);

const auto current_seg_idx = planner_data_->findEgoSegmentIndex(spline_shift_path.path.points);
return planner_data_->turn_signal_decider.overwrite_turn_signal(
spline_shift_path.path, getEgoPose(), current_seg_idx, original_signal, new_signal,
planner_data_->parameters.ego_nearest_dist_threshold,
planner_data_->parameters.ego_nearest_yaw_threshold);
}

BehaviorModuleOutput StaticObstacleAvoidanceModule::plan()
{
universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
Expand Down Expand Up @@ -992,48 +1098,9 @@ BehaviorModuleOutput StaticObstacleAvoidanceModule::plan()

BehaviorModuleOutput output;

const auto is_ignore_signal = [this](const UUID & uuid) {
if (!ignore_signal_.has_value()) {
return false;
}

return ignore_signal_.value() == uuid;
};

const auto update_ignore_signal = [this](const UUID & uuid, const bool is_ignore) {
ignore_signal_ = is_ignore ? std::make_optional(uuid) : std::nullopt;
};

const auto is_large_deviation = [this](const auto & path) {
constexpr double threshold = 1.0;
const auto current_seg_idx = planner_data_->findEgoSegmentIndex(path.points);
const auto lateral_deviation =
autoware::motion_utils::calcLateralOffset(path.points, getEgoPosition(), current_seg_idx);
return std::abs(lateral_deviation) > threshold;
};

// turn signal info
if (path_shifter_.getShiftLines().empty()) {
output.turn_signal_info = getPreviousModuleOutput().turn_signal_info;
} else if (is_ignore_signal(path_shifter_.getShiftLines().front().id)) {
output.turn_signal_info = getPreviousModuleOutput().turn_signal_info;
} else if (is_large_deviation(spline_shift_path.path)) {
output.turn_signal_info = getPreviousModuleOutput().turn_signal_info;
} 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(), 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.overwrite_turn_signal(
spline_shift_path.path, getEgoPose(), current_seg_idx, original_signal, new_signal,
planner_data_->parameters.ego_nearest_dist_threshold,
planner_data_->parameters.ego_nearest_yaw_threshold);
update_ignore_signal(path_shifter_.getShiftLines().front().id, is_ignore);
// turn signal
{
output.turn_signal_info = getTurnSignal(spline_shift_path, linear_shift_path);
}

// sparse resampling for computational cost
Expand All @@ -1046,7 +1113,7 @@ BehaviorModuleOutput StaticObstacleAvoidanceModule::plan()
{
updateEgoBehavior(data, spline_shift_path);
updateInfoMarker(avoid_data_);
updateDebugMarker(avoid_data_, path_shifter_, debug_data_);
updateDebugMarker(output, avoid_data_, path_shifter_, debug_data_);
}

if (isDrivingSameLane(helper_->getPreviousDrivingLanes(), data.current_lanelets)) {
Expand Down Expand Up @@ -1492,12 +1559,13 @@ void StaticObstacleAvoidanceModule::updateInfoMarker(const AvoidancePlanningData
}

void StaticObstacleAvoidanceModule::updateDebugMarker(
const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug) const
const BehaviorModuleOutput & output, const AvoidancePlanningData & data,
const PathShifter & shifter, const DebugData & debug) const
{
universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
debug_marker_.markers.clear();
debug_marker_ =
utils::static_obstacle_avoidance::createDebugMarkerArray(data, shifter, debug, parameters_);
debug_marker_ = utils::static_obstacle_avoidance::createDebugMarkerArray(
output, data, shifter, debug, parameters_);
}

void StaticObstacleAvoidanceModule::updateAvoidanceDebugData(
Expand Down

0 comments on commit 8a6c325

Please sign in to comment.