From 8781d1d783a65a4b8ba2a85d552384f7f85a8214 Mon Sep 17 00:00:00 2001
From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com>
Date: Wed, 20 Dec 2023 18:09:09 +0900
Subject: [PATCH 1/2] refactor(traffic_light_module): boost::optional to
 std::optional (#5912)

Signed-off-by: Muhammad Zulfaqar <zulfaqar.azmi@tier4.jp>
---
 .../behavior_velocity_traffic_light_module/package.xml     | 1 -
 .../behavior_velocity_traffic_light_module/src/debug.cpp   | 2 --
 .../behavior_velocity_traffic_light_module/src/manager.cpp | 1 -
 .../behavior_velocity_traffic_light_module/src/manager.hpp | 3 ++-
 .../behavior_velocity_traffic_light_module/src/scene.cpp   | 7 ++-----
 .../behavior_velocity_traffic_light_module/src/scene.hpp   | 7 +++----
 6 files changed, 7 insertions(+), 14 deletions(-)

diff --git a/planning/behavior_velocity_traffic_light_module/package.xml b/planning/behavior_velocity_traffic_light_module/package.xml
index c658f0890b986..39c454e59dcb2 100644
--- a/planning/behavior_velocity_traffic_light_module/package.xml
+++ b/planning/behavior_velocity_traffic_light_module/package.xml
@@ -24,7 +24,6 @@
   <depend>eigen</depend>
   <depend>geometry_msgs</depend>
   <depend>lanelet2_extension</depend>
-  <depend>libboost-dev</depend>
   <depend>motion_utils</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
diff --git a/planning/behavior_velocity_traffic_light_module/src/debug.cpp b/planning/behavior_velocity_traffic_light_module/src/debug.cpp
index 9a0ba4f37c3c0..b3dedaa29d6ad 100644
--- a/planning/behavior_velocity_traffic_light_module/src/debug.cpp
+++ b/planning/behavior_velocity_traffic_light_module/src/debug.cpp
@@ -26,8 +26,6 @@
 
 namespace behavior_velocity_planner
 {
-using tier4_autoware_utils::appendMarkerArray;
-
 visualization_msgs::msg::MarkerArray TrafficLightModule::createDebugMarkerArray()
 {
   visualization_msgs::msg::MarkerArray debug_marker_array;
diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp
index 908627540bdcc..fa1a516c107b0 100644
--- a/planning/behavior_velocity_traffic_light_module/src/manager.cpp
+++ b/planning/behavior_velocity_traffic_light_module/src/manager.cpp
@@ -23,7 +23,6 @@
 #include <memory>
 #include <set>
 #include <string>
-#include <unordered_map>
 #include <utility>
 namespace behavior_velocity_planner
 {
diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.hpp b/planning/behavior_velocity_traffic_light_module/src/manager.hpp
index c36c6af1128ce..87213d8a5ed3f 100644
--- a/planning/behavior_velocity_traffic_light_module/src/manager.hpp
+++ b/planning/behavior_velocity_traffic_light_module/src/manager.hpp
@@ -26,6 +26,7 @@
 
 #include <functional>
 #include <memory>
+#include <optional>
 
 namespace behavior_velocity_planner
 {
@@ -57,7 +58,7 @@ class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC
   // Debug
   rclcpp::Publisher<autoware_perception_msgs::msg::TrafficSignal>::SharedPtr pub_tl_state_;
 
-  boost::optional<int> first_ref_stop_path_point_index_;
+  std::optional<int> first_ref_stop_path_point_index_;
 };
 
 class TrafficLightModulePlugin : public PluginWrapper<TrafficLightModuleManager>
diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp
index a74ff8c0cb5e8..d2d18d74a5c1a 100644
--- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp
+++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp
@@ -19,7 +19,6 @@
 
 #include <boost/geometry/algorithms/distance.hpp>
 #include <boost/geometry/algorithms/intersection.hpp>
-#include <boost/optional.hpp>  // To be replaced by std::optional in C++17
 
 #include <tf2/utils.h>
 
@@ -31,9 +30,7 @@
 
 #include <algorithm>
 #include <limits>
-#include <map>
 #include <string>
-#include <utility>
 #include <vector>
 
 namespace behavior_velocity_planner
@@ -52,14 +49,14 @@ bool getBackwardPointFromBasePoint(
   return true;
 }
 
-boost::optional<Point2d> findNearestCollisionPoint(
+std::optional<Point2d> findNearestCollisionPoint(
   const LineString2d & line1, const LineString2d & line2, const Point2d & origin)
 {
   std::vector<Point2d> collision_points;
   bg::intersection(line1, line2, collision_points);
 
   if (collision_points.empty()) {
-    return boost::none;
+    return std::nullopt;
   }
 
   // check nearest collision point
diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_traffic_light_module/src/scene.hpp
index 6e46709267427..c7b2472289bf5 100644
--- a/planning/behavior_velocity_traffic_light_module/src/scene.hpp
+++ b/planning/behavior_velocity_traffic_light_module/src/scene.hpp
@@ -17,7 +17,6 @@
 
 #include <memory>
 #include <optional>
-#include <string>
 #include <tuple>
 #include <vector>
 
@@ -81,7 +80,7 @@ class TrafficLightModule : public SceneModuleInterface
 
   inline State getTrafficLightModuleState() const { return state_; }
 
-  inline boost::optional<int> getFirstRefStopPathPointIndex() const
+  inline std::optional<int> getFirstRefStopPathPointIndex() const
   {
     return first_ref_stop_path_point_index_;
   }
@@ -130,9 +129,9 @@ class TrafficLightModule : public SceneModuleInterface
   // prevent stop chattering
   std::unique_ptr<Time> stop_signal_received_time_ptr_{};
 
-  boost::optional<int> first_ref_stop_path_point_index_;
+  std::optional<int> first_ref_stop_path_point_index_;
 
-  boost::optional<Time> traffic_signal_stamp_;
+  std::optional<Time> traffic_signal_stamp_;
 
   // Traffic Light State
   TrafficSignal looking_tl_state_;

From bca290d1d89b0638a8a2163c4107a11b5e77c3c6 Mon Sep 17 00:00:00 2001
From: Fumiya Watanabe <rej55.g@gmail.com>
Date: Wed, 20 Dec 2023 18:10:51 +0900
Subject: [PATCH 2/2] fix(behavior_path_planner): publish steering factor from
 scene modules (#5914)

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>
---
 .../include/behavior_path_planner/planner_manager.hpp           | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp
index 7ceafb6f428de..3ccc92d04025f 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp
@@ -286,6 +286,8 @@ class PlannerManager
 
     module_ptr->publishRTCStatus();
 
+    module_ptr->publishSteeringFactor();
+
     module_ptr->publishObjectsOfInterestMarker();
 
     processing_time_.at(module_ptr->name()) += stop_watch_.toc(module_ptr->name(), true);