refactor(lane_change): replace sstream to fmt for marker's text

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/debug.hpp

@@ -45,7 +45,7 @@ struct Debug
   double distance_to_end_of_current_lane{std::numeric_limits<double>::max()};
   double distance_to_lane_change_finished{std::numeric_limits<double>::max()};
   double distance_to_abort_finished{std::numeric_limits<double>::max()};
-  bool is_able_to_return_to_current_lane{false};
+  bool is_able_to_return_to_current_lane{true};
   bool is_stuck{false};
   bool is_abort{false};
 
@@ -69,7 +69,7 @@ struct Debug
     distance_to_end_of_current_lane = std::numeric_limits<double>::max();
     distance_to_lane_change_finished = std::numeric_limits<double>::max();
     distance_to_abort_finished = std::numeric_limits<double>::max();
-    is_able_to_return_to_current_lane = false;
+    is_able_to_return_to_current_lane = true;
     is_stuck = false;
     is_abort = false;
   }

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml

@@ -26,6 +26,7 @@
   <depend>autoware_motion_utils</depend>
   <depend>autoware_rtc_interface</depend>
   <depend>autoware_universe_utils</depend>
+  <depend>fmt</depend>
   <depend>pluginlib</depend>
   <depend>range-v3</depend>
   <depend>rclcpp</depend>

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp

@@ -25,10 +25,11 @@
 #include <visualization_msgs/msg/detail/marker__struct.hpp>
 #include <visualization_msgs/msg/detail/marker_array__struct.hpp>
 
+#include <fmt/format.h>
+
 #include <algorithm>
 #include <cstdint>
 #include <cstdlib>
-#include <sstream>
 #include <string>
 #include <vector>
 
@@ -52,27 +53,6 @@
   const auto loop_size = std::min(lane_change_paths.size(), colors.size());
   marker_array.markers.reserve(loop_size);
 
-  const auto info_prep_to_string =
-    [](const autoware::behavior_path_planner::lane_change::Info & info) -> std::string {
-    std::ostringstream ss_text;
-    ss_text << std::setprecision(3) << "vel: " << info.velocity.prepare
-            << "[m/s] | lon_acc: " << info.longitudinal_acceleration.prepare
-            << "[m/s2] | t: " << info.duration.prepare << "[s] | L: " << info.length.prepare
-            << "[m]";
-    return ss_text.str();
-  };
-
-  const auto info_lc_to_string =
-    [](const autoware::behavior_path_planner::lane_change::Info & info) -> std::string {
-    std::ostringstream ss_text;
-    ss_text << std::setprecision(3) << "vel: " << info.velocity.lane_changing
-            << "[m/s] | lon_acc: " << info.longitudinal_acceleration.lane_changing
-            << "[m/s2] | lat_acc: " << info.lateral_acceleration
-            << "[m/s2] | t: " << info.duration.lane_changing
-            << "[s] | L: " << info.length.lane_changing << "[m]";
-    return ss_text.str();
-  };
-
   for (std::size_t idx = 0; idx < loop_size; ++idx) {
     int32_t id{0};
     const auto & lc_path = lane_change_paths.at(idx);
@@ -91,19 +71,30 @@
       marker.points.push_back(point.point.pose.position);
     }
 
+    const auto & info = lc_path.info;
     auto text_marker = createDefaultMarker(
       "map", current_time, ns_with_idx, ++id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING,
       createMarkerScale(0.1, 0.1, 0.8), colors::yellow());
     const auto prep_idx = points.size() / 4;
     text_marker.pose = points.at(prep_idx).point.pose;
     text_marker.pose.position.z += 2.0;
-    text_marker.text = info_prep_to_string(lc_path.info);
+    text_marker.text = fmt::format(
+      "vel: {vel:.3f}[m/s] | lon_acc: {lon_acc:.3f}[m/s2] | t: {time:.3f}[s] | L: {length:.3f}[m]",
+      fmt::arg("vel", info.velocity.prepare),
+      fmt::arg("lon_acc", info.longitudinal_acceleration.prepare),
+      fmt::arg("time", info.duration.prepare), fmt::arg("length", info.length.prepare));
     marker_array.markers.push_back(text_marker);
 
     const auto lc_idx = points.size() / 2;
     text_marker.id = ++id;
     text_marker.pose = points.at(lc_idx).point.pose;
-    text_marker.text = info_lc_to_string(lc_path.info);
+    text_marker.text = fmt::format(
+      "vel: {vel:.3f}[m/s] | lon_acc: {lon_acc:.3f}[m/s2] | lat_acc: {lat_acc:.3f}[m/s2] | t: "
+      "{time:.3f}[s] | L: {length:.3f}[m]",
+      fmt::arg("vel", info.velocity.lane_changing),
+      fmt::arg("lon_acc", info.longitudinal_acceleration.lane_changing),
+      fmt::arg("lat_acc", info.lateral_acceleration), fmt::arg("time", info.duration.lane_changing),
+      fmt::arg("length", info.length.lane_changing));
     marker_array.markers.push_back(text_marker);
 
     marker_array.markers.push_back(marker);
@@ -186,17 +177,10 @@
   safety_check_info_text.pose = ego_pose;
   safety_check_info_text.pose.position.z += 4.0;
 
-  std::ostringstream ss;
-
-  ss << "\nDistToEndOfCurrentLane: " << std::setprecision(5)
-     << debug_data.distance_to_end_of_current_lane
-     << "\nDistToLaneChangeFinished: " << debug_data.distance_to_lane_change_finished
-     << (debug_data.is_stuck ? "\nVehicleStuck" : "")
-     << (debug_data.is_able_to_return_to_current_lane ? "\nAbleToReturnToCurrentLane" : "")
-     << (debug_data.is_abort ? "\nAborting" : "")
-     << "\nDistanceToAbortFinished: " << debug_data.distance_to_abort_finished;
-
-  safety_check_info_text.text = ss.str();
+  safety_check_info_text.text = fmt::format(
+    "{stuck} | {return_lane} | {abort}", fmt::arg("stuck", debug_data.is_stuck ? "is stuck" : ""),
+    fmt::arg("return_lane", debug_data.is_able_to_return_to_current_lane ? "" : "can't return"),
+    fmt::arg("abort", debug_data.is_abort ? "aborting" : ""));
   marker_array.markers.push_back(safety_check_info_text);
   return marker_array;
 }