From 3a3fae328b36a12d854cb1a7af35a612ab0dca15 Mon Sep 17 00:00:00 2001 From: JianKangEgon Date: Fri, 17 Jan 2025 11:58:54 +0800 Subject: [PATCH] feat: port autoware_vehicle_info_utils into core Signed-off-by: JianKangEgon --- .../CMakeLists.txt | 31 ++++ common/autoware_vehicle_info_utils/README.md | 58 +++++++ .../config/polygon_remover.yaml | 41 +++++ .../config/vehicle_info.param.yaml | 13 ++ .../config/vehicle_mirror.param.yaml | 8 + .../vehicle_info.hpp | 86 +++++++++++ .../vehicle_info_utils.hpp | 44 ++++++ .../launch/sample.launch.py | 57 +++++++ .../launch/vehicle_info.launch.py | 36 +++++ .../autoware_vehicle_info_utils/package.xml | 32 ++++ .../scripts/min_turning_radius_calculator.py | 56 +++++++ .../src/vehicle_info.cpp | 143 ++++++++++++++++++ .../src/vehicle_info_utils.cpp | 62 ++++++++ .../test/test_vehicle_info_utils.cpp | 74 +++++++++ 14 files changed, 741 insertions(+) create mode 100644 common/autoware_vehicle_info_utils/CMakeLists.txt create mode 100644 common/autoware_vehicle_info_utils/README.md create mode 100644 common/autoware_vehicle_info_utils/config/polygon_remover.yaml create mode 100644 common/autoware_vehicle_info_utils/config/vehicle_info.param.yaml create mode 100644 common/autoware_vehicle_info_utils/config/vehicle_mirror.param.yaml create mode 100644 common/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info.hpp create mode 100644 common/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info_utils.hpp create mode 100644 common/autoware_vehicle_info_utils/launch/sample.launch.py create mode 100644 common/autoware_vehicle_info_utils/launch/vehicle_info.launch.py create mode 100644 common/autoware_vehicle_info_utils/package.xml create mode 100755 common/autoware_vehicle_info_utils/scripts/min_turning_radius_calculator.py create mode 100644 common/autoware_vehicle_info_utils/src/vehicle_info.cpp create mode 100644 common/autoware_vehicle_info_utils/src/vehicle_info_utils.cpp create mode 100644 common/autoware_vehicle_info_utils/test/test_vehicle_info_utils.cpp diff --git a/common/autoware_vehicle_info_utils/CMakeLists.txt b/common/autoware_vehicle_info_utils/CMakeLists.txt new file mode 100644 index 00000000..024d0428 --- /dev/null +++ b/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 +) diff --git a/common/autoware_vehicle_info_utils/README.md b/common/autoware_vehicle_info_utils/README.md new file mode 100644 index 00000000..537dc04a --- /dev/null +++ b/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 +``` + +## Assumptions / Known limits + +TBD. diff --git a/common/autoware_vehicle_info_utils/config/polygon_remover.yaml b/common/autoware_vehicle_info_utils/config/polygon_remover.yaml new file mode 100644 index 00000000..b1168b1a --- /dev/null +++ b/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 diff --git a/common/autoware_vehicle_info_utils/config/vehicle_info.param.yaml b/common/autoware_vehicle_info_utils/config/vehicle_info.param.yaml new file mode 100644 index 00000000..72c070c1 --- /dev/null +++ b/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] diff --git a/common/autoware_vehicle_info_utils/config/vehicle_mirror.param.yaml b/common/autoware_vehicle_info_utils/config/vehicle_mirror.param.yaml new file mode 100644 index 00000000..2f586d99 --- /dev/null +++ b/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 diff --git a/common/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info.hpp b/common/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info.hpp new file mode 100644 index 00000000..7091cf19 --- /dev/null +++ b/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{}; // front-left + static constexpr size_t RearRightIndex = 3; // front-right + static constexpr size_t RearLeftIndex = 4; // 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_ diff --git a/common/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info_utils.hpp b/common/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info_utils.hpp new file mode 100644 index 00000000..8c320e71 --- /dev/null +++ b/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 + +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_ diff --git a/common/autoware_vehicle_info_utils/launch/sample.launch.py b/common/autoware_vehicle_info_utils/launch/sample.launch.py new file mode 100644 index 00000000..5f83bc69 --- /dev/null +++ b/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), + ] + ) diff --git a/common/autoware_vehicle_info_utils/launch/vehicle_info.launch.py b/common/autoware_vehicle_info_utils/launch/vehicle_info.launch.py new file mode 100644 index 00000000..ea8760f9 --- /dev/null +++ b/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), + ] + ) diff --git a/common/autoware_vehicle_info_utils/package.xml b/common/autoware_vehicle_info_utils/package.xml new file mode 100644 index 00000000..1429ef4c --- /dev/null +++ b/common/autoware_vehicle_info_utils/package.xml @@ -0,0 +1,32 @@ + + + + autoware_vehicle_info_utils + 0.0.0 + The autoware_vehicle_info_utils package + + + Taiki Tanaka + Tomoya Kimura + Shumpei Wakabayashi + Mamoru Sobue + Jian Kang + + Apache License 2.0 + Yamato ANDO + + ament_cmake_auto + autoware_cmake + + autoware_utils + rclcpp + + ament_cmake_ros + ament_index_cpp + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/common/autoware_vehicle_info_utils/scripts/min_turning_radius_calculator.py b/common/autoware_vehicle_info_utils/scripts/min_turning_radius_calculator.py new file mode 100755 index 00000000..0b581786 --- /dev/null +++ b/common/autoware_vehicle_info_utils/scripts/min_turning_radius_calculator.py @@ -0,0 +1,56 @@ +#!/usr/bin/env python3 + +# 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. + +import argparse +import math + +from ament_index_python.packages import get_package_share_directory +import yaml + + +def main(yaml_path): + with open(yaml_path) as f: + config = yaml.safe_load(f) + + wheel_base = config["/**"]["ros__parameters"]["wheel_base"] + max_steer_angle = config["/**"]["ros__parameters"]["max_steer_angle"] + + rear_radius = wheel_base / math.tan(max_steer_angle) + front_radius = wheel_base / math.sin(max_steer_angle) + + print("yaml path is {}".format(yaml_path)) + print( + "Minimum turning radius is {} [m] for rear, {} [m] for front.".format( + rear_radius, front_radius + ) + ) + + +if __name__ == "__main__": + default_yaml_path = ( + get_package_share_directory("autoware_vehicle_info_utils") + + "/config/vehicle_info.param.yaml" + ) + + parser = argparse.ArgumentParser() + parser.add_argument( + "-y", "--yaml", default=default_yaml_path, help="vehicle_info.param.yaml path" + ) + + args = parser.parse_args() + yaml_path = args.yaml + + main(yaml_path) diff --git a/common/autoware_vehicle_info_utils/src/vehicle_info.cpp b/common/autoware_vehicle_info_utils/src/vehicle_info.cpp new file mode 100644 index 00000000..0367341d --- /dev/null +++ b/common/autoware_vehicle_info_utils/src/vehicle_info.cpp @@ -0,0 +1,143 @@ +// 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. + +#include "autoware_vehicle_info_utils/vehicle_info.hpp" + +#include + +#include + +namespace autoware::vehicle_info_utils +{ +autoware_utils::LinearRing2d VehicleInfo::createFootprint(const double margin) const +{ + return createFootprint(margin, margin); +} + +autoware_utils::LinearRing2d VehicleInfo::createFootprint( + const double lat_margin, const double lon_margin) const +{ + using autoware_utils::LinearRing2d; + using autoware_utils::Point2d; + + const double x_front = front_overhang_m + wheel_base_m + lon_margin; + const double x_center = wheel_base_m / 2.0; + const double x_rear = -(rear_overhang_m + lon_margin); + const double y_left = wheel_tread_m / 2.0 + left_overhang_m + lat_margin; + const double y_right = -(wheel_tread_m / 2.0 + right_overhang_m + lat_margin); + + LinearRing2d footprint; + footprint.push_back(Point2d{x_front, y_left}); + footprint.push_back(Point2d{x_front, y_right}); + footprint.push_back(Point2d{x_center, y_right}); + footprint.push_back(Point2d{x_rear, y_right}); + footprint.push_back(Point2d{x_rear, y_left}); + footprint.push_back(Point2d{x_center, y_left}); + footprint.push_back(Point2d{x_front, y_left}); + + return footprint; +} + +VehicleInfo createVehicleInfo( + 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_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; + const double min_longitudinal_offset_m_ = -rear_overhang_m; + const double max_longitudinal_offset_m_ = front_overhang_m + wheel_base_m; + const double min_lateral_offset_m_ = -(wheel_tread_m / 2.0 + right_overhang_m); + const double max_lateral_offset_m_ = wheel_tread_m / 2.0 + left_overhang_m; + const double min_height_offset_m_ = 0.0; + const double max_height_offset_m_ = vehicle_height_m; + + return VehicleInfo{ + // Base parameters + 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, + // Derived parameters + vehicle_length_m_, + vehicle_width_m_, + min_longitudinal_offset_m_, + max_longitudinal_offset_m_, + min_lateral_offset_m_, + max_lateral_offset_m_, + min_height_offset_m_, + max_height_offset_m_, + }; +} + +double VehicleInfo::calcMaxCurvature() const +{ + 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(steer_angle) < 1e-6) { + return std::numeric_limits::max(); + } + const double radius = wheel_base_m / std::tan(steer_angle); + const double curvature = 1.0 / radius; + return curvature; +} + +double VehicleInfo::calcSteerAngleFromCurvature(const double curvature) const +{ + if (std::abs(curvature) < 1e-6) { + return 0.0; + } + + const double radius = 1.0 / curvature; + const double steer_angle = std::atan2(wheel_base_m, radius); + return steer_angle; +} +} // namespace autoware::vehicle_info_utils diff --git a/common/autoware_vehicle_info_utils/src/vehicle_info_utils.cpp b/common/autoware_vehicle_info_utils/src/vehicle_info_utils.cpp new file mode 100644 index 00000000..fc60f463 --- /dev/null +++ b/common/autoware_vehicle_info_utils/src/vehicle_info_utils.cpp @@ -0,0 +1,62 @@ +// 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. + +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" + +#include + +namespace +{ +template +T getParameter(rclcpp::Node & node, const std::string & name) +{ + if (node.has_parameter(name)) { + return node.get_parameter(name).get_value(); + } + + try { + return node.declare_parameter(name); + } catch (const rclcpp::ParameterTypeException & ex) { + RCLCPP_ERROR( + node.get_logger(), "Failed to get parameter `%s`, please set it when you launch the node.", + name.c_str()); + throw(ex); + } +} +} // namespace + +namespace autoware::vehicle_info_utils +{ +VehicleInfoUtils::VehicleInfoUtils(rclcpp::Node & node) +{ + const auto wheel_radius_m = getParameter(node, "wheel_radius"); + const auto wheel_width_m = getParameter(node, "wheel_width"); + const auto wheel_base_m = getParameter(node, "wheel_base"); + const auto wheel_tread_m = getParameter(node, "wheel_tread"); + const auto front_overhang_m = getParameter(node, "front_overhang"); + const auto rear_overhang_m = getParameter(node, "rear_overhang"); + const auto left_overhang_m = getParameter(node, "left_overhang"); + const auto right_overhang_m = getParameter(node, "right_overhang"); + const auto vehicle_height_m = getParameter(node, "vehicle_height"); + const auto max_steer_angle_rad = getParameter(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() const +{ + return vehicle_info_; +} +} // namespace autoware::vehicle_info_utils diff --git a/common/autoware_vehicle_info_utils/test/test_vehicle_info_utils.cpp b/common/autoware_vehicle_info_utils/test/test_vehicle_info_utils.cpp new file mode 100644 index 00000000..ea660208 --- /dev/null +++ b/common/autoware_vehicle_info_utils/test/test_vehicle_info_utils.cpp @@ -0,0 +1,74 @@ +// 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 +#include + +#include + +#include + +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("test_vehicle_info_utils", options); + } + + void TearDown() override { rclcpp::shutdown(); } + + std::shared_ptr node_; +}; + +TEST_F(VehicleInfoUtilTest, check_vehicle_info_value) +{ + using autoware::vehicle_info_utils::VehicleInfo; + using autoware::vehicle_info_utils::VehicleInfoUtils; + std::optional 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); +}