Skip to content

Commit

Permalink
feat(intersection): memorize last observed valid traffic light inform…
Browse files Browse the repository at this point in the history 
…ation (autowarefoundation#6272)

Signed-off-by: Mamoru Sobue <[email protected]>
  • Loading branch information
@soblin @anhnv3991
soblin authored and anhnv3991 committed Feb 13, 2024
1 parent 5ce971a commit 5159bc0
Show file tree
Hide file tree
Showing 4 changed files with 163 additions and 55 deletions.
43 changes: 41 additions & 2 deletions planning/behavior_velocity_intersection_module/src/debug.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -111,7 +111,7 @@ visualization_msgs::msg::MarkerArray createPoseMarkerArray(
return msg;
}

visualization_msgs::msg::MarkerArray createLineMarkerArray(
visualization_msgs::msg::MarkerArray createArrowLineMarkerArray(
const geometry_msgs::msg::Point & point_start, const geometry_msgs::msg::Point & point_end,
const std::string & ns, const int64_t id, const double r, const double g, const double b)
{
Expand All @@ -137,6 +137,28 @@ visualization_msgs::msg::MarkerArray createLineMarkerArray(
return msg;
}

visualization_msgs::msg::MarkerArray createLineMarkerArray(
const geometry_msgs::msg::Point & point_start, const geometry_msgs::msg::Point & point_end,
const std::string & ns, const int64_t id, const double r, const double g, const double b)
{
visualization_msgs::msg::MarkerArray msg;

visualization_msgs::msg::Marker marker;
marker.header.frame_id = "map";
marker.ns = ns + "_line";
marker.id = id;
marker.lifetime = rclcpp::Duration::from_seconds(0.3);
marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
marker.action = visualization_msgs::msg::Marker::ADD;
marker.scale.x = 0.1;
marker.color = createMarkerColor(r, g, b, 0.999);
marker.points.push_back(point_start);
marker.points.push_back(point_end);

msg.markers.push_back(marker);
return msg;
}

[[maybe_unused]] visualization_msgs::msg::Marker createPointMarkerArray(
const geometry_msgs::msg::Point & point, const std::string & ns, const int64_t id, const double r,
const double g, const double b)
Expand Down Expand Up @@ -362,10 +384,27 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray(
if (debug_data_.nearest_occlusion_projection) {
const auto [point_start, point_end] = debug_data_.nearest_occlusion_projection.value();
appendMarkerArray(
::createLineMarkerArray(
::createArrowLineMarkerArray(
point_start, point_end, "nearest_occlusion_projection", lane_id_, 0.5, 0.5, 0.0),
&debug_marker_array, now);
}

if (debug_data_.traffic_light_observation) {
const auto GREEN = autoware_perception_msgs::msg::TrafficSignalElement::GREEN;
const auto YELLOW = autoware_perception_msgs::msg::TrafficSignalElement::AMBER;

const auto [ego, tl_point, id, color] = debug_data_.traffic_light_observation.value();
geometry_msgs::msg::Point tl_point_point;
tl_point_point.x = tl_point.x();
tl_point_point.y = tl_point.y();
tl_point_point.z = tl_point.z();
const auto tl_color = (color == GREEN) ? green : (color == YELLOW ? yellow : red);
const auto [r, g, b] = tl_color;
appendMarkerArray(
::createLineMarkerArray(
ego.position, tl_point_point, "intersection_traffic_light", lane_id_, r, g, b),
&debug_marker_array, now);
}
return debug_marker_array;
}

Expand Down
117 changes: 77 additions & 40 deletions planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@

#include <behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp> // for toGeomPoly
#include <behavior_velocity_planner_common/utilization/util.hpp>
#include <lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp>
#include <lanelet2_extension/utility/utilities.hpp>
#include <motion_utils/trajectory/trajectory.hpp>
#include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp> // for toPolygon2d
Expand Down Expand Up @@ -1143,25 +1144,11 @@ void IntersectionModule::reactRTCApproval(
bool IntersectionModule::isGreenSolidOn() const
{
using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement;
const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr();
const auto & lane = lanelet_map_ptr->laneletLayer.get(lane_id_);

std::optional<lanelet::Id> 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;
}
const auto tl_info_opt = planner_data_->getTrafficSignal(
tl_id.value(), true /* traffic light module keeps last observation*/);
if (!tl_info_opt) {
// the info of this traffic light is not available
if (!last_tl_valid_observation_) {
return false;
}
const auto & tl_info = tl_info_opt.value();
const auto & tl_info = last_tl_valid_observation_.value();
for (auto && tl_light : tl_info.signal.elements) {
if (
tl_light.color == TrafficSignalElement::GREEN &&
Expand All @@ -1176,38 +1163,88 @@ IntersectionModule::TrafficPrioritizedLevel IntersectionModule::getTrafficPriori
{
using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement;

auto corresponding_arrow = [&](const TrafficSignalElement & element) {
if (turn_direction_ == "straight" && element.shape == TrafficSignalElement::UP_ARROW) {
return true;
}
if (turn_direction_ == "left" && element.shape == TrafficSignalElement::LEFT_ARROW) {
return true;
}
if (turn_direction_ == "right" && element.shape == TrafficSignalElement::RIGHT_ARROW) {
return true;
}
return false;
};

// ==========================================================================================
// if no traffic light information has been available, it is UNKNOWN state which is treated as
// NOT_PRIORITIZED
//
// if there has been any information available in the past more than once, the last valid
// information is kept and used for planning
// ==========================================================================================
auto level = TrafficPrioritizedLevel::NOT_PRIORITIZED;
if (last_tl_valid_observation_) {
auto color = TrafficSignalElement::GREEN;
const auto & tl_info = last_tl_valid_observation_.value();
bool has_amber_signal{false};
for (auto && tl_light : tl_info.signal.elements) {
if (
tl_light.color == TrafficSignalElement::AMBER &&
tl_light.shape == TrafficSignalElement::CIRCLE) {
has_amber_signal = true;
}
if (
(tl_light.color == TrafficSignalElement::RED &&
tl_light.shape == TrafficSignalElement::CIRCLE) ||
(tl_light.color == TrafficSignalElement::GREEN && corresponding_arrow(tl_light))) {
// NOTE: Return here since the red signal has the highest priority.
level = TrafficPrioritizedLevel::FULLY_PRIORITIZED;
color = TrafficSignalElement::RED;
break;
}
}
if (has_amber_signal) {
level = TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED;
color = TrafficSignalElement::AMBER;
}
if (tl_id_and_point_) {
const auto [tl_id, point] = tl_id_and_point_.value();
debug_data_.traffic_light_observation =
std::make_tuple(planner_data_->current_odometry->pose, point, tl_id, color);
}
}
return level;
}

void IntersectionModule::updateTrafficSignalObservation()
{
const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr();
const auto & lane = lanelet_map_ptr->laneletLayer.get(lane_id_);

std::optional<lanelet::Id> tl_id = std::nullopt;
for (auto && tl_reg_elem : lane.regulatoryElementsAs<lanelet::TrafficLight>()) {
tl_id = tl_reg_elem->id();
break;
if (!tl_id_and_point_) {
for (auto && tl_reg_elem :
lane.regulatoryElementsAs<lanelet::autoware::AutowareTrafficLight>()) {
for (const auto & ls : tl_reg_elem->lightBulbs()) {
if (ls.hasAttribute("traffic_light_id")) {
tl_id_and_point_ = std::make_pair(tl_reg_elem->id(), ls.front());
break;
}
}
}
}
if (!tl_id) {
if (!tl_id_and_point_) {
// this lane has no traffic light
return TrafficPrioritizedLevel::NOT_PRIORITIZED;
return;
}
const auto tl_info_opt = planner_data_->getTrafficSignal(
tl_id.value(), true /* traffic light module keeps last observation*/);
const auto [tl_id, point] = tl_id_and_point_.value();
const auto tl_info_opt =
planner_data_->getTrafficSignal(tl_id, true /* traffic light module keeps last observation*/);
if (!tl_info_opt) {
return TrafficPrioritizedLevel::NOT_PRIORITIZED;
}
const auto & tl_info = tl_info_opt.value();
bool has_amber_signal{false};
for (auto && tl_light : tl_info.signal.elements) {
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;
}
}
if (has_amber_signal) {
return TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED;
// the info of this traffic light is not available
return;
}
return TrafficPrioritizedLevel::NOT_PRIORITIZED;
last_tl_valid_observation_ = tl_info_opt.value();
}

IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStatus(
Expand Down
24 changes: 24 additions & 0 deletions planning/behavior_velocity_intersection_module/src/scene_intersection.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -209,6 +209,9 @@ class IntersectionModule : public SceneModuleInterface
std::optional<std::pair<geometry_msgs::msg::Point, geometry_msgs::msg::Point>>
nearest_occlusion_projection{std::nullopt};
std::optional<double> static_occlusion_with_traffic_light_timeout{std::nullopt};

std::optional<std::tuple<geometry_msgs::msg::Pose, lanelet::ConstPoint3d, lanelet::Id, uint8_t>>
traffic_light_observation{std::nullopt};
};

using TimeDistanceArray = std::vector<std::pair<double /* time*/, double /* distance*/>>;
Expand Down Expand Up @@ -388,6 +391,20 @@ class IntersectionModule : public SceneModuleInterface
intersection::ObjectInfoManager object_info_manager_;
/** @} */

private:
/**
***********************************************************
***********************************************************
***********************************************************
* @defgroup collision-variables [var] collision detection
* @{
*/
//! save the last UNKNOWN traffic light information of this intersection(keep even if the info got
//! unavailable)
std::optional<std::pair<lanelet::Id, lanelet::ConstPoint3d>> tl_id_and_point_;
std::optional<TrafficSignalStamped> last_tl_valid_observation_{std::nullopt};
/** @} */

private:
/**
***********************************************************
Expand Down Expand Up @@ -556,6 +573,13 @@ class IntersectionModule : public SceneModuleInterface
* @brief find TrafficPrioritizedLevel
*/
TrafficPrioritizedLevel getTrafficPrioritizedLevel() const;

/**
* @brief update the valid traffic signal information if still available, otherwise keep last
* observation
*/
void updateTrafficSignalObservation();

/** @} */

private:
Expand Down
34 changes: 21 additions & 13 deletions planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -258,19 +258,27 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL
planner_data_->occupancy_grid->info.resolution);
}

const bool is_green_solid_on = isGreenSolidOn();
if (is_green_solid_on && !initial_green_light_observed_time_) {
const auto assigned_lane_begin_point = assigned_lanelet.centerline().front();
const bool approached_assigned_lane =
motion_utils::calcSignedArcLength(
path->points, closest_idx,
tier4_autoware_utils::createPoint(
assigned_lane_begin_point.x(), assigned_lane_begin_point.y(),
assigned_lane_begin_point.z())) <
planner_param_.collision_detection.yield_on_green_traffic_light
.distance_to_assigned_lanelet_start;
if (approached_assigned_lane) {
initial_green_light_observed_time_ = clock_->now();
// ==========================================================================================
// update traffic light information
// updateTrafficSignalObservation() must be called at first to because other traffic signal
// fuctions use last_valid_observation_
// ==========================================================================================
if (has_traffic_light_) {
updateTrafficSignalObservation();
const bool is_green_solid_on = isGreenSolidOn();
if (is_green_solid_on && !initial_green_light_observed_time_) {
const auto assigned_lane_begin_point = assigned_lanelet.centerline().front();
const bool approached_assigned_lane =
motion_utils::calcSignedArcLength(
path->points, closest_idx,
tier4_autoware_utils::createPoint(
assigned_lane_begin_point.x(), assigned_lane_begin_point.y(),
assigned_lane_begin_point.z())) <
planner_param_.collision_detection.yield_on_green_traffic_light
.distance_to_assigned_lanelet_start;
if (approached_assigned_lane) {
initial_green_light_observed_time_ = clock_->now();
}
}
}

Expand Down

0 comments on commit 5159bc0

Please sign in to comment.