Merge pull request tier4#1609 from tier4/feature/delay_curventure_calculation

Feature/delay curventure calculation

Author: takanotaiga

common/math/geometry/include/geometry/solver/polynomial_solver.hpp

@@ -101,6 +101,15 @@ class PolynomialSolver
    */
   constexpr static double tolerance = 1e-7;
 
+  /**
+   * @brief check the value0 and value1 is equal or not with considering tolerance.
+   * @param value0 the value you want to compare
+   * @param value1 the value you want to compared
+   * @return true value0 and value1 are equal
+   * @return false value0 and value1 are not equal
+   */
+  auto isApproximatelyEqualTo(const double value0, const double value1) const -> bool;
+
 private:
   /**
    * @brief solve cubic equation x^3 + a*x^2 + b*x + c = 0
@@ -119,14 +128,6 @@ class PolynomialSolver
   auto filterByRange(
     const std::vector<double> & values, const double min_value, const double max_value) const
     -> std::vector<double>;
-  /**
-   * @brief check the value0 and value1 is equal or not with considering tolerance.
-   * @param value0 the value you want to compare
-   * @param value1 the value you want to compared
-   * @return true value0 and value1 are equal
-   * @return false value0 and value1 are not equal
-   */
-  auto isApproximatelyEqualTo(const double value0, const double value1) const -> bool;
 };
 }  // namespace geometry
 }  // namespace math

common/math/geometry/include/geometry/spline/catmull_rom_spline.hpp

@@ -19,8 +19,10 @@
 #include <geometry/spline/catmull_rom_spline_interface.hpp>
 #include <geometry/spline/hermite_curve.hpp>
 #include <geometry_msgs/msg/point.hpp>
+#include <memory>
 #include <optional>
 #include <string>
+#include <traffic_simulator_msgs/msg/polyline_trajectory.hpp>
 #include <utility>
 #include <vector>
 
@@ -33,6 +35,9 @@ class CatmullRomSpline : public CatmullRomSplineInterface
 public:
   CatmullRomSpline() = default;
   explicit CatmullRomSpline(const std::vector<geometry_msgs::msg::Point> & control_points);
+  explicit CatmullRomSpline(
+    const geometry_msgs::msg::Point & start_point,
+    const std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory> & trajectory);
   auto getLength() const -> double override { return total_length_; }
   auto getMaximum2DCurvature() const -> double;
   auto getPoint(const double s) const -> geometry_msgs::msg::Point;
@@ -43,6 +48,8 @@ class CatmullRomSpline : public CatmullRomSplineInterface
   auto getTrajectory(
     const double start_s, const double end_s, const double resolution,
     const double offset = 0.0) const -> std::vector<geometry_msgs::msg::Point>;
+  auto getTrajectoryPoses(const double start_s, const double end_s, const double resolution) const
+    -> std::vector<geometry_msgs::msg::Pose>;
   auto getSValue(const geometry_msgs::msg::Pose & pose, double threshold_distance = 3.0) const
     -> std::optional<double>;
   auto getSquaredDistanceIn2D(const geometry_msgs::msg::Point & point, const double s) const
@@ -78,7 +85,8 @@ class CatmullRomSpline : public CatmullRomSplineInterface
   std::vector<LineSegment> line_segments_;
   std::vector<HermiteCurve> curves_;
   std::vector<double> length_list_;
-  std::vector<double> maximum_2d_curvatures_;
+  /// @note Since curvature calculation requires a large computational cost, it is inefficient to calculate it in the member initializer list and must be calculated when the getMaximum2DCurvature function is first called.
+  mutable std::vector<double> maximum_2d_curvatures_;
   double total_length_;
 };
 }  // namespace geometry

common/math/geometry/include/geometry/vector3/operator.hpp

@@ -20,6 +20,7 @@
 #include <geometry_msgs/msg/point.hpp>
 #include <geometry_msgs/msg/quaternion.hpp>
 #include <geometry_msgs/msg/vector3.hpp>
+#include <limits>
 
 namespace math
 {
@@ -131,6 +132,16 @@ auto operator+=(T & a, const U & b) -> decltype(auto)
   a.z += b.z;
   return a;
 }
+
+template <
+  typename T, typename U,
+  std::enable_if_t<std::conjunction_v<IsLikeVector3<T>, IsLikeVector3<U>>, std::nullptr_t> =
+    nullptr>
+auto operator==(const T & a, const U & b) -> bool
+{
+  constexpr decltype(a.x) e = std::numeric_limits<decltype(a.x)>::epsilon();
+  return (std::abs(a.x - b.x) < e) && (std::abs(a.y - b.y) < e) && (std::abs(a.z - b.z) < e);
+}
 }  // namespace geometry
 }  // namespace math
 

common/math/geometry/src/polygon/line_segment.cpp

