fix(crane_geometry): Intervalの境界一致処理（erase/append）を修正

utility/crane_geometry/include/crane_geometry/interval.hpp

@@ -32,8 +32,8 @@ class Interval
     std::ranges::sort(uppers);
     std::ranges::sort(lowers);
     for (size_t i = 1; i < uppers.size(); i++) {
-      // 重なっている
-      if (uppers[i - 1] > lowers[i]) {
+      // 重なっている（境界が接する場合も連続区間として結合する）
+      if (uppers[i - 1] >= lowers[i]) {
         uppers[i - 1] = uppers[i];
         lowers.erase(lowers.begin() + i);
         uppers.erase(uppers.begin() + i);
@@ -44,35 +44,56 @@ class Interval
 
   auto erase(double a, double b) -> void
   {
-    double upper = std::max(a, b);
-    double lower = std::min(a, b);
+    // 消去区間 [L, U]
+    double U = std::max(a, b);
+    double L = std::min(a, b);
+
+    // 各既存区間 [lo, hi] から [L, U] を引いた結果を再構築する。
+    // uppers/lowers をペアとして扱い、対応関係を崩さないようにする。
+    std::vector<double> new_lowers;
+    std::vector<double> new_uppers;
+    new_lowers.reserve(lowers.size() + 1);
+    new_uppers.reserve(uppers.size() + 1);
+
     for (size_t i = 0; i < uppers.size(); i++) {
-      // 完全消去
-      if (uppers[i] < upper && lowers[i] > lower) {
-        lowers.erase(lowers.begin() + i);
-        uppers.erase(uppers.begin() + i);
-        i--;
+      double lo = lowers[i];
+      double hi = uppers[i];
+
+      // 重なりなし（境界一致を含む）：区間はそのまま残る
+      if (U <= lo || L >= hi) {
+        new_lowers.emplace_back(lo);
+        new_uppers.emplace_back(hi);
         continue;
       }
-      // 中抜き
-      if (uppers[i] > upper && lowers[i] < lower) {
-        uppers.emplace_back(lower);
-        lowers.emplace_back(upper);
-        std::ranges::sort(uppers);
-        std::ranges::sort(lowers);
+
+      // 完全に覆われる：区間を削除（何も追加しない）
+      if (L <= lo && U >= hi) {
+        continue;
       }
 
-      // 上限修正
-      if (lower < uppers[i] && upper > uppers[i]) {
-        uppers[i] = lower;
+      // 中抜き（2分割）：[lo, L] と [U, hi]
+      if (L > lo && U < hi) {
+        new_lowers.emplace_back(lo);
+        new_uppers.emplace_back(L);
+        new_lowers.emplace_back(U);
+        new_uppers.emplace_back(hi);
+        continue;
       }
-      // 下限修正
-      if (lowers[i] < upper && lowers[i] > lower) {
-        lowers[i] = upper;
+
+      // 下端を縮める：[U, hi]（L <= lo < U < hi）
+      if (L <= lo) {
+        new_lowers.emplace_back(U);
+        new_uppers.emplace_back(hi);
+        continue;
       }
+
+      // 上端を縮める：[lo, L]（lo < L < hi <= U）
+      new_lowers.emplace_back(lo);
+      new_uppers.emplace_back(L);
     }
-    std::ranges::sort(uppers);
-    std::ranges::sort(lowers);
+
+    lowers = std::move(new_lowers);
+    uppers = std::move(new_uppers);
   }
 
   auto getWidth() const -> double

utility/crane_geometry/test/test_interval.cpp

@@ -54,4 +54,33 @@ TEST(IntervalTest, EraseIntervals)
   EXPECT_DOUBLE_EQ(largest.first, 5.0);
   EXPECT_DOUBLE_EQ(largest.second, 10.0);
 }
+
+// 境界がちょうど接する区間を追加した場合、1つの連続区間としてマージされることを確認する
+TEST(IntervalTest, AppendTouchingBoundaryMerges)
+{
+  Interval interval;
+
+  interval.append(1.0, 8.0);
+  interval.append(8.0, 19.0);
+  EXPECT_DOUBLE_EQ(interval.getWidth(), 18.0);
+
+  auto largest = interval.getLargestInterval();
+  EXPECT_DOUBLE_EQ(largest.first, 1.0);
+  EXPECT_DOUBLE_EQ(largest.second, 19.0);
+}
+
+// erase範囲の境界が既存区間自身の境界と厳密に一致する場合でも、
+// 区間が正しく縮まることを確認する（単一区間・単一eraseで再現する最小ケース）
+TEST(IntervalTest, EraseExactBoundaryMatchShrinksInterval)
+{
+  Interval interval;
+
+  interval.append(1.0, 19.0);
+  interval.erase(1.0, 7.0);
+  EXPECT_DOUBLE_EQ(interval.getWidth(), 12.0);
+
+  auto largest = interval.getLargestInterval();
+  EXPECT_DOUBLE_EQ(largest.first, 7.0);
+  EXPECT_DOUBLE_EQ(largest.second, 19.0);
+}
 }  // namespace crane