refactor(vehicle_info): add API description, add owner (#9151)

Signed-off-by: Mamoru Sobue <[email protected]>

common/autoware_vehicle_info_utils/CMakeLists.txt

@@ -9,13 +9,23 @@ ament_auto_add_library(vehicle_info_utils SHARED
   src/vehicle_info_utils.cpp
 )
 
-ament_auto_package(
-  INSTALL_TO_SHARE
-    config
-    launch
-)
+if(BUILD_TESTING)
+  file(GLOB_RECURSE test_files test/**/*.cpp)
+
+  ament_add_ros_isolated_gtest(test_${PROJECT_NAME} test/test_vehicle_info_utils.cpp)
+
+  target_link_libraries(test_${PROJECT_NAME}
+    vehicle_info_utils
+  )
+endif()
 
 install(PROGRAMS
   scripts/min_turning_radius_calculator.py
   DESTINATION lib/${PROJECT_NAME}
 )
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+    config
+    launch
+)

common/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info.hpp

@@ -24,30 +24,48 @@ struct VehicleInfo
 {
   // Base parameters. These describe the vehicle's bounding box and the
   // position and radius of the wheels.
-  double wheel_radius_m;
-  double wheel_width_m;
-  double wheel_base_m;
-  double wheel_tread_m;
-  double front_overhang_m;
-  double rear_overhang_m;
-  double left_overhang_m;
-  double right_overhang_m;
-  double vehicle_height_m;
-  double max_steer_angle_rad;
+  double wheel_radius_m{};       //<! should be positive
+  double wheel_width_m{};        //<! should be positive
+  double wheel_base_m{};         //<! should be positive
+  double wheel_tread_m{};        //<! should be positive
+  double front_overhang_m{};     //<! should be positive
+  double rear_overhang_m{};      //<! should be positive
+  double left_overhang_m{};      //<! should be positive
+  double right_overhang_m{};     //<! should be positive
+  double vehicle_height_m{};     //<! should be positive
+  double max_steer_angle_rad{};  //<! should be positive
 
   // Derived parameters, i.e. calculated from base parameters
   // The offset values are relative to the base frame origin, which is located
   // on the ground below the middle of the rear axle, and can be negative.
-  double vehicle_length_m;
-  double vehicle_width_m;
-  double min_longitudinal_offset_m;
-  double max_longitudinal_offset_m;
-  double min_lateral_offset_m;
-  double max_lateral_offset_m;
-  double min_height_offset_m;
-  double max_height_offset_m;
+  double vehicle_length_m{};
+  double vehicle_width_m{};
+  double min_longitudinal_offset_m{};
+  double max_longitudinal_offset_m{};
+  double min_lateral_offset_m{};
+  double max_lateral_offset_m{};
+  double min_height_offset_m{};
+  double max_height_offset_m{};
 
+  static constexpr size_t FrontLeftIndex = 0;   //<! the point index of front-left edge == 0
+  static constexpr size_t FrontRightIndex = 1;  //<! the point index of front-right > front-left
+  static constexpr size_t RearRightIndex = 3;   //<! the point index of rear-right > front-right
+  static constexpr size_t RearLeftIndex = 4;    //<! the point index of rear-left > rear-right
+
+  /**
+   * @brief calculate the vehicle footprint in clockwise manner starting from the front-left edge,
+   * through front-right edge, center-right point, to front-left edge again to form a enclosed
+   * polygon
+   * @param margin the longitudinal and lateral inflation margin
+   */
   autoware::universe_utils::LinearRing2d createFootprint(const double margin = 0.0) const;
+
+  /**
+   * @brief calculate the vehicle footprint in clockwise manner starting from the front-left edge,
+   * through front-right edge, center-right point, to front-left edge again to form a enclosed
+   * polygon
+   * @param margin the longitudinal and lateral inflation margin
+   */
   autoware::universe_utils::LinearRing2d createFootprint(
     const double lat_margin, const double lon_margin) const;
 

common/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info_utils.hpp

@@ -17,7 +17,7 @@
 
 #include "autoware_vehicle_info_utils/vehicle_info.hpp"
 
-#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/node.hpp>
 
 namespace autoware::vehicle_info_utils
 {
@@ -28,10 +28,11 @@ class VehicleInfoUtils
 {
 public:
   /// Constructor
+  // NOTE(soblin): this throws which should be replaced with a factory
   explicit VehicleInfoUtils(rclcpp::Node & node);
 
   /// Get vehicle info
-  VehicleInfo getVehicleInfo();
+  VehicleInfo getVehicleInfo() const;
 
 private:
   /// Buffer for base parameters

common/autoware_vehicle_info_utils/package.xml

@@ -9,6 +9,7 @@
   <maintainer email="[email protected]">Taiki Tanaka</maintainer>
   <maintainer email="[email protected]">Tomoya Kimura</maintainer>
   <maintainer email="[email protected]">Shumpei Wakabayashi</maintainer>
+  <maintainer email="[email protected]">Mamoru Sobue</maintainer>
 
   <license>Apache License 2.0</license>
   <author email="[email protected]">Yamato ANDO</author>
@@ -19,6 +20,8 @@
   <depend>autoware_universe_utils</depend>
   <depend>rclcpp</depend>
 
+  <test_depend>ament_cmake_ros</test_depend>
+  <test_depend>ament_index_cpp</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
 

common/autoware_vehicle_info_utils/src/vehicle_info.cpp

@@ -14,6 +14,8 @@
 
 #include "autoware_vehicle_info_utils/vehicle_info.hpp"
 
+#include <rclcpp/rclcpp.hpp>
+
 namespace autoware::vehicle_info_utils
 {
 autoware::universe_utils::LinearRing2d VehicleInfo::createFootprint(const double margin) const
@@ -46,11 +48,36 @@ autoware::universe_utils::LinearRing2d VehicleInfo::createFootprint(
 }
 
 VehicleInfo createVehicleInfo(
-  const double wheel_radius_m, const double wheel_width_m, const double wheel_base_m,
+  const double wheel_radius_m, const double wheel_width_m, const double wheel_base_m_arg,
   const double wheel_tread_m, const double front_overhang_m, const double rear_overhang_m,
   const double left_overhang_m, const double right_overhang_m, const double vehicle_height_m,
-  const double max_steer_angle_rad)
+  const double max_steer_angle_rad_arg)
 {
+  double wheel_base_m = wheel_base_m_arg;
+  static constexpr double MIN_WHEEL_BASE_M = 1e-6;
+  if (std::abs(wheel_base_m) < MIN_WHEEL_BASE_M) {
+    RCLCPP_ERROR(
+      rclcpp::get_logger("vehicle_info"), "wheel_base_m %f is almost 0.0, clamping to %f",
+      wheel_base_m, MIN_WHEEL_BASE_M);
+    wheel_base_m = MIN_WHEEL_BASE_M;
+  }
+  double max_steer_angle_rad = max_steer_angle_rad_arg;
+  static constexpr double MAX_STEER_ANGLE_RAD = 1e-6;
+  if (std::abs(max_steer_angle_rad) < MAX_STEER_ANGLE_RAD) {
+    RCLCPP_ERROR(
+      rclcpp::get_logger("vehicle_info"), "max_steer_angle_rad %f is almost 0.0, clamping to %f",
+      max_steer_angle_rad, MAX_STEER_ANGLE_RAD);
+    max_steer_angle_rad = MAX_STEER_ANGLE_RAD;
+  }
+
+  if (
+    wheel_radius_m <= 0 || wheel_width_m <= 0 || wheel_base_m <= 0 || wheel_tread_m <= 0 ||
+    front_overhang_m <= 0 || rear_overhang_m <= 0 || left_overhang_m <= 0 ||
+    right_overhang_m <= 0 || vehicle_height_m <= 0 || max_steer_angle_rad <= 0) {
+    RCLCPP_ERROR(
+      rclcpp::get_logger("vehicle_info"), "given parameters contain non positive values");
+  }
+
   // Calculate derived parameters
   const double vehicle_length_m_ = front_overhang_m + wheel_base_m + rear_overhang_m;
   const double vehicle_width_m_ = wheel_tread_m + left_overhang_m + right_overhang_m;
@@ -87,26 +114,15 @@ VehicleInfo createVehicleInfo(
 
 double VehicleInfo::calcMaxCurvature() const
 {
-  if (std::abs(wheel_base_m) < 1e-6) {
-    throw std::invalid_argument("wheel_base_m is 0.");
-  }
-  if (std::abs(max_steer_angle_rad) < 1e-6) {
-    return std::numeric_limits<double>::max();
-  }
-
   const double radius = wheel_base_m / std::tan(max_steer_angle_rad);
   const double curvature = 1.0 / radius;
   return curvature;
 }
 double VehicleInfo::calcCurvatureFromSteerAngle(const double steer_angle) const
 {
-  if (std::abs(wheel_base_m) < 1e-6) {
-    throw std::invalid_argument("wheel_base_m is 0.");
-  }
   if (std::abs(steer_angle) < 1e-6) {
     return std::numeric_limits<double>::max();
   }
-
   const double radius = wheel_base_m / std::tan(steer_angle);
   const double curvature = 1.0 / radius;
   return curvature;

common/autoware_vehicle_info_utils/src/vehicle_info_utils.cpp

@@ -40,24 +40,23 @@ namespace autoware::vehicle_info_utils
 {
 VehicleInfoUtils::VehicleInfoUtils(rclcpp::Node & node)
 {
-  vehicle_info_.wheel_radius_m = getParameter<double>(node, "wheel_radius");
-  vehicle_info_.wheel_width_m = getParameter<double>(node, "wheel_width");
-  vehicle_info_.wheel_base_m = getParameter<double>(node, "wheel_base");
-  vehicle_info_.wheel_tread_m = getParameter<double>(node, "wheel_tread");
-  vehicle_info_.front_overhang_m = getParameter<double>(node, "front_overhang");
-  vehicle_info_.rear_overhang_m = getParameter<double>(node, "rear_overhang");
-  vehicle_info_.left_overhang_m = getParameter<double>(node, "left_overhang");
-  vehicle_info_.right_overhang_m = getParameter<double>(node, "right_overhang");
-  vehicle_info_.vehicle_height_m = getParameter<double>(node, "vehicle_height");
-  vehicle_info_.max_steer_angle_rad = getParameter<double>(node, "max_steer_angle");
+  const auto wheel_radius_m = getParameter<double>(node, "wheel_radius");
+  const auto wheel_width_m = getParameter<double>(node, "wheel_width");
+  const auto wheel_base_m = getParameter<double>(node, "wheel_base");
+  const auto wheel_tread_m = getParameter<double>(node, "wheel_tread");
+  const auto front_overhang_m = getParameter<double>(node, "front_overhang");
+  const auto rear_overhang_m = getParameter<double>(node, "rear_overhang");
+  const auto left_overhang_m = getParameter<double>(node, "left_overhang");
+  const auto right_overhang_m = getParameter<double>(node, "right_overhang");
+  const auto vehicle_height_m = getParameter<double>(node, "vehicle_height");
+  const auto max_steer_angle_rad = getParameter<double>(node, "max_steer_angle");
+  vehicle_info_ = createVehicleInfo(
+    wheel_radius_m, wheel_width_m, wheel_base_m, wheel_tread_m, front_overhang_m, rear_overhang_m,
+    left_overhang_m, right_overhang_m, vehicle_height_m, max_steer_angle_rad);
 }
 
-VehicleInfo VehicleInfoUtils::getVehicleInfo()
+VehicleInfo VehicleInfoUtils::getVehicleInfo() const
 {
-  return createVehicleInfo(
-    vehicle_info_.wheel_radius_m, vehicle_info_.wheel_width_m, vehicle_info_.wheel_base_m,
-    vehicle_info_.wheel_tread_m, vehicle_info_.front_overhang_m, vehicle_info_.rear_overhang_m,
-    vehicle_info_.left_overhang_m, vehicle_info_.right_overhang_m, vehicle_info_.vehicle_height_m,
-    vehicle_info_.max_steer_angle_rad);
+  return vehicle_info_;
 }
 }  // namespace autoware::vehicle_info_utils

common/autoware_vehicle_info_utils/test/test_vehicle_info_utils.cpp

@@ -0,0 +1,72 @@
+// Copyright 2020-2024 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 <ament_index_cpp/get_package_share_directory.hpp>
+#include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
+
+#include <gtest/gtest.h>
+
+class VehicleInfoUtilTest : public ::testing::Test
+{
+protected:
+  void SetUp() override
+  {
+    rclcpp::init(0, nullptr);
+    rclcpp::NodeOptions options;
+    options.arguments(
+      {"--ros-args", "--params-file",
+       ament_index_cpp::get_package_share_directory("autoware_vehicle_info_utils") +
+         "/config/vehicle_info.param.yaml"});
+    node_ = std::make_shared<rclcpp::Node>("test_vehicle_info_utils", options);
+  }
+
+  void TearDown() override { rclcpp::shutdown(); }
+
+  std::shared_ptr<rclcpp::Node> node_;
+};
+
+TEST_F(VehicleInfoUtilTest, check_vehicle_info_value)
+{
+  using autoware::vehicle_info_utils::VehicleInfo;
+  using autoware::vehicle_info_utils::VehicleInfoUtils;
+  std::optional<VehicleInfoUtils> vehicle_info_util_opt{std::nullopt};
+  ASSERT_NO_THROW({ vehicle_info_util_opt.emplace(VehicleInfoUtils(*node_)); });
+  const auto & vehicle_info_util = vehicle_info_util_opt.value();
+  const auto vehicle_info = vehicle_info_util.getVehicleInfo();
+
+  EXPECT_FLOAT_EQ(0.39, vehicle_info.wheel_radius_m);
+  EXPECT_FLOAT_EQ(0.42, vehicle_info.wheel_width_m);
+  EXPECT_FLOAT_EQ(2.74, vehicle_info.wheel_base_m);
+  EXPECT_FLOAT_EQ(1.63, vehicle_info.wheel_tread_m);
+  EXPECT_FLOAT_EQ(1.0, vehicle_info.front_overhang_m);
+  EXPECT_FLOAT_EQ(1.03, vehicle_info.rear_overhang_m);
+  EXPECT_FLOAT_EQ(0.1, vehicle_info.left_overhang_m);
+  EXPECT_FLOAT_EQ(0.1, vehicle_info.right_overhang_m);
+  EXPECT_FLOAT_EQ(2.5, vehicle_info.vehicle_height_m);
+  EXPECT_FLOAT_EQ(0.7, vehicle_info.max_steer_angle_rad);
+
+  const auto footprint = vehicle_info.createFootprint();
+  // front-left
+  EXPECT_FLOAT_EQ(footprint.at(VehicleInfo::FrontLeftIndex).x(), 2.74 + 1.0);
+  EXPECT_FLOAT_EQ(footprint.at(VehicleInfo::FrontLeftIndex).y(), 1.63 / 2 + 0.1);
+  // front-right
+  EXPECT_FLOAT_EQ(footprint.at(VehicleInfo::FrontRightIndex).x(), 2.74 + 1.0);
+  EXPECT_FLOAT_EQ(footprint.at(VehicleInfo::FrontRightIndex).y(), -(1.63 / 2 + 0.1));
+  // rear-right
+  EXPECT_FLOAT_EQ(footprint.at(VehicleInfo::RearRightIndex).x(), -1.03);
+  EXPECT_FLOAT_EQ(footprint.at(VehicleInfo::RearRightIndex).y(), -(1.63 / 2 + 0.1));
+  // rear-left
+  EXPECT_FLOAT_EQ(footprint.at(VehicleInfo::RearLeftIndex).x(), -1.03);
+  EXPECT_FLOAT_EQ(footprint.at(VehicleInfo::RearLeftIndex).y(), 1.63 / 2 + 0.1);
+}