Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(intersection): 信号連携周りの改善 #997

Merged
merged 2 commits into from
Nov 2, 2023
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -24,12 +24,15 @@
state_transit_margin_time: 1.0
min_predicted_path_confidence: 0.05
minimum_ego_predicted_velocity: 1.388 # [m/s]
normal:
collision_start_margin_time: 4.0 # [s] this + state_transit_margin_time should be higher to account for collision with fast/accelerating object
collision_end_margin_time: 6.0 # [s] this + state_transit_margin_time should be higher to account for collision with slow/decelerating object
relaxed:
fully_prioritized:
collision_start_margin_time: 2.0
collision_end_margin_time: 0.0
partially_prioritized:
collision_start_margin_time: 2.0
collision_end_margin_time: 2.0
not_prioritized:
collision_start_margin_time: 4.0 # [s] this + state_transit_margin_time should be higher to account for collision with fast/accelerating object
collision_end_margin_time: 6.0 # [s] this + state_transit_margin_time should be higher to account for collision with slow/decelerating object
keep_detection_vel_thr: 0.833 # == 3.0km/h. keep detection if ego is ego.vel < keep_detection_vel_thr

occlusion:
Expand Down
25 changes: 17 additions & 8 deletions planning/behavior_velocity_intersection_module/src/manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -81,14 +81,23 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
node.declare_parameter<double>(ns + ".collision_detection.minimum_ego_predicted_velocity");
ip.collision_detection.state_transit_margin_time =
node.declare_parameter<double>(ns + ".collision_detection.state_transit_margin_time");
ip.collision_detection.normal.collision_start_margin_time =
node.declare_parameter<double>(ns + ".collision_detection.normal.collision_start_margin_time");
ip.collision_detection.normal.collision_end_margin_time =
node.declare_parameter<double>(ns + ".collision_detection.normal.collision_end_margin_time");
ip.collision_detection.relaxed.collision_start_margin_time =
node.declare_parameter<double>(ns + ".collision_detection.relaxed.collision_start_margin_time");
ip.collision_detection.relaxed.collision_end_margin_time =
node.declare_parameter<double>(ns + ".collision_detection.relaxed.collision_end_margin_time");
ip.collision_detection.fully_prioritized.collision_start_margin_time =
node.declare_parameter<double>(
ns + ".collision_detection.fully_prioritized.collision_start_margin_time");
ip.collision_detection.fully_prioritized.collision_end_margin_time =
node.declare_parameter<double>(
ns + ".collision_detection.fully_prioritized.collision_end_margin_time");
ip.collision_detection.partially_prioritized.collision_start_margin_time =
node.declare_parameter<double>(
ns + ".collision_detection.partially_prioritized.collision_start_margin_time");
ip.collision_detection.partially_prioritized.collision_end_margin_time =
node.declare_parameter<double>(
ns + ".collision_detection.partially_prioritized.collision_end_margin_time");
ip.collision_detection.not_prioritized.collision_start_margin_time =
node.declare_parameter<double>(
ns + ".collision_detection.not_prioritized.collision_start_margin_time");
ip.collision_detection.not_prioritized.collision_end_margin_time = node.declare_parameter<double>(
ns + ".collision_detection.not_prioritized.collision_end_margin_time");
ip.collision_detection.keep_detection_vel_thr =
node.declare_parameter<double>(ns + ".collision_detection.keep_detection_vel_thr");

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -853,10 +853,12 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(

// at the very first time of registration of this module, the path may not be conflicting with the
// attention area, so update() is called to update the internal data as well as traffic light info
const bool tl_arrow_solid_on =
util::isTrafficLightArrowActivated(assigned_lanelet, planner_data_->traffic_light_id_map);
const auto traffic_prioritized_level =
util::getTrafficPrioritizedLevel(assigned_lanelet, planner_data_->traffic_light_id_map);
const bool is_prioritized =
traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED;
const auto footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0);
intersection_lanelets.update(tl_arrow_solid_on, interpolated_path_info, footprint);
intersection_lanelets.update(is_prioritized, interpolated_path_info, footprint);

// this is abnormal
const auto & conflicting_lanelets = intersection_lanelets.conflicting();
Expand Down Expand Up @@ -1013,20 +1015,20 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
auto target_objects = generateTargetObjects(intersection_lanelets, intersection_area);

