From c9e0b91f2bc6db03fcb29614c4db225d527b1b43 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Tue, 7 Nov 2023 12:51:00 +0900 Subject: [PATCH] refactor(behavior_path_planner): traffic light utils (#5503) * refactor(behavior_path_planner): traffic light utils Signed-off-by: Muhammad Zulfaqar Azmi * Update planning/behavior_path_planner/src/utils/traffic_light_utils.cpp Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> * Update planning/behavior_path_planner/include/behavior_path_planner/utils/traffic_light_utils.hpp Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> --------- Signed-off-by: Muhammad Zulfaqar Azmi Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> --- planning/behavior_path_planner/CMakeLists.txt | 1 + .../utils/traffic_light_utils.hpp | 112 +++++++++++++ .../behavior_path_planner/utils/utils.hpp | 3 - .../src/utils/avoidance/utils.cpp | 84 +--------- .../src/utils/traffic_light_utils.cpp | 157 ++++++++++++++++++ .../behavior_path_planner/src/utils/utils.cpp | 56 ------- 6 files changed, 274 insertions(+), 139 deletions(-) create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/utils/traffic_light_utils.hpp create mode 100644 planning/behavior_path_planner/src/utils/traffic_light_utils.cpp diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index ee6e50f5a9553..e71d1dd9d86b4 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -30,6 +30,7 @@ ament_auto_add_library(behavior_path_planner_node SHARED src/turn_signal_decider.cpp src/utils/utils.cpp src/utils/path_utils.cpp + src/utils/traffic_light_utils.cpp src/utils/path_safety_checker/safety_check.cpp src/utils/path_safety_checker/objects_filtering.cpp src/utils/start_goal_planner_common/utils.cpp diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/traffic_light_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/traffic_light_utils.hpp new file mode 100644 index 0000000000000..aa3e7f4833134 --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/traffic_light_utils.hpp @@ -0,0 +1,112 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef BEHAVIOR_PATH_PLANNER__UTILS__TRAFFIC_LIGHT_UTILS_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__TRAFFIC_LIGHT_UTILS_HPP_ + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include + +namespace behavior_path_planner::utils::traffic_light +{ + +using autoware_perception_msgs::msg::TrafficSignal; +using autoware_perception_msgs::msg::TrafficSignalElement; +using geometry_msgs::msg::Pose; + +/** + * @brief Checks if a traffic light state includes a circle-shaped light with the specified color. + * + * Iterates through the traffic light elements to find a circle-shaped light that matches the given + * color. + * + * @param tl_state The traffic light state to check. + * @param lamp_color The color to look for in the traffic light's circle-shaped lamps. + * @return True if a circle-shaped light with the specified color is found, false otherwise. + */ +bool hasTrafficLightCircleColor(const TrafficSignal & tl_state, const uint8_t & lamp_color); + +/** + * @brief Checks if a traffic light state includes a light with the specified shape. + * + * Searches through the traffic light elements to find a light that matches the given shape. + * + * @param tl_state The traffic light state to check. + * @param shape The shape to look for in the traffic light's lights. + * @return True if a light with the specified shape is found, false otherwise. + */ +bool hasTrafficLightShape(const TrafficSignal & tl_state, const uint8_t & lamp_shape); + +/** + * @brief Determines if a traffic signal indicates a stop for the given lanelet. + * + * Evaluates the current state of the traffic light, considering if it's green or unknown, + * which would not necessitate a stop. Then, it checks the turn direction attribute of the lanelet + * against the traffic light's arrow shapes to determine whether a vehicle must stop or if it can + * proceed based on allowed turn directions. + * + * @param lanelet The lanelet to check for a stop signal at its traffic light. + * @param tl_state The current state of the traffic light associated with the lanelet. + * @return True if the traffic signal indicates a stop is required, false otherwise. + */ +bool isTrafficSignalStop(const lanelet::ConstLanelet & lanelet, const TrafficSignal & tl_state); + +/** + * @brief Computes the distance from the current position to the next traffic light along a set of + * lanelets. + * + * This function finds the closest lanelet to the current position and then searches for the + * next traffic light within the lanelet sequence. If a traffic light is found, it calculates + * the distance to the stop line of that traffic light from the current position. + * + * @param current_pose The current position of ego vehicle. + * @param lanelets A collection of lanelets representing the road ahead. + * @return The distance to the next traffic light's stop line or infinity if no traffic light is + * found ahead. + */ +double getDistanceToNextTrafficLight( + const Pose & current_pose, const lanelet::ConstLanelets & lanelets); + +/** + * @brief Calculates the signed distance from the ego vehicle to the stop line of the nearest red + * traffic light. + * + * Iterates over lanelets to find traffic lights. If a red traffic light is found, + * computes the distance to its stop line from the ego vehicle's position along a specified path. + * + * @param lanelets Collection of lanelets to inspect for traffic light regulatory elements. + * @param path The path along which the distance from ego current position to the stop line is + * measured. + * @param planner_data Shared pointer to planner data with vehicle odometry and traffic signal + * information. + * @return Optional double value with the signed distance to the stop line, or std::nullopt if no + * red traffic light is detected. + */ +std::optional calcDistanceToRedTrafficLight( + const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path, + const std::shared_ptr & planner_data); +} // namespace behavior_path_planner::utils::traffic_light + +#endif // BEHAVIOR_PATH_PLANNER__UTILS__TRAFFIC_LIGHT_UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index bfa83c43b061c..3f9591f6c2143 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -130,9 +130,6 @@ double l2Norm(const Vector3 vector); double getDistanceToEndOfLane(const Pose & current_pose, const lanelet::ConstLanelets & lanelets); -double getDistanceToNextTrafficLight( - const Pose & current_pose, const lanelet::ConstLanelets & lanelets); - double getDistanceToNextIntersection( const Pose & current_pose, const lanelet::ConstLanelets & lanelets); diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index f1bbb3bc28a36..ce4c9ad7ac72d 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -18,6 +18,7 @@ #include "behavior_path_planner/utils/avoidance/utils.hpp" #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/traffic_light_utils.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include @@ -49,6 +50,8 @@ namespace behavior_path_planner::utils::avoidance { using autoware_perception_msgs::msg::TrafficSignalElement; +using behavior_path_planner::utils::traffic_light::calcDistanceToRedTrafficLight; +using behavior_path_planner::utils::traffic_light::getDistanceToNextTrafficLight; using motion_utils::calcLongitudinalOffsetPoint; using motion_utils::calcSignedArcLength; using motion_utils::findNearestIndex; @@ -227,85 +230,6 @@ void pushUniqueVector(T & base_vector, const T & additional_vector) base_vector.insert(base_vector.end(), additional_vector.begin(), additional_vector.end()); } -bool hasTrafficLightCircleColor(const TrafficSignal & tl_state, const uint8_t & lamp_color) -{ - const auto it_lamp = - std::find_if(tl_state.elements.begin(), tl_state.elements.end(), [&lamp_color](const auto & x) { - return x.shape == TrafficSignalElement::CIRCLE && x.color == lamp_color; - }); - - return it_lamp != tl_state.elements.end(); -} - -bool hasTrafficLightShape(const TrafficSignal & tl_state, const uint8_t & lamp_shape) -{ - const auto it_lamp = std::find_if( - tl_state.elements.begin(), tl_state.elements.end(), - [&lamp_shape](const auto & x) { return x.shape == lamp_shape; }); - - return it_lamp != tl_state.elements.end(); -} - -bool isTrafficSignalStop(const lanelet::ConstLanelet & lanelet, const TrafficSignal & tl_state) -{ - if (hasTrafficLightCircleColor(tl_state, TrafficSignalElement::GREEN)) { - return false; - } - - if (hasTrafficLightCircleColor(tl_state, TrafficSignalElement::UNKNOWN)) { - return false; - } - - const std::string turn_direction = lanelet.attributeOr("turn_direction", "else"); - - if (turn_direction == "else") { - return true; - } - if ( - turn_direction == "right" && - hasTrafficLightShape(tl_state, TrafficSignalElement::RIGHT_ARROW)) { - return false; - } - if ( - turn_direction == "left" && hasTrafficLightShape(tl_state, TrafficSignalElement::LEFT_ARROW)) { - return false; - } - if ( - turn_direction == "straight" && - hasTrafficLightShape(tl_state, TrafficSignalElement::UP_ARROW)) { - return false; - } - - return true; -} - -std::optional calcDistanceToRedTrafficLight( - const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path, - const std::shared_ptr & planner_data) -{ - for (const auto & lanelet : lanelets) { - for (const auto & element : lanelet.regulatoryElementsAs()) { - const auto traffic_signal_stamped = planner_data->getTrafficSignal(element->id()); - if (!traffic_signal_stamped.has_value()) { - continue; - } - - if (!isTrafficSignalStop(lanelet, traffic_signal_stamped.value().signal)) { - continue; - } - - const auto & ego_pos = planner_data->self_odometry->pose.pose.position; - lanelet::ConstLineString3d stop_line = *(element->stopLine()); - const auto x = 0.5 * (stop_line.front().x() + stop_line.back().x()); - const auto y = 0.5 * (stop_line.front().y() + stop_line.back().y()); - const auto z = 0.5 * (stop_line.front().z() + stop_line.back().z()); - - return calcSignedArcLength(path.points, ego_pos, tier4_autoware_utils::createPoint(x, y, z)); - } - } - - return std::nullopt; -} } // namespace bool isOnRight(const ObjectData & obj) @@ -426,7 +350,7 @@ bool isForceAvoidanceTarget( bool not_parked_object = true; // check traffic light - const auto to_traffic_light = utils::getDistanceToNextTrafficLight(object_pose, extend_lanelets); + const auto to_traffic_light = getDistanceToNextTrafficLight(object_pose, extend_lanelets); { not_parked_object = to_traffic_light < parameters->object_ignore_section_traffic_light_in_front_distance; diff --git a/planning/behavior_path_planner/src/utils/traffic_light_utils.cpp b/planning/behavior_path_planner/src/utils/traffic_light_utils.cpp new file mode 100644 index 0000000000000..9ec8531818d83 --- /dev/null +++ b/planning/behavior_path_planner/src/utils/traffic_light_utils.cpp @@ -0,0 +1,157 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +namespace behavior_path_planner::utils::traffic_light +{ +using motion_utils::calcSignedArcLength; + +bool hasTrafficLightCircleColor(const TrafficSignal & tl_state, const uint8_t & lamp_color) +{ + const auto it_lamp = + std::find_if(tl_state.elements.begin(), tl_state.elements.end(), [&lamp_color](const auto & x) { + return x.shape == TrafficSignalElement::CIRCLE && x.color == lamp_color; + }); + + return it_lamp != tl_state.elements.end(); +} + +bool hasTrafficLightShape(const TrafficSignal & tl_state, const uint8_t & lamp_shape) +{ + const auto it_lamp = std::find_if( + tl_state.elements.begin(), tl_state.elements.end(), + [&lamp_shape](const auto & x) { return x.shape == lamp_shape; }); + + return it_lamp != tl_state.elements.end(); +} + +bool isTrafficSignalStop(const lanelet::ConstLanelet & lanelet, const TrafficSignal & tl_state) +{ + if (hasTrafficLightCircleColor(tl_state, TrafficSignalElement::GREEN)) { + return false; + } + + if (hasTrafficLightCircleColor(tl_state, TrafficSignalElement::UNKNOWN)) { + return false; + } + + const std::string turn_direction = lanelet.attributeOr("turn_direction", "else"); + + if (turn_direction == "else") { + return true; + } + if ( + turn_direction == "right" && + hasTrafficLightShape(tl_state, TrafficSignalElement::RIGHT_ARROW)) { + return false; + } + if ( + turn_direction == "left" && hasTrafficLightShape(tl_state, TrafficSignalElement::LEFT_ARROW)) { + return false; + } + if ( + turn_direction == "straight" && + hasTrafficLightShape(tl_state, TrafficSignalElement::UP_ARROW)) { + return false; + } + + return true; +} + +double getDistanceToNextTrafficLight( + const Pose & current_pose, const lanelet::ConstLanelets & lanelets) +{ + lanelet::ConstLanelet current_lanelet; + if (!lanelet::utils::query::getClosestLanelet(lanelets, current_pose, ¤t_lanelet)) { + return std::numeric_limits::infinity(); + } + + const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(current_pose.position); + const auto to_object = lanelet::geometry::toArcCoordinates( + lanelet::utils::to2D(current_lanelet.centerline()), + lanelet::utils::to2D(lanelet_point).basicPoint()); + + for (const auto & element : current_lanelet.regulatoryElementsAs()) { + lanelet::ConstLineString3d lanelet_stop_lines = element->stopLine().get(); + + const auto to_stop_line = lanelet::geometry::toArcCoordinates( + lanelet::utils::to2D(current_lanelet.centerline()), + lanelet::utils::to2D(lanelet_stop_lines).front().basicPoint()); + + const auto distance_object_to_stop_line = to_stop_line.length - to_object.length; + + if (distance_object_to_stop_line > 0.0) { + return distance_object_to_stop_line; + } + } + + double distance = lanelet::utils::getLaneletLength3d(current_lanelet); + + bool found_current_lane = false; + for (const auto & llt : lanelets) { + if (llt.id() == current_lanelet.id()) { + found_current_lane = true; + continue; + } + + if (!found_current_lane) { + continue; + } + + for (const auto & element : llt.regulatoryElementsAs()) { + lanelet::ConstLineString3d lanelet_stop_lines = element->stopLine().get(); + + const auto to_stop_line = lanelet::geometry::toArcCoordinates( + lanelet::utils::to2D(llt.centerline()), + lanelet::utils::to2D(lanelet_stop_lines).front().basicPoint()); + + return distance + to_stop_line.length - to_object.length; + } + + distance += lanelet::utils::getLaneletLength3d(llt); + } + + return std::numeric_limits::infinity(); +} + +std::optional calcDistanceToRedTrafficLight( + const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path, + const std::shared_ptr & planner_data) +{ + for (const auto & lanelet : lanelets) { + for (const auto & element : lanelet.regulatoryElementsAs()) { + const auto traffic_signal_stamped = planner_data->getTrafficSignal(element->id()); + if (!traffic_signal_stamped.has_value()) { + continue; + } + + if (!isTrafficSignalStop(lanelet, traffic_signal_stamped.value().signal)) { + continue; + } + + const auto & ego_pos = planner_data->self_odometry->pose.pose.position; + lanelet::ConstLineString3d stop_line = *(element->stopLine()); + const auto x = 0.5 * (stop_line.front().x() + stop_line.back().x()); + const auto y = 0.5 * (stop_line.front().y() + stop_line.back().y()); + const auto z = 0.5 * (stop_line.front().z() + stop_line.back().z()); + + return calcSignedArcLength(path.points, ego_pos, tier4_autoware_utils::createPoint(x, y, z)); + } + } + + return std::nullopt; +} +} // namespace behavior_path_planner::utils::traffic_light diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 37a628f54607e..93f74e670ecdd 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -2289,62 +2289,6 @@ double getDistanceToEndOfLane(const Pose & current_pose, const lanelet::ConstLan return lanelet_length - arc_coordinates.length; } -double getDistanceToNextTrafficLight( - const Pose & current_pose, const lanelet::ConstLanelets & lanelets) -{ - lanelet::ConstLanelet current_lanelet; - if (!lanelet::utils::query::getClosestLanelet(lanelets, current_pose, ¤t_lanelet)) { - return std::numeric_limits::infinity(); - } - - const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(current_pose.position); - const auto to_object = lanelet::geometry::toArcCoordinates( - lanelet::utils::to2D(current_lanelet.centerline()), - lanelet::utils::to2D(lanelet_point).basicPoint()); - - for (const auto & element : current_lanelet.regulatoryElementsAs()) { - lanelet::ConstLineString3d lanelet_stop_lines = element->stopLine().get(); - - const auto to_stop_line = lanelet::geometry::toArcCoordinates( - lanelet::utils::to2D(current_lanelet.centerline()), - lanelet::utils::to2D(lanelet_stop_lines).front().basicPoint()); - - const auto distance_object_to_stop_line = to_stop_line.length - to_object.length; - - if (distance_object_to_stop_line > 0.0) { - return distance_object_to_stop_line; - } - } - - double distance = lanelet::utils::getLaneletLength3d(current_lanelet); - - bool found_current_lane = false; - for (const auto & llt : lanelets) { - if (llt.id() == current_lanelet.id()) { - found_current_lane = true; - continue; - } - - if (!found_current_lane) { - continue; - } - - for (const auto & element : llt.regulatoryElementsAs()) { - lanelet::ConstLineString3d lanelet_stop_lines = element->stopLine().get(); - - const auto to_stop_line = lanelet::geometry::toArcCoordinates( - lanelet::utils::to2D(llt.centerline()), - lanelet::utils::to2D(lanelet_stop_lines).front().basicPoint()); - - return distance + to_stop_line.length - to_object.length; - } - - distance += lanelet::utils::getLaneletLength3d(llt); - } - - return std::numeric_limits::infinity(); -} - double getDistanceToNextIntersection( const Pose & current_pose, const lanelet::ConstLanelets & lanelets) {