diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py index 62d4c5b7188ee..f535fe1184d74 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py @@ -84,6 +84,10 @@ def launch_setup(context, *args, **kwargs): "~/input/costmap", "/planning/scenario_planning/parking/costmap_generator/occupancy_grid", ), + ( + "~/input/traffic_signals", + "/perception/traffic_light_recognition/traffic_signals", + ), ("~/input/odometry", "/localization/kinematic_state"), ("~/input/accel", "/localization/acceleration"), ("~/input/scenario", "/planning/scenario_planning/scenario"), diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index 4ff862c7852c6..8ffa682030a99 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -8,6 +8,7 @@ + diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml index d333cbfd778cb..b4d323de45cde 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -2,6 +2,7 @@ ros__parameters: verbose: false max_iteration_num: 100 + traffic_light_signal_timeout: 1.0 planning_hz: 10.0 backward_path_length: 5.0 forward_path_length: 300.0 diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 420a5a8aa6ee5..1a9b23be00a3f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -58,6 +58,7 @@ using autoware_auto_planning_msgs::msg::Path; using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; +using autoware_perception_msgs::msg::TrafficSignalArray; using autoware_planning_msgs::msg::LaneletRoute; using autoware_planning_msgs::msg::PoseWithUuidStamped; using nav_msgs::msg::OccupancyGrid; @@ -93,6 +94,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node rclcpp::Subscription::SharedPtr perception_subscriber_; rclcpp::Subscription::SharedPtr occupancy_grid_subscriber_; rclcpp::Subscription::SharedPtr costmap_subscriber_; + rclcpp::Subscription::SharedPtr traffic_signals_subscriber_; rclcpp::Subscription::SharedPtr lateral_offset_subscriber_; rclcpp::Subscription::SharedPtr operation_mode_subscriber_; rclcpp::Publisher::SharedPtr path_publisher_; @@ -136,6 +138,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node void onPerception(const PredictedObjects::ConstSharedPtr msg); void onOccupancyGrid(const OccupancyGrid::ConstSharedPtr msg); void onCostMap(const OccupancyGrid::ConstSharedPtr msg); + void onTrafficSignals(const TrafficSignalArray::ConstSharedPtr msg); void onMap(const HADMapBin::ConstSharedPtr map_msg); void onRoute(const LaneletRoute::ConstSharedPtr route_msg); void onOperationMode(const OperationModeState::ConstSharedPtr msg); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp index 9280a81e643ca..ca58144464731 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp @@ -20,6 +20,7 @@ #include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" #include "motion_utils/trajectory/trajectory.hpp" +#include #include #include @@ -28,6 +29,7 @@ #include #include #include +#include #include #include #include @@ -39,6 +41,7 @@ #include #include +#include #include #include #include @@ -51,6 +54,7 @@ using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; +using autoware_perception_msgs::msg::TrafficSignal; using autoware_planning_msgs::msg::PoseWithUuidStamped; using geometry_msgs::msg::AccelWithCovarianceStamped; using geometry_msgs::msg::PoseStamped; @@ -59,8 +63,15 @@ using nav_msgs::msg::Odometry; using route_handler::RouteHandler; using tier4_planning_msgs::msg::LateralOffset; using PlanResult = PathWithLaneId::SharedPtr; +using lanelet::TrafficLight; using unique_identifier_msgs::msg::UUID; +struct TrafficSignalStamped +{ + builtin_interfaces::msg::Time stamp; + TrafficSignal signal; +}; + struct BoolStamped { explicit BoolStamped(bool in_data) : data(in_data) {} @@ -145,6 +156,7 @@ struct PlannerData std::optional prev_modified_goal{}; std::optional prev_route_id{}; std::shared_ptr route_handler{std::make_shared()}; + std::map traffic_light_id_map; BehaviorPathPlannerParameters parameters{}; drivable_area_expansion::DrivableAreaExpansionParameters drivable_area_expansion_parameters{}; @@ -162,6 +174,21 @@ struct PlannerData route_handler, path, turn_signal_info, current_pose, current_vel, parameters, debug_data); } + std::optional getTrafficSignal(const int id) const + { + if (traffic_light_id_map.count(id) == 0) { + return std::nullopt; + } + + const auto elapsed_time = + (rclcpp::Clock{RCL_ROS_TIME}.now() - traffic_light_id_map.at(id).stamp).seconds(); + if (elapsed_time > parameters.traffic_light_signal_timeout) { + return std::nullopt; + } + + return traffic_light_id_map.at(id); + } + template size_t findEgoIndex(const std::vector & points) const { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index 5dceeeff3aada..1998e7b99dd65 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -77,6 +77,7 @@ struct BehaviorPathPlannerParameters { bool verbose; size_t max_iteration_num{100}; + double traffic_light_signal_timeout{1.0}; ModuleConfigParameters config_avoidance; ModuleConfigParameters config_avoidance_by_lc; diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml index 24ee1056e5a47..95ad5f1efb9ac 100644 --- a/planning/behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/package.xml @@ -46,6 +46,7 @@ autoware_auto_planning_msgs autoware_auto_tf2 autoware_auto_vehicle_msgs + autoware_perception_msgs behaviortree_cpp_v3 freespace_planning_algorithms geometry_msgs diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index bd46bf069e846..ef44bee663985 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -106,6 +106,10 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod costmap_subscriber_ = create_subscription( "~/input/costmap", 1, std::bind(&BehaviorPathPlannerNode::onCostMap, this, _1), createSubscriptionOptions(this)); + traffic_signals_subscriber_ = + this->create_subscription( + "~/input/traffic_signals", 1, std::bind(&BehaviorPathPlannerNode::onTrafficSignals, this, _1), + createSubscriptionOptions(this)); lateral_offset_subscriber_ = this->create_subscription( "~/input/lateral_offset", 1, std::bind(&BehaviorPathPlannerNode::onLateralOffset, this, _1), createSubscriptionOptions(this)); @@ -277,6 +281,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.verbose = declare_parameter("verbose"); p.max_iteration_num = declare_parameter("max_iteration_num"); + p.traffic_light_signal_timeout = declare_parameter("traffic_light_signal_timeout"); const auto get_scene_module_manager_param = [&](std::string && ns) { ModuleConfigParameters config; @@ -934,6 +939,17 @@ void BehaviorPathPlannerNode::onCostMap(const OccupancyGrid::ConstSharedPtr msg) const std::lock_guard lock(mutex_pd_); planner_data_->costmap = msg; } +void BehaviorPathPlannerNode::onTrafficSignals(const TrafficSignalArray::ConstSharedPtr msg) +{ + std::lock_guard lock(mutex_pd_); + + for (const auto & signal : msg->signals) { + TrafficSignalStamped traffic_signal; + traffic_signal.stamp = msg->stamp; + traffic_signal.signal = signal; + planner_data_->traffic_light_id_map[signal.traffic_signal_id] = traffic_signal; + } +} void BehaviorPathPlannerNode::onMap(const HADMapBin::ConstSharedPtr msg) { const std::lock_guard lock(mutex_map_);