From 8049d2e84da0097467316112b4ac30e1fadd3848 Mon Sep 17 00:00:00 2001
From: Yuki Takagi <yuki.takagi@tier4.jp>
Date: Wed, 31 Jan 2024 21:19:34 +0900
Subject: [PATCH] refactor

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>
---
 .../config/map_based_prediction.param.yaml    |  1 +
 .../map_based_prediction_node.hpp             |  3 ++
 .../src/map_based_prediction_node.cpp         | 36 ++++++++++++-------
 3 files changed, 28 insertions(+), 12 deletions(-)

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<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);
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<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>