Skip to content

Commit

Permalink
feat(start_planner): add general turn signal method to start planner (#…
Browse files Browse the repository at this point in the history 
…6628)

* Add general turnSignal method to start planner

Signed-off-by: Daniel Sanchez <[email protected]>

* add description to ignore signal

Signed-off-by: Daniel Sanchez <[email protected]>

---------

Signed-off-by: Daniel Sanchez <[email protected]>
  • Loading branch information
@danielsanchezaran
danielsanchezaran authored Mar 18, 2024
1 parent c24c3b0 commit 546df44
Show file tree
Hide file tree
Showing 2 changed files with 59 additions and 87 deletions.
7 changes: 6 additions & 1 deletion ..._start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down Expand Up @@ -240,6 +241,10 @@ class StartPlannerModule : public SceneModuleInterface
PullOutStatus status_;
mutable StartPlannerDebugData debug_data_;

// Keeps track of lanelets that should be ignored when calculating the turnSignalInfo for this
// module's output. If the ego vehicle is in this lanelet, the calculation is skipped.
std::optional<lanelet::Id> ignore_signal_{std::nullopt};

std::deque<nav_msgs::msg::Odometry::ConstSharedPtr> odometry_buffer_;

std::unique_ptr<rclcpp::Time> last_route_received_time_;
Expand All @@ -264,7 +269,7 @@ class StartPlannerModule : public SceneModuleInterface
std::shared_ptr<LaneDepartureChecker> lane_departure_checker_;

// turn signal
TurnSignalInfo calcTurnSignalInfo() const;
TurnSignalInfo calcTurnSignalInfo();

void incrementPathIndex();
PathWithLaneId getCurrentPath() const;
Expand Down
139 changes: 53 additions & 86 deletions planning/behavior_path_start_planner_module/src/start_planner_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1154,103 +1154,70 @@ bool StartPlannerModule::hasFinishedCurrentPath()
return is_near_target && isStopped();
}

TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const
TurnSignalInfo StartPlannerModule::calcTurnSignalInfo()
{
TurnSignalInfo turn_signal{}; // output
const auto path = getFullPath();
if (path.points.empty()) return getPreviousModuleOutput().turn_signal_info;

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);
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);
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;
};

if (path.points.empty()) {
return {};
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;
}

// 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 double current_shift_length =
lanelet::utils::getArcCoordinates(current_lanes, current_pose).distance;

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;
});
constexpr bool egos_lane_is_shifted = true;
constexpr bool is_pull_out = true;

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;
}
// 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;
}
// 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;
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);

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
Expand Down

0 comments on commit 546df44

Please sign in to comment.