// calculate dynamic collision around attention area
const double time_to_restart = (is_go_out_ || tl_arrow_solid_on)
const double time_to_restart = (is_go_out_ || is_prioritized)
? 0.0
: (planner_param_.collision_detection.state_transit_margin_time -
collision_state_machine_.getDuration());

const bool has_collision = checkCollision(
*path, &target_objects, path_lanelets, closest_idx, time_to_restart, tl_arrow_solid_on);
*path, &target_objects, path_lanelets, closest_idx, time_to_restart, traffic_prioritized_level);
collision_state_machine_.setStateWithMarginTime(
has_collision ? StateMachine::State::STOP : StateMachine::State::GO,
logger_.get_child("collision state_machine"), *clock_);
const bool has_collision_with_margin =
collision_state_machine_.getState() == StateMachine::State::STOP;

if (tl_arrow_solid_on) {
if (is_prioritized) {
return TrafficLightArrowSolidOn{
has_collision_with_margin, closest_idx, collision_stop_line_idx, occlusion_stop_line_idx};
}
Expand All @@ -1045,7 +1047,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
std::pow(planner_param_.occlusion.max_vehicle_velocity_for_rss, 2) /
(2 * planner_param_.occlusion.min_vehicle_brake_for_rss));
auto occlusion_status =
(enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !tl_arrow_solid_on)
(enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !is_prioritized)
? getOcclusionStatus(
*planner_data_->occupancy_grid, occlusion_attention_area, adjacent_lanelets,
first_attention_area, interpolated_path_info, occlusion_attention_divisions,
Expand Down Expand Up @@ -1256,7 +1258,8 @@ util::TargetObjects IntersectionModule::generateTargetObjects(
bool IntersectionModule::checkCollision(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
util::TargetObjects * target_objects, const util::PathLanelets & path_lanelets,
const int closest_idx, const double time_delay, const bool tl_arrow_solid_on)
const int closest_idx, const double time_delay,
const util::TrafficPrioritizedLevel & traffic_prioritized_level)
{
using lanelet::utils::getArcCoordinates;
using lanelet::utils::getPolygonFromArcLength;
Expand All @@ -1277,12 +1280,21 @@ bool IntersectionModule::checkCollision(
debug_data_.ego_lane = ego_lane.polygon3d();
const auto ego_poly = ego_lane.polygon2d().basicPolygon();
// check collision between predicted_path and ego_area
const double collision_start_margin_time =
tl_arrow_solid_on ? planner_param_.collision_detection.relaxed.collision_start_margin_time
: planner_param_.collision_detection.normal.collision_start_margin_time;
const double collision_end_margin_time =
tl_arrow_solid_on ? planner_param_.collision_detection.relaxed.collision_end_margin_time
: planner_param_.collision_detection.normal.collision_end_margin_time;
const auto [collision_start_margin_time, collision_end_margin_time] = [&]() {
if (traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED) {
return std::make_pair(
planner_param_.collision_detection.fully_prioritized.collision_start_margin_time,
planner_param_.collision_detection.fully_prioritized.collision_end_margin_time);
}
if (traffic_prioritized_level == util::TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) {
return std::make_pair(
planner_param_.collision_detection.partially_prioritized.collision_start_margin_time,
planner_param_.collision_detection.partially_prioritized.collision_end_margin_time);
}
return std::make_pair(
planner_param_.collision_detection.not_prioritized.collision_start_margin_time,
planner_param_.collision_detection.not_prioritized.collision_end_margin_time);
}();
bool collision_detected = false;
for (const auto & target_object : target_objects->all_attention_objects) {
const auto & object = target_object.object;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -78,16 +78,21 @@ class IntersectionModule : public SceneModuleInterface
//! minimum confidence value of predicted path to use for collision detection
double minimum_ego_predicted_velocity; //! used to calculate ego's future velocity profile
double state_transit_margin_time;
struct Normal
struct FullyPrioritized
{
double collision_start_margin_time; //! start margin time to check collision
double collision_end_margin_time; //! end margin time to check collision
} normal;
struct Relaxed
} fully_prioritized;
struct PartiallyPrioritized
{
double collision_start_margin_time;
double collision_end_margin_time;
} relaxed;
double collision_start_margin_time; //! start margin time to check collision
double collision_end_margin_time; //! end margin time to check collision
} partially_prioritized;
struct NotPrioritized
{
double collision_start_margin_time; //! start margin time to check collision
double collision_end_margin_time; //! end margin time to check collision
} not_prioritized;
double keep_detection_vel_thr; //! keep detection if ego is ego.vel < keep_detection_vel_thr
} collision_detection;
struct Occlusion
Expand Down Expand Up @@ -295,7 +300,8 @@ class IntersectionModule : public SceneModuleInterface
bool checkCollision(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
util::TargetObjects * target_objects, const util::PathLanelets & path_lanelets,
const int closest_idx, const double time_delay, const bool tl_arrow_solid_on);
const int closest_idx, const double time_delay,
const util::TrafficPrioritizedLevel & traffic_prioritized_level);

