update

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>
autowarefoundation · Jul 21, 2022 · ef3a045 · ef3a045
1 parent 5515dd5
commit ef3a045
Show file tree

Hide file tree

Showing 2 changed files with 3 additions and 2 deletions.
diff --git a/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp b/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp
@@ -86,13 +86,13 @@ class TrajectoryAnalyzer
 
     TrajectoryDebugInfo data;
     data.stamp = node_->now();
+    data.size = points.size();
     data.curvature = calcCurvature(points);
     const auto arclength_offset = motion_utils::calcSignedArcLength(points, 0, ego_p);
     data.arclength = calcPathArcLengthArray(points, -arclength_offset);
     data.velocity = getVelocityArray(points);
 
-    const auto s = data.curvature.size();
-    if (s != data.arclength.size() || s != data.velocity.size()) {
+    if (data.size != data.arclength.size() || data.size != data.velocity.size()) {
       RCLCPP_ERROR(node_->get_logger(), "computation failed.");
       return;
     }

diff --git a/planning/planning_debug_tools/msg/TrajectoryDebugInfo.msg b/planning/planning_debug_tools/msg/TrajectoryDebugInfo.msg
@@ -1,4 +1,5 @@
 builtin_interfaces/Time stamp
+uint32 size
 float64[] arclength
 float64[] curvature
 float64[] velocity