fix(motion_velocity_smoother): curvature calculation (autowarefoundation#1460)

* fix(motion_velocity_smoother): curvature calculation

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix cppcheck for namespace

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* remove debug code

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

Author: TakaHoribe

planning/motion_velocity_smoother/CMakeLists.txt

@@ -46,6 +46,16 @@ rclcpp_components_register_node(motion_velocity_smoother_node
   EXECUTABLE motion_velocity_smoother
 )
 
+if(BUILD_TESTING)
+  ament_add_ros_isolated_gmock(test_smoother_functions
+    test/test_smoother_functions.cpp
+  )
+  target_link_libraries(test_smoother_functions
+    smoother
+  )
+endif()
+
+
 ament_auto_package(
   INSTALL_TO_SHARE
     launch

planning/motion_velocity_smoother/package.xml

@@ -28,6 +28,7 @@
   <depend>tier4_debug_msgs</depend>
   <depend>tier4_planning_msgs</depend>
 
+  <test_depend>ament_cmake_ros</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
 

planning/motion_velocity_smoother/src/trajectory_utils.cpp

@@ -199,9 +199,17 @@ std::vector<double> calcTrajectoryCurvatureFrom3Points(
   using tier4_autoware_utils::calcCurvature;
   using tier4_autoware_utils::getPoint;
 
+  if (trajectory.size() < 3) {
+    const std::vector<double> k_arr(trajectory.size(), 0.0);
+    return k_arr;
+  }
+
   // if the idx size is not enough, change the idx_dist
-  if (trajectory.size() < 2 * idx_dist + 1) {
-    idx_dist = std::ceil((trajectory.size() - 1) / 2.0);
+  const auto max_idx_dist = static_cast<size_t>(std::floor((trajectory.size() - 1) / 2.0));
+  idx_dist = std::max(1ul, std::min(idx_dist, max_idx_dist));
+
+  if (idx_dist < 1) {
+    throw std::logic_error("idx_dist less than 1 is not expected");
   }
 
   // calculate curvature by circle fitting from three points
@@ -217,14 +225,6 @@ std::vector<double> calcTrajectoryCurvatureFrom3Points(
     }
   }
 
-  // for debug
-  if (k_arr.empty()) {
-    RCLCPP_ERROR(
-      rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"),
-      "k_arr.size() = 0, something wrong. pls check.");
-    return {};
-  }
-
   // first and last curvature is copied from next value
   for (size_t i = 0; i < idx_dist; ++i) {
     k_arr.insert(k_arr.begin(), k_arr.front());

planning/motion_velocity_smoother/test/test_smoother_functions.cpp

@@ -0,0 +1,63 @@
+// Copyright 2022 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "motion_velocity_smoother/trajectory_utils.hpp"
+
+#include <gtest/gtest.h>
+
+#include <vector>
+
+using autoware_auto_planning_msgs::msg::TrajectoryPoint;
+using motion_velocity_smoother::trajectory_utils::TrajectoryPoints;
+
+TrajectoryPoints genStraightTrajectory(const size_t size)
+{
+  double x = 0.0;
+  double dx = 1.0;
+  geometry_msgs::msg::Quaternion o;
+  o.x = 0.0;
+  o.y = 0.0;
+  o.z = 0.0;
+  o.w = 1.0;
+
+  TrajectoryPoints tps;
+  TrajectoryPoint p;
+  for (size_t i = 0; i < size; ++i) {
+    p.pose.position.x = x;
+    p.pose.orientation = o;
+    x += dx;
+    tps.push_back(p);
+  }
+  return tps;
+}
+
+TEST(TestTrajectoryUtils, CalcTrajectoryCurvatureFrom3Points)
+{
+  // the output curvature vector should have the same size of the input trajectory.
+  const auto checkOutputSize = [](const size_t trajectory_size, const size_t idx_dist) {
+    const auto trajectory_points = genStraightTrajectory(trajectory_size);
+    const auto curvatures =
+      motion_velocity_smoother::trajectory_utils::calcTrajectoryCurvatureFrom3Points(
+        trajectory_points, idx_dist);
+    EXPECT_EQ(curvatures.size(), trajectory_size) << ", idx_dist = " << idx_dist;
+  };
+
+  const auto trajectory_size_arr = {0, 1, 2, 3, 4, 5, 10, 100};
+  const auto idx_dist_arr = {0, 1, 2, 3, 4, 5, 10, 100};
+  for (const auto trajectory_size : trajectory_size_arr) {
+    for (const auto idx_dist : idx_dist_arr) {
+      checkOutputSize(trajectory_size, idx_dist);
+    }
+  }
+}