diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 75ce4e675e..469b429753 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,4 +1,5 @@ ### Automatically generated from package.xml ### +common/autoware_component_interface_specs/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-core-global-codeowners common/autoware_geography_utils/** anh.nguyen.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp @autowarefoundation/autoware-core-global-codeowners common/autoware_interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-core-global-codeowners common/autoware_kalman_filter/** koji.minoda@tier4.jp takeshi.ishita@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-core-global-codeowners @@ -8,9 +9,14 @@ common/autoware_osqp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4. common/autoware_point_types/** cynthia.liu@autocore.ai david.wong@tier4.jp max.schmeller@tier4.jp @autowarefoundation/autoware-core-global-codeowners common/autoware_qp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-core-global-codeowners common/autoware_trajectory/** mamoru.sobue@tier4.jp takayuki.murooka@tier4.jp yukinari.hisaki.2@tier4.jp @autowarefoundation/autoware-core-global-codeowners +common/autoware_vehicle_info_utils/** egon.kang@autocore.ai mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-core-global-codeowners demos/autoware_test_node/** mfc@autoware.org @autowarefoundation/autoware-core-global-codeowners localization/autoware_ekf_localizer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp takamasa.horibe@tier4.jp takeshi.ishita@tier4.jp yamato.ando@tier4.jp @autowarefoundation/autoware-core-global-codeowners localization/autoware_localization_util/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp lxg19892021@gmail.com masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp @autowarefoundation/autoware-core-global-codeowners +planning/autoware_objects_of_interest_marker_interface/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp lxg19892021@gmail.com zulfaqar.azmi@tier4.jp @autowarefoundation/autoware-core-global-codeowners +planning/autoware_path_generator/** mitsuhiro.sakamoto@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-core-global-codeowners +sensing/autoware_gnss_poser/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp lxg19892021@gmail.com masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp @autowarefoundation/autoware-core-global-codeowners +testing/autoware_pyplot/** mamoru.sobue@tier4.jp yukinari.hisaki.2@tier4.jp @autowarefoundation/autoware-core-global-codeowners testing/autoware_testing/** adam.dabrowski@robotec.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-core-global-codeowners ### Copied from .github/CODEOWNERS-manual ### diff --git a/.github/sync-files.yaml b/.github/sync-files.yaml index 448d57c084..591807cfa8 100644 --- a/.github/sync-files.yaml +++ b/.github/sync-files.yaml @@ -28,6 +28,7 @@ - source: .github/workflows/sync-files.yaml - source: .github/workflows/update-codeowners-from-packages.yaml pre-commands: | + sd " tag:update-codeowners-from-packages\n" " tag:update-codeowners-from-packages\n run:build-and-test-differential\n" {source} sd " auto-merge-method: squash\n" " auto-merge-method: squash\n global-codeowners: \"@autowarefoundation/autoware-core-global-codeowners\"\n" {source} - source: .clang-format - source: .clang-tidy diff --git a/.github/workflows/clang-tidy-pr-comments-manually.yaml b/.github/workflows/clang-tidy-pr-comments-manually.yaml index 747c62cdcd..2c05e2b26f 100644 --- a/.github/workflows/clang-tidy-pr-comments-manually.yaml +++ b/.github/workflows/clang-tidy-pr-comments-manually.yaml @@ -15,7 +15,7 @@ jobs: runs-on: ubuntu-22.04 steps: - name: Check out repository - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Download analysis results run: | @@ -40,7 +40,7 @@ jobs: - name: Check out PR head if: ${{ steps.check-fixes-yaml-existence.outputs.exists == 'true' }} - uses: actions/checkout@v3 + uses: actions/checkout@v4 with: repository: ${{ steps.set-variables.outputs.pr-head-repo }} ref: ${{ steps.set-variables.outputs.pr-head-ref }} diff --git a/.github/workflows/clang-tidy-pr-comments.yaml b/.github/workflows/clang-tidy-pr-comments.yaml index c8df6a62c9..2e9510fc3d 100644 --- a/.github/workflows/clang-tidy-pr-comments.yaml +++ b/.github/workflows/clang-tidy-pr-comments.yaml @@ -17,7 +17,7 @@ jobs: runs-on: ubuntu-22.04 steps: - name: Check out repository - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Download analysis results run: | @@ -41,7 +41,7 @@ jobs: - name: Check out PR head if: ${{ steps.check-fixes-yaml-existence.outputs.exists == 'true' }} - uses: actions/checkout@v3 + uses: actions/checkout@v4 with: repository: ${{ steps.set-variables.outputs.pr-head-repo }} ref: ${{ steps.set-variables.outputs.pr-head-ref }} diff --git a/.github/workflows/delete-closed-pr-docs.yaml b/.github/workflows/delete-closed-pr-docs.yaml index b8ff4f6d14..86301c06a9 100644 --- a/.github/workflows/delete-closed-pr-docs.yaml +++ b/.github/workflows/delete-closed-pr-docs.yaml @@ -14,7 +14,7 @@ jobs: runs-on: ubuntu-22.04 steps: - name: Check out repository - uses: actions/checkout@v3 + uses: actions/checkout@v4 with: fetch-depth: 0 diff --git a/.github/workflows/deploy-docs.yaml b/.github/workflows/deploy-docs.yaml index 47009a25e6..3af7a51d36 100644 --- a/.github/workflows/deploy-docs.yaml +++ b/.github/workflows/deploy-docs.yaml @@ -34,7 +34,7 @@ jobs: runs-on: ubuntu-22.04 steps: - name: Check out repository - uses: actions/checkout@v3 + uses: actions/checkout@v4 with: fetch-depth: 0 ref: ${{ github.event.pull_request.head.sha }} diff --git a/.github/workflows/update-codeowners-from-packages.yaml b/.github/workflows/update-codeowners-from-packages.yaml index e71e6f6275..a975315193 100644 --- a/.github/workflows/update-codeowners-from-packages.yaml +++ b/.github/workflows/update-codeowners-from-packages.yaml @@ -34,5 +34,6 @@ jobs: pr-labels: | tag:bot tag:update-codeowners-from-packages + run:build-and-test-differential auto-merge-method: squash global-codeowners: "@autowarefoundation/autoware-core-global-codeowners" diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 48a97c13ef..9d7c0531a4 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -74,7 +74,7 @@ repos: args: [--line-length=100] - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v19.1.5 + rev: v19.1.6 hooks: - id: clang-format types_or: [c++, c, cuda] diff --git a/common/autoware_vehicle_info_utils/CMakeLists.txt b/common/autoware_vehicle_info_utils/CMakeLists.txt new file mode 100644 index 0000000000..024d0428b3 --- /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 0000000000..537dc04aa8 --- /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 0000000000..b1168b1a62 --- /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 0000000000..72c070c17b --- /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 0000000000..2f586d99ad --- /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 0000000000..088487d717 --- /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 0000000000..383e6eee8a --- /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/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 0000000000..e400e67b8a --- /dev/null +++ b/common/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info.hpp @@ -0,0 +1,23 @@ +// 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. + +// this file is for backward compatibility +// TODO(NorahXiong): remove this file after the transition to the new vehicle info utils + +#ifndef AUTOWARE_VEHICLE_INFO_UTILS__VEHICLE_INFO_HPP_ +#define AUTOWARE_VEHICLE_INFO_UTILS__VEHICLE_INFO_HPP_ + +#include "autoware/vehicle_info_utils/vehicle_info.hpp" + +#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 0000000000..222be48a08 --- /dev/null +++ b/common/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info_utils.hpp @@ -0,0 +1,23 @@ +// 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. + +// this file is for backward compatibility +// TODO(NorahXiong): remove this file after the transition to the new vehicle info utils + +#ifndef AUTOWARE_VEHICLE_INFO_UTILS__VEHICLE_INFO_UTILS_HPP_ +#define AUTOWARE_VEHICLE_INFO_UTILS__VEHICLE_INFO_UTILS_HPP_ + +#include + +#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 0000000000..5f83bc6952 --- /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 0000000000..ea8760f959 --- /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 0000000000..1429ef4c1d --- /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 0000000000..0b581786b2 --- /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 0000000000..507c0c5889 --- /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 0000000000..dc3dbdc594 --- /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 0000000000..5b6d833888 --- /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); +} diff --git a/planning/autoware_path_generator/CMakeLists.txt b/planning/autoware_path_generator/CMakeLists.txt new file mode 100644 index 0000000000..bac4fb7498 --- /dev/null +++ b/planning/autoware_path_generator/CMakeLists.txt @@ -0,0 +1,28 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_path_generator) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +generate_parameter_library(path_generator_parameters + param/path_generator_parameters.yaml +) + +ament_auto_add_library(${PROJECT_NAME} SHARED + DIRECTORY src +) + +target_link_libraries(${PROJECT_NAME} + path_generator_parameters +) + +target_compile_options(${PROJECT_NAME} PRIVATE + -Wno-error=deprecated-declarations +) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::path_generator::PathGenerator" + EXECUTABLE path_generator_node +) + +ament_auto_package() diff --git a/planning/autoware_path_generator/README.md b/planning/autoware_path_generator/README.md new file mode 100644 index 0000000000..71f5618085 --- /dev/null +++ b/planning/autoware_path_generator/README.md @@ -0,0 +1,79 @@ +# Path Generator + +The `path_generator` node receives a route from `mission_planner` and converts the center line into a path. +If the route has waypoints set, it generates a path passing through them. + +This package is a simple alternative of `behavior_path_generator`. + +## Path generation + +When input data is ready, it first searches for the lanelet closest to the vehicle. +If found, it gets the lanelets within a distance of `backward_path_length` behind and `forward_path_length` in front. +Their center lines are concatenated to generate a path. + +If waypoints exist in the route, it replaces the overlapped segment of the center line with them. +The overlap interval is determined as shown in the following figure. + +![waypoint_group_overlap_interval_determination](./media/waypoint_group_overlap_interval_determination.drawio.svg) + +## Flowchart + +![Flowchart](https://www.plantuml.com/plantuml/svg/RL91RiCW4BppYZqwNt0EZTHh_u0sOHAZKa2mJkhV5qDmN2cMB2spEyCCSCh2IUOVfyIA0wNPgmefDGf_GniMFgenGtHqx3tI4x9N6cok2vt0PZcGJF0qBCW71PT1Wmy7HPGbH0Llx5MfMmfpf5L9HvQfT1joGr5cGKU9nXlaquMCB5_iuI373TUk8La_h4pMww8XsFKVlCHWlUYgdbjdTSn-eTwayYFTsL5DdEcCecMeYt_yIqIqgSU4kmrpxZRUk2YJ-1Nir1fUZw5MZyawq70DYS2NYIvOtP2p7bJjjIsRxN17SXmZ39rt0MgwTpORg_jq2xq4dkFRuFwc-ZUoCTikcIJGUYqzMK_nbj_PbRfdxdcrgWUWQjc2g5UbnZYVdStJawxg6pgLWV9K_m00) + +```plantuml +@startuml +title run +start + +:take_data; +:set_planner_data; +if (is_data_ready) then (yes) +else (no) + stop +endif + +group plan_path + group generate_path + :getClosestLanelet; + :get_lanelets_within_route; + :get_waypoint_groups; + if (any waypoint interval starts behind lanelets?) then (yes) + :get_previous_lanelet_within_route; + :extend lanelets; + endif + while (for each center line point) + if (overlapped by waypoint group?) then (yes) + if (previously overlapped?) then + else (no) + :add waypoints to path; + endif + else (no) + :add point to path; + endif + endwhile + end group +end group + +:publish path; + +stop +@enduml +``` + +## Input topics + +| Name | Type | Description | +| :------------------- | :------------------------------------------ | :------------------------------- | +| `~/input/odometry` | `nav_msgs::msg::Odometry` | ego pose | +| `~/input/vector_map` | `autoware_map_msgs::msg::LaneletMapBin` | vector map information | +| `~/input/route` | `autoware_planning_msgs::msg::LaneletRoute` | current route from start to goal | + +## Output topics + +| Name | Type | Description | QoS Durability | +| :-------------- | :----------------------------------------------------- | :------------- | :------------- | +| `~/output/path` | `autoware_internal_planning_msgs::msg::PathWithLaneId` | generated path | `volatile` | + +## Parameters + +{{ json_to_markdown("planning/autoware_path_generator/schema/path_generator.schema.json") }} diff --git a/planning/autoware_path_generator/config/path_generator.param.yaml b/planning/autoware_path_generator/config/path_generator.param.yaml new file mode 100644 index 0000000000..b6ed2383e3 --- /dev/null +++ b/planning/autoware_path_generator/config/path_generator.param.yaml @@ -0,0 +1,9 @@ +/**: + ros__parameters: + planning_hz: 10.0 + path_length: + backward: 5.0 + forward: 300.0 + waypoint_group: + separation_threshold: 1.0 + interval_margin_ratio: 10.0 diff --git a/planning/autoware_path_generator/include/autoware/path_generator/common_structs.hpp b/planning/autoware_path_generator/include/autoware/path_generator/common_structs.hpp new file mode 100644 index 0000000000..a1b774e93a --- /dev/null +++ b/planning/autoware_path_generator/include/autoware/path_generator/common_structs.hpp @@ -0,0 +1,43 @@ +// Copyright 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. + +#ifndef AUTOWARE__PATH_GENERATOR__COMMON_STRUCTS_HPP_ +#define AUTOWARE__PATH_GENERATOR__COMMON_STRUCTS_HPP_ + +#include + +#include +#include + +#include + +namespace autoware::path_generator +{ +struct PlannerData +{ + lanelet::LaneletMapPtr lanelet_map_ptr{nullptr}; + lanelet::traffic_rules::TrafficRulesPtr traffic_rules_ptr{nullptr}; + lanelet::routing::RoutingGraphPtr routing_graph_ptr{nullptr}; + + std::string route_frame_id{}; + geometry_msgs::msg::Pose goal_pose{}; + + lanelet::ConstLanelets route_lanelets{}; + lanelet::ConstLanelets preferred_lanelets{}; + lanelet::ConstLanelets start_lanelets{}; + lanelet::ConstLanelets goal_lanelets{}; +}; +} // namespace autoware::path_generator + +#endif // AUTOWARE__PATH_GENERATOR__COMMON_STRUCTS_HPP_ diff --git a/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp b/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp new file mode 100644 index 0000000000..466ecf6b8f --- /dev/null +++ b/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp @@ -0,0 +1,119 @@ +// Copyright 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. + +#ifndef AUTOWARE__PATH_GENERATOR__UTILS_HPP_ +#define AUTOWARE__PATH_GENERATOR__UTILS_HPP_ + +#include "autoware/path_generator/common_structs.hpp" + +#include + +#include +#include +#include + +namespace autoware::path_generator +{ +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; + +namespace utils +{ +/** + * @brief get lanelets within route that are in specified distance forward or backward from + * current position + * @param lanelet lanelet where ego vehicle is on + * @param planner_data planner data + * @param current_pose current pose of ego vehicle + * @param backward_distance backward distance from ego vehicle + * @param forward_distance forward distance from ego vehicle + * @return lanelets in range (std::nullopt if target lanelet is not within route) + */ +std::optional get_lanelets_within_route( + const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, + const geometry_msgs::msg::Pose & current_pose, const double backward_distance, + const double forward_distance); + +/** + * @brief get lanelets within route that are in specified distance backward from target + * lanelet + * @param lanelet target lanelet + * @param planner_data planner data + * @param distance backward distance from beginning of target lanelet + * @return lanelets in range (std::nullopt if target lanelet is not within route) + */ +std::optional get_lanelets_within_route_up_to( + const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, const double distance); + +/** + * @brief get lanelets within route that are in specified distance forward from target + * lanelet + * @param lanelet target lanelet + * @param planner_data planner data + * @param distance forward distance from end of target lanelet + * @return lanelets in range (std::nullopt if target lanelet is not within route) + */ +std::optional get_lanelets_within_route_after( + const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, const double distance); + +/** + * @brief get previous lanelet within route + * @param lanelet target lanelet + * @param planner_data planner data + * @return lanelets in range (std::nullopt if previous lanelet is not found or not + * within route) + */ +std::optional get_previous_lanelet_within_route( + const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data); + +/** + * @brief get next lanelet within route + * @param lanelet target lanelet + * @param planner_data planner data + * @return lanelets in range (std::nullopt if next lanelet is not found or not + * within route) + */ +std::optional get_next_lanelet_within_route( + const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data); + +/** + * @brief get waypoints in lanelet sequence and group them + * @param lanelet_sequence lanelet sequence + * @param lanelet_map lanelet map to get waypoints + * @param group_separation_threshold maximum distance between waypoints to belong to same + * group (see figure in README) + * @param interval_margin_ratio ratio to expand interval bound of group according to the + * lateral distance of first and last point of group + * @return waypoint groups (each group is a pair of points and its interval) + */ +std::vector>> get_waypoint_groups( + const lanelet::LaneletSequence & lanelet_sequence, const lanelet::LaneletMap & lanelet_map, + const double group_separation_threshold, const double interval_margin_ratio); + +/** + * @brief get bound of path cropped within specified range + * @param lanelet_bound original bound of lanelet + * @param lanelet_centerline centerline of lanelet + * @param s_start longitudinal distance of start of bound + * @param s_end longitudinal distance of end of bound + * @return cropped bound + */ +std::vector get_path_bound( + const lanelet::CompoundLineString2d & lanelet_bound, + const lanelet::CompoundLineString2d & lanelet_centerline, const double s_start, + const double s_end); +} // namespace utils +} // namespace autoware::path_generator + +#endif // AUTOWARE__PATH_GENERATOR__UTILS_HPP_ diff --git a/planning/autoware_path_generator/media/waypoint_group_overlap_interval_determination.drawio.svg b/planning/autoware_path_generator/media/waypoint_group_overlap_interval_determination.drawio.svg new file mode 100644 index 0000000000..64e5ffb8c1 --- /dev/null +++ b/planning/autoware_path_generator/media/waypoint_group_overlap_interval_determination.drawio.svg @@ -0,0 +1,625 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ larger than + waypoint_group_separation_threshold +
+
+
+ + larger than waypoint_group_separation_threshold + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ margin +
+
+
+ + margin + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ overlap interval +
+
+
+ + overlap interval + + + + + + + + + + + + + + + + + +
+
+
+ margin × +
+ waypoint_group_interval_margin_ratio +
+
+
+
+ + margin ×... + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ smaller than + waypoint_group_separation_threshold +
+
+
+ + smaller than waypoint_group_separation_threshold + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ waypoint group +
+
+
+ + waypoint group + + + + + + + + + + + + +
+
+
+ waypoint +
+
+
+ + waypoint + + + + + + + + + + + + + + + + + +
+
+
+ generated path +
+
+
+ + generated path + + + + + + + + + + + + + + + + + +
+
+
+ centerline point +
+
+
+ + centerline point + + + + + + + + + + + + + + + + + +
+
+
+ overlap interval +
+
+
+ + overlap interval + + + + + + + + diff --git a/planning/autoware_path_generator/package.xml b/planning/autoware_path_generator/package.xml new file mode 100644 index 0000000000..f3fef3f60d --- /dev/null +++ b/planning/autoware_path_generator/package.xml @@ -0,0 +1,31 @@ + + + + autoware_path_generator + 0.0.0 + The autoware_path_generator package + Satoshi Ota + Takayuki Murooka + Mitsuhiro Sakamoto + Apache License 2.0 + + Satoshi Ota + Takayuki Murooka + Mitsuhiro Sakamoto + + ament_cmake_auto + autoware_cmake + + autoware_internal_planning_msgs + autoware_lanelet2_extension + autoware_motion_utils + autoware_trajectory + autoware_vehicle_info_utils + generate_parameter_library + rclcpp + rclcpp_components + + + ament_cmake + + diff --git a/planning/autoware_path_generator/param/path_generator_parameters.yaml b/planning/autoware_path_generator/param/path_generator_parameters.yaml new file mode 100644 index 0000000000..2c7192427b --- /dev/null +++ b/planning/autoware_path_generator/param/path_generator_parameters.yaml @@ -0,0 +1,17 @@ +path_generator: + planning_hz: + type: double + + path_length: + backward: + type: double + + forward: + type: double + + waypoint_group: + separation_threshold: + type: double + + interval_margin_ratio: + type: double diff --git a/planning/autoware_path_generator/schema/path_generator.schema.json b/planning/autoware_path_generator/schema/path_generator.schema.json new file mode 100644 index 0000000000..8bb9c005f7 --- /dev/null +++ b/planning/autoware_path_generator/schema/path_generator.schema.json @@ -0,0 +1,50 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Path Generator Node", + "type": "object", + "definitions": { + "autoware_path_generator": { + "type": "object", + "properties": { + "planning_hz": { + "type": "number", + "description": "Planning frequency [Hz]", + "default": "10.0", + "minimum": 0.0 + }, + "backward_path_length": { + "type": "number", + "description": "Length of generated path behind vehicle [m]", + "default": "5.0", + "minimum": 0.0 + }, + "forward_path_length": { + "type": "number", + "description": "Length of generated path in front of vehicle [m]", + "default": "300.0", + "minimum": 0.0 + }, + "waypoint_group_separation_threshold": { + "type": "number", + "description": "Maximum distance at which consecutive waypoints are considered to belong to the same group [m]", + "default": "1.0", + "minimum": 0.0 + }, + "waypoint_group_interval_margin_ratio": { + "type": "number", + "description": "Ratio for determining length of switching section from centerline to waypoints", + "default": "10.0", + "minimum": 0.0 + } + }, + "required": [ + "planning_hz", + "backward_path_length", + "forward_path_length", + "waypoint_group_separation_threshold", + "waypoint_group_interval_margin_ratio" + ], + "additionalProperties": false + } + } +} diff --git a/planning/autoware_path_generator/src/node.cpp b/planning/autoware_path_generator/src/node.cpp new file mode 100644 index 0000000000..b869491aad --- /dev/null +++ b/planning/autoware_path_generator/src/node.cpp @@ -0,0 +1,378 @@ +// Copyright 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 "node.hpp" + +#include "autoware/path_generator/utils.hpp" + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +namespace +{ +template +double get_arc_length_along_centerline(const LaneletT & lanelet, const PointT & point) +{ + return lanelet::geometry::toArcCoordinates(lanelet.centerline2d(), lanelet::utils::to2D(point)) + .length; +} +} // namespace + +namespace autoware::path_generator +{ +PathGenerator::PathGenerator(const rclcpp::NodeOptions & node_options) +: Node("path_generator", node_options) +{ + param_listener_ = + std::make_shared<::path_generator::ParamListener>(this->get_node_parameters_interface()); + + path_publisher_ = create_publisher("~/output/path", 1); + + vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); + + const auto params = param_listener_->get_params(); + timer_ = rclcpp::create_timer( + this, get_clock(), rclcpp::Rate(params.planning_hz).period(), + std::bind(&PathGenerator::run, this)); +} + +void PathGenerator::run() +{ + const auto input_data = take_data(); + set_planner_data(input_data); + if (!is_data_ready(input_data)) { + return; + } + + const auto path = plan_path(input_data); + if (!path) { + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 5000, "output path is invalid"); + return; + } + + path_publisher_->publish(*path); +} + +PathGenerator::InputData PathGenerator::take_data() +{ + InputData input_data; + + // route + if (const auto msg = route_subscriber_.take_data()) { + if (msg->segments.empty()) { + RCLCPP_ERROR(get_logger(), "input route is empty, ignoring..."); + } else { + input_data.route_ptr = msg; + } + } + + // map + if (const auto msg = vector_map_subscriber_.take_data()) { + input_data.lanelet_map_bin_ptr = msg; + } + + // velocity + if (const auto msg = odometry_subscriber_.take_data()) { + input_data.odometry_ptr = msg; + } + + return input_data; +} + +void PathGenerator::set_planner_data(const InputData & input_data) +{ + if (input_data.lanelet_map_bin_ptr) { + planner_data_.lanelet_map_ptr = std::make_shared(); + lanelet::utils::conversion::fromBinMsg( + *input_data.lanelet_map_bin_ptr, planner_data_.lanelet_map_ptr, + &planner_data_.traffic_rules_ptr, &planner_data_.routing_graph_ptr); + } + + if (input_data.route_ptr) { + set_route(input_data.route_ptr); + } +} + +void PathGenerator::set_route(const LaneletRoute::ConstSharedPtr & route_ptr) +{ + planner_data_.route_frame_id = route_ptr->header.frame_id; + planner_data_.goal_pose = route_ptr->goal_pose; + + planner_data_.route_lanelets.clear(); + planner_data_.preferred_lanelets.clear(); + planner_data_.start_lanelets.clear(); + planner_data_.goal_lanelets.clear(); + + size_t primitives_num = 0; + for (const auto & route_section : route_ptr->segments) { + primitives_num += route_section.primitives.size(); + } + planner_data_.route_lanelets.reserve(primitives_num); + + for (const auto & route_section : route_ptr->segments) { + for (const auto & primitive : route_section.primitives) { + const auto id = primitive.id; + const auto & lanelet = planner_data_.lanelet_map_ptr->laneletLayer.get(id); + planner_data_.route_lanelets.push_back(lanelet); + if (id == route_section.preferred_primitive.id) { + planner_data_.preferred_lanelets.push_back(lanelet); + } + } + } + + const auto set_lanelets_from_segment = + [&]( + const autoware_planning_msgs::msg::LaneletSegment & segment, + lanelet::ConstLanelets & lanelets) { + lanelets.reserve(segment.primitives.size()); + for (const auto & primitive : segment.primitives) { + const auto & lanelet = planner_data_.lanelet_map_ptr->laneletLayer.get(primitive.id); + lanelets.push_back(lanelet); + } + }; + set_lanelets_from_segment(route_ptr->segments.front(), planner_data_.start_lanelets); + set_lanelets_from_segment(route_ptr->segments.back(), planner_data_.goal_lanelets); +} + +bool PathGenerator::is_data_ready(const InputData & input_data) +{ + const auto notify_waiting = [this](const std::string & name) { + RCLCPP_INFO_SKIPFIRST_THROTTLE( + get_logger(), *get_clock(), 5000, "waiting for %s", name.c_str()); + }; + + if (!planner_data_.lanelet_map_ptr) { + notify_waiting("map"); + return false; + } + + if (planner_data_.route_lanelets.empty()) { + notify_waiting("route"); + return false; + } + + if (!input_data.odometry_ptr) { + notify_waiting("odometry"); + return false; + } + + return true; +} + +std::optional PathGenerator::plan_path(const InputData & input_data) +{ + const auto path = + generate_path(input_data.odometry_ptr->pose.pose, param_listener_->get_params()); + + if (!path) { + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 5000, "output path is invalid"); + return std::nullopt; + } else if (path->points.empty()) { + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 5000, "output path is empty"); + return std::nullopt; + } + + return path; +} + +std::optional PathGenerator::generate_path( + const geometry_msgs::msg::Pose & current_pose, const Params & params) const +{ + lanelet::ConstLanelet current_lane; + if (!lanelet::utils::query::getClosestLanelet( + planner_data_.preferred_lanelets, current_pose, ¤t_lane)) { + return std::nullopt; + } + + const auto lanelets = utils::get_lanelets_within_route( + current_lane, planner_data_, current_pose, params.path_length.backward, + params.path_length.forward); + if (!lanelets) { + return std::nullopt; + } + + return generate_path(*lanelets, current_pose, params); +} + +std::optional PathGenerator::generate_path( + const lanelet::LaneletSequence & lanelet_sequence, const geometry_msgs::msg::Pose & current_pose, + const Params & params) const +{ + if (lanelet_sequence.empty()) { + return std::nullopt; + } + + const auto & lanelets = lanelet_sequence.lanelets(); + + const auto arc_coordinates = lanelet::utils::getArcCoordinates(lanelets, current_pose); + const auto s = arc_coordinates.length; // s denotes longitudinal position in Frenet coordinates + const auto s_start = std::max(0., s - params.path_length.backward); + const auto s_end = [&]() { + auto s_end = s + params.path_length.forward; + + if (!utils::get_next_lanelet_within_route(lanelets.back(), planner_data_)) { + s_end = std::min(s_end, lanelet::utils::getLaneletLength2d(lanelets)); + } + + if (std::any_of( + planner_data_.goal_lanelets.begin(), planner_data_.goal_lanelets.end(), + [&](const auto & goal_lanelet) { return lanelets.back().id() == goal_lanelet.id(); })) { + const auto goal_arc_coordinates = + lanelet::utils::getArcCoordinates(lanelets, planner_data_.goal_pose); + s_end = std::min(s_end, goal_arc_coordinates.length); + } + + return s_end; + }(); + + return generate_path(lanelet_sequence, s_start, s_end, params); +} + +std::optional PathGenerator::generate_path( + const lanelet::LaneletSequence & lanelet_sequence, const double s_start, const double s_end, + const Params & params) const +{ + std::vector path_points_with_lane_id{}; + + const auto add_path_point = [&](const auto & path_point, const lanelet::ConstLanelet & lanelet) { + PathPointWithLaneId path_point_with_lane_id{}; + path_point_with_lane_id.lane_ids.push_back(lanelet.id()); + path_point_with_lane_id.point.pose.position = + lanelet::utils::conversion::toGeomMsgPt(path_point); + path_point_with_lane_id.point.longitudinal_velocity_mps = + planner_data_.traffic_rules_ptr->speedLimit(lanelet).speedLimit.value(); + path_points_with_lane_id.push_back(std::move(path_point_with_lane_id)); + }; + + const auto waypoint_groups = utils::get_waypoint_groups( + lanelet_sequence, *planner_data_.lanelet_map_ptr, params.waypoint_group.separation_threshold, + params.waypoint_group.interval_margin_ratio); + + auto extended_lanelets = lanelet_sequence.lanelets(); + auto s_offset = 0.; + + for (const auto & [waypoints, interval] : waypoint_groups) { + if (interval.first > 0.) { + continue; + } + const auto prev_lanelet = + utils::get_previous_lanelet_within_route(extended_lanelets.front(), planner_data_); + if (!prev_lanelet) { + break; + } + extended_lanelets.insert(extended_lanelets.begin(), *prev_lanelet); + s_offset = lanelet::geometry::length2d(*prev_lanelet); + break; + } + + const lanelet::LaneletSequence extended_lanelet_sequence(extended_lanelets); + std::optional overlapping_waypoint_group_index = std::nullopt; + + for (auto lanelet_it = extended_lanelet_sequence.begin(); + lanelet_it != extended_lanelet_sequence.end(); ++lanelet_it) { + const auto & centerline = lanelet_it->centerline(); + auto s = get_arc_length_along_centerline(extended_lanelet_sequence, centerline.front()); + + for (auto point_it = centerline.begin(); point_it != centerline.end(); ++point_it) { + if (point_it != centerline.begin()) { + s += lanelet::geometry::distance2d(*std::prev(point_it), *point_it); + } else if (lanelet_it != extended_lanelet_sequence.begin()) { + continue; + } + + if (overlapping_waypoint_group_index) { + const auto & [waypoints, interval] = waypoint_groups[*overlapping_waypoint_group_index]; + if (s >= interval.first + s_offset && s <= interval.second + s_offset) { + continue; + } + overlapping_waypoint_group_index = std::nullopt; + } + + for (size_t i = 0; i < waypoint_groups.size(); ++i) { + const auto & [waypoints, interval] = waypoint_groups[i]; + if (s < interval.first + s_offset || s > interval.second + s_offset) { + continue; + } + for (const auto & waypoint : waypoints) { + const auto s_waypoint = + get_arc_length_along_centerline(extended_lanelet_sequence, waypoint); + for (auto waypoint_lanelet_it = extended_lanelet_sequence.begin(); + waypoint_lanelet_it != extended_lanelet_sequence.end(); ++waypoint_lanelet_it) { + if ( + s_waypoint > get_arc_length_along_centerline( + extended_lanelet_sequence, waypoint_lanelet_it->centerline().back())) { + continue; + } + add_path_point(waypoint, *waypoint_lanelet_it); + break; + } + } + overlapping_waypoint_group_index = i; + break; + } + + if (overlapping_waypoint_group_index) { + continue; + } + add_path_point(*point_it, *lanelet_it); + } + } + + auto trajectory = Trajectory::Builder().build(path_points_with_lane_id); + if (!trajectory) { + return std::nullopt; + } + + trajectory->align_orientation_with_trajectory_direction(); + trajectory->crop( + s_offset + s_start - + get_arc_length_along_centerline( + extended_lanelet_sequence, lanelet::utils::conversion::toLaneletPoint( + path_points_with_lane_id.front().point.pose.position)), + s_end - s_start); + + PathWithLaneId path{}; + path.header.frame_id = planner_data_.route_frame_id; + path.header.stamp = now(); + path.points = trajectory->restore(); + + const auto get_path_bound = [&](const lanelet::CompoundLineString2d & lanelet_bound) { + const auto s_bound_start = + std::max(0., s_offset + s_start - vehicle_info_.max_longitudinal_offset_m); + const auto s_bound_end = + std::max(0., s_offset + s_end + vehicle_info_.max_longitudinal_offset_m); + return utils::get_path_bound( + lanelet_bound, extended_lanelet_sequence.centerline2d(), s_bound_start, s_bound_end); + }; + path.left_bound = get_path_bound(extended_lanelet_sequence.leftBound2d()); + path.right_bound = get_path_bound(extended_lanelet_sequence.rightBound2d()); + + return path; +} +} // namespace autoware::path_generator + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::path_generator::PathGenerator) diff --git a/planning/autoware_path_generator/src/node.hpp b/planning/autoware_path_generator/src/node.hpp new file mode 100644 index 0000000000..7a7db18563 --- /dev/null +++ b/planning/autoware_path_generator/src/node.hpp @@ -0,0 +1,100 @@ +// Copyright 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. + +#ifndef NODE_HPP_ +#define NODE_HPP_ + +#include "autoware/path_generator/common_structs.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace autoware::path_generator +{ +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; +using autoware_map_msgs::msg::LaneletMapBin; +using autoware_planning_msgs::msg::LaneletRoute; +using nav_msgs::msg::Odometry; +using ::path_generator::Params; +using Trajectory = autoware::trajectory::Trajectory; + +class PathGenerator : public rclcpp::Node +{ +public: + explicit PathGenerator(const rclcpp::NodeOptions & node_options); + +private: + struct InputData + { + LaneletRoute::ConstSharedPtr route_ptr{nullptr}; + LaneletMapBin::ConstSharedPtr lanelet_map_bin_ptr{nullptr}; + Odometry::ConstSharedPtr odometry_ptr{nullptr}; + }; + + // subscriber + autoware_utils::InterProcessPollingSubscriber< + LaneletRoute, autoware_utils::polling_policy::Newest> + route_subscriber_{this, "~/input/route", rclcpp::QoS{1}.transient_local()}; + autoware_utils::InterProcessPollingSubscriber< + LaneletMapBin, autoware_utils::polling_policy::Newest> + vector_map_subscriber_{this, "~/input/vector_map", rclcpp::QoS{1}.transient_local()}; + autoware_utils::InterProcessPollingSubscriber odometry_subscriber_{ + this, "~/input/odometry"}; + + // publisher + rclcpp::Publisher::SharedPtr path_publisher_; + + rclcpp::TimerBase::SharedPtr timer_; + + std::shared_ptr<::path_generator::ParamListener> param_listener_; + + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; + PlannerData planner_data_; + + void run(); + + InputData take_data(); + + void set_planner_data(const InputData & input_data); + + void set_route(const LaneletRoute::ConstSharedPtr & route_ptr); + + bool is_data_ready(const InputData & input_data); + + std::optional plan_path(const InputData & input_data); + + std::optional generate_path( + const geometry_msgs::msg::Pose & current_pose, const Params & params) const; + + std::optional generate_path( + const lanelet::LaneletSequence & lanelet_sequence, + const geometry_msgs::msg::Pose & current_pose, const Params & params) const; + + std::optional generate_path( + const lanelet::LaneletSequence & lanelet_sequence, const double s_start, const double s_end, + const Params & params) const; +}; +} // namespace autoware::path_generator + +#endif // NODE_HPP_ diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp new file mode 100644 index 0000000000..de993907ac --- /dev/null +++ b/planning/autoware_path_generator/src/utils.cpp @@ -0,0 +1,253 @@ +// Copyright 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 "autoware/path_generator/utils.hpp" + +#include +#include +#include + +#include + +#include +#include + +namespace autoware::path_generator +{ +namespace utils +{ +namespace +{ +template +bool exists(const std::vector & vec, const T & item) +{ + return std::find(vec.begin(), vec.end(), item) != vec.end(); +} +} // namespace + +std::optional get_lanelets_within_route( + const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, + const geometry_msgs::msg::Pose & current_pose, const double backward_distance, + const double forward_distance) +{ + if (!exists(planner_data.route_lanelets, lanelet)) { + return std::nullopt; + } + + const auto arc_coordinates = lanelet::utils::getArcCoordinates({lanelet}, current_pose); + const auto lanelet_length = lanelet::utils::getLaneletLength2d(lanelet); + + const auto backward_lanelets = get_lanelets_within_route_up_to( + lanelet, planner_data, backward_distance - arc_coordinates.length); + if (!backward_lanelets) { + return std::nullopt; + } + + const auto forward_lanelets = get_lanelets_within_route_after( + lanelet, planner_data, forward_distance - (lanelet_length - arc_coordinates.length)); + if (!forward_lanelets) { + return std::nullopt; + } + + lanelet::ConstLanelets lanelets(std::move(*backward_lanelets)); + lanelets.push_back(lanelet); + std::move(forward_lanelets->begin(), forward_lanelets->end(), std::back_inserter(lanelets)); + + return lanelets; +} + +std::optional get_lanelets_within_route_up_to( + const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, const double distance) +{ + if (!exists(planner_data.route_lanelets, lanelet)) { + return std::nullopt; + } + + lanelet::ConstLanelets lanelets{}; + auto current_lanelet = lanelet; + auto length = 0.; + + while (rclcpp::ok() && length < distance) { + const auto prev_lanelet = get_previous_lanelet_within_route(current_lanelet, planner_data); + if (!prev_lanelet) { + break; + } + + lanelets.push_back(*prev_lanelet); + current_lanelet = *prev_lanelet; + length += lanelet::utils::getLaneletLength2d(*prev_lanelet); + } + + std::reverse(lanelets.begin(), lanelets.end()); + return lanelets; +} + +std::optional get_lanelets_within_route_after( + const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, const double distance) +{ + if (!exists(planner_data.route_lanelets, lanelet)) { + return std::nullopt; + } + + lanelet::ConstLanelets lanelets{}; + auto current_lanelet = lanelet; + auto length = 0.; + + while (rclcpp::ok() && length < distance) { + const auto next_lanelet = get_next_lanelet_within_route(current_lanelet, planner_data); + if (!next_lanelet) { + break; + } + + lanelets.push_back(*next_lanelet); + current_lanelet = *next_lanelet; + length += lanelet::utils::getLaneletLength2d(*next_lanelet); + } + + return lanelets; +} + +std::optional get_previous_lanelet_within_route( + const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data) +{ + if (exists(planner_data.start_lanelets, lanelet)) { + return std::nullopt; + } + + const auto prev_lanelets = planner_data.routing_graph_ptr->previous(lanelet); + if (prev_lanelets.empty()) { + return std::nullopt; + } + + const auto prev_lanelet_itr = std::find_if( + prev_lanelets.cbegin(), prev_lanelets.cend(), + [&](const lanelet::ConstLanelet & l) { return exists(planner_data.route_lanelets, l); }); + if (prev_lanelet_itr == prev_lanelets.cend()) { + return std::nullopt; + } + return *prev_lanelet_itr; +} + +std::optional get_next_lanelet_within_route( + const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data) +{ + if (planner_data.preferred_lanelets.empty()) { + return std::nullopt; + } + + if (exists(planner_data.goal_lanelets, lanelet)) { + return std::nullopt; + } + + const auto next_lanelets = planner_data.routing_graph_ptr->following(lanelet); + if ( + next_lanelets.empty() || + next_lanelets.front().id() == planner_data.preferred_lanelets.front().id()) { + return std::nullopt; + } + + const auto next_lanelet_itr = std::find_if( + next_lanelets.cbegin(), next_lanelets.cend(), + [&](const lanelet::ConstLanelet & l) { return exists(planner_data.route_lanelets, l); }); + if (next_lanelet_itr == next_lanelets.cend()) { + return std::nullopt; + } + return *next_lanelet_itr; +} + +std::vector>> get_waypoint_groups( + const lanelet::LaneletSequence & lanelet_sequence, const lanelet::LaneletMap & lanelet_map, + const double group_separation_threshold, const double interval_margin_ratio) +{ + std::vector>> waypoint_groups{}; + + const auto get_interval_bound = + [&](const lanelet::ConstPoint3d & point, const double lateral_distance_factor) { + const auto arc_coordinates = lanelet::geometry::toArcCoordinates( + lanelet_sequence.centerline2d(), lanelet::utils::to2D(point)); + return arc_coordinates.length + lateral_distance_factor * std::abs(arc_coordinates.distance); + }; + + for (const auto & lanelet : lanelet_sequence) { + if (!lanelet.hasAttribute("waypoints")) { + continue; + } + + const auto waypoints_id = lanelet.attribute("waypoints").asId().value(); + const auto & waypoints = lanelet_map.lineStringLayer.get(waypoints_id); + + if ( + waypoint_groups.empty() || + lanelet::geometry::distance2d(waypoint_groups.back().first.back(), waypoints.front()) > + group_separation_threshold) { + waypoint_groups.emplace_back().second.first = + get_interval_bound(waypoints.front(), -interval_margin_ratio); + } + waypoint_groups.back().second.second = + get_interval_bound(waypoints.back(), interval_margin_ratio); + + waypoint_groups.back().first.insert( + waypoint_groups.back().first.end(), waypoints.begin(), waypoints.end()); + } + + return waypoint_groups; +} + +std::vector get_path_bound( + const lanelet::CompoundLineString2d & lanelet_bound, + const lanelet::CompoundLineString2d & lanelet_centerline, const double s_start, + const double s_end) +{ + const auto path_start_point = + lanelet::geometry::interpolatedPointAtDistance(lanelet_centerline, s_start); + const auto path_end_point = + lanelet::geometry::interpolatedPointAtDistance(lanelet_centerline, s_end); + + auto s_bound_start = + lanelet::geometry::toArcCoordinates( + lanelet::utils::to2D(lanelet_bound.lineStrings().front()), path_start_point) + .length; + auto s_bound_end = lanelet::geometry::toArcCoordinates(lanelet_bound, path_end_point).length; + + std::vector path_bound{}; + auto s = 0.; + + for (auto it = lanelet_bound.begin(); it != std::prev(lanelet_bound.end()); ++it) { + s += lanelet::geometry::distance2d(*it, *std::next(it)); + if (s < s_bound_start) { + continue; + } + + if (path_bound.empty()) { + const auto interpolated_point = + lanelet::geometry::interpolatedPointAtDistance(lanelet_bound, s_bound_start); + path_bound.push_back( + lanelet::utils::conversion::toGeomMsgPt(lanelet::utils::to3D(interpolated_point))); + } else { + path_bound.push_back(lanelet::utils::conversion::toGeomMsgPt(*it)); + } + + if (s >= s_bound_end) { + const auto interpolated_point = + lanelet::geometry::interpolatedPointAtDistance(lanelet_bound, s_bound_end); + path_bound.push_back( + lanelet::utils::conversion::toGeomMsgPt(lanelet::utils::to3D(interpolated_point))); + break; + } + } + + return path_bound; +} +} // namespace utils +} // namespace autoware::path_generator