diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index f61dc75ffb85b..61ec1ef7c2984 100644 --- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -29,6 +29,7 @@ #include #include #include +#include #include #include #include @@ -42,6 +43,7 @@ #include #include #include +#include #include #include #include @@ -107,6 +109,9 @@ using autoware_auto_perception_msgs::msg::TrackedObject; using autoware_auto_perception_msgs::msg::TrackedObjectKinematics; using autoware_auto_perception_msgs::msg::TrackedObjects; using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_perception_msgs::msg::TrafficSignal; +using autoware_perception_msgs::msg::TrafficSignalArray; +using autoware_perception_msgs::msg::TrafficSignalElement; using tier4_autoware_utils::StopWatch; using tier4_debug_msgs::msg::StringStamped; using TrajectoryPoints = std::vector; @@ -122,6 +127,7 @@ class MapBasedPredictionNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_calculation_time_; rclcpp::Subscription::SharedPtr sub_objects_; rclcpp::Subscription::SharedPtr sub_map_; + rclcpp::Subscription::SharedPtr sub_traffic_signals_; // Object History std::unordered_map> objects_history_; @@ -131,6 +137,8 @@ class MapBasedPredictionNode : public rclcpp::Node std::shared_ptr routing_graph_ptr_; std::shared_ptr traffic_rules_ptr_; + std::unordered_map traffic_signal_id_map; + // parameter update OnSetParametersCallbackHandle::SharedPtr set_param_res_; rcl_interfaces::msg::SetParametersResult onParam( @@ -186,6 +194,7 @@ class MapBasedPredictionNode : public rclcpp::Node // Member Functions void mapCallback(const HADMapBin::ConstSharedPtr msg); + void trafficSignalsCallback(const TrafficSignalArray::ConstSharedPtr msg); void objectsCallback(const TrackedObjects::ConstSharedPtr in_objects); bool doesPathCrossAnyFence(const PredictedPath & predicted_path); @@ -249,6 +258,7 @@ class MapBasedPredictionNode : public rclcpp::Node const LaneletsData & lanelets_data); bool isDuplicated( const PredictedPath & predicted_path, const std::vector & predicted_paths); + std::optional getTrafficSignalId(const lanelet::ConstLanelet & way_lanelet); visualization_msgs::msg::Marker getDebugMarker( const TrackedObject & object, const Maneuver & maneuver, const size_t obj_num); diff --git a/perception/map_based_prediction/launch/map_based_prediction.launch.xml b/perception/map_based_prediction/launch/map_based_prediction.launch.xml index 619c0c9507e0b..7f81971b2f8d7 100644 --- a/perception/map_based_prediction/launch/map_based_prediction.launch.xml +++ b/perception/map_based_prediction/launch/map_based_prediction.launch.xml @@ -3,12 +3,14 @@ + + diff --git a/perception/map_based_prediction/package.xml b/perception/map_based_prediction/package.xml index b07d9855f9821..8efea9efa1844 100644 --- a/perception/map_based_prediction/package.xml +++ b/perception/map_based_prediction/package.xml @@ -16,6 +16,7 @@ autoware_cmake autoware_auto_perception_msgs + autoware_perception_msgs interpolation lanelet2_extension libgoogle-glog-dev diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index c949eae21aeff..a6c27d47633d9 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -801,6 +801,9 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ sub_map_ = this->create_subscription( "/vector_map", rclcpp::QoS{1}.transient_local(), std::bind(&MapBasedPredictionNode::mapCallback, this, std::placeholders::_1)); + sub_traffic_signals_ = this->create_subscription( + "/traffic_signals", 1, + std::bind(&MapBasedPredictionNode::trafficSignalsCallback, this, std::placeholders::_1)); pub_objects_ = this->create_publisher("~/output/objects", rclcpp::QoS{1}); pub_debug_markers_ = @@ -872,6 +875,14 @@ void MapBasedPredictionNode::mapCallback(const HADMapBin::ConstSharedPtr msg) crosswalks_.insert(crosswalks_.end(), walkways.begin(), walkways.end()); } +void MapBasedPredictionNode::trafficSignalsCallback(const TrafficSignalArray::ConstSharedPtr msg) +{ + traffic_signal_id_map.clear(); + for (const auto & signal : msg->signals) { + traffic_signal_id_map[signal.traffic_signal_id] = signal; + } +} + void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPtr in_objects) { stop_watch_.tic(); @@ -1218,6 +1229,23 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( } // try to find the edge points for all crosswalks and generate path to the crosswalk edge for (const auto & crosswalk : crosswalks_) { + const auto crosswalk_signal_id_opt = getTrafficSignalId(crosswalk); + if (crosswalk_signal_id_opt.has_value()) { + // If the crosswalk has traffic light, do something. + if (traffic_signal_id_map.count(crosswalk_signal_id_opt.value()) != 0) { + const auto & signal_elements = + traffic_signal_id_map.at(crosswalk_signal_id_opt.value()).elements; + if (signal_elements.size() > 1) { + RCLCPP_ERROR( + get_logger(), "[Map Based Prediction]: Multiple TrafficSignalElement_ are recieved."); + } else if ( + !signal_elements.empty() && signal_elements.front().color == TrafficSignalElement::RED) { + continue; + } + } + // TODO(yuki takagi): If a flag is setted, wait for timeout while the pedestrian is stopping. + } + const auto edge_points = getCrosswalkEdgePoints(crosswalk); const auto reachable_first = hasPotentialToReach( @@ -2211,6 +2239,24 @@ bool MapBasedPredictionNode::isDuplicated( return false; } + +std::optional MapBasedPredictionNode::getTrafficSignalId( + const lanelet::ConstLanelet & way_lanelet) +{ + const auto traffic_light_reg_elems = + way_lanelet.regulatoryElementsAs(); + if (traffic_light_reg_elems.empty()) { + return std::nullopt; + } else if (traffic_light_reg_elems.size() > 1) { + RCLCPP_ERROR( + get_logger(), + "[Map Based Prediction]: " + "Multiple regulatory elements as TrafficLight are defined to one lanelet object."); + return std::nullopt; // Is it appropriate? + } + return traffic_light_reg_elems.front()->id(); +} + } // namespace map_based_prediction #include