diff --git a/common/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info.hpp b/common/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info.hpp index 0e783cd006986..51c63418acc29 100644 --- a/common/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info.hpp +++ b/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 diff --git a/common/autoware_vehicle_info_utils/src/vehicle_info.cpp b/common/autoware_vehicle_info_utils/src/vehicle_info.cpp index c0a4c240ba44a..c75a4351fb3f3 100644 --- a/common/autoware_vehicle_info_utils/src/vehicle_info.cpp +++ b/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 diff --git a/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml b/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml index 9ccc5a183be45..0590009ab378c 100644 --- a/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml +++ b/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml @@ -48,7 +48,7 @@ - + diff --git a/planning/autoware_static_centerline_generator/package.xml b/planning/autoware_static_centerline_generator/package.xml index a6d97d6fae2f7..6ba1494e714cd 100644 --- a/planning/autoware_static_centerline_generator/package.xml +++ b/planning/autoware_static_centerline_generator/package.xml @@ -20,9 +20,11 @@ autoware_global_parameter_loader autoware_interpolation autoware_lanelet2_extension + autoware_lanelet2_map_visualizer autoware_map_loader autoware_map_msgs autoware_map_projection_loader + autoware_map_tf_generator autoware_mission_planner autoware_motion_utils autoware_osqp_interface diff --git a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp index 7f0264d02434e..f172e3e0cb1cd 100644 --- a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp +++ b/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) {