fix: standardize the width

Signed-off-by: Shumpei Wakabayashi <[email protected]>
autowarefoundation · May 5, 2024 · 4a2f0ba · 4a2f0ba
1 parent 502a7fd
commit 4a2f0ba
Showing 1 changed file with 6 additions and 2 deletions.
diff --git a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp
@@ -80,8 +80,12 @@ void visualizeBound(
       if (diff_angle == 0.0) {
         return std::make_pair(normal_vector_angle, width);
       }
-
-      return std::make_pair(normal_vector_angle, width / std::sin(diff_angle));
+      const float adjusted_width = width / std::sin(diff_angle);
+      if (std::abs(adjusted_width) > 1.0) {
+          return std::make_pair(normal_vector_angle, adjusted_width / std::abs(adjusted_width));
+      } else {
+          return std::make_pair(normal_vector_angle, adjusted_width);
+      }
     }();
 
     normal_vector_angles.push_back(normal_vector_angle);