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 72489c79d5a3a..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 @@ -189,6 +189,8 @@ class MapBasedPredictionNode : public rclcpp::Node double speed_limit_multiplier_; double acceleration_exponential_half_life_; + bool use_crosswalk_signal_; + // Stop watch StopWatch stop_watch_; @@ -259,6 +261,7 @@ class MapBasedPredictionNode : public rclcpp::Node 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/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index fa86e063279a0..2ddc55e933915 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_); @@ -1230,18 +1232,13 @@ 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 received."); - } else if ( - !signal_elements.empty() && signal_elements.front().color == TrafficSignalElement::RED) { - continue; - } + if (crosswalk_signal_id_opt.has_value() && use_crosswalk_signal_) { + const auto signal_element_opt = getTrafficSignalElement(crosswalk_signal_id_opt.value()); + const auto signal_color = + signal_element_opt ? signal_element_opt.value().color : TrafficSignalElement::UNKNOWN; + + if (signal_color == TrafficSignalElement::RED) { + continue; } } @@ -2255,6 +2252,21 @@ std::optional MapBasedPredictionNode::getTrafficSignalId( 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