OcclusionType getOcclusionStatus(
const nav_msgs::msg::OccupancyGrid & occ_grid,
Expand Down
31 changes: 17 additions & 14 deletions planning/behavior_velocity_intersection_module/src/util.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -975,37 +975,40 @@
return tl_id.has_value();
}

bool isTrafficLightArrowActivated(
TrafficPrioritizedLevel getTrafficPrioritizedLevel(
lanelet::ConstLanelet lane, const std::map<int, TrafficSignalStamped> & tl_infos)
{
using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement;

const auto & turn_direction = lane.attributeOr("turn_direction", "else");
std::optional<int> tl_id = std::nullopt;
for (auto && tl_reg_elem : lane.regulatoryElementsAs<lanelet::TrafficLight>()) {
tl_id = tl_reg_elem->id();
break;
}
if (!tl_id) {
// this lane has no traffic light
return false;
return TrafficPrioritizedLevel::NOT_PRIORITIZED;
}
const auto tl_info_it = tl_infos.find(tl_id.value());
if (tl_info_it == tl_infos.end()) {
// the info of this traffic light is not available
return false;
return TrafficPrioritizedLevel::NOT_PRIORITIZED;
}
const auto & tl_info = tl_info_it->second;
bool has_amber_signal{false};
for (auto && tl_light : tl_info.signal.elements) {
if (tl_light.color != TrafficSignalElement::GREEN) continue;
if (tl_light.status != TrafficSignalElement::SOLID_ON) continue;
if (turn_direction == std::string("left") && tl_light.shape == TrafficSignalElement::LEFT_ARROW)
return true;
if (
turn_direction == std::string("right") && tl_light.shape == TrafficSignalElement::RIGHT_ARROW)
return true;
if (tl_light.color == TrafficSignalElement::AMBER) {
has_amber_signal = true;
}
if (tl_light.color == TrafficSignalElement::RED) {
// NOTE: Return here since the red signal has the highest priority.
return TrafficPrioritizedLevel::FULLY_PRIORITIZED;
}
}
return false;
if (has_amber_signal) {
return TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED;
}
return TrafficPrioritizedLevel::NOT_PRIORITIZED;
}

double getHighestCurvature(const lanelet::ConstLineString3d & centerline)
Expand Down Expand Up @@ -1335,10 +1338,10 @@
}

