feat: port autoware_vehicle_info_utils into core

Signed-off-by: JianKangEgon <[email protected]>

common/autoware_vehicle_info_utils/CMakeLists.txt

@@ -0,0 +1,31 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_vehicle_info_utils)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_library(vehicle_info_utils SHARED
+  src/vehicle_info.cpp
+  src/vehicle_info_utils.cpp
+)
+
+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/README.md

@@ -0,0 +1,58 @@
+# Vehicle Info Util
+
+## Purpose
+
+This package is to get vehicle info parameters.
+
+### Description
+
+In [here](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/components/vehicle-dimensions/), you can check the vehicle dimensions with more detail.
+The current format supports only the Ackermann model. This file defines the model assumed in autoware path planning, control, etc. and does not represent the exact physical model. If a model other than the Ackermann model is used, it is assumed that a vehicle interface will be designed to change the control output for the model.
+
+### Versioning Policy
+
+We have implemented a versioning system for the `vehicle_info.param.yaml` file to ensure clarity and consistency in file format across different versions of Autoware and its external applications. Please see [discussion](https://github.com/orgs/autowarefoundation/discussions/4050) for the details.
+
+#### How to Operate
+
+- The current file format is set as an unversioned base version (`version:` field is commented out).
+- For the next update involving changes (such as additions, deletions, or modifications):
+  - Uncomment and update the version line at the beginning of the file.
+  - Initiate versioning by assigning a version number, starting from `0.1.0`. Follow the semantic versioning format (MAJOR.MINOR.PATCH).
+  - Update this Readme.md too.
+- For subsequent updates, continue incrementing the version number in accordance with the changes made.
+  - Discuss how to increment version depending on the amount of changes made to the file.
+
+```yaml
+/**:
+  ros__parameters:
+    # version: 0.1.0 # Uncomment and update this line for future format changes.
+    wheel_radius: 0.383
+    ...
+```
+
+#### Why Versioning?
+
+- Consistency Across Updates: Implementing version control will allow accurate tracking of changes over time and changes in vehicle information parameters.
+- Clarity for External Applications: External applications that depend on `vehicle_info.param.yaml` need to reference the correct file version for optimal compatibility and functionality.
+- Simplified Management for Customized Branches: Assigning versions directly to the `vehicle_info.param.yaml` file simplifies management compared to maintaining separate versions for multiple customized Autoware branches. This approach streamlines version tracking and reduces complexity.
+
+### Scripts
+
+#### Minimum turning radius
+
+```sh
+$ ros2 run autoware_vehicle_info_utils min_turning_radius_calculator.py
+yaml path is /home/autoware/pilot-auto/install/autoware_vehicle_info_utils/share/autoware_vehicle_info_utils/config/vehicle_info.param.yaml
+Minimum turning radius is 3.253042620027102 [m] for rear, 4.253220695862465 [m] for front.
+```
+
+You can designate yaml file with `-y` option as follows.
+
+```sh
+ros2 run autoware_vehicle_info_utils min_turning_radius_calculator.py -y <path-to-yaml>
+```
+
+## Assumptions / Known limits
+
+TBD.

common/autoware_vehicle_info_utils/config/polygon_remover.yaml

@@ -0,0 +1,41 @@
+/**:
+  ros__parameters:
+    polygon_vertices:
+      # This is the coordinates of polygon vertices
+      # The first two numbers are the horizontal and vertical coordinates of the first point respectively
+      # Axis directions are as same as base_link
+      [
+        -1.03,
+        0.815,
+        2.0,
+        0.815,
+        1.9,
+        1.115,
+        2.1,
+        1.115,
+        2.2,
+        1.015,
+        3.0,
+        1.015,
+        3.2,
+        0.815,
+        3.74,
+        0.815,
+        3.74,
+        -0.815,
+        3.2,
+        -0.815,
+        3.0,
+        -1.015,
+        2.2,
+        -1.015,
+        2.1,
+        -1.115,
+        1.9,
+        -1.115,
+        2.0,
+        -0.815,
+        -1.03,
+        -0.815,
+      ] # car
+    will_visualize: true

common/autoware_vehicle_info_utils/config/vehicle_info.param.yaml

@@ -0,0 +1,13 @@
+/**:
+  ros__parameters:
+    # version: 0.1.0 # uncomment this line in the next update of this file format. please check Readme.md
+    wheel_radius: 0.39
+    wheel_width: 0.42
+    wheel_base: 2.74 # between front wheel center and rear wheel center
+    wheel_tread: 1.63 # between left wheel center and right wheel center
+    front_overhang: 1.0 # between front wheel center and vehicle front
+    rear_overhang: 1.03 # between rear wheel center and vehicle rear
+    left_overhang: 0.1 # between left wheel center and vehicle left
+    right_overhang: 0.1 # between right wheel center and vehicle right
+    vehicle_height: 2.5
+    max_steer_angle: 0.70 # [rad]

common/autoware_vehicle_info_utils/config/vehicle_mirror.param.yaml

@@ -0,0 +1,8 @@
+/**:
+  ros__parameters:
+    min_longitudinal_offset: 1.8
+    max_longitudinal_offset: 3.2
+    min_lateral_offset: -1.4
+    max_lateral_offset: 1.4
+    min_height_offset: 0.8
+    max_height_offset: 1.5

common/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info.hpp

@@ -0,0 +1,86 @@
+// Copyright 2015-2021 Autoware Foundation
+//
+// 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.
+
+#ifndef AUTOWARE_VEHICLE_INFO_UTILS__VEHICLE_INFO_HPP_
+#define AUTOWARE_VEHICLE_INFO_UTILS__VEHICLE_INFO_HPP_
+
+#include "autoware_utils/geometry/boost_geometry.hpp"
+
+namespace autoware::vehicle_info_utils
+{
+/// Data class for vehicle info
+struct VehicleInfo
+{
+  // Base parameters. These describe the vehicle's bounding box and the
+  // position and radius of the wheels.
+  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{};
+
+  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_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_utils::LinearRing2d createFootprint(
+    const double lat_margin, const double lon_margin) const;
+
+  double calcMaxCurvature() const;
+  double calcCurvatureFromSteerAngle(const double steer_angle) const;
+  double calcSteerAngleFromCurvature(const double curvature) const;
+};
+
+/// Create vehicle info from base parameters
+VehicleInfo createVehicleInfo(
+  const double wheel_radius_m, const double wheel_width_m, const double wheel_base_m,
+  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);
+
+}  // namespace autoware::vehicle_info_utils
+
+#endif  // AUTOWARE_VEHICLE_INFO_UTILS__VEHICLE_INFO_HPP_

common/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info_utils.hpp

@@ -0,0 +1,44 @@
+// Copyright 2015-2021 Autoware Foundation
+//
+// 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.
+
+#ifndef AUTOWARE_VEHICLE_INFO_UTILS__VEHICLE_INFO_UTILS_HPP_
+#define AUTOWARE_VEHICLE_INFO_UTILS__VEHICLE_INFO_UTILS_HPP_
+
+#include "autoware_vehicle_info_utils/vehicle_info.hpp"
+
+#include <rclcpp/node.hpp>
+
+namespace autoware::vehicle_info_utils
+{
+/// This is a convenience class for saving you from declaring all parameters
+/// manually and calculating derived parameters.
+/// This class supposes that necessary parameters are set when the node is launched.
+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() const;
+
+private:
+  /// Buffer for base parameters
+  VehicleInfo vehicle_info_;
+};
+
+}  // namespace autoware::vehicle_info_utils
+
+#endif  // AUTOWARE_VEHICLE_INFO_UTILS__VEHICLE_INFO_UTILS_HPP_

common/autoware_vehicle_info_utils/launch/sample.launch.py

@@ -0,0 +1,57 @@
+# Copyright 2021 Tier IV, Inc. All rights reserved.
+#
+# 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.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.actions import IncludeLaunchDescription
+from launch.actions import OpaqueFunction
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import SetParameter
+from launch_ros.substitutions import FindPackageShare
+
+
+def launch_setup(context, *args, **kwargs):
+    # use_sim_time
+    set_use_sim_time = SetParameter(name="use_sim_time", value=LaunchConfiguration("use_sim_time"))
+
+    vehicle_info_param_path = os.path.join(
+        get_package_share_directory("autoware_vehicle_info_utils"),
+        "config",
+        "vehicle_info.param.yaml",
+    )
+    # vehicle_info
+    load_vehicle_info = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [FindPackageShare("autoware_vehicle_info_utils"), "/launch/vehicle_info.launch.py"]
+        ),
+        launch_arguments={"vehicle_info_param_file": [vehicle_info_param_path]}.items(),
+    )
+
+    return [
+        set_use_sim_time,
+        load_vehicle_info,
+    ]
+
+
+def generate_launch_description():
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument("use_sim_time", default_value="false"),
+            OpaqueFunction(function=launch_setup),
+        ]
+    )

common/autoware_vehicle_info_utils/launch/vehicle_info.launch.py

@@ -0,0 +1,36 @@
+# Copyright 2021 Tier IV, Inc. All rights reserved.
+#
+# 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.
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.actions import OpaqueFunction
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import SetParameter
+import yaml
+
+
+def launch_setup(context, *args, **kwargs):
+    vehicle_param_file = LaunchConfiguration("vehicle_info_param_file").perform(context)
+    with open(vehicle_param_file, "r") as f:
+        vehicle_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+    return [SetParameter(name=k, value=v) for (k, v) in vehicle_param.items()]
+
+
+def generate_launch_description():
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument("vehicle_info_param_file"),
+            OpaqueFunction(function=launch_setup),
+        ]
+    )