fix(static_centerline_generator): several bug fixes (#9426)

* fix: dependent packages

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* feat: use steer angle, use warn for steer angle failure, calc curvature dicontinuously

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix cppcheck

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

---------

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

Author: takayuki5168

common/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info.hpp

@@ -71,6 +71,7 @@ struct VehicleInfo
 
   double calcMaxCurvature() const;
   double calcCurvatureFromSteerAngle(const double steer_angle) const;
+  double calcSteerAngleFromCurvature(const double curvature) const;
 };
 
 /// Create vehicle info from base parameters

common/autoware_vehicle_info_utils/src/vehicle_info.cpp

@@ -129,4 +129,15 @@ double VehicleInfo::calcCurvatureFromSteerAngle(const double steer_angle) const
   const double curvature = 1.0 / radius;
   return curvature;
 }
+
+double VehicleInfo::calcSteerAngleFromCurvature(const double curvature) const
+{
+  if (std::abs(curvature) < 1e-6) {
+    return 0.0;
+  }
+
+  const double radius = 1.0 / curvature;
+  const double steer_angle = std::atan2(wheel_base_m, radius);
+  return steer_angle;
+}
 }  // namespace autoware::vehicle_info_utils

planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml

@@ -48,7 +48,7 @@
   </node>
 
   <!-- visualize map -->
-  <node pkg="map_loader" exec="lanelet2_map_visualization" name="lanelet2_map_visualization">
+  <node pkg="autoware_lanelet2_map_visualizer" exec="lanelet2_map_visualization" name="lanelet2_map_visualization">
     <remap from="input/lanelet2_map" to="$(var lanelet2_map_topic)"/>
     <remap from="output/lanelet2_map_marker" to="$(var lanelet2_map_marker_topic)"/>
   </node>

planning/autoware_static_centerline_generator/package.xml

@@ -20,9 +20,11 @@
   <depend>autoware_global_parameter_loader</depend>
   <depend>autoware_interpolation</depend>
   <depend>autoware_lanelet2_extension</depend>
+  <depend>autoware_lanelet2_map_visualizer</depend>
   <depend>autoware_map_loader</depend>
   <depend>autoware_map_msgs</depend>
   <depend>autoware_map_projection_loader</depend>
+  <depend>autoware_map_tf_generator</depend>
   <depend>autoware_mission_planner</depend>
   <depend>autoware_motion_utils</depend>
   <depend>autoware_osqp_interface</depend>

planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp

@@ -14,13 +14,14 @@
 
 #include "static_centerline_generator_node.hpp"
 
-#include "autoware/interpolation/spline_interpolation_points_2d.hpp"
 #include "autoware/map_loader/lanelet2_map_loader_node.hpp"
 #include "autoware/map_projection_loader/load_info_from_lanelet2_map.hpp"
 #include "autoware/map_projection_loader/map_projection_loader.hpp"
 #include "autoware/motion_utils/resample/resample.hpp"
 #include "autoware/motion_utils/trajectory/conversion.hpp"
+#include "autoware/motion_utils/trajectory/trajectory.hpp"
 #include "autoware/universe_utils/geometry/geometry.hpp"
+#include "autoware/universe_utils/math/unit_conversion.hpp"
 #include "autoware/universe_utils/ros/parameter.hpp"
 #include "autoware_lanelet2_extension/utility/message_conversion.hpp"
 #include "autoware_lanelet2_extension/utility/query.hpp"
@@ -59,6 +60,7 @@
 
 #define RESET_TEXT "\x1B[0m"
 #define RED_TEXT "\x1B[31m"
+#define YELLOW_TEXT "\x1b[33m"
 #define BOLD_TEXT "\x1B[1m"
 
 namespace autoware::static_centerline_generator
@@ -626,10 +628,8 @@ void StaticCenterlineGeneratorNode::validate()
   }
 
   // calculate curvature
