diff --git a/perception/map_based_prediction/config/map_based_prediction.param.yaml b/perception/map_based_prediction/config/map_based_prediction.param.yaml index fdd64174de9be..ed1193f1c49a0 100644 --- a/perception/map_based_prediction/config/map_based_prediction.param.yaml +++ b/perception/map_based_prediction/config/map_based_prediction.param.yaml @@ -19,6 +19,7 @@ use_vehicle_acceleration: false # whether to consider current vehicle acceleration when predicting paths or not speed_limit_multiplier: 1.5 # When using vehicle acceleration. Set vehicle's maximum predicted speed as the legal speed limit in that lanelet times this value acceleration_exponential_half_life: 2.5 # [s] When using vehicle acceleration. The decaying acceleration model considers that the current vehicle acceleration will be halved after this many seconds + use_crosswalk_signal: true # parameter for shoulder lane prediction prediction_time_horizon_rate_for_validate_shoulder_lane_length: 0.8 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..2864d1c5bf393 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( @@ -181,11 +189,14 @@ class MapBasedPredictionNode : public rclcpp::Node double speed_limit_multiplier_; double acceleration_exponential_half_life_; + bool use_crosswalk_signal_; + // Stop watch StopWatch stop_watch_; // 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 +260,8 @@ 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); + std::optional getTrafficSignalElement(const lanelet::Id & id); 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..168b874ccbcce 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -788,6 +788,8 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ acceleration_exponential_half_life_ = declare_parameter("acceleration_exponential_half_life"); + use_crosswalk_signal_ = declare_parameter("use_crosswalk_signal"); + path_generator_ = std::make_shared( prediction_time_horizon_, lateral_control_time_horizon_, prediction_sampling_time_interval_, min_crosswalk_user_velocity_); @@ -801,6 +803,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 +877,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 +1231,18 @@ 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() && use_crosswalk_signal_) { + const auto signal_color = [&] { + const auto elem_opt = getTrafficSignalElement(crosswalk_signal_id_opt.value()); + return elem_opt ? elem_opt.value().color : TrafficSignalElement::UNKNOWN; + }(); + + if (signal_color == TrafficSignalElement::RED) { + continue; + } + } + const auto edge_points = getCrosswalkEdgePoints(crosswalk); const auto reachable_first = hasPotentialToReach( @@ -2211,6 +2236,38 @@ 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 traffic_light_reg_elems.front()->id(); +} + +std::optional MapBasedPredictionNode::getTrafficSignalElement( + const lanelet::Id & id) +{ + if (traffic_signal_id_map_.count(id) != 0) { + const auto & signal_elements = traffic_signal_id_map_.at(id).elements; + if (signal_elements.size() > 1) { + RCLCPP_ERROR( + get_logger(), "[Map Based Prediction]: Multiple TrafficSignalElement_ are received."); + } else if (!signal_elements.empty()) { + return signal_elements.front(); + } + } + return std::nullopt; +} + } // namespace map_based_prediction #include