void IntersectionLanelets::update(
const bool tl_arrow_solid_on, const InterpolatedPathInfo & interpolated_path_info,
const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info,
const tier4_autoware_utils::LinearRing2d & footprint)
{
tl_arrow_solid_on_ = tl_arrow_solid_on;
is_prioritized_ = is_prioritized;
// find the first conflicting/detection area polygon intersecting the path
if (!first_conflicting_area_) {
auto first =
Expand All @@ -1361,14 +1364,14 @@
static lanelet::ConstLanelets getPrevLanelets(
const lanelet::ConstLanelets & lanelets_on_path, const std::set<int> & associative_ids)
{
lanelet::ConstLanelets prevs;

Check warning on line 1367 in planning/behavior_velocity_intersection_module/src/util.cpp

View workflow job for this annotation

GitHub Actions / spell-check-differential

Unknown word (prevs)
for (const auto & ll : lanelets_on_path) {
if (associative_ids.find(ll.id()) != associative_ids.end()) {
return prevs;

Check warning on line 1370 in planning/behavior_velocity_intersection_module/src/util.cpp

View workflow job for this annotation

GitHub Actions / spell-check-differential

Unknown word (prevs)
}
prevs.push_back(ll);

Check warning on line 1372 in planning/behavior_velocity_intersection_module/src/util.cpp

View workflow job for this annotation

GitHub Actions / spell-check-differential

Unknown word (prevs)
}
return prevs;

Check warning on line 1374 in planning/behavior_velocity_intersection_module/src/util.cpp

View workflow job for this annotation

GitHub Actions / spell-check-differential

Unknown word (prevs)
}

std::optional<PathLanelets> generatePathLanelets(
Expand Down
3 changes: 2 additions & 1 deletion planning/behavior_velocity_intersection_module/src/util.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -103,7 +103,8 @@ std::optional<Polygon2d> getIntersectionArea(
lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr);

bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane);
bool isTrafficLightArrowActivated(

TrafficPrioritizedLevel getTrafficPrioritizedLevel(
lanelet::ConstLanelet lane, const std::map<int, TrafficSignalStamped> & tl_infos);

std::vector<lanelet::ConstLineString3d> generateDetectionLaneDivisions(
Expand Down
21 changes: 15 additions & 6 deletions planning/behavior_velocity_intersection_module/src/util_type.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -71,25 +71,25 @@
{
public:
void update(
const bool tl_arrow_solid_on, const InterpolatedPathInfo & interpolated_path_info,
const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info,
const tier4_autoware_utils::LinearRing2d & footprint);
const lanelet::ConstLanelets & attention() const
{
return tl_arrow_solid_on_ ? attention_non_preceding_ : attention_;
return is_prioritized_ ? attention_non_preceding_ : attention_;
}
const std::vector<std::optional<lanelet::ConstLineString3d>> & attention_stop_lines() const
{
return tl_arrow_solid_on_ ? attention_non_preceding_stop_lines_ : attention_stop_lines_;
return is_prioritized_ ? attention_non_preceding_stop_lines_ : attention_stop_lines_;
}
const lanelet::ConstLanelets & conflicting() const { return conflicting_; }
const lanelet::ConstLanelets & adjacent() const { return adjacent_; }
const lanelet::ConstLanelets & occlusion_attention() const
{
return tl_arrow_solid_on_ ? attention_non_preceding_ : occlusion_attention_;
return is_prioritized_ ? attention_non_preceding_ : occlusion_attention_;
}
const std::vector<lanelet::CompoundPolygon3d> & attention_area() const
{
return tl_arrow_solid_on_ ? attention_non_preceding_area_ : attention_area_;
return is_prioritized_ ? attention_non_preceding_area_ : attention_area_;
}
const std::vector<lanelet::CompoundPolygon3d> & conflicting_area() const
{
Expand Down Expand Up @@ -141,7 +141,7 @@
std::optional<lanelet::CompoundPolygon3d> first_conflicting_area_{std::nullopt};
std::optional<lanelet::ConstLanelet> first_attention_lane_{std::nullopt};
std::optional<lanelet::CompoundPolygon3d> first_attention_area_{std::nullopt};
bool tl_arrow_solid_on_ = false;
bool is_prioritized_ = false;
};

struct IntersectionStopLines
Expand All @@ -163,7 +163,7 @@
struct PathLanelets
{
lanelet::ConstLanelets prev;
// lanelet::Constlanelet entry2ego; this is included in `all` if exists

Check warning on line 166 in planning/behavior_velocity_intersection_module/src/util_type.hpp

View workflow job for this annotation

GitHub Actions / spell-check-differential

Unknown word (Constlanelet)
lanelet::ConstLanelet
ego_or_entry2exit; // this is `assigned lane` part of the path(not from
// ego) if ego is before the intersection, otherwise from ego to exit
Expand Down Expand Up @@ -192,6 +192,15 @@
std::vector<TargetObject> intersection_area_objects;
std::vector<TargetObject> all_attention_objects; // TODO(Mamoru Sobue): avoid copy
};

enum class TrafficPrioritizedLevel {
// The target lane's traffic signal is red or the ego's traffic signal has an arrow.
FULLY_PRIORITIZED = 0,
// The target lane's traffic signal is amber
PARTIALLY_PRIORITIZED,
// The target lane's traffic signal is green
NOT_PRIORITIZED
};
} // namespace behavior_velocity_planner::util

#endif // UTIL_TYPE_HPP_
Loading