-  autoware::interpolation::SplineInterpolationPoints2d centerline_spline(centerline);
-  const auto curvature_vec = centerline_spline.getSplineInterpolatedCurvatures();
-  const double curvature_threshold = vehicle_info_.calcCurvatureFromSteerAngle(
-    vehicle_info_.max_steer_angle_rad - max_steer_angle_margin);
+  const auto curvature_vec = autoware::motion_utils::calcCurvature(centerline);
+  const double steer_angle_threshold = vehicle_info_.max_steer_angle_rad - max_steer_angle_margin;
 
   // calculate the distance between footprint and right/left bounds
   MarkerArray marker_array;
@@ -676,6 +676,7 @@ void StaticCenterlineGeneratorNode::validate()
       max_curvature = std::abs(curvature);
     }
   }
+  const double max_steer_angle = vehicle_info_.calcSteerAngleFromCurvature(max_curvature);
 
   // publish road boundaries
   const auto left_bound = convertToGeometryPoints(lanelet_left_bound);
@@ -693,30 +694,36 @@ void StaticCenterlineGeneratorNode::validate()
     std::cerr << "1. Footprints inside Lanelets:" << std::endl;
     if (dist_thresh_to_road_border < min_dist) {
       std::cerr << "  The generated centerline is inside the lanelet. (threshold:"
-                << dist_thresh_to_road_border << " < actual:" << min_dist << ")" << std::endl
+                << dist_thresh_to_road_border << "[m] < actual:" << min_dist << "[m])" << std::endl
                 << "  Passed." << std::endl;
       return true;
     }
     std::cerr << RED_TEXT
               << " The generated centerline is outside the lanelet. (actual:" << min_dist
-              << " <= threshold:" << dist_thresh_to_road_border << ")" << std::endl
+              << "[m] <= threshold:" << dist_thresh_to_road_border << "[m])" << std::endl
               << "  Failed." << RESET_TEXT << std::endl;
     return false;
   }();
   // 2. centerline's curvature
-  const bool is_curvature_low = [&]() {
-    std::cerr << "2. Curvature:" << std::endl;
-    if (max_curvature < curvature_threshold) {
-      std::cerr << "  The generated centerline has no high curvature. (actual:" << max_curvature
-                << " < threshold:" << curvature_threshold << ")"
-                << "  Passed." << std::endl;
-      return true;
-    }
-    std::cerr << RED_TEXT << "  The generated centerline has a too high curvature. (threshold:"
-              << curvature_threshold << " <= actual:" << max_curvature << ")"
-              << "  Failed." << RESET_TEXT << std::endl;
-    return false;
-  }();
+  std::cerr << "2. Curvature:" << std::endl;
+  const bool is_curvature_low =
+    true;  // always tre for now since the curvature is just estimated and not enough precise.
+  if (max_steer_angle < steer_angle_threshold) {
+    std::cerr << "  The generated centerline has no high steer angle. (estimated:"
+              << autoware::universe_utils::rad2deg(max_steer_angle)
+              << "[deg] < threshold:" << autoware::universe_utils::rad2deg(steer_angle_threshold)
+              << "[deg])" << std::endl
+              << "  Passed." << std::endl;
+  } else {
+    std::cerr << YELLOW_TEXT << "  The generated centerline has a too high steer angle. (threshold:"
+              << autoware::universe_utils::rad2deg(steer_angle_threshold)
+              << "[deg] <= estimated:" << autoware::universe_utils::rad2deg(max_steer_angle)
+              << "[deg])" << std::endl
+              << "  However, the estimated steer angle is not enough precise, so the result is "
+                 "conditional pass."
+              << std::endl
+              << "  Conditionally Passed." << RESET_TEXT << std::endl;
+  }
   // 3. result
   std::cerr << std::endl << BOLD_TEXT << "Result:" << RESET_TEXT << std::endl;
   if (are_footprints_inside_lanelets && is_curvature_low) {