diff --git a/common/autoware_vehicle_info_utils/CHANGELOG.rst b/common/autoware_vehicle_info_utils/CHANGELOG.rst deleted file mode 100644 index a02256d5f9cea..0000000000000 --- a/common/autoware_vehicle_info_utils/CHANGELOG.rst +++ /dev/null @@ -1,56 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_vehicle_info_utils -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.40.0 (2024-12-12) -------------------- -* Merge branch 'main' into release-0.40.0 -* Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" - This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5. -* fix(static_centerline_generator): several bug fixes (`#9426 `_) - * fix: dependent packages - * feat: use steer angle, use warn for steer angle failure, calc curvature dicontinuously - * fix cppcheck - --------- -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* chore(package.xml): bump version to 0.39.0 (`#9587 `_) - * chore(package.xml): bump version to 0.39.0 - * fix: fix ticket links in CHANGELOG.rst - * fix: remove unnecessary diff - --------- - Co-authored-by: Yutaka Kondo -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* fix(cpplint): include what you use - common (`#9564 `_) -* 0.39.0 -* update changelog -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Fumiya Watanabe, M. Fatih Cırıt, Ryohsuke Mitsudome, Takayuki Murooka, Yutaka Kondo - -0.39.0 (2024-11-25) -------------------- -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.38.0 (2024-11-08) -------------------- -* unify package.xml version to 0.37.0 -* refactor(vehicle_info): add API description, add owner (`#9151 `_) -* chore(autoware_vehicle_info_utils): move package to common directory (`#8708 `_) -* Contributors: Mamoru Sobue, Ryohsuke Mitsudome, Yutaka Kondo - -0.26.0 (2024-04-03) -------------------- diff --git a/common/autoware_vehicle_info_utils/CMakeLists.txt b/common/autoware_vehicle_info_utils/CMakeLists.txt deleted file mode 100644 index 024d0428b3576..0000000000000 --- a/common/autoware_vehicle_info_utils/CMakeLists.txt +++ /dev/null @@ -1,31 +0,0 @@ -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 deleted file mode 100644 index 537dc04aa8c69..0000000000000 --- a/common/autoware_vehicle_info_utils/Readme.md +++ /dev/null @@ -1,58 +0,0 @@ -# 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 deleted file mode 100644 index b1168b1a62b7c..0000000000000 --- a/common/autoware_vehicle_info_utils/config/polygon_remover.yaml +++ /dev/null @@ -1,41 +0,0 @@ -/**: - 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 deleted file mode 100644 index 72c070c17b52c..0000000000000 --- a/common/autoware_vehicle_info_utils/config/vehicle_info.param.yaml +++ /dev/null @@ -1,13 +0,0 @@ -/**: - 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 deleted file mode 100644 index 2f586d99add43..0000000000000 --- a/common/autoware_vehicle_info_utils/config/vehicle_mirror.param.yaml +++ /dev/null @@ -1,8 +0,0 @@ -/**: - 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 deleted file mode 100644 index 51c63418acc29..0000000000000 --- a/common/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info.hpp +++ /dev/null @@ -1,86 +0,0 @@ -// 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/universe_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::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; - - 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 deleted file mode 100644 index 8c320e711cfaf..0000000000000 --- a/common/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info_utils.hpp +++ /dev/null @@ -1,44 +0,0 @@ -// 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 deleted file mode 100644 index 5f83bc695230e..0000000000000 --- a/common/autoware_vehicle_info_utils/launch/sample.launch.py +++ /dev/null @@ -1,57 +0,0 @@ -# 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 deleted file mode 100644 index ea8760f959bbb..0000000000000 --- a/common/autoware_vehicle_info_utils/launch/vehicle_info.launch.py +++ /dev/null @@ -1,36 +0,0 @@ -# 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 deleted file mode 100644 index b6a2f1ad5154a..0000000000000 --- a/common/autoware_vehicle_info_utils/package.xml +++ /dev/null @@ -1,31 +0,0 @@ - - - - autoware_vehicle_info_utils - 0.40.0 - The autoware_vehicle_info_utils package - - - Taiki Tanaka - Tomoya Kimura - Shumpei Wakabayashi - Mamoru Sobue - - Apache License 2.0 - Yamato ANDO - - ament_cmake_auto - autoware_cmake - - autoware_universe_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 deleted file mode 100755 index 0b581786b25ad..0000000000000 --- a/common/autoware_vehicle_info_utils/scripts/min_turning_radius_calculator.py +++ /dev/null @@ -1,56 +0,0 @@ -#!/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 deleted file mode 100644 index c75a4351fb3f3..0000000000000 --- a/common/autoware_vehicle_info_utils/src/vehicle_info.cpp +++ /dev/null @@ -1,143 +0,0 @@ -// 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::universe_utils::LinearRing2d VehicleInfo::createFootprint(const double margin) const -{ - return createFootprint(margin, margin); -} - -autoware::universe_utils::LinearRing2d VehicleInfo::createFootprint( - const double lat_margin, const double lon_margin) const -{ - using autoware::universe_utils::LinearRing2d; - using autoware::universe_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 deleted file mode 100644 index fc60f46315ae8..0000000000000 --- a/common/autoware_vehicle_info_utils/src/vehicle_info_utils.cpp +++ /dev/null @@ -1,62 +0,0 @@ -// 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 deleted file mode 100644 index ea660208da2be..0000000000000 --- a/common/autoware_vehicle_info_utils/test/test_vehicle_info_utils.cpp +++ /dev/null @@ -1,74 +0,0 @@ -// 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); -}