refactor

Signed-off-by: Yuki Takagi <[email protected]>

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
 

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<std::chrono::milliseconds> stop_watch_;
 
@@ -259,6 +261,7 @@ class MapBasedPredictionNode : public rclcpp::Node
   bool isDuplicated(
     const PredictedPath & predicted_path, const std::vector<PredictedPath> & predicted_paths);
   std::optional<lanelet::Id> getTrafficSignalId(const lanelet::ConstLanelet & way_lanelet);
+  std::optional<TrafficSignalElement> getTrafficSignalElement(const lanelet::Id & id);
 
   visualization_msgs::msg::Marker getDebugMarker(
     const TrackedObject & object, const Maneuver & maneuver, const size_t obj_num);

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<double>("acceleration_exponential_half_life");
 
+  use_crosswalk_signal_ = declare_parameter<bool>("use_crosswalk_signal");
+
   path_generator_ = std::make_shared<PathGenerator>(
     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<lanelet::Id> MapBasedPredictionNode::getTrafficSignalId(
   return traffic_light_reg_elems.front()->id();
 }
 
+std::optional<TrafficSignalElement> 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 <rclcpp_components/register_node_macro.hpp>