@@ -314,11 +314,15 @@ auto getLineSegments(
   } else {
     std::vector<LineSegment> seg;
     for (size_t i = 0; i < points.size() - 1; i++) {
-      seg.emplace_back(points[i], points[i + 1]);
+      if (points[i] != points[i + 1]) {
+        seg.emplace_back(points[i], points[i + 1]);
+      }
     }
     /// @note If true, the end point(points[points.size() - 1]) and start point(points[0]) was connected.
     if (close_start_end) {
-      seg.emplace_back(points[points.size() - 1], points[0]);
+      if (points[points.size() - 1] != points[0]) {
+        seg.emplace_back(points[points.size() - 1], points[0]);
+      }
     }
     return seg;
   }

common/math/geometry/src/spline/catmull_rom_spline.cpp

@@ -101,6 +101,12 @@ auto CatmullRomSpline::getTrajectory(
   const double start_s, const double end_s, const double resolution, const double offset) const
   -> std::vector<geometry_msgs::msg::Point>
 {
+  if (std::fabs(resolution) <= std::numeric_limits<double>::epsilon()) {
+    THROW_SIMULATION_ERROR(
+      "Resolution should not be zero.",
+      "This message is not originally intended to be displayed, if you see it, please contact ",
+      "the developer of traffic_simulator.");
+  }
   if (start_s > end_s) {
     std::vector<geometry_msgs::msg::Point> ret;
     double s = start_s;
@@ -122,8 +128,70 @@ auto CatmullRomSpline::getTrajectory(
   }
 }
 
-CatmullRomSpline::CatmullRomSpline(const std::vector<geometry_msgs::msg::Point> & control_points)
-: control_points(control_points), line_segments_(getLineSegments(control_points)), total_length_(0)
+auto CatmullRomSpline::getTrajectoryPoses(
+  const double start_s, const double end_s, const double resolution) const
+  -> std::vector<geometry_msgs::msg::Pose>
+{
+  if (std::fabs(resolution) <= std::numeric_limits<double>::epsilon()) {
+    THROW_SIMULATION_ERROR(
+      "Resolution should not be zero.",
+      "This message is not originally intended to be displayed, if you see it, please contact ",
+      "the developer of traffic_simulator.");
+  }
+  if (start_s > end_s) {
+    std::vector<geometry_msgs::msg::Pose> ret;
+    double s = start_s;
+    while (s > end_s) {
+      ret.emplace_back(getPose(s));
+      s = s - std::fabs(resolution);
+    }
+    ret.emplace_back(getPose(end_s));
+    return ret;
+  } else {
+    std::vector<geometry_msgs::msg::Pose> ret;
+    double s = start_s;
+    while (s < end_s) {
+      ret.emplace_back(getPose(s));
+      s = s + std::fabs(resolution);
+    }
+    ret.emplace_back(getPose(end_s));
+    return ret;
+  }
+}
+
+CatmullRomSpline::CatmullRomSpline(
+  const geometry_msgs::msg::Point & start_point,
+  const std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory> & trajectory)
+: CatmullRomSpline([start_point, trajectory] {
+    if (!trajectory) {
+      THROW_SIMULATION_ERROR(
+        "Trajectory is empty.",
+        "This message is not originally intended to be displayed, if you see it, please contact "
+        "the developer of traffic_simulator.");
+    }
+    std::vector<geometry_msgs::msg::Point> control_points;
+    control_points.reserve(trajectory->shape.vertices.size() + 2UL);
+    control_points.emplace_back(start_point);
+    for (const auto & vertex : trajectory->shape.vertices) {
+      control_points.emplace_back(vertex.position.position);
+    }
+    if (trajectory->closed) {
+      control_points.emplace_back(control_points[0]);
+    }
+    return control_points;
+  }())
+{
+}
+
+CatmullRomSpline::CatmullRomSpline(
+  const std::vector<geometry_msgs::msg::Point> & raw_control_points)
+: control_points([raw_control_points]() {
+    std::vector<geometry_msgs::msg::Point> ret = raw_control_points;
+    ret.erase(std::unique(ret.begin(), ret.end()), ret.end());
+    return ret;
+  }()),
+  line_segments_(getLineSegments(control_points)),
+  total_length_(0)
 {
   switch (control_points.size()) {
     case 0:
@@ -232,7 +300,6 @@ CatmullRomSpline::CatmullRomSpline(const std::vector<geometry_msgs::msg::Point>
         }
         for (const auto & curve : curves_) {
           length_list_.emplace_back(curve.getLength());
-          maximum_2d_curvatures_.emplace_back(curve.getMaximum2DCurvature());
         }
         total_length_ = 0;
         for (const auto & length : length_list_) {
@@ -585,8 +652,17 @@ auto CatmullRomSpline::getPoint(const double s, const double offset) const
 
 auto CatmullRomSpline::getMaximum2DCurvature() const -> double
 {
+  if (curves_.empty()) {
+    THROW_SEMANTIC_ERROR(
+      "Curves are empty. We cannot determine the maximum 2D curvature of the spline.",
+      "This message is not originally intended to be displayed, if you see it, please contact "
+      "the developer of traffic_simulator.");
+  }
+  /// @note Maximum 2D curvature is empty means that it is not calculated yet.
   if (maximum_2d_curvatures_.empty()) {
-    THROW_SIMULATION_ERROR("maximum 2D curvature vector size is 0.");  // LCOV_EXCL_LINE
+    for (const auto & curve : curves_) {
+      maximum_2d_curvatures_.emplace_back(curve.getMaximum2DCurvature());
+    }
   }
   const auto [min, max] =
     std::minmax_element(maximum_2d_curvatures_.begin(), maximum_2d_curvatures_.end());

common/math/geometry/src/spline/hermite_curve.cpp

@@ -135,21 +135,16 @@ std::set<double> HermiteCurve::getCollisionPointsIn2D(
   double d = dy_ * ex - dx_ * ey - ex * fy + ey * fx;
 
   const auto get_solutions = [search_backward, a, b, c, d, this]() -> std::vector<double> {
-    try {
+    if (
+      solver_.isApproximatelyEqualTo(a, 0.0) && solver_.isApproximatelyEqualTo(b, 0.0) &&
+      solver_.isApproximatelyEqualTo(c, 0.0) && solver_.isApproximatelyEqualTo(d, 0.0)) {
       /**
-       * @note Obtain a solution to the cubic equation ax^3 + bx^2 + cx + d = 0 that falls within the range [0, 1].
+       * @note If all coefficients are zero, the cubic equation is satisfied for all x values.
+       * In this case, return 0 or 1 depending on the search direction.
        */
-      return solver_.solveCubicEquation(a, b, c, d, 0, 1);
-    }
-    /**
-     * @note PolynomialSolver::solveCubicEquation throws common::SimulationError when any x value can satisfy the equation, 
-     * so the beginning and end point of this curve can collide with the line segment.
-     * If search_backward = true, the line segment collisions at the end of the curve. So return 1.
-     * If search_backward = false, the line segment collisions at the start of the curve. So return 0.
-     */
-    catch (const common::SimulationError &) {
       return {search_backward ? 1.0 : 0.0};
     }
+    return solver_.solveCubicEquation(a, b, c, d, 0, 1);
   };
 
   /**

common/math/geometry/test/src/polygon/test_line_segment.cpp

@@ -752,13 +752,7 @@ TEST(LineSegment, getLineSegmentsVectorIdentical)
   const std::vector<geometry_msgs::msg::Point> points{point, point, point, point};
   const std::vector<math::geometry::LineSegment> lines =
     math::geometry::getLineSegments(points, false);
-  EXPECT_EQ(lines.size(), size_t(3));
-  EXPECT_POINT_EQ(lines[0].start_point, point);
-  EXPECT_POINT_EQ(lines[0].end_point, point);
-  EXPECT_POINT_EQ(lines[1].start_point, point);
-  EXPECT_POINT_EQ(lines[1].end_point, point);
-  EXPECT_POINT_EQ(lines[2].start_point, point);
-  EXPECT_POINT_EQ(lines[2].end_point, point);
+  EXPECT_EQ(lines.size(), size_t(0));
 }
 
 TEST(LineSegment, getLineSegmentsVectorIdentical_closeStartEnd)
@@ -767,15 +761,7 @@ TEST(LineSegment, getLineSegmentsVectorIdentical_closeStartEnd)
   const std::vector<geometry_msgs::msg::Point> points{point, point, point, point};
   const std::vector<math::geometry::LineSegment> lines =
     math::geometry::getLineSegments(points, true);
-  EXPECT_EQ(lines.size(), size_t(4));
-  EXPECT_POINT_EQ(lines[0].start_point, point);
-  EXPECT_POINT_EQ(lines[0].end_point, point);
-  EXPECT_POINT_EQ(lines[1].start_point, point);
-  EXPECT_POINT_EQ(lines[1].end_point, point);
-  EXPECT_POINT_EQ(lines[2].start_point, point);
-  EXPECT_POINT_EQ(lines[2].end_point, point);
-  EXPECT_POINT_EQ(lines[3].start_point, point);
-  EXPECT_POINT_EQ(lines[3].end_point, point);
+  EXPECT_EQ(lines.size(), size_t(0));
 }
 
 int main(int argc, char ** argv)

mock/cpp_mock_scenarios/launch/mock_test.launch.py

@@ -107,6 +107,7 @@ def launch_setup(context, *args, **kwargs):
     autoware_launch_package             = LaunchConfiguration("autoware_launch_package",                default=default_autoware_launch_package_of(architecture_type.perform(context)))
     consider_acceleration_by_road_slope = LaunchConfiguration("consider_acceleration_by_road_slope",    default=False)
     consider_pose_by_road_slope         = LaunchConfiguration("consider_pose_by_road_slope",            default=True)
+    enable_perf                         = LaunchConfiguration("enable_perf",                            default=False)
     global_frame_rate                   = LaunchConfiguration("global_frame_rate",                      default=20.0)
     global_real_time_factor             = LaunchConfiguration("global_real_time_factor",                default=1.0)
     global_timeout                      = LaunchConfiguration("global_timeout",                         default=180)
@@ -135,6 +136,7 @@ def launch_setup(context, *args, **kwargs):
     print(f"autoware_launch_package             := {autoware_launch_package.perform(context)}")
     print(f"consider_acceleration_by_road_slope := {consider_acceleration_by_road_slope.perform(context)}")
     print(f"consider_pose_by_road_slope         := {consider_pose_by_road_slope.perform(context)}")
+    print(f"enable_perf                         := {enable_perf.perform(context)}")
     print(f"global_frame_rate                   := {global_frame_rate.perform(context)}")
     print(f"global_real_time_factor             := {global_real_time_factor.perform(context)}")
     print(f"global_timeout                      := {global_timeout.perform(context)}")
@@ -182,6 +184,13 @@ def make_parameters():
         parameters += [parameter_file_path.perform(context)]
         return parameters
 
+    def make_launch_prefix():
+        if enable_perf.perform(context) == "True":
+            return "perf record -F 10000"
+        else:
+            return ""
+
+
     def make_vehicle_parameters():
         parameters = []
 
@@ -219,6 +228,7 @@ def description():
         DeclareLaunchArgument("autoware_launch_package",             default_value=autoware_launch_package            ),
         DeclareLaunchArgument("consider_acceleration_by_road_slope", default_value=consider_acceleration_by_road_slope),
         DeclareLaunchArgument("consider_pose_by_road_slope",         default_value=consider_pose_by_road_slope        ),
+        DeclareLaunchArgument("enable_perf",                         default_value=enable_perf                        ),
         DeclareLaunchArgument("global_frame_rate",                   default_value=global_frame_rate                  ),
         DeclareLaunchArgument("global_real_time_factor",             default_value=global_real_time_factor            ),
         DeclareLaunchArgument("global_timeout",                      default_value=global_timeout                     ),
@@ -253,6 +263,7 @@ def description():
             name="visualizer",
             output="screen",
             remappings=[("/simulation/entity/status", "/entity/status")],
+            prefix=make_launch_prefix(),
         ),
         Node(
             package="rviz2",

simulation/do_nothing_plugin/src/plugin.cpp

@@ -20,6 +20,7 @@
 #include <geometry/vector3/operator.hpp>
 #include <geometry_msgs/msg/accel.hpp>
 #include <geometry_msgs/msg/twist.hpp>
+#include <traffic_simulator/lanelet_wrapper/lanelet_map.hpp>
 
 namespace entity_behavior
 {
@@ -180,7 +181,7 @@ void DoNothingBehavior::update(double current_time, double step_time)
   canonicalized_entity_status_->setTime(current_time);
   if (getRequest() == traffic_simulator::behavior::Request::FOLLOW_POLYLINE_TRAJECTORY) {
     canonicalized_entity_status_->set(
-      interpolate_entity_status_on_polyline_trajectory(),
+      interpolate_entity_status_on_polyline_trajectory(), getRouteLanelets(),
       getDefaultMatchingDistanceForLaneletPoseCalculation());
     if (
       getCurrentTime() + getStepTime() >=
@@ -190,7 +191,7 @@ void DoNothingBehavior::update(double current_time, double step_time)
   } else {
     canonicalized_entity_status_->set(
       static_cast<traffic_simulator::EntityStatus>(*canonicalized_entity_status_),
-      getDefaultMatchingDistanceForLaneletPoseCalculation());
+      getRouteLanelets(), getDefaultMatchingDistanceForLaneletPoseCalculation());
   }
 }
 auto DoNothingBehavior::getCurrentAction() -> const std::string &

simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp

@@ -65,6 +65,7 @@ class EntityManager
     lanelet_marker_pub_ptr_(rclcpp::create_publisher<MarkerArray>(
       node, "lanelet/marker", LaneletMarkerQoS(),
       rclcpp::PublisherOptionsWithAllocator<AllocatorT>())),
+    entities_(128),
     hdmap_utils_ptr_(std::make_shared<hdmap_utils::HdMapUtils>(
       configuration_.lanelet2_map_path(), getOrigin(*node))),
     markers_raw_(hdmap_utils_ptr_->generateMarker())