diff --git a/.cspell-partial.json b/.cspell-partial.json index 45b85e779c39d..f45ae8961c56b 100644 --- a/.cspell-partial.json +++ b/.cspell-partial.json @@ -1,5 +1,9 @@ { - "ignorePaths": ["**/perception/**", "**/sensing/**", "perception/bytetrack/lib/**"], + "ignorePaths": [ + "**/perception/**", + "sensing/tier4_pcl_extensions/include/**", + "perception/bytetrack/lib/**" + ], "ignoreRegExpList": [], "words": [] } diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index fb6df15ab7679..03f9f22b56015 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -14,6 +14,7 @@ common/component_interface_tools/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp m common/component_interface_utils/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp yukihiro.saito@tier4.jp common/cuda_utils/** daisuke.nishimatsu@tier4.jp manato.hirabayashi@tier4.jp common/fake_test_node/** opensource@apex.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +common/geography_utils/** koji.minoda@tier4.jp common/global_parameter_loader/** ryohsuke.mitsudome@tier4.jp common/glog_component/** takamasa.horibe@tier4.jp common/goal_distance_calculator/** taiki.tanaka@tier4.jp @@ -117,7 +118,7 @@ perception/lidar_apollo_segmentation_tvm/** ambroise.vincent@arm.com xinyu.wang@ perception/lidar_apollo_segmentation_tvm_nodes/** ambroise.vincent@arm.com xinyu.wang@tier4.jp perception/lidar_centerpoint/** kenzo.lobos@tier4.jp yusuke.muramatsu@tier4.jp perception/lidar_centerpoint_tvm/** carl.liu@autocore.ai xinyu.wang@tier4.jp -perception/map_based_prediction/** shunsuke.miura@tier4.jp tomoya.kimura@tier4.jp yoshi.ri@tier4.jp yutaka.shimizu@tier4.jp +perception/map_based_prediction/** kyoichi.sugahara@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yoshi.ri@tier4.jp yutaka.shimizu@tier4.jp perception/multi_object_tracker/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/object_merger/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/object_range_splitter/** yukihiro.saito@tier4.jp @@ -166,7 +167,7 @@ planning/freespace_planning_algorithms/** kosuke.takeuchi@tier4.jp takamasa.hori planning/mission_planner/** isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp ryohsuke.mitsudome@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp planning/motion_velocity_smoother/** fumiya.watanabe@tier4.jp makoto.kurihara@tier4.jp takamasa.horibe@tier4.jp yutaka.shimizu@tier4.jp planning/obstacle_avoidance_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -planning/obstacle_cruise_planner/** takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp +planning/obstacle_cruise_planner/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp planning/obstacle_stop_planner/** berkay@leodrive.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp planning/obstacle_velocity_limiter/** maxime.clement@tier4.jp planning/path_smoother/** maxime.clement@tier4.jp takayuki.murooka@tier4.jp diff --git a/.github/workflows/beta-to-tier4-main-sync.yaml b/.github/workflows/beta-to-tier4-main-sync.yaml new file mode 100644 index 0000000000000..53f40f1e811ab --- /dev/null +++ b/.github/workflows/beta-to-tier4-main-sync.yaml @@ -0,0 +1,34 @@ +name: beta-to-tier4-main-sync + +on: + workflow_dispatch: + inputs: + source_branch: + description: Source branch + required: true + type: string + +jobs: + sync-beta-branch: + runs-on: ubuntu-latest + steps: + - name: Generate token + id: generate-token + uses: tibdex/github-app-token@v1 + with: + app_id: ${{ secrets.APP_ID }} + private_key: ${{ secrets.PRIVATE_KEY }} + + - name: Run sync-branches + uses: autowarefoundation/autoware-github-actions/sync-branches@v1 + with: + token: ${{ steps.generate-token.outputs.token }} + base-branch: tier4/main + sync-pr-branch: beta-to-tier4-main-sync + sync-target-repository: https://github.com/tier4/autoware.universe.git + sync-target-branch: ${{ inputs.source_branch }} + pr-title: "chore: sync beta branch ${{ inputs.source_branch }} with tier4/main" + pr-labels: | + bot + sync-beta-branch + auto-merge-method: merge diff --git a/.github/workflows/dispatch-push-event.yaml b/.github/workflows/dispatch-push-event.yaml new file mode 100644 index 0000000000000..492083c8038f6 --- /dev/null +++ b/.github/workflows/dispatch-push-event.yaml @@ -0,0 +1,45 @@ +name: dispatch-push-event +on: + push: + +jobs: + search-dispatch-repo: + runs-on: ubuntu-latest + strategy: + matrix: + include: + - { version: beta/v0.3.**, dispatch-repo: pilot-auto.x1.eve } + outputs: + dispatch-repo: ${{ steps.search-dispatch-repo.outputs.value }} + steps: + - name: Search dispatch repo + id: search-dispatch-repo + run: | + if [[ ${{ github.ref_name }} =~ ${{ matrix.version }} ]]; then + echo ::set-output name=value::"${{ matrix.dispatch-repo }}" + echo "Detected beta branch: ${{ github.ref_name }}" + echo "Dispatch repository: ${{ matrix.dispatch-repo }}" + fi + + dispatch-push-event: + runs-on: ubuntu-latest + needs: search-dispatch-repo + if: ${{ needs.search-dispatch-repo.outputs.dispatch-repo != '' }} + steps: + - name: Generate token + id: generate-token + uses: tibdex/github-app-token@v1 + with: + app_id: ${{ secrets.INTERNAL_APP_ID }} + private_key: ${{ secrets.INTERNAL_PRIVATE_KEY }} + + # 注意: workflow_dispatchで指定するブランチはmain固定となっているため、dispatch-repoのmainブランチにupdate-beta-branch.yamlが存在することが前提条件。 + - name: Dispatch the update-beta-branch workflow + run: | + curl -L \ + -X POST \ + -H "Accept: application/vnd.github+json" \ + -H "Authorization: Bearer ${{ steps.generate-token.outputs.token }}" \ + -H "X-GitHub-Api-Version: 2022-11-28" \ + https://api.github.com/repos/tier4/${{ needs.search-dispatch-repo.outputs.dispatch-repo }}/actions/workflows/update-beta-branch.yaml/dispatches \ + -d '{"ref":"main"}' diff --git a/.github/workflows/sync-upstream.yaml b/.github/workflows/sync-upstream.yaml index 267fe2135a627..90420ec404042 100644 --- a/.github/workflows/sync-upstream.yaml +++ b/.github/workflows/sync-upstream.yaml @@ -1,9 +1,12 @@ name: sync-upstream on: - schedule: - - cron: 0 1 * * 1-5 workflow_dispatch: + inputs: + target_branch: + description: Target branch + required: true + type: string jobs: sync-upstream: @@ -28,12 +31,17 @@ jobs: script: | const holiday_jp = require(`${process.env.GITHUB_WORKSPACE}/node_modules/@holiday-jp/holiday_jp`) core.setOutput('holiday', holiday_jp.isHoliday(new Date())); + - name: Print warning for invalid branch name + if: ${{ inputs.target_branch == 'tier4/main' }} + run: | + echo This action cannot be performed on 'tier4/main' branch + - name: Run sync-branches + if: ${{ inputs.target_branch != 'tier4/main' }} uses: autowarefoundation/autoware-github-actions/sync-branches@v1 - if: ${{ steps.is-holiday.outputs.holiday != 'true' || github.event_name == 'workflow_dispatch' }} with: token: ${{ steps.generate-token.outputs.token }} - base-branch: tier4/main + base-branch: ${{ inputs.target_branch }} sync-pr-branch: sync-upstream sync-target-repository: https://github.com/autowarefoundation/autoware.universe.git sync-target-branch: main diff --git a/common/geography_utils/CMakeLists.txt b/common/geography_utils/CMakeLists.txt new file mode 100644 index 0000000000000..b705e819c059b --- /dev/null +++ b/common/geography_utils/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 3.14) +project(geography_utils) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +# GeographicLib +find_package(PkgConfig) +find_path(GeographicLib_INCLUDE_DIR GeographicLib/Config.h + PATH_SUFFIXES GeographicLib +) +set(GeographicLib_INCLUDE_DIRS ${GeographicLib_INCLUDE_DIR}) +find_library(GeographicLib_LIBRARIES NAMES Geographic) + +ament_auto_add_library(geography_utils SHARED + src/height.cpp + src/projection.cpp + src/lanelet2_projector.cpp +) + +target_link_libraries(geography_utils + ${GeographicLib_LIBRARIES} +) + +if(BUILD_TESTING) + find_package(ament_cmake_ros REQUIRED) + + file(GLOB_RECURSE test_files test/*.cpp) + + ament_add_ros_isolated_gtest(test_geography_utils ${test_files}) + + target_link_libraries(test_geography_utils + geography_utils + ) +endif() + +ament_auto_package() diff --git a/common/geography_utils/README.md b/common/geography_utils/README.md new file mode 100644 index 0000000000000..fb4c2dc3a8312 --- /dev/null +++ b/common/geography_utils/README.md @@ -0,0 +1,5 @@ +# geography_utils + +## Purpose + +This package contains geography-related functions used by other packages, so please refer to them as needed. diff --git a/common/geography_utils/include/geography_utils/height.hpp b/common/geography_utils/include/geography_utils/height.hpp new file mode 100644 index 0000000000000..8bcda41f84cda --- /dev/null +++ b/common/geography_utils/include/geography_utils/height.hpp @@ -0,0 +1,33 @@ +// Copyright 2023 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 GEOGRAPHY_UTILS__HEIGHT_HPP_ +#define GEOGRAPHY_UTILS__HEIGHT_HPP_ + +#include + +namespace geography_utils +{ + +typedef double (*HeightConversionFunction)( + const double height, const double latitude, const double longitude); +double convert_wgs84_to_egm2008(const double height, const double latitude, const double longitude); +double convert_egm2008_to_wgs84(const double height, const double latitude, const double longitude); +double convert_height( + const double height, const double latitude, const double longitude, + const std::string & source_vertical_datum, const std::string & target_vertical_datum); + +} // namespace geography_utils + +#endif // GEOGRAPHY_UTILS__HEIGHT_HPP_ diff --git a/common/geography_utils/include/geography_utils/lanelet2_projector.hpp b/common/geography_utils/include/geography_utils/lanelet2_projector.hpp new file mode 100644 index 0000000000000..739abe7019023 --- /dev/null +++ b/common/geography_utils/include/geography_utils/lanelet2_projector.hpp @@ -0,0 +1,32 @@ +// Copyright 2023 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 GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_ +#define GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_ + +#include + +#include + +#include + +namespace geography_utils +{ +using MapProjectorInfo = tier4_map_msgs::msg::MapProjectorInfo; + +std::unique_ptr get_lanelet2_projector(const MapProjectorInfo & projector_info); + +} // namespace geography_utils + +#endif // GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_ diff --git a/common/geography_utils/include/geography_utils/projection.hpp b/common/geography_utils/include/geography_utils/projection.hpp new file mode 100644 index 0000000000000..be616d854a466 --- /dev/null +++ b/common/geography_utils/include/geography_utils/projection.hpp @@ -0,0 +1,33 @@ +// Copyright 2023 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 GEOGRAPHY_UTILS__PROJECTION_HPP_ +#define GEOGRAPHY_UTILS__PROJECTION_HPP_ + +#include +#include +#include + +namespace geography_utils +{ +using MapProjectorInfo = tier4_map_msgs::msg::MapProjectorInfo; +using GeoPoint = geographic_msgs::msg::GeoPoint; +using LocalPoint = geometry_msgs::msg::Point; + +LocalPoint project_forward(const GeoPoint & geo_point, const MapProjectorInfo & projector_info); +GeoPoint project_reverse(const LocalPoint & local_point, const MapProjectorInfo & projector_info); + +} // namespace geography_utils + +#endif // GEOGRAPHY_UTILS__PROJECTION_HPP_ diff --git a/common/geography_utils/package.xml b/common/geography_utils/package.xml new file mode 100644 index 0000000000000..414364e36ad29 --- /dev/null +++ b/common/geography_utils/package.xml @@ -0,0 +1,27 @@ + + + + geography_utils + 0.1.0 + The geography_utils package + Koji Minoda + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + geographic_msgs + geographiclib + geometry_msgs + lanelet2_extension + lanelet2_io + tier4_map_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/common/geography_utils/src/height.cpp b/common/geography_utils/src/height.cpp new file mode 100644 index 0000000000000..fe69557f25a76 --- /dev/null +++ b/common/geography_utils/src/height.cpp @@ -0,0 +1,63 @@ +// Copyright 2023 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 +#include +#include + +namespace geography_utils +{ + +double convert_wgs84_to_egm2008(const double height, const double latitude, const double longitude) +{ + GeographicLib::Geoid egm2008("egm2008-1"); + // cSpell: ignore ELLIPSOIDTOGEOID + return egm2008.ConvertHeight(latitude, longitude, height, GeographicLib::Geoid::ELLIPSOIDTOGEOID); +} + +double convert_egm2008_to_wgs84(const double height, const double latitude, const double longitude) +{ + GeographicLib::Geoid egm2008("egm2008-1"); + // cSpell: ignore GEOIDTOELLIPSOID + return egm2008.ConvertHeight(latitude, longitude, height, GeographicLib::Geoid::GEOIDTOELLIPSOID); +} + +double convert_height( + const double height, const double latitude, const double longitude, + const std::string & source_vertical_datum, const std::string & target_vertical_datum) +{ + if (source_vertical_datum == target_vertical_datum) { + return height; + } + std::map, HeightConversionFunction> conversion_map; + conversion_map[{"WGS84", "EGM2008"}] = convert_wgs84_to_egm2008; + conversion_map[{"EGM2008", "WGS84"}] = convert_egm2008_to_wgs84; + + auto key = std::make_pair(source_vertical_datum, target_vertical_datum); + if (conversion_map.find(key) != conversion_map.end()) { + return conversion_map[key](height, latitude, longitude); + } else { + std::string error_message = + "Invalid conversion types: " + std::string(source_vertical_datum.c_str()) + " to " + + std::string(target_vertical_datum.c_str()); + + throw std::invalid_argument(error_message); + } +} + +} // namespace geography_utils diff --git a/common/geography_utils/src/lanelet2_projector.cpp b/common/geography_utils/src/lanelet2_projector.cpp new file mode 100644 index 0000000000000..d487a5273ce79 --- /dev/null +++ b/common/geography_utils/src/lanelet2_projector.cpp @@ -0,0 +1,43 @@ +// Copyright 2023 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 + +namespace geography_utils +{ + +std::unique_ptr get_lanelet2_projector(const MapProjectorInfo & projector_info) +{ + if (projector_info.projector_type == MapProjectorInfo::LOCAL_CARTESIAN_UTM) { + lanelet::GPSPoint position{ + projector_info.map_origin.latitude, projector_info.map_origin.longitude, + projector_info.map_origin.altitude}; + lanelet::Origin origin{position}; + lanelet::projection::UtmProjector projector{origin}; + return std::make_unique(projector); + } else if (projector_info.projector_type == MapProjectorInfo::MGRS) { + lanelet::projection::MGRSProjector projector{}; + return std::make_unique(projector); + } else { + const std::string error_msg = "Invalid map projector type: " + projector_info.projector_type + + ". Currently supported types: MGRS, and LocalCartesianUTM"; + throw std::invalid_argument(error_msg); + } +} + +} // namespace geography_utils diff --git a/common/geography_utils/src/projection.cpp b/common/geography_utils/src/projection.cpp new file mode 100644 index 0000000000000..ac9ce19052702 --- /dev/null +++ b/common/geography_utils/src/projection.cpp @@ -0,0 +1,95 @@ +// Copyright 2023 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 + +namespace geography_utils +{ + +Eigen::Vector3d to_basic_point_3d_pt(const LocalPoint src) +{ + Eigen::Vector3d dst; + dst.x() = src.x; + dst.y() = src.y; + dst.z() = src.z; + return dst; +} + +LocalPoint project_forward(const GeoPoint & geo_point, const MapProjectorInfo & projector_info) +{ + std::unique_ptr projector = get_lanelet2_projector(projector_info); + lanelet::GPSPoint position{geo_point.latitude, geo_point.longitude, geo_point.altitude}; + + lanelet::BasicPoint3d projected_local_point; + if (projector_info.projector_type == MapProjectorInfo::MGRS) { + const int mgrs_precision = 9; // set precision as 100 micro meter + const auto mgrs_projector = dynamic_cast(projector.get()); + + // project x and y using projector + // note that the altitude is ignored in MGRS projection conventionally + projected_local_point = mgrs_projector->forward(position, mgrs_precision); + } else { + // project x and y using projector + // note that the original projector such as UTM projector does not compensate for the altitude + // offset + projected_local_point = projector->forward(position); + + // correct z based on the map origin + // note that the converted altitude in local point is in the same vertical datum as the geo + // point + projected_local_point.z() = geo_point.altitude - projector_info.map_origin.altitude; + } + + LocalPoint local_point; + local_point.x = projected_local_point.x(); + local_point.y = projected_local_point.y(); + local_point.z = projected_local_point.z(); + + return local_point; +} + +GeoPoint project_reverse(const LocalPoint & local_point, const MapProjectorInfo & projector_info) +{ + std::unique_ptr projector = get_lanelet2_projector(projector_info); + + lanelet::GPSPoint projected_gps_point; + if (projector_info.projector_type == MapProjectorInfo::MGRS) { + const auto mgrs_projector = dynamic_cast(projector.get()); + // project latitude and longitude using projector + // note that the z is ignored in MGRS projection conventionally + projected_gps_point = + mgrs_projector->reverse(to_basic_point_3d_pt(local_point), projector_info.mgrs_grid); + } else { + // project latitude and longitude using projector + // note that the original projector such as UTM projector does not compensate for the altitude + // offset + projected_gps_point = projector->reverse(to_basic_point_3d_pt(local_point)); + + // correct altitude based on the map origin + // note that the converted altitude in local point is in the same vertical datum as the geo + // point + projected_gps_point.ele = local_point.z + projector_info.map_origin.altitude; + } + + GeoPoint geo_point; + geo_point.latitude = projected_gps_point.lat; + geo_point.longitude = projected_gps_point.lon; + geo_point.altitude = projected_gps_point.ele; + return geo_point; +} + +} // namespace geography_utils diff --git a/common/geography_utils/test/test_geography_utils.cpp b/common/geography_utils/test/test_geography_utils.cpp new file mode 100644 index 0000000000000..480e17b8f49a4 --- /dev/null +++ b/common/geography_utils/test/test_geography_utils.cpp @@ -0,0 +1,26 @@ +// Copyright 2023 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 "geography_utils/height.hpp" +#include "geography_utils/lanelet2_projector.hpp" +#include "geography_utils/projection.hpp" + +#include + +int main(int argc, char * argv[]) +{ + testing::InitGoogleTest(&argc, argv); + bool result = RUN_ALL_TESTS(); + return result; +} diff --git a/common/geography_utils/test/test_height.cpp b/common/geography_utils/test/test_height.cpp new file mode 100644 index 0000000000000..15297089ee131 --- /dev/null +++ b/common/geography_utils/test/test_height.cpp @@ -0,0 +1,86 @@ +// Copyright 2023 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 + +// Test case to verify if same source and target datums return original height +TEST(GeographyUtils, SameSourceTargetDatum) +{ + const double height = 10.0; + const double latitude = 35.0; + const double longitude = 139.0; + const std::string datum = "WGS84"; + + double converted_height = + geography_utils::convert_height(height, latitude, longitude, datum, datum); + + EXPECT_DOUBLE_EQ(height, converted_height); +} + +// Test case to verify valid source and target datums +TEST(GeographyUtils, ValidSourceTargetDatum) +{ + // Calculated with + // https://www.unavco.org/software/geodetic-utilities/geoid-height-calculator/geoid-height-calculator.html + const double height = 10.0; + const double latitude = 35.0; + const double longitude = 139.0; + const double target_height = -30.18; + + double converted_height = + geography_utils::convert_height(height, latitude, longitude, "WGS84", "EGM2008"); + + EXPECT_NEAR(target_height, converted_height, 0.1); +} + +// Test case to verify invalid source and target datums +TEST(GeographyUtils, InvalidSourceTargetDatum) +{ + const double height = 10.0; + const double latitude = 35.0; + const double longitude = 139.0; + + EXPECT_THROW( + geography_utils::convert_height(height, latitude, longitude, "INVALID1", "INVALID2"), + std::invalid_argument); +} + +// Test case to verify invalid source datums +TEST(GeographyUtils, InvalidSourceDatum) +{ + const double height = 10.0; + const double latitude = 35.0; + const double longitude = 139.0; + + EXPECT_THROW( + geography_utils::convert_height(height, latitude, longitude, "INVALID1", "WGS84"), + std::invalid_argument); +} + +// Test case to verify invalid target datums +TEST(GeographyUtils, InvalidTargetDatum) +{ + const double height = 10.0; + const double latitude = 35.0; + const double longitude = 139.0; + + EXPECT_THROW( + geography_utils::convert_height(height, latitude, longitude, "WGS84", "INVALID2"), + std::invalid_argument); +} diff --git a/common/geography_utils/test/test_projection.cpp b/common/geography_utils/test/test_projection.cpp new file mode 100644 index 0000000000000..74ffb616cde6f --- /dev/null +++ b/common/geography_utils/test/test_projection.cpp @@ -0,0 +1,161 @@ +// Copyright 2023 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 + +TEST(GeographyUtilsProjection, ProjectForwardToMGRS) +{ + // source point + geographic_msgs::msg::GeoPoint geo_point; + geo_point.latitude = 35.62426; + geo_point.longitude = 139.74252; + geo_point.altitude = 10.0; + + // target point + geometry_msgs::msg::Point local_point; + local_point.x = 86128.0; + local_point.y = 43002.0; + local_point.z = 10.0; + + // projector info + tier4_map_msgs::msg::MapProjectorInfo projector_info; + projector_info.projector_type = tier4_map_msgs::msg::MapProjectorInfo::MGRS; + projector_info.mgrs_grid = "54SUE"; + projector_info.vertical_datum = tier4_map_msgs::msg::MapProjectorInfo::WGS84; + + // conversion + const geometry_msgs::msg::Point converted_point = + geography_utils::project_forward(geo_point, projector_info); + + EXPECT_NEAR(converted_point.x, local_point.x, 1.0); + EXPECT_NEAR(converted_point.y, local_point.y, 1.0); + EXPECT_NEAR(converted_point.z, local_point.z, 1.0); +} + +TEST(GeographyUtilsProjection, ProjectReverseFromMGRS) +{ + // source point + geometry_msgs::msg::Point local_point; + local_point.x = 86128.0; + local_point.y = 43002.0; + local_point.z = 10.0; + + // target point + geographic_msgs::msg::GeoPoint geo_point; + geo_point.latitude = 35.62426; + geo_point.longitude = 139.74252; + geo_point.altitude = 10.0; + + // projector info + tier4_map_msgs::msg::MapProjectorInfo projector_info; + projector_info.projector_type = tier4_map_msgs::msg::MapProjectorInfo::MGRS; + projector_info.mgrs_grid = "54SUE"; + projector_info.vertical_datum = tier4_map_msgs::msg::MapProjectorInfo::WGS84; + + // conversion + const geographic_msgs::msg::GeoPoint converted_point = + geography_utils::project_reverse(local_point, projector_info); + + EXPECT_NEAR(converted_point.latitude, geo_point.latitude, 0.0001); + EXPECT_NEAR(converted_point.longitude, geo_point.longitude, 0.0001); + EXPECT_NEAR(converted_point.altitude, geo_point.altitude, 0.0001); +} + +TEST(GeographyUtilsProjection, ProjectForwardAndReverseMGRS) +{ + // source point + geographic_msgs::msg::GeoPoint geo_point; + geo_point.latitude = 35.62426; + geo_point.longitude = 139.74252; + geo_point.altitude = 10.0; + + // projector info + tier4_map_msgs::msg::MapProjectorInfo projector_info; + projector_info.projector_type = tier4_map_msgs::msg::MapProjectorInfo::MGRS; + projector_info.mgrs_grid = "54SUE"; + projector_info.vertical_datum = tier4_map_msgs::msg::MapProjectorInfo::WGS84; + + // conversion + const geometry_msgs::msg::Point converted_local_point = + geography_utils::project_forward(geo_point, projector_info); + const geographic_msgs::msg::GeoPoint converted_geo_point = + geography_utils::project_reverse(converted_local_point, projector_info); + + EXPECT_NEAR(converted_geo_point.latitude, geo_point.latitude, 0.0001); + EXPECT_NEAR(converted_geo_point.longitude, geo_point.longitude, 0.0001); + EXPECT_NEAR(converted_geo_point.altitude, geo_point.altitude, 0.0001); +} + +TEST(GeographyUtilsProjection, ProjectForwardToLocalCartesianUTMOrigin) +{ + // source point + geographic_msgs::msg::GeoPoint geo_point; + geo_point.latitude = 35.62406; + geo_point.longitude = 139.74252; + geo_point.altitude = 10.0; + + // target point + geometry_msgs::msg::Point local_point; + local_point.x = 0.0; + local_point.y = -22.18; + local_point.z = 20.0; + + // projector info + tier4_map_msgs::msg::MapProjectorInfo projector_info; + projector_info.projector_type = tier4_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN_UTM; + projector_info.vertical_datum = tier4_map_msgs::msg::MapProjectorInfo::WGS84; + projector_info.map_origin.latitude = 35.62426; + projector_info.map_origin.longitude = 139.74252; + projector_info.map_origin.altitude = -10.0; + + // conversion + const geometry_msgs::msg::Point converted_point = + geography_utils::project_forward(geo_point, projector_info); + + EXPECT_NEAR(converted_point.x, local_point.x, 1.0); + EXPECT_NEAR(converted_point.y, local_point.y, 1.0); + EXPECT_NEAR(converted_point.z, local_point.z, 1.0); +} + +TEST(GeographyUtilsProjection, ProjectForwardAndReverseLocalCartesianUTMOrigin) +{ + // source point + geographic_msgs::msg::GeoPoint geo_point; + geo_point.latitude = 35.62426; + geo_point.longitude = 139.74252; + geo_point.altitude = 10.0; + + // projector info + tier4_map_msgs::msg::MapProjectorInfo projector_info; + projector_info.projector_type = tier4_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN_UTM; + projector_info.vertical_datum = tier4_map_msgs::msg::MapProjectorInfo::WGS84; + projector_info.map_origin.latitude = 35.0; + projector_info.map_origin.longitude = 139.0; + projector_info.map_origin.altitude = 0.0; + + // conversion + const geometry_msgs::msg::Point converted_local_point = + geography_utils::project_forward(geo_point, projector_info); + const geographic_msgs::msg::GeoPoint converted_geo_point = + geography_utils::project_reverse(converted_local_point, projector_info); + + EXPECT_NEAR(converted_geo_point.latitude, geo_point.latitude, 0.0001); + EXPECT_NEAR(converted_geo_point.longitude, geo_point.longitude, 0.0001); + EXPECT_NEAR(converted_geo_point.altitude, geo_point.altitude, 0.0001); +} diff --git a/common/motion_utils/include/motion_utils/marker/marker_helper.hpp b/common/motion_utils/include/motion_utils/marker/marker_helper.hpp index 6433f1b9ae2f9..b6d10bbf40de5 100644 --- a/common/motion_utils/include/motion_utils/marker/marker_helper.hpp +++ b/common/motion_utils/include/motion_utils/marker/marker_helper.hpp @@ -16,7 +16,8 @@ #define MOTION_UTILS__MARKER__MARKER_HELPER_HPP_ #include "motion_utils/resample/resample_utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#include #include #include diff --git a/common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp b/common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp index 331b3dd5275fd..afead3a3614d4 100644 --- a/common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp +++ b/common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp @@ -18,8 +18,6 @@ #include "motion_utils/marker/marker_helper.hpp" #include -#include -#include #include #include diff --git a/common/motion_utils/src/marker/marker_helper.cpp b/common/motion_utils/src/marker/marker_helper.cpp index 25d8616949b02..c869271222387 100644 --- a/common/motion_utils/src/marker/marker_helper.cpp +++ b/common/motion_utils/src/marker/marker_helper.cpp @@ -14,6 +14,10 @@ #include "motion_utils/marker/marker_helper.hpp" +#include "tier4_autoware_utils/ros/marker_helper.hpp" + +#include + #include using tier4_autoware_utils::appendMarkerArray; diff --git a/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp b/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp index 63c456ab24fb0..114c4f87907c0 100644 --- a/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp +++ b/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp @@ -14,6 +14,8 @@ #include "motion_utils/marker/virtual_wall_marker_creator.hpp" +#include + namespace motion_utils { diff --git a/common/motion_utils/src/resample/resample.cpp b/common/motion_utils/src/resample/resample.cpp index 091f3405e2815..1a800a38d1051 100644 --- a/common/motion_utils/src/resample/resample.cpp +++ b/common/motion_utils/src/resample/resample.cpp @@ -109,13 +109,16 @@ std::vector resamplePointVector( } std::vector resamplePoseVector( - const std::vector & points, + const std::vector & points_raw, const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, const bool use_lerp_for_z) { + // Remove overlap points for resampling + const auto points = motion_utils::removeOverlapPoints(points_raw); + // validate arguments if (!resample_utils::validate_arguments(points, resampled_arclength)) { - return points; + return points_raw; } std::vector position(points.size()); diff --git a/common/object_recognition_utils/include/object_recognition_utils/transform.hpp b/common/object_recognition_utils/include/object_recognition_utils/transform.hpp index 4446c87427e88..31892853a855f 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/transform.hpp +++ b/common/object_recognition_utils/include/object_recognition_utils/transform.hpp @@ -15,15 +15,22 @@ #ifndef OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_ #define OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_ +#include + #include +#include +#include #include #include #include #ifdef ROS_DISTRO_GALACTIC +#include #include #else +#include + #include #endif @@ -45,6 +52,23 @@ namespace detail return boost::none; } } + +[[maybe_unused]] inline boost::optional getTransformMatrix( + const std::string & in_target_frame, const std_msgs::msg::Header & in_cloud_header, + const tf2_ros::Buffer & tf_buffer) +{ + try { + geometry_msgs::msg::TransformStamped transform_stamped; + transform_stamped = tf_buffer.lookupTransform( + in_target_frame, in_cloud_header.frame_id, in_cloud_header.stamp, + rclcpp::Duration::from_seconds(1.0)); + Eigen::Matrix4f mat = tf2::transformToEigen(transform_stamped.transform).matrix().cast(); + return mat; + } catch (tf2::TransformException & e) { + RCLCPP_WARN_STREAM(rclcpp::get_logger("detail::getTransformMatrix"), e.what()); + return boost::none; + } +} } // namespace detail namespace object_recognition_utils @@ -79,6 +103,46 @@ bool transformObjects( } return true; } +template +bool transformObjectsWithFeature( + const T & input_msg, const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer, + T & output_msg) +{ + output_msg = input_msg; + if (input_msg.header.frame_id != target_frame_id) { + output_msg.header.frame_id = target_frame_id; + tf2::Transform tf_target2objects_world; + tf2::Transform tf_target2objects; + tf2::Transform tf_objects_world2objects; + const auto ros_target2objects_world = detail::getTransform( + tf_buffer, input_msg.header.frame_id, target_frame_id, input_msg.header.stamp); + if (!ros_target2objects_world) { + return false; + } + const auto tf_matrix = detail::getTransformMatrix(target_frame_id, input_msg.header, tf_buffer); + if (!tf_matrix) { + RCLCPP_WARN( + rclcpp::get_logger("object_recognition_utils:"), "failed to get transformed matrix"); + return false; + } + for (auto & feature_object : output_msg.feature_objects) { + // transform object + tf2::fromMsg( + feature_object.object.kinematics.pose_with_covariance.pose, tf_objects_world2objects); + tf_target2objects = tf_target2objects_world * tf_objects_world2objects; + tf2::toMsg(tf_target2objects, feature_object.object.kinematics.pose_with_covariance.pose); + + // transform cluster + sensor_msgs::msg::PointCloud2 transformed_cluster; + pcl_ros::transformPointCloud(*tf_matrix, feature_object.feature.cluster, transformed_cluster); + transformed_cluster.header.frame_id = target_frame_id; + feature_object.feature.cluster = transformed_cluster; + } + output_msg.header.frame_id = target_frame_id; + return true; + } + return true; +} } // namespace object_recognition_utils #endif // OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_ diff --git a/common/object_recognition_utils/package.xml b/common/object_recognition_utils/package.xml index 95925e846a55c..2f2472515ebad 100644 --- a/common/object_recognition_utils/package.xml +++ b/common/object_recognition_utils/package.xml @@ -17,7 +17,13 @@ geometry_msgs interpolation libboost-dev + pcl_conversions + pcl_ros rclcpp + sensor_msgs + std_msgs + tf2 + tf2_eigen tier4_autoware_utils ament_cmake_ros diff --git a/common/path_distance_calculator/src/path_distance_calculator.cpp b/common/path_distance_calculator/src/path_distance_calculator.cpp index e4c9c8e027f7b..aa20dd1ffc7ce 100644 --- a/common/path_distance_calculator/src/path_distance_calculator.cpp +++ b/common/path_distance_calculator/src/path_distance_calculator.cpp @@ -14,8 +14,7 @@ #include "path_distance_calculator.hpp" -#include -#include +#include #include #include diff --git a/common/perception_utils/package.xml b/common/perception_utils/package.xml index 6cae7f9ce8c4f..5304d481737e5 100644 --- a/common/perception_utils/package.xml +++ b/common/perception_utils/package.xml @@ -4,7 +4,6 @@ perception_utils 0.1.0 The perception_utils package - Mingyu Li Satoshi Tanaka Yusuke Muramatsu Shunsuke Miura diff --git a/common/tier4_planning_rviz_plugin/include/path/display_base.hpp b/common/tier4_planning_rviz_plugin/include/path/display_base.hpp index 55c7a9c53d72d..ccedcceefaaf9 100644 --- a/common/tier4_planning_rviz_plugin/include/path/display_base.hpp +++ b/common/tier4_planning_rviz_plugin/include/path/display_base.hpp @@ -506,6 +506,10 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay vehicle_footprint_info_ = std::make_shared( vehicle_info_->vehicle_length_m, vehicle_info_->vehicle_width_m, vehicle_info_->rear_overhang_m); + + property_vehicle_length_.setValue(vehicle_info_->vehicle_length_m); + property_vehicle_width_.setValue(vehicle_info_->vehicle_width_m); + property_rear_overhang_.setValue(vehicle_info_->rear_overhang_m); } else { const float length{property_vehicle_length_.getFloat()}; const float width{property_vehicle_width_.getFloat()}; diff --git a/common/tier4_target_object_type_rviz_plugin/CMakeLists.txt b/common/tier4_target_object_type_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000000000..ed458cf924e33 --- /dev/null +++ b/common/tier4_target_object_type_rviz_plugin/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.14) +project(tier4_target_object_type_rviz_plugin) + +find_package(autoware_cmake REQUIRED) +autoware_package() +find_package(OpenCV REQUIRED) +find_package(Qt5 REQUIRED Core Widgets) +set(QT_LIBRARIES Qt5::Widgets) +set(CMAKE_AUTOMOC ON) +set(CMAKE_INCLUDE_CURRENT_DIR ON) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/target_object_type_panel.hpp + src/target_object_type_panel.cpp +) +target_link_libraries(${PROJECT_NAME} + ${QT_LIBRARIES} + ${OpenCV_LIBRARIES} +) +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + +ament_auto_package( + INSTALL_TO_SHARE + plugins +) diff --git a/common/tier4_target_object_type_rviz_plugin/README.md b/common/tier4_target_object_type_rviz_plugin/README.md new file mode 100644 index 0000000000000..1296bd3442ab3 --- /dev/null +++ b/common/tier4_target_object_type_rviz_plugin/README.md @@ -0,0 +1,9 @@ +# tier4_target_object_type_rviz_plugin + +This plugin allows you to check which types of the dynamic object is being used by each planner. + +![window](./image/window.png) + +## Limitations + +Currently, which parameters of which module to check are hardcoded. In the future, this will be parameterized using YAML. diff --git a/common/tier4_target_object_type_rviz_plugin/image/window.png b/common/tier4_target_object_type_rviz_plugin/image/window.png new file mode 100644 index 0000000000000..33aa9a1e5a26e Binary files /dev/null and b/common/tier4_target_object_type_rviz_plugin/image/window.png differ diff --git a/common/tier4_target_object_type_rviz_plugin/package.xml b/common/tier4_target_object_type_rviz_plugin/package.xml new file mode 100644 index 0000000000000..6209a5aaaee6b --- /dev/null +++ b/common/tier4_target_object_type_rviz_plugin/package.xml @@ -0,0 +1,27 @@ + + + + tier4_target_object_type_rviz_plugin + 0.0.1 + The tier4_target_object_type_rviz_plugin package + Takamasa Horibe + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + libqt5-core + libqt5-gui + libqt5-widgets + qtbase5-dev + rclcpp + rviz_common + rviz_rendering + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + diff --git a/common/tier4_target_object_type_rviz_plugin/plugins/plugin_description.xml b/common/tier4_target_object_type_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 0000000000000..eb3350e4333bd --- /dev/null +++ b/common/tier4_target_object_type_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,9 @@ + + + + TargetObjectTypePanel + + + diff --git a/common/tier4_target_object_type_rviz_plugin/src/target_object_type_panel.cpp b/common/tier4_target_object_type_rviz_plugin/src/target_object_type_panel.cpp new file mode 100644 index 0000000000000..cf5960cac281f --- /dev/null +++ b/common/tier4_target_object_type_rviz_plugin/src/target_object_type_panel.cpp @@ -0,0 +1,248 @@ +// Copyright 2023 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. + +#include "target_object_type_panel.hpp" + +#include +#include +#include +#include +#include + +TargetObjectTypePanel::TargetObjectTypePanel(QWidget * parent) : rviz_common::Panel(parent) +{ + node_ = std::make_shared("matrix_display_node"); + + setParameters(); + + matrix_widget_ = new QTableWidget(modules_.size(), targets_.size(), this); + for (size_t i = 0; i < modules_.size(); i++) { + matrix_widget_->setVerticalHeaderItem( + i, new QTableWidgetItem(QString::fromStdString(modules_[i]))); + } + for (size_t j = 0; j < targets_.size(); j++) { + matrix_widget_->setHorizontalHeaderItem( + j, new QTableWidgetItem(QString::fromStdString(targets_[j]))); + } + + updateMatrix(); + + reload_button_ = new QPushButton("Reload", this); + connect( + reload_button_, &QPushButton::clicked, this, &TargetObjectTypePanel::onReloadButtonClicked); + + QVBoxLayout * layout = new QVBoxLayout; + layout->addWidget(matrix_widget_); + layout->addWidget(reload_button_); + setLayout(layout); +} + +void TargetObjectTypePanel::onReloadButtonClicked() +{ + RCLCPP_INFO(node_->get_logger(), "Reload button clicked. Update parameter data."); + updateMatrix(); +} + +void TargetObjectTypePanel::setParameters() +{ + // Parameter will be investigated for these modules: + modules_ = { + "avoidance", + "avoidance_by_lane_change", + "lane_change", + "obstacle_cruise (inside)", + "obstacle_cruise (outside)", + "obstacle_stop", + "obstacle_slowdown"}; + + // Parameter will be investigated for targets in each module + targets_ = {"car", "truck", "bus", "trailer", "unknown", "bicycle", "motorcycle", "pedestrian"}; + + // TODO(Horibe): If the param naming strategy is aligned, this should be done automatically based + // on the modules_ and targets_. + + // default + ParamNameEnableObject default_param_name; + default_param_name.name.emplace("car", "car"); + default_param_name.name.emplace("truck", "truck"); + default_param_name.name.emplace("bus", "bus"); + default_param_name.name.emplace("trailer", "trailer"); + default_param_name.name.emplace("unknown", "unknown"); + default_param_name.name.emplace("bicycle", "bicycle"); + default_param_name.name.emplace("motorcycle", "motorcycle"); + default_param_name.name.emplace("pedestrian", "pedestrian"); + + // avoidance + { + const auto module = "avoidance"; + ParamNameEnableObject param_name; + param_name.node = + "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner"; + param_name.ns = "avoidance.target_object"; + param_name.name.emplace("car", "car.is_target"); + param_name.name.emplace("truck", "truck.is_target"); + param_name.name.emplace("bus", "bus.is_target"); + param_name.name.emplace("trailer", "trailer.is_target"); + param_name.name.emplace("unknown", "unknown.is_target"); + param_name.name.emplace("bicycle", "bicycle.is_target"); + param_name.name.emplace("motorcycle", "motorcycle.is_target"); + param_name.name.emplace("pedestrian", "pedestrian.is_target"); + param_names_.emplace(module, param_name); + } + + // avoidance_by_lane_change + { + const auto module = "avoidance_by_lane_change"; + ParamNameEnableObject param_name; + param_name.node = + "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner"; + param_name.ns = "avoidance_by_lane_change.target_object"; + param_name.name.emplace("car", "car.is_target"); + param_name.name.emplace("truck", "truck.is_target"); + param_name.name.emplace("bus", "bus.is_target"); + param_name.name.emplace("trailer", "trailer.is_target"); + param_name.name.emplace("unknown", "unknown.is_target"); + param_name.name.emplace("bicycle", "bicycle.is_target"); + param_name.name.emplace("motorcycle", "motorcycle.is_target"); + param_name.name.emplace("pedestrian", "pedestrian.is_target"); + param_names_.emplace(module, param_name); + } + + // lane_change + { + const auto module = "lane_change"; + ParamNameEnableObject param_name; + param_name.node = + "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner"; + param_name.ns = "lane_change.target_object"; + param_name.name = default_param_name.name; + param_names_.emplace(module, param_name); + } + + // obstacle cruise (inside) + { + const auto module = "obstacle_cruise (inside)"; + ParamNameEnableObject param_name; + param_name.node = + "/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner"; + param_name.ns = "common.cruise_obstacle_type.inside"; + param_name.name = default_param_name.name; + param_names_.emplace(module, param_name); + } + + // obstacle cruise (outside) + { + const auto module = "obstacle_cruise (outside)"; + ParamNameEnableObject param_name; + param_name.node = + "/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner"; + param_name.ns = "common.cruise_obstacle_type.outside"; + param_name.name = default_param_name.name; + param_names_.emplace(module, param_name); + } + + // obstacle stop + { + const auto module = "obstacle_stop"; + ParamNameEnableObject param_name; + param_name.node = + "/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner"; + param_name.ns = "common.stop_obstacle_type"; + param_name.name = default_param_name.name; + param_names_.emplace(module, param_name); + } + + // obstacle slowdown + { + const auto module = "obstacle_slowdown"; + ParamNameEnableObject param_name; + param_name.node = + "/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner"; + param_name.ns = "common.slow_down_obstacle_type"; + param_name.name = default_param_name.name; + param_names_.emplace(module, param_name); + } +} + +void TargetObjectTypePanel::updateMatrix() +{ + // blue base + // const QColor color_in_use("#6eb6cc"); + // const QColor color_no_use("#1d3e48"); + // const QColor color_undefined("#9e9e9e"); + + // green base + const QColor color_in_use("#afff70"); + const QColor color_no_use("#44642b"); + const QColor color_undefined("#9e9e9e"); + + const auto set_undefined = [&](const auto i, const auto j) { + QTableWidgetItem * item = new QTableWidgetItem("N/A"); + item->setForeground(QBrush(Qt::black)); // set the text color to black + item->setBackground(color_undefined); + matrix_widget_->setItem(i, j, item); + }; + + for (size_t i = 0; i < modules_.size(); i++) { + const auto & module = modules_[i]; + + // Check if module exists in param_names_ + if (param_names_.find(module) == param_names_.end()) { + RCLCPP_WARN_STREAM(node_->get_logger(), module << " is not in the param names"); + continue; + } + + const auto & module_params = param_names_.at(module); + auto parameters_client = + std::make_shared(node_, module_params.node); + if (!parameters_client->wait_for_service(std::chrono::microseconds(500))) { + RCLCPP_WARN_STREAM( + node_->get_logger(), "Failed to find parameter service for node: " << module_params.node); + for (size_t j = 0; j < targets_.size(); j++) { + set_undefined(i, j); + } + continue; + } + + for (size_t j = 0; j < targets_.size(); j++) { + const auto & target = targets_[j]; + + // Check if target exists in module's name map + if (module_params.name.find(target) == module_params.name.end()) { + RCLCPP_WARN_STREAM( + node_->get_logger(), target << " parameter is not set in the " << module); + continue; + } + + std::string param_name = module_params.ns + "." + module_params.name.at(target); + auto parameter_result = parameters_client->get_parameters({param_name}); + + if (!parameter_result.empty()) { + bool value = parameter_result[0].as_bool(); + QTableWidgetItem * item = new QTableWidgetItem(value ? "O" : "X"); + item->setForeground(QBrush(value ? Qt::black : Qt::black)); // set the text color to black + item->setBackground(QBrush(value ? color_in_use : color_no_use)); + matrix_widget_->setItem(i, j, item); + } else { + RCLCPP_WARN_STREAM( + node_->get_logger(), + "Failed to get parameter " << module_params.node << " " << param_name); + + set_undefined(i, j); + } + } + } +} + +PLUGINLIB_EXPORT_CLASS(TargetObjectTypePanel, rviz_common::Panel) diff --git a/common/tier4_target_object_type_rviz_plugin/src/target_object_type_panel.hpp b/common/tier4_target_object_type_rviz_plugin/src/target_object_type_panel.hpp new file mode 100644 index 0000000000000..1f3934369bfba --- /dev/null +++ b/common/tier4_target_object_type_rviz_plugin/src/target_object_type_panel.hpp @@ -0,0 +1,60 @@ +// Copyright 2023 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. + +#ifndef TARGET_OBJECT_TYPE_PANEL_HPP_ +#define TARGET_OBJECT_TYPE_PANEL_HPP_ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +class TargetObjectTypePanel : public rviz_common::Panel +{ + Q_OBJECT + +public: + explicit TargetObjectTypePanel(QWidget * parent = 0); + +protected: + QTableWidget * matrix_widget_; + std::shared_ptr node_; + std::vector modules_; + std::vector targets_; + + struct ParamNameEnableObject + { + std::string node; + std::string ns; + std::unordered_map name; + }; + std::unordered_map param_names_; + +private slots: + void onReloadButtonClicked(); + +private: + QPushButton * reload_button_; + + void updateMatrix(); + void setParameters(); +}; + +#endif // TARGET_OBJECT_TYPE_PANEL_HPP_ diff --git a/common/tier4_traffic_light_rviz_plugin/README.md b/common/tier4_traffic_light_rviz_plugin/README.md index 953412a917432..91fb5bc124cb7 100644 --- a/common/tier4_traffic_light_rviz_plugin/README.md +++ b/common/tier4_traffic_light_rviz_plugin/README.md @@ -8,9 +8,9 @@ This plugin panel publishes dummy traffic light signals. ### Output -| Name | Type | Description | -| ------------------------------------------------------- | -------------------------------------------------------- | ----------------------------- | -| `/perception/traffic_light_recognition/traffic_signals` | `autoware_auto_perception_msgs::msg::TrafficSignalArray` | Publish traffic light signals | +| Name | Type | Description | +| ------------------------------------------------------- | --------------------------------------------------- | ----------------------------- | +| `/perception/traffic_light_recognition/traffic_signals` | `autoware_perception_msgs::msg::TrafficSignalArray` | Publish traffic light signals | ## HowToUse diff --git a/common/tier4_traffic_light_rviz_plugin/package.xml b/common/tier4_traffic_light_rviz_plugin/package.xml index 21d8bae8f6cff..2b118b429f1af 100644 --- a/common/tier4_traffic_light_rviz_plugin/package.xml +++ b/common/tier4_traffic_light_rviz_plugin/package.xml @@ -11,7 +11,7 @@ autoware_cmake autoware_auto_mapping_msgs - autoware_auto_perception_msgs + autoware_perception_msgs lanelet2_extension libqt5-core libqt5-gui diff --git a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp index e4fb0095b8d0a..35c5a88a2f8a6 100644 --- a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp +++ b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp @@ -34,6 +34,7 @@ #include #include +#undef signals namespace rviz_plugins { TrafficLightPublishPanel::TrafficLightPublishPanel(QWidget * parent) : rviz_common::Panel(parent) @@ -138,55 +139,55 @@ void TrafficLightPublishPanel::onSetTrafficLightState() const auto shape = light_shape_combo_->currentText(); const auto status = light_status_combo_->currentText(); - TrafficLight traffic_light; + TrafficSignalElement traffic_light; traffic_light.confidence = traffic_light_confidence_input_->value(); if (color == "RED") { - traffic_light.color = TrafficLight::RED; + traffic_light.color = TrafficSignalElement::RED; } else if (color == "AMBER") { - traffic_light.color = TrafficLight::AMBER; + traffic_light.color = TrafficSignalElement::AMBER; } else if (color == "GREEN") { - traffic_light.color = TrafficLight::GREEN; + traffic_light.color = TrafficSignalElement::GREEN; } else if (color == "WHITE") { - traffic_light.color = TrafficLight::WHITE; + traffic_light.color = TrafficSignalElement::WHITE; } else if (color == "UNKNOWN") { - traffic_light.color = TrafficLight::UNKNOWN; + traffic_light.color = TrafficSignalElement::UNKNOWN; } if (shape == "CIRCLE") { - traffic_light.shape = TrafficLight::CIRCLE; + traffic_light.shape = TrafficSignalElement::CIRCLE; } else if (shape == "LEFT_ARROW") { - traffic_light.shape = TrafficLight::LEFT_ARROW; + traffic_light.shape = TrafficSignalElement::LEFT_ARROW; } else if (shape == "RIGHT_ARROW") { - traffic_light.shape = TrafficLight::RIGHT_ARROW; + traffic_light.shape = TrafficSignalElement::RIGHT_ARROW; } else if (shape == "UP_ARROW") { - traffic_light.shape = TrafficLight::UP_ARROW; + traffic_light.shape = TrafficSignalElement::UP_ARROW; } else if (shape == "DOWN_ARROW") { - traffic_light.shape = TrafficLight::DOWN_ARROW; + traffic_light.shape = TrafficSignalElement::DOWN_ARROW; } else if (shape == "DOWN_LEFT_ARROW") { - traffic_light.shape = TrafficLight::DOWN_LEFT_ARROW; + traffic_light.shape = TrafficSignalElement::DOWN_LEFT_ARROW; } else if (shape == "DOWN_RIGHT_ARROW") { - traffic_light.shape = TrafficLight::DOWN_RIGHT_ARROW; + traffic_light.shape = TrafficSignalElement::DOWN_RIGHT_ARROW; } else if (shape == "UNKNOWN") { - traffic_light.shape = TrafficLight::UNKNOWN; + traffic_light.shape = TrafficSignalElement::UNKNOWN; } if (status == "SOLID_OFF") { - traffic_light.status = TrafficLight::SOLID_OFF; + traffic_light.status = TrafficSignalElement::SOLID_OFF; } else if (status == "SOLID_ON") { - traffic_light.status = TrafficLight::SOLID_ON; + traffic_light.status = TrafficSignalElement::SOLID_ON; } else if (status == "FLASHING") { - traffic_light.status = TrafficLight::FLASHING; + traffic_light.status = TrafficSignalElement::FLASHING; } else if (status == "UNKNOWN") { - traffic_light.status = TrafficLight::UNKNOWN; + traffic_light.status = TrafficSignalElement::UNKNOWN; } TrafficSignal traffic_signal; - traffic_signal.lights.push_back(traffic_light); - traffic_signal.map_primitive_id = traffic_light_id; + traffic_signal.elements.push_back(traffic_light); + traffic_signal.traffic_signal_id = traffic_light_id; for (auto & signal : extra_traffic_signals_.signals) { - if (signal.map_primitive_id == traffic_light_id) { + if (signal.traffic_signal_id == traffic_light_id) { signal = traffic_signal; return; } @@ -247,7 +248,7 @@ void TrafficLightPublishPanel::createWallTimer() void TrafficLightPublishPanel::onTimer() { if (enable_publish_) { - extra_traffic_signals_.header.stamp = rclcpp::Clock().now(); + extra_traffic_signals_.stamp = rclcpp::Clock().now(); pub_traffic_signals_->publish(extra_traffic_signals_); } @@ -260,35 +261,35 @@ void TrafficLightPublishPanel::onTimer() for (size_t i = 0; i < extra_traffic_signals_.signals.size(); ++i) { const auto & signal = extra_traffic_signals_.signals.at(i); - if (signal.lights.empty()) { + if (signal.elements.empty()) { continue; } - auto id_label = new QLabel(QString::number(signal.map_primitive_id)); + auto id_label = new QLabel(QString::number(signal.traffic_signal_id)); id_label->setAlignment(Qt::AlignCenter); auto color_label = new QLabel(); color_label->setAlignment(Qt::AlignCenter); - const auto & light = signal.lights.front(); + const auto & light = signal.elements.front(); switch (light.color) { - case TrafficLight::RED: + case TrafficSignalElement::RED: color_label->setText("RED"); color_label->setStyleSheet("background-color: #FF0000;"); break; - case TrafficLight::AMBER: + case TrafficSignalElement::AMBER: color_label->setText("AMBER"); color_label->setStyleSheet("background-color: #FFBF00;"); break; - case TrafficLight::GREEN: + case TrafficSignalElement::GREEN: color_label->setText("GREEN"); color_label->setStyleSheet("background-color: #7CFC00;"); break; - case TrafficLight::WHITE: + case TrafficSignalElement::WHITE: color_label->setText("WHITE"); color_label->setStyleSheet("background-color: #FFFFFF;"); break; - case TrafficLight::UNKNOWN: + case TrafficSignalElement::UNKNOWN: color_label->setText("UNKNOWN"); color_label->setStyleSheet("background-color: #808080;"); break; @@ -300,31 +301,28 @@ void TrafficLightPublishPanel::onTimer() shape_label->setAlignment(Qt::AlignCenter); switch (light.shape) { - case TrafficLight::CIRCLE: + case TrafficSignalElement::CIRCLE: shape_label->setText("CIRCLE"); break; - case TrafficLight::LEFT_ARROW: + case TrafficSignalElement::LEFT_ARROW: shape_label->setText("LEFT_ARROW"); break; - case TrafficLight::RIGHT_ARROW: + case TrafficSignalElement::RIGHT_ARROW: shape_label->setText("RIGHT_ARROW"); break; - case TrafficLight::UP_ARROW: + case TrafficSignalElement::UP_ARROW: shape_label->setText("UP_ARROW"); break; - case TrafficLight::DOWN_ARROW: + case TrafficSignalElement::DOWN_ARROW: shape_label->setText("DOWN_ARROW"); break; - case TrafficLight::DOWN_LEFT_ARROW: + case TrafficSignalElement::DOWN_LEFT_ARROW: shape_label->setText("DOWN_LEFT_ARROW"); break; - case TrafficLight::DOWN_RIGHT_ARROW: + case TrafficSignalElement::DOWN_RIGHT_ARROW: shape_label->setText("DOWN_RIGHT_ARROW"); break; - case TrafficLight::FLASHING: - shape_label->setText("FLASHING"); - break; - case TrafficLight::UNKNOWN: + case TrafficSignalElement::UNKNOWN: shape_label->setText("UNKNOWN"); break; default: @@ -335,16 +333,16 @@ void TrafficLightPublishPanel::onTimer() status_label->setAlignment(Qt::AlignCenter); switch (light.status) { - case TrafficLight::SOLID_OFF: + case TrafficSignalElement::SOLID_OFF: status_label->setText("SOLID_OFF"); break; - case TrafficLight::SOLID_ON: + case TrafficSignalElement::SOLID_ON: status_label->setText("SOLID_ON"); break; - case TrafficLight::FLASHING: + case TrafficSignalElement::FLASHING: status_label->setText("FLASHING"); break; - case TrafficLight::UNKNOWN: + case TrafficSignalElement::UNKNOWN: status_label->setText("UNKNOWN"); break; default: @@ -375,11 +373,9 @@ void TrafficLightPublishPanel::onVectorMap(const HADMapBin::ConstSharedPtr msg) std::string info = "Fetching traffic lights :"; std::string delim = " "; for (auto && tl_reg_elem_ptr : tl_reg_elems) { - for (auto && traffic_light : tl_reg_elem_ptr->trafficLights()) { - auto id = static_cast(traffic_light.id()); - info += (std::exchange(delim, ", ") + std::to_string(id)); - traffic_light_ids_.insert(id); - } + auto id = static_cast(tl_reg_elem_ptr->id()); + info += (std::exchange(delim, ", ") + std::to_string(id)); + traffic_light_ids_.insert(id); } RCLCPP_INFO_STREAM(raw_node_->get_logger(), info); received_vector_map_ = true; diff --git a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp index 75e6405417084..e54b6a301873b 100644 --- a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp +++ b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp @@ -27,7 +27,7 @@ #include #include -#include +#include #endif #include @@ -36,10 +36,9 @@ namespace rviz_plugins { using autoware_auto_mapping_msgs::msg::HADMapBin; -using autoware_auto_perception_msgs::msg::TrafficLight; -using autoware_auto_perception_msgs::msg::TrafficSignal; -using autoware_auto_perception_msgs::msg::TrafficSignalArray; - +using autoware_perception_msgs::msg::TrafficSignal; +using autoware_perception_msgs::msg::TrafficSignalArray; +using autoware_perception_msgs::msg::TrafficSignalElement; class TrafficLightPublishPanel : public rviz_common::Panel { Q_OBJECT diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.hpp b/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.hpp index e71003ea7f61c..cba0fa16b50fe 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.hpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.hpp @@ -25,7 +25,8 @@ #include #include #include -#include +#include +#include #include #endif diff --git a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp index 8859367c25e9b..a8f260795d485 100644 --- a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp +++ b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp @@ -28,6 +28,7 @@ #include #include #include +#include #include diff --git a/control/autonomous_emergency_braking/src/node.cpp b/control/autonomous_emergency_braking/src/node.cpp index 7e10ca9864f11..6eff270e78326 100644 --- a/control/autonomous_emergency_braking/src/node.cpp +++ b/control/autonomous_emergency_braking/src/node.cpp @@ -15,7 +15,9 @@ #include "autonomous_emergency_braking/node.hpp" #include -#include +#include +#include +#include #include #include diff --git a/control/control_validator/src/control_validator.cpp b/control/control_validator/src/control_validator.cpp index 59b798a786722..6c3996bfd274b 100644 --- a/control/control_validator/src/control_validator.cpp +++ b/control/control_validator/src/control_validator.cpp @@ -163,7 +163,9 @@ void ControlValidator::publishDebugInfo() void ControlValidator::validate(const Trajectory & predicted_trajectory) { if (predicted_trajectory.points.size() < 2) { - RCLCPP_ERROR(get_logger(), "predicted_trajectory size is less than 2. Cannot validate."); + RCLCPP_ERROR_THROTTLE( + get_logger(), *get_clock(), 1000, + "predicted_trajectory size is less than 2. Cannot validate."); return; } @@ -204,7 +206,9 @@ void ControlValidator::displayStatus() const auto & s = validation_status_; - warn(s.is_valid_max_distance_deviation, "planning trajectory is too far from ego!!"); + warn( + s.is_valid_max_distance_deviation, + "predicted trajectory is too far from planning trajectory!!"); } } // namespace control_validator diff --git a/control/control_validator/src/utils.cpp b/control/control_validator/src/utils.cpp index 2aade12d5f1cf..1b11d1e55c376 100644 --- a/control/control_validator/src/utils.cpp +++ b/control/control_validator/src/utils.cpp @@ -44,7 +44,8 @@ void insertPointInPredictedTrajectory( TrajectoryPoints reverseTrajectoryPoints(const TrajectoryPoints & trajectory_points) { - TrajectoryPoints reversed_trajectory_points(trajectory_points.size()); + TrajectoryPoints reversed_trajectory_points; + reversed_trajectory_points.reserve(trajectory_points.size()); std::reverse_copy( trajectory_points.begin(), trajectory_points.end(), std::back_inserter(reversed_trajectory_points)); @@ -106,7 +107,7 @@ Trajectory alignTrajectoryWithReferenceTrajectory( // predicted_trajectory: 　　　　p1-----p2-----p3----//------pN // trajectory: t1--------//------tN // ↓ - // predicted_trajectory: 　　　　 tNew--p3----//------pN + // predicted_trajectory: 　　　　 pNew--p3----//------pN // trajectory: t1--------//------tN auto predicted_trajectory_point_removed = removeFrontTrajectoryPoint( trajectory_points, modified_trajectory_points, predicted_trajectory_points); diff --git a/control/lane_departure_checker/README.md b/control/lane_departure_checker/README.md index 5148d6a998f50..67b770aefb3d2 100644 --- a/control/lane_departure_checker/README.md +++ b/control/lane_departure_checker/README.md @@ -63,15 +63,30 @@ This package includes the following features: ### Node Parameters -| Name | Type | Description | Default value | -| :---------------------------- | :----- | :------------------------------------------------------------ | :------------ | -| update_rate | double | Frequency for publishing [Hz] | 10.0 | -| visualize_lanelet | bool | Flag for visualizing lanelet | False | -| include_right_lanes | bool | Flag for including right lanelet in borders | False | -| include_left_lanes | bool | Flag for including left lanelet in borders | False | -| include_opposite_lanes | bool | Flag for including opposite lanelet in borders | False | -| include_conflicting_lanes | bool | Flag for including conflicting lanelet in borders | False | -| road_border_departure_checker | bool | Flag for checking if the vehicle will departs the road border | False | +#### General Parameters + +| Name | Type | Description | Default value | +| :------------------------- | :----- | :---------------------------------------------------------------------------------------------------------- | :------------ | +| will_out_of_lane_checker | bool | Enable checker whether ego vehicle footprint will depart from lane | True | +| out_of_lane_checker | bool | Enable checker whether ego vehicle footprint is out of lane | True | +| boundary_departure_checker | bool | Enable checker whether ego vehicle footprint wil depart from boundary specified by boundary_types_to_detect | False | +| update_rate | double | Frequency for publishing [Hz] | 10.0 | +| visualize_lanelet | bool | Flag for visualizing lanelet | False | + +#### Parameters For Lane Departure + +| Name | Type | Description | Default value | +| :------------------------ | :--- | :------------------------------------------------ | :------------ | +| include_right_lanes | bool | Flag for including right lanelet in borders | False | +| include_left_lanes | bool | Flag for including left lanelet in borders | False | +| include_opposite_lanes | bool | Flag for including opposite lanelet in borders | False | +| include_conflicting_lanes | bool | Flag for including conflicting lanelet in borders | False | + +#### Parameters For Road Border Departure + +| Name | Type | Description | Default value | +| :----------------------- | :------------------------- | :---------------------------------------------------------- | :------------ | +| boundary_types_to_detect | std::vector\ | line_string types to detect with boundary_departure_checker | [road_border] | ### Core Parameters diff --git a/control/lane_departure_checker/config/lane_departure_checker.param.yaml b/control/lane_departure_checker/config/lane_departure_checker.param.yaml index 008832b1cab21..f0a8df21c425b 100644 --- a/control/lane_departure_checker/config/lane_departure_checker.param.yaml +++ b/control/lane_departure_checker/config/lane_departure_checker.param.yaml @@ -1,5 +1,10 @@ /**: ros__parameters: + # Enable feature + will_out_of_lane_checker: true + out_of_lane_checker: true + boundary_departure_checker: false + # Node update_rate: 10.0 visualize_lanelet: false @@ -7,7 +12,8 @@ include_left_lanes: false include_opposite_lanes: false include_conflicting_lanes: false - road_border_departure_checker: false + boundary_types_to_detect: [road_border] + # Core footprint_margin_scale: 1.0 diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp index 9822fd820dc3c..c77503106e485 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp @@ -18,7 +18,6 @@ #include #include #include -#include #include #include @@ -73,6 +72,7 @@ struct Input lanelet::ConstLanelets shoulder_lanelets{}; Trajectory::ConstSharedPtr reference_trajectory{}; Trajectory::ConstSharedPtr predicted_trajectory{}; + std::vector boundary_types_to_detect{}; }; struct Output @@ -80,7 +80,7 @@ struct Output std::map processing_time_map{}; bool will_leave_lane{}; bool is_out_of_lane{}; - bool will_cross_road_border{}; + bool will_cross_boundary{}; PoseDeviation trajectory_deviation{}; lanelet::ConstLanelets candidate_lanelets{}; TrajectoryPoints resampled_trajectory{}; @@ -137,9 +137,10 @@ class LaneDepartureChecker const lanelet::ConstLanelets & candidate_lanelets, const std::vector & vehicle_footprints); - static bool willCrossRoadBorder( + static bool willCrossBoundary( const lanelet::ConstLanelets & candidate_lanelets, - const std::vector & vehicle_footprints); + const std::vector & vehicle_footprints, + const std::vector & boundary_types_to_detects); static bool isCrossingRoadBorder( const lanelet::BasicLineString2d & road_border, const std::vector & footprints); diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp index f7f7bcfda7d51..c146957bbd314 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp @@ -37,6 +37,7 @@ #include #include +#include #include namespace lane_departure_checker @@ -45,13 +46,17 @@ using autoware_auto_mapping_msgs::msg::HADMapBin; struct NodeParam { + bool will_out_of_lane_checker; + bool out_of_lane_checker; + bool boundary_departure_checker; + double update_rate; bool visualize_lanelet; bool include_right_lanes; bool include_left_lanes; bool include_opposite_lanes; bool include_conflicting_lanes; - bool road_border_departure_checker; + std::vector boundary_types_to_detect; }; class LaneDepartureCheckerNode : public rclcpp::Node diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp index f1d8d75452df1..9d413b2bf0a70 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp @@ -67,19 +67,19 @@ bool isInAnyLane(const lanelet::ConstLanelets & candidate_lanelets, const Point2 return false; } -bool isCrossingWithRoadBorder( - const lanelet::BasicLineString2d & road_border, const std::vector & footprints) +bool isCrossingWithBoundary( + const lanelet::BasicLineString2d & boundary, const std::vector & footprints) { for (auto & footprint : footprints) { for (size_t i = 0; i < footprint.size() - 1; ++i) { auto footprint1 = footprint.at(i).to_3d(); auto footprint2 = footprint.at(i + 1).to_3d(); - for (size_t i = 0; i < road_border.size() - 1; ++i) { - auto road_border1 = road_border.at(i); - auto road_border2 = road_border.at(i + 1); + for (size_t i = 0; i < boundary.size() - 1; ++i) { + auto boundary1 = boundary.at(i); + auto boundary2 = boundary.at(i + 1); if (tier4_autoware_utils::intersect( tier4_autoware_utils::toMsg(footprint1), tier4_autoware_utils::toMsg(footprint2), - fromVector2dToMsg(road_border1), fromVector2dToMsg(road_border2))) { + fromVector2dToMsg(boundary1), fromVector2dToMsg(boundary2))) { return true; } } @@ -172,9 +172,9 @@ Output LaneDepartureChecker::update(const Input & input) output.is_out_of_lane = isOutOfLane(output.candidate_lanelets, output.vehicle_footprints.front()); output.processing_time_map["isOutOfLane"] = stop_watch.toc(true); - output.will_cross_road_border = - willCrossRoadBorder(output.candidate_lanelets, output.vehicle_footprints); - output.processing_time_map["willCrossRoadBorder"] = stop_watch.toc(true); + output.will_cross_boundary = willCrossBoundary( + output.candidate_lanelets, output.vehicle_footprints, input.boundary_types_to_detect); + output.processing_time_map["willCrossBoundary"] = stop_watch.toc(true); return output; } @@ -334,15 +334,18 @@ bool LaneDepartureChecker::isOutOfLane( return false; } -bool LaneDepartureChecker::willCrossRoadBorder( +bool LaneDepartureChecker::willCrossBoundary( const lanelet::ConstLanelets & candidate_lanelets, - const std::vector & vehicle_footprints) + const std::vector & vehicle_footprints, + const std::vector & boundary_types_to_detect) { for (const auto & candidate_lanelet : candidate_lanelets) { const std::string r_type = candidate_lanelet.rightBound().attributeOr(lanelet::AttributeName::Type, "none"); - if (r_type == "road_border") { - if (isCrossingWithRoadBorder( + if ( + std::find(boundary_types_to_detect.begin(), boundary_types_to_detect.end(), r_type) != + boundary_types_to_detect.end()) { + if (isCrossingWithBoundary( candidate_lanelet.rightBound2d().basicLineString(), vehicle_footprints)) { // std::cerr << "The crossed road_border's line string id: " // << candidate_lanelet.rightBound().id() << std::endl; @@ -351,8 +354,10 @@ bool LaneDepartureChecker::willCrossRoadBorder( } const std::string l_type = candidate_lanelet.leftBound().attributeOr(lanelet::AttributeName::Type, "none"); - if (l_type == "road_border") { - if (isCrossingWithRoadBorder( + if ( + std::find(boundary_types_to_detect.begin(), boundary_types_to_detect.end(), l_type) != + boundary_types_to_detect.end()) { + if (isCrossingWithBoundary( candidate_lanelet.leftBound2d().basicLineString(), vehicle_footprints)) { // std::cerr << "The crossed road_border's line string id: " // << candidate_lanelet.leftBound().id() << std::endl; diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp index 20d535a82bfa1..f788e64316171 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp @@ -125,6 +125,11 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o { using std::placeholders::_1; + // Enable feature + node_param_.will_out_of_lane_checker = declare_parameter("will_out_of_lane_checker"); + node_param_.out_of_lane_checker = declare_parameter("out_of_lane_checker"); + node_param_.boundary_departure_checker = declare_parameter("boundary_departure_checker"); + // Node Parameter node_param_.update_rate = declare_parameter("update_rate"); node_param_.visualize_lanelet = declare_parameter("visualize_lanelet"); @@ -132,8 +137,10 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o node_param_.include_left_lanes = declare_parameter("include_left_lanes"); node_param_.include_opposite_lanes = declare_parameter("include_opposite_lanes"); node_param_.include_conflicting_lanes = declare_parameter("include_conflicting_lanes"); - node_param_.road_border_departure_checker = - declare_parameter("road_border_departure_checker"); + + // Boundary_departure_checker + node_param_.boundary_types_to_detect = + declare_parameter>("boundary_types_to_detect"); // Vehicle Info const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); @@ -338,6 +345,7 @@ void LaneDepartureCheckerNode::onTimer() input_.shoulder_lanelets = shoulder_lanelets_; input_.reference_trajectory = reference_trajectory_; input_.predicted_trajectory = predicted_trajectory_; + input_.boundary_types_to_detect = node_param_.boundary_types_to_detect; processing_time_map["Node: setInputData"] = stop_watch.toc(true); output_ = lane_departure_checker_->update(input_); @@ -377,12 +385,19 @@ rcl_interfaces::msg::SetParametersResult LaneDepartureCheckerNode::onParameter( result.reason = "success"; try { + // Enable feature + update_param(parameters, "will_out_of_lane_checker", node_param_.will_out_of_lane_checker); + update_param(parameters, "out_of_lane_checker", node_param_.out_of_lane_checker); + update_param(parameters, "boundary_departure_checker", node_param_.boundary_departure_checker); + // Node update_param(parameters, "visualize_lanelet", node_param_.visualize_lanelet); update_param(parameters, "include_right_lanes", node_param_.include_right_lanes); update_param(parameters, "include_left_lanes", node_param_.include_left_lanes); update_param(parameters, "include_opposite_lanes", node_param_.include_opposite_lanes); update_param(parameters, "include_conflicting_lanes", node_param_.include_conflicting_lanes); + update_param(parameters, "boundary_departure_checker", node_param_.boundary_departure_checker); + update_param(parameters, "boundary_types_to_detect", node_param_.boundary_types_to_detect); // Core update_param(parameters, "footprint_margin_scale", param_.footprint_margin_scale); @@ -409,19 +424,19 @@ void LaneDepartureCheckerNode::checkLaneDeparture( int8_t level = DiagStatus::OK; std::string msg = "OK"; - if (output_.will_leave_lane) { + if (output_.will_leave_lane && node_param_.will_out_of_lane_checker) { level = DiagStatus::WARN; msg = "vehicle will leave lane"; } - if (output_.is_out_of_lane) { + if (output_.is_out_of_lane && node_param_.out_of_lane_checker) { level = DiagStatus::ERROR; msg = "vehicle is out of lane"; } - if (output_.will_cross_road_border && node_param_.road_border_departure_checker) { + if (output_.will_cross_boundary && node_param_.boundary_departure_checker) { level = DiagStatus::ERROR; - msg = "vehicle will cross road border"; + msg = "vehicle will cross boundary"; } stat.summary(level, msg); diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp index ee33062854ab9..b108e7602939c 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp @@ -52,11 +52,13 @@ using tier4_debug_msgs::msg::Float32Stamped; class MpcLateralController : public trajectory_follower::LateralControllerBase { public: + /// \param node Reference to the node used only for the component and parameter initialization. explicit MpcLateralController(rclcpp::Node & node); virtual ~MpcLateralController(); private: - rclcpp::Node * node_; + rclcpp::Clock::SharedPtr clock_; + rclcpp::Logger logger_; rclcpp::Publisher::SharedPtr m_pub_predicted_traj; rclcpp::Publisher::SharedPtr m_pub_debug_values; @@ -144,23 +146,27 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase * @param wheelbase Vehicle's wheelbase. * @param steer_lim Steering command limit. * @param steer_tau Steering time constant. + * @param node Reference to the node. * @return Pointer to the created vehicle model. */ std::shared_ptr createVehicleModel( - const double wheelbase, const double steer_lim, const double steer_tau); + const double wheelbase, const double steer_lim, const double steer_tau, rclcpp::Node & node); /** * @brief Create the quadratic problem solver interface. + * @param node Reference to the node. * @return Pointer to the created QP solver interface. */ - std::shared_ptr createQPSolverInterface(); + std::shared_ptr createQPSolverInterface(rclcpp::Node & node); /** * @brief Create the steering offset estimator for offset compensation. * @param wheelbase Vehicle's wheelbase. + * @param node Reference to the node. * @return Pointer to the created steering offset estimator. */ - std::shared_ptr createSteerOffsetEstimator(const double wheelbase); + std::shared_ptr createSteerOffsetEstimator( + const double wheelbase, rclcpp::Node & node); /** * @brief Check if all necessary data is received and ready to run the control. @@ -250,8 +256,9 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase /** * @brief Declare MPC parameters as ROS parameters to allow tuning on the fly. + * @param node Reference to the node. */ - void declareMPCparameters(); + void declareMPCparameters(rclcpp::Node & node); /** * @brief Callback function called when parameters are changed outside of the node. @@ -264,13 +271,13 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase template inline void info_throttle(Args &&... args) { - RCLCPP_INFO_THROTTLE(node_->get_logger(), *node_->get_clock(), 5000, args...); + RCLCPP_INFO_THROTTLE(logger_, *clock_, 5000, args...); } template inline void warn_throttle(Args &&... args) { - RCLCPP_WARN_THROTTLE(node_->get_logger(), *node_->get_clock(), 5000, args...); + RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, args...); } }; } // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp index 643f8a6f91023..82ebadafa17bf 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -14,8 +14,10 @@ #include "mpc_lateral_controller/mpc.hpp" -#include "motion_utils/motion_utils.hpp" +#include "interpolation/linear_interpolation.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "mpc_lateral_controller/mpc_utils.hpp" +#include "tier4_autoware_utils/math/unit_conversion.hpp" #include #include diff --git a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp index af680b5050037..751873cacbf12 100644 --- a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -14,7 +14,7 @@ #include "mpc_lateral_controller/mpc_lateral_controller.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" #include "mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp" #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" @@ -35,13 +35,14 @@ namespace autoware::motion::control::mpc_lateral_controller { -MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} +MpcLateralController::MpcLateralController(rclcpp::Node & node) +: clock_(node.get_clock()), logger_(node.get_logger()) { - const auto dp_int = [&](const std::string & s) { return node_->declare_parameter(s); }; - const auto dp_bool = [&](const std::string & s) { return node_->declare_parameter(s); }; - const auto dp_double = [&](const std::string & s) { return node_->declare_parameter(s); }; + const auto dp_int = [&](const std::string & s) { return node.declare_parameter(s); }; + const auto dp_bool = [&](const std::string & s) { return node.declare_parameter(s); }; + const auto dp_double = [&](const std::string & s) { return node.declare_parameter(s); }; - m_mpc.m_ctrl_period = node_->get_parameter("ctrl_period").as_double(); + m_mpc.m_ctrl_period = node.get_parameter("ctrl_period").as_double(); auto & p_filt = m_trajectory_filtering_param; p_filt.enable_path_smoothing = dp_bool("enable_path_smoothing"); @@ -66,16 +67,16 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} m_mpc_converged_threshold_rps = dp_double("mpc_converged_threshold_rps"); // [rad/s] /* mpc parameters */ - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*node_).getVehicleInfo(); + const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); const double wheelbase = vehicle_info.wheel_base_m; constexpr double deg2rad = static_cast(M_PI) / 180.0; m_mpc.m_steer_lim = vehicle_info.max_steer_angle_rad; // steer rate limit depending on curvature const auto steer_rate_lim_dps_list_by_curvature = - node_->declare_parameter>("steer_rate_lim_dps_list_by_curvature"); + node.declare_parameter>("steer_rate_lim_dps_list_by_curvature"); const auto curvature_list_for_steer_rate_lim = - node_->declare_parameter>("curvature_list_for_steer_rate_lim"); + node.declare_parameter>("curvature_list_for_steer_rate_lim"); for (size_t i = 0; i < steer_rate_lim_dps_list_by_curvature.size(); ++i) { m_mpc.m_steer_rate_lim_map_by_curvature.emplace_back( curvature_list_for_steer_rate_lim.at(i), @@ -84,9 +85,9 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} // steer rate limit depending on velocity const auto steer_rate_lim_dps_list_by_velocity = - node_->declare_parameter>("steer_rate_lim_dps_list_by_velocity"); + node.declare_parameter>("steer_rate_lim_dps_list_by_velocity"); const auto velocity_list_for_steer_rate_lim = - node_->declare_parameter>("velocity_list_for_steer_rate_lim"); + node.declare_parameter>("velocity_list_for_steer_rate_lim"); for (size_t i = 0; i < steer_rate_lim_dps_list_by_velocity.size(); ++i) { m_mpc.m_steer_rate_lim_map_by_velocity.emplace_back( velocity_list_for_steer_rate_lim.at(i), steer_rate_lim_dps_list_by_velocity.at(i) * deg2rad); @@ -94,12 +95,12 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} /* vehicle model setup */ auto vehicle_model_ptr = - createVehicleModel(wheelbase, m_mpc.m_steer_lim, m_mpc.m_param.steer_tau); + createVehicleModel(wheelbase, m_mpc.m_steer_lim, m_mpc.m_param.steer_tau, node); m_mpc.setVehicleModel(vehicle_model_ptr); /* QP solver setup */ m_mpc.setVehicleModel(vehicle_model_ptr); - auto qpsolver_ptr = createQPSolverInterface(); + auto qpsolver_ptr = createQPSolverInterface(node); m_mpc.setQPSolver(qpsolver_ptr); /* delay compensation */ @@ -113,7 +114,7 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} /* steering offset compensation */ enable_auto_steering_offset_removal_ = dp_bool("steering_offset.enable_auto_steering_offset_removal"); - steering_offset_ = createSteerOffsetEstimator(wheelbase); + steering_offset_ = createSteerOffsetEstimator(wheelbase, node); /* initialize low-pass filter */ { @@ -124,30 +125,29 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} // ego nearest index search const auto check_and_get_param = [&](const auto & param) { - return node_->has_parameter(param) ? node_->get_parameter(param).as_double() : dp_double(param); + return node.has_parameter(param) ? node.get_parameter(param).as_double() : dp_double(param); }; m_ego_nearest_dist_threshold = check_and_get_param("ego_nearest_dist_threshold"); m_ego_nearest_yaw_threshold = check_and_get_param("ego_nearest_yaw_threshold"); m_mpc.ego_nearest_dist_threshold = m_ego_nearest_dist_threshold; m_mpc.ego_nearest_yaw_threshold = m_ego_nearest_yaw_threshold; - m_pub_predicted_traj = node_->create_publisher("~/output/predicted_trajectory", 1); + m_pub_predicted_traj = node.create_publisher("~/output/predicted_trajectory", 1); m_pub_debug_values = - node_->create_publisher("~/output/lateral_diagnostic", 1); - m_pub_steer_offset = - node_->create_publisher("~/output/estimated_steer_offset", 1); + node.create_publisher("~/output/lateral_diagnostic", 1); + m_pub_steer_offset = node.create_publisher("~/output/estimated_steer_offset", 1); - declareMPCparameters(); + declareMPCparameters(node); /* get parameter updates */ using std::placeholders::_1; - m_set_param_res = node_->add_on_set_parameters_callback( - std::bind(&MpcLateralController::paramCallback, this, _1)); + m_set_param_res = + node.add_on_set_parameters_callback(std::bind(&MpcLateralController::paramCallback, this, _1)); m_mpc.initializeSteeringPredictor(); - m_mpc.setLogger(node_->get_logger()); - m_mpc.setClock(node_->get_clock()); + m_mpc.setLogger(logger_); + m_mpc.setClock(clock_); } MpcLateralController::~MpcLateralController() @@ -155,12 +155,11 @@ MpcLateralController::~MpcLateralController() } std::shared_ptr MpcLateralController::createVehicleModel( - const double wheelbase, const double steer_lim, const double steer_tau) + const double wheelbase, const double steer_lim, const double steer_tau, rclcpp::Node & node) { std::shared_ptr vehicle_model_ptr; - const std::string vehicle_model_type = - node_->declare_parameter("vehicle_model_type"); + const std::string vehicle_model_type = node.declare_parameter("vehicle_model_type"); if (vehicle_model_type == "kinematics") { vehicle_model_ptr = std::make_shared(wheelbase, steer_lim, steer_tau); @@ -173,12 +172,12 @@ std::shared_ptr MpcLateralController::createVehicleModel( } if (vehicle_model_type == "dynamics") { - const double mass_fl = node_->declare_parameter("vehicle.mass_fl"); - const double mass_fr = node_->declare_parameter("vehicle.mass_fr"); - const double mass_rl = node_->declare_parameter("vehicle.mass_rl"); - const double mass_rr = node_->declare_parameter("vehicle.mass_rr"); - const double cf = node_->declare_parameter("vehicle.cf"); - const double cr = node_->declare_parameter("vehicle.cr"); + const double mass_fl = node.declare_parameter("vehicle.mass_fl"); + const double mass_fr = node.declare_parameter("vehicle.mass_fr"); + const double mass_rl = node.declare_parameter("vehicle.mass_rl"); + const double mass_rr = node.declare_parameter("vehicle.mass_rr"); + const double cf = node.declare_parameter("vehicle.cf"); + const double cr = node.declare_parameter("vehicle.cr"); // vehicle_model_ptr is only assigned in ctor, so parameter value have to be passed at init time vehicle_model_ptr = @@ -186,15 +185,16 @@ std::shared_ptr MpcLateralController::createVehicleModel( return vehicle_model_ptr; } - RCLCPP_ERROR(node_->get_logger(), "vehicle_model_type is undefined"); + RCLCPP_ERROR(logger_, "vehicle_model_type is undefined"); return vehicle_model_ptr; } -std::shared_ptr MpcLateralController::createQPSolverInterface() +std::shared_ptr MpcLateralController::createQPSolverInterface( + rclcpp::Node & node) { std::shared_ptr qpsolver_ptr; - const std::string qp_solver_type = node_->declare_parameter("qp_solver_type"); + const std::string qp_solver_type = node.declare_parameter("qp_solver_type"); if (qp_solver_type == "unconstraint_fast") { qpsolver_ptr = std::make_shared(); @@ -202,22 +202,22 @@ std::shared_ptr MpcLateralController::createQPSolverInterface } if (qp_solver_type == "osqp") { - qpsolver_ptr = std::make_shared(node_->get_logger()); + qpsolver_ptr = std::make_shared(logger_); return qpsolver_ptr; } - RCLCPP_ERROR(node_->get_logger(), "qp_solver_type is undefined"); + RCLCPP_ERROR(logger_, "qp_solver_type is undefined"); return qpsolver_ptr; } std::shared_ptr MpcLateralController::createSteerOffsetEstimator( - const double wheelbase) + const double wheelbase, rclcpp::Node & node) { const std::string ns = "steering_offset."; - const auto vel_thres = node_->declare_parameter(ns + "update_vel_threshold"); - const auto steer_thres = node_->declare_parameter(ns + "update_steer_threshold"); - const auto limit = node_->declare_parameter(ns + "steering_offset_limit"); - const auto num = node_->declare_parameter(ns + "average_num"); + const auto vel_thres = node.declare_parameter(ns + "update_vel_threshold"); + const auto steer_thres = node.declare_parameter(ns + "update_steer_threshold"); + const auto limit = node.declare_parameter(ns + "steering_offset_limit"); + const auto num = node.declare_parameter(ns + "average_num"); steering_offset_ = std::make_shared(wheelbase, num, vel_thres, steer_thres, limit); return steering_offset_; @@ -306,7 +306,7 @@ bool MpcLateralController::isSteerConverged(const AckermannLateralCommand & cmd) // wait for a while to propagate the trajectory shape to the output command when the trajectory // shape is changed. if (!m_has_received_first_trajectory || isTrajectoryShapeChanged()) { - RCLCPP_DEBUG(node_->get_logger(), "trajectory shaped is changed"); + RCLCPP_DEBUG(logger_, "trajectory shaped is changed"); return false; } @@ -344,12 +344,12 @@ void MpcLateralController::setTrajectory(const Trajectory & msg) m_current_trajectory = msg; if (msg.points.size() < 3) { - RCLCPP_DEBUG(node_->get_logger(), "received path size is < 3, not enough."); + RCLCPP_DEBUG(logger_, "received path size is < 3, not enough."); return; } if (!isValidTrajectory(msg)) { - RCLCPP_ERROR(node_->get_logger(), "Trajectory is invalid!! stop computing."); + RCLCPP_ERROR(logger_, "Trajectory is invalid!! stop computing."); return; } @@ -424,32 +424,32 @@ AckermannLateralCommand MpcLateralController::createCtrlCmdMsg( const AckermannLateralCommand & ctrl_cmd) { auto out = ctrl_cmd; - out.stamp = node_->now(); + out.stamp = clock_->now(); m_steer_cmd_prev = out.steering_tire_angle; return out; } void MpcLateralController::publishPredictedTraj(Trajectory & predicted_traj) const { - predicted_traj.header.stamp = node_->now(); + predicted_traj.header.stamp = clock_->now(); predicted_traj.header.frame_id = m_current_trajectory.header.frame_id; m_pub_predicted_traj->publish(predicted_traj); } void MpcLateralController::publishDebugValues(Float32MultiArrayStamped & debug_values) const { - debug_values.stamp = node_->now(); + debug_values.stamp = clock_->now(); m_pub_debug_values->publish(debug_values); Float32Stamped offset; - offset.stamp = node_->now(); + offset.stamp = clock_->now(); offset.data = steering_offset_->getOffset(); m_pub_steer_offset->publish(offset); } void MpcLateralController::setSteeringToHistory(const AckermannLateralCommand & steering) { - const auto time = node_->now(); + const auto time = clock_->now(); if (m_mpc_steering_history.empty()) { m_mpc_steering_history.emplace_back(steering, time); m_is_mpc_history_filled = false; @@ -501,12 +501,12 @@ bool MpcLateralController::isMpcConverged() return (max_steering_value - min_steering_value) < m_mpc_converged_threshold_rps; } -void MpcLateralController::declareMPCparameters() +void MpcLateralController::declareMPCparameters(rclcpp::Node & node) { - m_mpc.m_param.prediction_horizon = node_->declare_parameter("mpc_prediction_horizon"); - m_mpc.m_param.prediction_dt = node_->declare_parameter("mpc_prediction_dt"); + m_mpc.m_param.prediction_horizon = node.declare_parameter("mpc_prediction_horizon"); + m_mpc.m_param.prediction_dt = node.declare_parameter("mpc_prediction_dt"); - const auto dp = [&](const auto & param) { return node_->declare_parameter(param); }; + const auto dp = [&](const auto & param) { return node.declare_parameter(param); }; auto & nw = m_mpc.m_param.nominal_weight; nw.lat_error = dp("mpc_weight_lat_error"); diff --git a/control/mpc_lateral_controller/src/mpc_utils.cpp b/control/mpc_lateral_controller/src/mpc_utils.cpp index 891a48299c81d..6d6e980675086 100644 --- a/control/mpc_lateral_controller/src/mpc_utils.cpp +++ b/control/mpc_lateral_controller/src/mpc_utils.cpp @@ -16,7 +16,7 @@ #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/math/normalization.hpp" diff --git a/control/operation_mode_transition_manager/README.md b/control/operation_mode_transition_manager/README.md index 3bbc3a006c73a..3cb07a9bd7821 100644 --- a/control/operation_mode_transition_manager/README.md +++ b/control/operation_mode_transition_manager/README.md @@ -25,7 +25,27 @@ There is also an `In Transition` state that occurs during each mode transitions. ## Design - +A rough design of the relationship between `operation_mode_transition_manager`` and the other nodes is shown below. + +![transition_rough_structure](image/transition_rough_structure.drawio.svg) + +A more detailed structure is below. + +![transition_detailed_structure](image/transition_detailed_structure.drawio.svg) + +Here we see that `operation_mode_transition_manager` has multiple state transitions as follows + +- **AUTOWARE ENABLED <---> DISABLED** + - **ENABLED**: the vehicle is controlled by Autoware. + - **DISABLED**: the vehicle is out of Autoware control, expecting the e.g. manual driving. +- **AUTOWARE ENABLED <---> AUTO/LOCAL/REMOTE/NONE** + - **AUTO**: the vehicle is controlled by Autoware, with the autonomous control command calculated by the planning/control component. + - **LOCAL**: the vehicle is controlled by Autoware, with the locally connected operator, e.g. joystick controller. + - **REMOTE**: the vehicle is controlled by Autoware, with the remotely connected operator. + - **NONE**: the vehicle is not controlled by any operator. +- **IN TRANSITION <---> COMPLETED** + - **IN TRANSITION**: the mode listed above is in the transition process, expecting the former operator to have a responsibility to confirm the transition is completed. + - **COMPLETED**: the mode transition is completed. ## Inputs / Outputs / API @@ -63,13 +83,14 @@ For the backward compatibility (to be removed): ## Parameters -| Name | Type | Description | Default value | -| :--------------------------------- | :------- | :------------------------------------------------------------------------------------------------ | :------------ | -| `transition_timeout` | `double` | If the state transition is not completed within this time, it is considered a transition failure. | 10.0 | -| `frequency_hz` | `double` | running hz | 10.0 | -| `check_engage_condition` | `double` | If false, autonomous transition is always available | 0.1 | -| `nearest_dist_deviation_threshold` | `double` | distance threshold used to find nearest trajectory point | 3.0 | -| `nearest_yaw_deviation_threshold` | `double` | angle threshold used to find nearest trajectory point | 1.57 | +| Name | Type | Description | Default value | +| :--------------------------------- | :------- | :---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| `transition_timeout` | `double` | If the state transition is not completed within this time, it is considered a transition failure. | 10.0 | +| `frequency_hz` | `double` | running hz | 10.0 | +| `enable_engage_on_driving` | `bool` | Set true if you want to engage the autonomous driving mode while the vehicle is driving. If set to false, it will deny Engage in any situation where the vehicle speed is not zero. Note that if you use this feature without adjusting the parameters, it may cause issues like sudden deceleration. Before using, please ensure the engage condition and the vehicle_cmd_gate transition filter are appropriately adjusted. | 0.1 | +| `check_engage_condition` | `bool` | If false, autonomous transition is always available | 0.1 | +| `nearest_dist_deviation_threshold` | `double` | distance threshold used to find nearest trajectory point | 3.0 | +| `nearest_yaw_deviation_threshold` | `double` | angle threshold used to find nearest trajectory point | 1.57 | For `engage_acceptable_limits` related parameters: @@ -94,6 +115,21 @@ For `stable_check` related parameters: | `speed_upper_threshold` | `double` | the velocity deviation between control command and ego vehicle must be within this threshold to complete `Autonomous` transition. | 2.0 | | `speed_lower_threshold` | `double` | the velocity deviation between control command and ego vehicle must be within this threshold to complete `Autonomous` transition. | 2.0 | +## Engage check behavior on each parameter setting + +This matrix describes the scenarios in which the vehicle can be engaged based on the combinations of parameter settings: + +| `enable_engage_on_driving` | `check_engage_condition` | `allow_autonomous_in_stopped` | Scenarios where engage is permitted | +| :------------------------: | :----------------------: | :---------------------------: | :---------------------------------------------------------------- | +| x | x | x | Only when the vehicle is stationary. | +| x | x | o | Only when the vehicle is stationary. | +| x | o | x | When the vehicle is stationary and all engage conditions are met. | +| x | o | o | Only when the vehicle is stationary. | +| o | x | x | At any time (Caution: Not recommended). | +| o | x | o | At any time (Caution: Not recommended). | +| o | o | x | When all engage conditions are met, regardless of vehicle status. | +| o | o | o | When all engage conditions are met or the vehicle is stationary. | + ## Future extensions / Unimplemented parts - Need to remove backward compatibility interfaces. diff --git a/control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml b/control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml index a86443f5cabdb..67ce4b485e8c3 100644 --- a/control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml +++ b/control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml @@ -2,11 +2,16 @@ ros__parameters: transition_timeout: 10.0 frequency_hz: 10.0 + + # set true if you want to engage the autonomous driving mode while the vehicle is driving. If set to false, it will deny Engage in any situation where the vehicle speed is not zero. Note that if you use this feature without adjusting the parameters, it may cause issues like sudden deceleration. Before using, please ensure the engage condition and the vehicle_cmd_gate transition filter are appropriately adjusted. + enable_engage_on_driving: false + check_engage_condition: false # set false if you do not want to care about the engage condition. + nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index engage_acceptable_limits: - allow_autonomous_in_stopped: true # no check if the velocity is zero, always allowed. + allow_autonomous_in_stopped: true # if true, all engage check is skipped. dist_threshold: 1.5 yaw_threshold: 0.524 speed_upper_threshold: 10.0 diff --git a/control/operation_mode_transition_manager/image/transition_detailed_structure.drawio.svg b/control/operation_mode_transition_manager/image/transition_detailed_structure.drawio.svg new file mode 100644 index 0000000000000..3492dca6dd86b --- /dev/null +++ b/control/operation_mode_transition_manager/image/transition_detailed_structure.drawio.svg @@ -0,0 +1,610 @@ + + + + + + + + + + + + +
+
+
+ Control Command +
+ (Auto) +
+
+
+ + Control Command... + + + + + + +
+
+
+ Trajectory Follower +
+
+
+ + Trajectory Follower + + + + + + + + + +
+
+
+ Control Command +
+
+
+ + Control Command + + + + + + +
+
+
+ Vehicle Cmd Gate +
+
+
+ + Vehicle Cmd Gate + + + + + + +
+
+
+ Vehicle Interface +
+
+
+ + Vehicle Interface + + + + + + + +
+
+
+ Current Mode +
+
+
+ + Current Mode + + + + + + + +
+
+
+ Auto mode +
+ Availability +
+
+
+ + Auto mode... + + + + + + + +
+
+
+ Mode Change +
+ Request +
+
+
+ + Mode Change... + + + + + + +
+
+
+ Operation Mode +
+ Transition Manager +
+
+
+ + Operation Mode... + + + + + + + +
+
+
+ Mode Change +
+ Request +
+
+
+ + Mode Change... + + + + + + +
+
+
+ API +
+
+
+ + API + + + + + + +
+
+
+ Trajectory (From Planning) +
+
+
+ + Trajectory (From Planning) + + + + + + + +
+
+
+ Mode Change +
+ Response +
+
+
+ + Mode Change... + + + + + + + + + + +
+
+
+ Ego pose, twist, acceleration (From Localization) +
+
+
+ + Ego pose, twist, accelera... + + + + + + + +
+
+
+ External Control Command +
+
+
+ + External C... + + + + + + +
+
+
+ AUTOWARE ENABLED +
+
+
+ + AUTOWARE ENABLED + + + + + + +
+
+
+ REMOTE +
+
+
+ + REMOTE + + + + + + + + + +
+
+
+ NONE +
+
+
+ + NONE + + + + + + + + + + + + +
+
+
+ AUTO +
+
+
+ + AUTO + + + + + + + + + + + + +
+
+
+ LOCAL +
+
+
+ + LOCAL + + + + + + + + + +
+
+
+ + AUTOWARE +
+ DISABLED + +
+
+
+ + AUTOWARE... + + + + + + + + + +
+
+
+ TRANSITION +
+
+
+ + TRANSITION + + + + + + +
+
+
+ IN TRANSITION +
+
+
+ + IN TRANSITION + + + + + + +
+
+
+ COMPLETED +
+
+
+ + COMPLETED + + + + + + + + + + + +
+
+
+ + Vehicle Control mode +
+ (From Vehicle) + +
+
+
+ + Vehicle Control mode... + + + + + + +
+
+
+ + Control Component + +
+
+
+ + Control Component + + + + + + +
+
+
+ External Command +
+ Selector +
+
+
+ + External Command... + + + + + + + +
+
+
+ Control Command +
+ (Local) +
+
+
+ + Control Co... + + + + + + + +
+
+
+ Control Command +
+ (Remote) +
+
+
+ + Control Command... + + + + + + + Text is not SVG - cannot display + + + diff --git a/control/operation_mode_transition_manager/image/transition_rough_structure.drawio.svg b/control/operation_mode_transition_manager/image/transition_rough_structure.drawio.svg new file mode 100644 index 0000000000000..073a2315355be --- /dev/null +++ b/control/operation_mode_transition_manager/image/transition_rough_structure.drawio.svg @@ -0,0 +1,321 @@ + + + + + + + + + + + +
+
+
+ Control Command +
+
+
+ + Control Command + + + + + + +
+
+
+ Trajectory Follower +
+
+
+ + Trajectory Follower + + + + + + + +
+
+
+ Control Command +
+
+
+ + Control Command + + + + + + +
+
+
+ Vehicle Cmd Gate +
+
+
+ + Vehicle Cmd Gate + + + + + + +
+
+
+ Vehicle Interface +
+
+
+ + Vehicle Interface + + + + + + + +
+
+
+ Current Mode +
+
+
+ + Current Mode + + + + + + + +
+
+
+ Auto mode +
+ Availability +
+
+
+ + Auto mode... + + + + + + + +
+
+
+ Mode Change +
+ Request +
+
+
+ + Mode Change... + + + + + + +
+
+
+ Operation Mode +
+ Transition Manager +
+
+
+ + Operation Mode... + + + + + + + +
+
+
+ Mode Change +
+ Request +
+
+
+ + Mode Change... + + + + + + +
+
+
+ API +
+
+
+ + API + + + + + + + + +
+
+
+ Change the control mode (e.g. Auto/Manual) for the request, follow the control command in the Auto mode +
+
+
+ + Change the control mode (e.g. Auto/Ma... + + + + + + +
+
+
+ Manage the operation mode transition (e.g. Manual/Auto). +
+ It also rejects the request based on the driving conditrion, checks the transition is complited. +
+
+
+ + Manage the operation mode transition... + + + + + + + + +
+
+
+ Manage the operation mode transition (e.g. Manual/Auto). +
+ It also rejects the request, checks the transition is complited. +
+
+
+ + Manage the operation mode transition... + + + + + + +
+
+
+ Apply filter based on the current operation mode (e.g. Manual/Auto, in the process of the transition) +
+
+
+ + Apply filter based on t... + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/control/operation_mode_transition_manager/src/state.cpp b/control/operation_mode_transition_manager/src/state.cpp index f8434bbdb081f..55b672aa48964 100644 --- a/control/operation_mode_transition_manager/src/state.cpp +++ b/control/operation_mode_transition_manager/src/state.cpp @@ -16,8 +16,9 @@ #include "util.hpp" -#include -#include +#include +#include +#include #include #include @@ -45,6 +46,7 @@ AutonomousMode::AutonomousMode(rclcpp::Node * node) "trajectory", 1, [this](const Trajectory::SharedPtr msg) { trajectory_ = *msg; }); check_engage_condition_ = node->declare_parameter("check_engage_condition"); + enable_engage_on_driving_ = node->declare_parameter("enable_engage_on_driving"); nearest_dist_deviation_threshold_ = node->declare_parameter("nearest_dist_deviation_threshold"); nearest_yaw_deviation_threshold_ = @@ -121,14 +123,24 @@ bool AutonomousMode::isModeChangeCompleted() const auto closest_point = trajectory_.points.at(*closest_idx); // check for lateral deviation - const auto dist_deviation = calcDistance2d(closest_point.pose, kinematics_.pose.pose); + const auto dist_deviation = + motion_utils::calcLateralOffset(trajectory_.points, kinematics_.pose.pose.position); + if (std::isnan(dist_deviation)) { + RCLCPP_INFO(logger_, "Not stable yet: lateral offset calculation failed."); + return unstable(); + } if (dist_deviation > stable_check_param_.dist_threshold) { RCLCPP_INFO(logger_, "Not stable yet: distance deviation is too large: %f", dist_deviation); return unstable(); } // check for yaw deviation - const auto yaw_deviation = calcYawDeviation(closest_point.pose, kinematics_.pose.pose); + const auto yaw_deviation = + motion_utils::calcYawDeviation(trajectory_.points, kinematics_.pose.pose); + if (std::isnan(yaw_deviation)) { + RCLCPP_INFO(logger_, "Not stable yet: lateral offset calculation failed."); + return unstable(); + } if (yaw_deviation > stable_check_param_.yaw_threshold) { RCLCPP_INFO(logger_, "Not stable yet: yaw deviation is too large: %f", yaw_deviation); return unstable(); @@ -207,6 +219,15 @@ bool AutonomousMode::isModeChangeAvailable() const auto target_control_speed = control_cmd_.longitudinal.speed; const auto & param = engage_acceptable_param_; + if (!enable_engage_on_driving_ && std::fabs(current_speed) > 1.0e-2) { + RCLCPP_INFO( + logger_, + "Engage unavailable: enable_engage_on_driving is false, and the vehicle is not " + "stationary."); + debug_info_ = DebugInfo{}; // all false + return false; + } + if (trajectory_.points.size() < 2) { RCLCPP_WARN_SKIPFIRST_THROTTLE( logger_, *clock_, 5000, "Engage unavailable: trajectory size must be > 2"); diff --git a/control/operation_mode_transition_manager/src/state.hpp b/control/operation_mode_transition_manager/src/state.hpp index 9d384857bbe3d..de0dd8a543b15 100644 --- a/control/operation_mode_transition_manager/src/state.hpp +++ b/control/operation_mode_transition_manager/src/state.hpp @@ -73,6 +73,7 @@ class AutonomousMode : public ModeChangeBase rclcpp::Clock::SharedPtr clock_; bool check_engage_condition_ = true; // if false, the vehicle is engaged without any checks. + bool enable_engage_on_driving_ = false; // if false, engage is not permited on driving double nearest_dist_deviation_threshold_; // [m] for finding nearest index double nearest_yaw_deviation_threshold_; // [rad] for finding nearest index EngageAcceptableParam engage_acceptable_param_; diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp index c2dbc67f011dc..41d98a4d15541 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -56,6 +56,7 @@ namespace trajectory_follower = ::autoware::motion::control::trajectory_follower class PidLongitudinalController : public trajectory_follower::LongitudinalControllerBase { public: + /// \param node Reference to the node used only for the component and parameter initialization. explicit PidLongitudinalController(rclcpp::Node & node); private: @@ -77,7 +78,9 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro double slope_angle{0.0}; double dt{0.0}; }; - rclcpp::Node * node_; + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_; + rclcpp::Clock::SharedPtr clock_; + rclcpp::Logger logger_; // ros variables rclcpp::Publisher::SharedPtr m_pub_slope; rclcpp::Publisher::SharedPtr m_pub_debug; @@ -202,7 +205,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro // debug values DebugValues m_debug_values; - std::shared_ptr m_last_running_time{std::make_shared(node_->now())}; + std::shared_ptr m_last_running_time{std::make_shared(clock_->now())}; // Diagnostic diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index 337a63bc7dc76..6d3489811b195 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -14,8 +14,9 @@ #include "pid_longitudinal_controller/pid_longitudinal_controller.hpp" -#include "motion_utils/motion_utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/math/normalization.hpp" #include #include @@ -27,110 +28,110 @@ namespace autoware::motion::control::pid_longitudinal_controller { PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) -: node_{&node}, diagnostic_updater_(&node) +: node_parameters_(node.get_node_parameters_interface()), + clock_(node.get_clock()), + logger_(node.get_logger()), + diagnostic_updater_(&node) { using std::placeholders::_1; // parameters timer - m_longitudinal_ctrl_period = node_->get_parameter("ctrl_period").as_double(); + m_longitudinal_ctrl_period = node.get_parameter("ctrl_period").as_double(); - m_wheel_base = vehicle_info_util::VehicleInfoUtil(*node_).getVehicleInfo().wheel_base_m; + m_wheel_base = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo().wheel_base_m; // parameters for delay compensation - m_delay_compensation_time = node_->declare_parameter("delay_compensation_time"); // [s] + m_delay_compensation_time = node.declare_parameter("delay_compensation_time"); // [s] // parameters to enable functions - m_enable_smooth_stop = node_->declare_parameter("enable_smooth_stop"); - m_enable_overshoot_emergency = node_->declare_parameter("enable_overshoot_emergency"); + m_enable_smooth_stop = node.declare_parameter("enable_smooth_stop"); + m_enable_overshoot_emergency = node.declare_parameter("enable_overshoot_emergency"); m_enable_large_tracking_error_emergency = - node_->declare_parameter("enable_large_tracking_error_emergency"); - m_enable_slope_compensation = node_->declare_parameter("enable_slope_compensation"); + node.declare_parameter("enable_large_tracking_error_emergency"); + m_enable_slope_compensation = node.declare_parameter("enable_slope_compensation"); m_enable_keep_stopped_until_steer_convergence = - node_->declare_parameter("enable_keep_stopped_until_steer_convergence"); + node.declare_parameter("enable_keep_stopped_until_steer_convergence"); // parameters for state transition { auto & p = m_state_transition_params; // drive - p.drive_state_stop_dist = node_->declare_parameter("drive_state_stop_dist"); // [m] + p.drive_state_stop_dist = node.declare_parameter("drive_state_stop_dist"); // [m] p.drive_state_offset_stop_dist = - node_->declare_parameter("drive_state_offset_stop_dist"); // [m] + node.declare_parameter("drive_state_offset_stop_dist"); // [m] // stopping - p.stopping_state_stop_dist = - node_->declare_parameter("stopping_state_stop_dist"); // [m] + p.stopping_state_stop_dist = node.declare_parameter("stopping_state_stop_dist"); // [m] p.stopped_state_entry_duration_time = - node_->declare_parameter("stopped_state_entry_duration_time"); // [s] + node.declare_parameter("stopped_state_entry_duration_time"); // [s] // stop - p.stopped_state_entry_vel = - node_->declare_parameter("stopped_state_entry_vel"); // [m/s] + p.stopped_state_entry_vel = node.declare_parameter("stopped_state_entry_vel"); // [m/s] p.stopped_state_entry_acc = - node_->declare_parameter("stopped_state_entry_acc"); // [m/s²] + node.declare_parameter("stopped_state_entry_acc"); // [m/s²] // emergency p.emergency_state_overshoot_stop_dist = - node_->declare_parameter("emergency_state_overshoot_stop_dist"); // [m] + node.declare_parameter("emergency_state_overshoot_stop_dist"); // [m] p.emergency_state_traj_trans_dev = - node_->declare_parameter("emergency_state_traj_trans_dev"); // [m] + node.declare_parameter("emergency_state_traj_trans_dev"); // [m] p.emergency_state_traj_rot_dev = - node_->declare_parameter("emergency_state_traj_rot_dev"); // [m] + node.declare_parameter("emergency_state_traj_rot_dev"); // [m] } // parameters for drive state { // initialize PID gain - const double kp{node_->declare_parameter("kp")}; - const double ki{node_->declare_parameter("ki")}; - const double kd{node_->declare_parameter("kd")}; + const double kp{node.declare_parameter("kp")}; + const double ki{node.declare_parameter("ki")}; + const double kd{node.declare_parameter("kd")}; m_pid_vel.setGains(kp, ki, kd); // initialize PID limits - const double max_pid{node_->declare_parameter("max_out")}; // [m/s^2] - const double min_pid{node_->declare_parameter("min_out")}; // [m/s^2] - const double max_p{node_->declare_parameter("max_p_effort")}; // [m/s^2] - const double min_p{node_->declare_parameter("min_p_effort")}; // [m/s^2] - const double max_i{node_->declare_parameter("max_i_effort")}; // [m/s^2] - const double min_i{node_->declare_parameter("min_i_effort")}; // [m/s^2] - const double max_d{node_->declare_parameter("max_d_effort")}; // [m/s^2] - const double min_d{node_->declare_parameter("min_d_effort")}; // [m/s^2] + const double max_pid{node.declare_parameter("max_out")}; // [m/s^2] + const double min_pid{node.declare_parameter("min_out")}; // [m/s^2] + const double max_p{node.declare_parameter("max_p_effort")}; // [m/s^2] + const double min_p{node.declare_parameter("min_p_effort")}; // [m/s^2] + const double max_i{node.declare_parameter("max_i_effort")}; // [m/s^2] + const double min_i{node.declare_parameter("min_i_effort")}; // [m/s^2] + const double max_d{node.declare_parameter("max_d_effort")}; // [m/s^2] + const double min_d{node.declare_parameter("min_d_effort")}; // [m/s^2] m_pid_vel.setLimits(max_pid, min_pid, max_p, min_p, max_i, min_i, max_d, min_d); // set lowpass filter for vel error and pitch - const double lpf_vel_error_gain{node_->declare_parameter("lpf_vel_error_gain")}; + const double lpf_vel_error_gain{node.declare_parameter("lpf_vel_error_gain")}; m_lpf_vel_error = std::make_shared(0.0, lpf_vel_error_gain); m_current_vel_threshold_pid_integrate = - node_->declare_parameter("current_vel_threshold_pid_integration"); // [m/s] + node.declare_parameter("current_vel_threshold_pid_integration"); // [m/s] m_enable_brake_keeping_before_stop = - node_->declare_parameter("enable_brake_keeping_before_stop"); // [-] - m_brake_keeping_acc = node_->declare_parameter("brake_keeping_acc"); // [m/s^2] + node.declare_parameter("enable_brake_keeping_before_stop"); // [-] + m_brake_keeping_acc = node.declare_parameter("brake_keeping_acc"); // [m/s^2] } // parameters for smooth stop state { const double max_strong_acc{ - node_->declare_parameter("smooth_stop_max_strong_acc")}; // [m/s^2] + node.declare_parameter("smooth_stop_max_strong_acc")}; // [m/s^2] const double min_strong_acc{ - node_->declare_parameter("smooth_stop_min_strong_acc")}; // [m/s^2] - const double weak_acc{node_->declare_parameter("smooth_stop_weak_acc")}; // [m/s^2] + node.declare_parameter("smooth_stop_min_strong_acc")}; // [m/s^2] + const double weak_acc{node.declare_parameter("smooth_stop_weak_acc")}; // [m/s^2] const double weak_stop_acc{ - node_->declare_parameter("smooth_stop_weak_stop_acc")}; // [m/s^2] + node.declare_parameter("smooth_stop_weak_stop_acc")}; // [m/s^2] const double strong_stop_acc{ - node_->declare_parameter("smooth_stop_strong_stop_acc")}; // [m/s^2] + node.declare_parameter("smooth_stop_strong_stop_acc")}; // [m/s^2] - const double max_fast_vel{ - node_->declare_parameter("smooth_stop_max_fast_vel")}; // [m/s] + const double max_fast_vel{node.declare_parameter("smooth_stop_max_fast_vel")}; // [m/s] const double min_running_vel{ - node_->declare_parameter("smooth_stop_min_running_vel")}; // [m/s] + node.declare_parameter("smooth_stop_min_running_vel")}; // [m/s] const double min_running_acc{ - node_->declare_parameter("smooth_stop_min_running_acc")}; // [m/s^2] + node.declare_parameter("smooth_stop_min_running_acc")}; // [m/s^2] const double weak_stop_time{ - node_->declare_parameter("smooth_stop_weak_stop_time")}; // [s] + node.declare_parameter("smooth_stop_weak_stop_time")}; // [s] const double weak_stop_dist{ - node_->declare_parameter("smooth_stop_weak_stop_dist")}; // [m] + node.declare_parameter("smooth_stop_weak_stop_dist")}; // [m] const double strong_stop_dist{ - node_->declare_parameter("smooth_stop_strong_stop_dist")}; // [m] + node.declare_parameter("smooth_stop_strong_stop_dist")}; // [m] m_smooth_stop.setParams( max_strong_acc, min_strong_acc, weak_acc, weak_stop_acc, strong_stop_acc, max_fast_vel, @@ -140,52 +141,52 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) // parameters for stop state { auto & p = m_stopped_state_params; - p.vel = node_->declare_parameter("stopped_vel"); // [m/s] - p.acc = node_->declare_parameter("stopped_acc"); // [m/s^2] - p.jerk = node_->declare_parameter("stopped_jerk"); // [m/s^3] + p.vel = node.declare_parameter("stopped_vel"); // [m/s] + p.acc = node.declare_parameter("stopped_acc"); // [m/s^2] + p.jerk = node.declare_parameter("stopped_jerk"); // [m/s^3] } // parameters for emergency state { auto & p = m_emergency_state_params; - p.vel = node_->declare_parameter("emergency_vel"); // [m/s] - p.acc = node_->declare_parameter("emergency_acc"); // [m/s^2] - p.jerk = node_->declare_parameter("emergency_jerk"); // [m/s^3] + p.vel = node.declare_parameter("emergency_vel"); // [m/s] + p.acc = node.declare_parameter("emergency_acc"); // [m/s^2] + p.jerk = node.declare_parameter("emergency_jerk"); // [m/s^3] } // parameters for acceleration limit - m_max_acc = node_->declare_parameter("max_acc"); // [m/s^2] - m_min_acc = node_->declare_parameter("min_acc"); // [m/s^2] + m_max_acc = node.declare_parameter("max_acc"); // [m/s^2] + m_min_acc = node.declare_parameter("min_acc"); // [m/s^2] // parameters for jerk limit - m_max_jerk = node_->declare_parameter("max_jerk"); // [m/s^3] - m_min_jerk = node_->declare_parameter("min_jerk"); // [m/s^3] + m_max_jerk = node.declare_parameter("max_jerk"); // [m/s^3] + m_min_jerk = node.declare_parameter("min_jerk"); // [m/s^3] // parameters for slope compensation - m_use_traj_for_pitch = node_->declare_parameter("use_trajectory_for_pitch_calculation"); - const double lpf_pitch_gain{node_->declare_parameter("lpf_pitch_gain")}; + m_use_traj_for_pitch = node.declare_parameter("use_trajectory_for_pitch_calculation"); + const double lpf_pitch_gain{node.declare_parameter("lpf_pitch_gain")}; m_lpf_pitch = std::make_shared(0.0, lpf_pitch_gain); - m_max_pitch_rad = node_->declare_parameter("max_pitch_rad"); // [rad] - m_min_pitch_rad = node_->declare_parameter("min_pitch_rad"); // [rad] + m_max_pitch_rad = node.declare_parameter("max_pitch_rad"); // [rad] + m_min_pitch_rad = node.declare_parameter("min_pitch_rad"); // [rad] // ego nearest index search m_ego_nearest_dist_threshold = - node_->has_parameter("ego_nearest_dist_threshold") - ? node_->get_parameter("ego_nearest_dist_threshold").as_double() - : node_->declare_parameter("ego_nearest_dist_threshold"); // [m] + node.has_parameter("ego_nearest_dist_threshold") + ? node.get_parameter("ego_nearest_dist_threshold").as_double() + : node.declare_parameter("ego_nearest_dist_threshold"); // [m] m_ego_nearest_yaw_threshold = - node_->has_parameter("ego_nearest_yaw_threshold") - ? node_->get_parameter("ego_nearest_yaw_threshold").as_double() - : node_->declare_parameter("ego_nearest_yaw_threshold"); // [rad] + node.has_parameter("ego_nearest_yaw_threshold") + ? node.get_parameter("ego_nearest_yaw_threshold").as_double() + : node.declare_parameter("ego_nearest_yaw_threshold"); // [rad] // subscriber, publisher - m_pub_slope = node_->create_publisher( + m_pub_slope = node.create_publisher( "~/output/slope_angle", rclcpp::QoS{1}); - m_pub_debug = node_->create_publisher( + m_pub_debug = node.create_publisher( "~/output/longitudinal_diagnostic", rclcpp::QoS{1}); // set parameter callback - m_set_param_res = node_->add_on_set_parameters_callback( + m_set_param_res = node.add_on_set_parameters_callback( std::bind(&PidLongitudinalController::paramCallback, this, _1)); // diagnostic @@ -212,14 +213,12 @@ void PidLongitudinalController::setTrajectory( const autoware_auto_planning_msgs::msg::Trajectory & msg) { if (!longitudinal_utils::isValidTrajectory(msg)) { - RCLCPP_ERROR_THROTTLE( - node_->get_logger(), *node_->get_clock(), 3000, "received invalid trajectory. ignore."); + RCLCPP_ERROR_THROTTLE(logger_, *clock_, 3000, "received invalid trajectory. ignore."); return; } if (msg.points.size() < 2) { - RCLCPP_WARN_THROTTLE( - node_->get_logger(), *node_->get_clock(), 3000, "Unexpected trajectory size < 2. Ignored."); + RCLCPP_WARN_THROTTLE(logger_, *clock_, 3000, "Unexpected trajectory size < 2. Ignored."); return; } @@ -258,22 +257,22 @@ rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallbac // drive state { - double kp{node_->get_parameter("kp").as_double()}; - double ki{node_->get_parameter("ki").as_double()}; - double kd{node_->get_parameter("kd").as_double()}; + double kp{node_parameters_->get_parameter("kp").as_double()}; + double ki{node_parameters_->get_parameter("ki").as_double()}; + double kd{node_parameters_->get_parameter("kd").as_double()}; update_param("kp", kp); update_param("ki", ki); update_param("kd", kd); m_pid_vel.setGains(kp, ki, kd); - double max_pid{node_->get_parameter("max_out").as_double()}; - double min_pid{node_->get_parameter("min_out").as_double()}; - double max_p{node_->get_parameter("max_p_effort").as_double()}; - double min_p{node_->get_parameter("min_p_effort").as_double()}; - double max_i{node_->get_parameter("max_i_effort").as_double()}; - double min_i{node_->get_parameter("min_i_effort").as_double()}; - double max_d{node_->get_parameter("max_d_effort").as_double()}; - double min_d{node_->get_parameter("min_d_effort").as_double()}; + double max_pid{node_parameters_->get_parameter("max_out").as_double()}; + double min_pid{node_parameters_->get_parameter("min_out").as_double()}; + double max_p{node_parameters_->get_parameter("max_p_effort").as_double()}; + double min_p{node_parameters_->get_parameter("min_p_effort").as_double()}; + double max_i{node_parameters_->get_parameter("max_i_effort").as_double()}; + double min_i{node_parameters_->get_parameter("min_i_effort").as_double()}; + double max_d{node_parameters_->get_parameter("max_d_effort").as_double()}; + double min_d{node_parameters_->get_parameter("min_d_effort").as_double()}; update_param("max_out", max_pid); update_param("min_out", min_pid); update_param("max_p_effort", max_p); @@ -289,17 +288,25 @@ rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallbac // stopping state { - double max_strong_acc{node_->get_parameter("smooth_stop_max_strong_acc").as_double()}; - double min_strong_acc{node_->get_parameter("smooth_stop_min_strong_acc").as_double()}; - double weak_acc{node_->get_parameter("smooth_stop_weak_acc").as_double()}; - double weak_stop_acc{node_->get_parameter("smooth_stop_weak_stop_acc").as_double()}; - double strong_stop_acc{node_->get_parameter("smooth_stop_strong_stop_acc").as_double()}; - double max_fast_vel{node_->get_parameter("smooth_stop_max_fast_vel").as_double()}; - double min_running_vel{node_->get_parameter("smooth_stop_min_running_vel").as_double()}; - double min_running_acc{node_->get_parameter("smooth_stop_min_running_acc").as_double()}; - double weak_stop_time{node_->get_parameter("smooth_stop_weak_stop_time").as_double()}; - double weak_stop_dist{node_->get_parameter("smooth_stop_weak_stop_dist").as_double()}; - double strong_stop_dist{node_->get_parameter("smooth_stop_strong_stop_dist").as_double()}; + double max_strong_acc{ + node_parameters_->get_parameter("smooth_stop_max_strong_acc").as_double()}; + double min_strong_acc{ + node_parameters_->get_parameter("smooth_stop_min_strong_acc").as_double()}; + double weak_acc{node_parameters_->get_parameter("smooth_stop_weak_acc").as_double()}; + double weak_stop_acc{node_parameters_->get_parameter("smooth_stop_weak_stop_acc").as_double()}; + double strong_stop_acc{ + node_parameters_->get_parameter("smooth_stop_strong_stop_acc").as_double()}; + double max_fast_vel{node_parameters_->get_parameter("smooth_stop_max_fast_vel").as_double()}; + double min_running_vel{ + node_parameters_->get_parameter("smooth_stop_min_running_vel").as_double()}; + double min_running_acc{ + node_parameters_->get_parameter("smooth_stop_min_running_acc").as_double()}; + double weak_stop_time{ + node_parameters_->get_parameter("smooth_stop_weak_stop_time").as_double()}; + double weak_stop_dist{ + node_parameters_->get_parameter("smooth_stop_weak_stop_dist").as_double()}; + double strong_stop_dist{ + node_parameters_->get_parameter("smooth_stop_strong_stop_dist").as_double()}; update_param("smooth_stop_max_strong_acc", max_strong_acc); update_param("smooth_stop_min_strong_acc", min_strong_acc); update_param("smooth_stop_weak_acc", weak_acc); @@ -470,8 +477,7 @@ PidLongitudinalController::Motion PidLongitudinalController::calcEmergencyCtrlCm longitudinal_utils::applyDiffLimitFilter(p.acc, m_prev_raw_ctrl_cmd.acc, dt, p.jerk); RCLCPP_ERROR_THROTTLE( - node_->get_logger(), *node_->get_clock(), 3000, "[Emergency stop] vel: %3.3f, acc: %3.3f", vel, - acc); + logger_, *clock_, 3000, "[Emergency stop] vel: %3.3f, acc: %3.3f", vel, acc); return Motion{vel, acc}; } @@ -496,11 +502,11 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d if ( std::fabs(current_vel) > p.stopped_state_entry_vel || std::fabs(current_acc) > p.stopped_state_entry_acc) { - m_last_running_time = std::make_shared(node_->now()); + m_last_running_time = std::make_shared(clock_->now()); } const bool stopped_condition = m_last_running_time - ? (node_->now() - *m_last_running_time).seconds() > p.stopped_state_entry_duration_time + ? (clock_->now() - *m_last_running_time).seconds() > p.stopped_state_entry_duration_time : false; static constexpr double vel_epsilon = @@ -515,15 +521,14 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d const auto changeState = [this](const auto s) { if (s != m_control_state) { RCLCPP_DEBUG_STREAM( - node_->get_logger(), - "controller state changed: " << toStr(m_control_state) << " -> " << toStr(s)); + logger_, "controller state changed: " << toStr(m_control_state) << " -> " << toStr(s)); } m_control_state = s; return; }; const auto info_throttle = [this](const auto & s) { - RCLCPP_INFO_SKIPFIRST_THROTTLE(node_->get_logger(), *node_->get_clock(), 5000, "%s", s); + RCLCPP_INFO_SKIPFIRST_THROTTLE(logger_, *clock_, 5000, "%s", s); }; // if current operation mode is not autonomous mode, then change state to stopped @@ -610,7 +615,7 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d return; } - RCLCPP_FATAL(node_->get_logger(), "invalid state found."); + RCLCPP_FATAL(logger_, "invalid state found."); return; } @@ -641,7 +646,7 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( raw_ctrl_cmd.vel = target_motion.vel; raw_ctrl_cmd.acc = applyVelocityFeedback(target_motion, control_data.dt, pred_vel_in_target); RCLCPP_DEBUG( - node_->get_logger(), + logger_, "[feedback control] vel: %3.3f, acc: %3.3f, dt: %3.3f, v_curr: %3.3f, v_ref: %3.3f " "feedback_ctrl_cmd.ac: %3.3f", raw_ctrl_cmd.vel, raw_ctrl_cmd.acc, control_data.dt, current_vel, target_motion.vel, @@ -652,8 +657,8 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( raw_ctrl_cmd.vel = m_stopped_state_params.vel; RCLCPP_DEBUG( - node_->get_logger(), "[smooth stop]: Smooth stopping. vel: %3.3f, acc: %3.3f", - raw_ctrl_cmd.vel, raw_ctrl_cmd.acc); + logger_, "[smooth stop]: Smooth stopping. vel: %3.3f, acc: %3.3f", raw_ctrl_cmd.vel, + raw_ctrl_cmd.acc); } else if (m_control_state == ControlState::STOPPED) { // This acceleration is without slope compensation const auto & p = m_stopped_state_params; @@ -661,8 +666,7 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( raw_ctrl_cmd.acc = longitudinal_utils::applyDiffLimitFilter( p.acc, m_prev_raw_ctrl_cmd.acc, control_data.dt, p.jerk); - RCLCPP_DEBUG( - node_->get_logger(), "[Stopped]. vel: %3.3f, acc: %3.3f", raw_ctrl_cmd.vel, raw_ctrl_cmd.acc); + RCLCPP_DEBUG(logger_, "[Stopped]. vel: %3.3f, acc: %3.3f", raw_ctrl_cmd.vel, raw_ctrl_cmd.acc); } else if (m_control_state == ControlState::EMERGENCY) { raw_ctrl_cmd = calcEmergencyCtrlCmd(control_data.dt); } @@ -686,12 +690,12 @@ autoware_auto_control_msgs::msg::LongitudinalCommand PidLongitudinalController:: { // publish control command autoware_auto_control_msgs::msg::LongitudinalCommand cmd{}; - cmd.stamp = node_->now(); + cmd.stamp = clock_->now(); cmd.speed = static_cast(ctrl_cmd.vel); cmd.acceleration = static_cast(ctrl_cmd.acc); // store current velocity history - m_vel_hist.push_back({node_->now(), current_vel}); + m_vel_hist.push_back({clock_->now(), current_vel}); while (m_vel_hist.size() > static_cast(0.5 / m_longitudinal_ctrl_period)) { m_vel_hist.erase(m_vel_hist.begin()); } @@ -714,7 +718,7 @@ void PidLongitudinalController::publishDebugData( // publish debug values tier4_debug_msgs::msg::Float32MultiArrayStamped debug_msg{}; - debug_msg.stamp = node_->now(); + debug_msg.stamp = clock_->now(); for (const auto & v : m_debug_values.getValues()) { debug_msg.data.push_back(static_cast(v)); } @@ -722,7 +726,7 @@ void PidLongitudinalController::publishDebugData( // slope angle tier4_debug_msgs::msg::Float32MultiArrayStamped slope_msg{}; - slope_msg.stamp = node_->now(); + slope_msg.stamp = clock_->now(); slope_msg.data.push_back( static_cast(control_data.slope_angle)); m_pub_slope->publish(slope_msg); @@ -733,10 +737,10 @@ double PidLongitudinalController::getDt() double dt; if (!m_prev_control_time) { dt = m_longitudinal_ctrl_period; - m_prev_control_time = std::make_shared(node_->now()); + m_prev_control_time = std::make_shared(clock_->now()); } else { - dt = (node_->now() - *m_prev_control_time).seconds(); - *m_prev_control_time = node_->now(); + dt = (clock_->now() - *m_prev_control_time).seconds(); + *m_prev_control_time = clock_->now(); } const double max_dt = m_longitudinal_ctrl_period * 2.0; const double min_dt = m_longitudinal_ctrl_period * 0.5; @@ -785,7 +789,7 @@ void PidLongitudinalController::storeAccelCmd(const double accel) if (m_control_state == ControlState::DRIVE) { // convert format autoware_auto_control_msgs::msg::LongitudinalCommand cmd; - cmd.stamp = node_->now(); + cmd.stamp = clock_->now(); cmd.acceleration = static_cast(accel); // store published ctrl cmd @@ -799,7 +803,7 @@ void PidLongitudinalController::storeAccelCmd(const double accel) if (m_ctrl_cmd_vec.size() <= 2) { return; } - if ((node_->now() - m_ctrl_cmd_vec.at(1).stamp).seconds() > m_delay_compensation_time) { + if ((clock_->now() - m_ctrl_cmd_vec.at(1).stamp).seconds() > m_delay_compensation_time) { m_ctrl_cmd_vec.erase(m_ctrl_cmd_vec.begin()); } } @@ -886,9 +890,9 @@ double PidLongitudinalController::predictedVelocityInTargetPoint( double pred_vel = current_vel_abs; const auto past_delay_time = - node_->now() - rclcpp::Duration::from_seconds(delay_compensation_time); + clock_->now() - rclcpp::Duration::from_seconds(delay_compensation_time); for (std::size_t i = 0; i < m_ctrl_cmd_vec.size(); ++i) { - if ((node_->now() - m_ctrl_cmd_vec.at(i).stamp).seconds() < m_delay_compensation_time) { + if ((clock_->now() - m_ctrl_cmd_vec.at(i).stamp).seconds() < m_delay_compensation_time) { if (i == 0) { // size of m_ctrl_cmd_vec is less than m_delay_compensation_time pred_vel = current_vel_abs + @@ -907,7 +911,7 @@ double PidLongitudinalController::predictedVelocityInTargetPoint( const double last_acc = m_ctrl_cmd_vec.at(m_ctrl_cmd_vec.size() - 1).acceleration; const double time_to_current = - (node_->now() - m_ctrl_cmd_vec.at(m_ctrl_cmd_vec.size() - 1).stamp).seconds(); + (clock_->now() - m_ctrl_cmd_vec.at(m_ctrl_cmd_vec.size() - 1).stamp).seconds(); pred_vel += last_acc * time_to_current; // avoid to change sign of current_vel and pred_vel diff --git a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp b/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp index e9e57cf08bcc4..daaebaacde8de 100644 --- a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp +++ b/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp @@ -100,10 +100,12 @@ struct DebugData class PurePursuitLateralController : public LateralControllerBase { public: + /// \param node Reference to the node used only for the component and parameter initialization. explicit PurePursuitLateralController(rclcpp::Node & node); private: - rclcpp::Node::SharedPtr node_; + rclcpp::Clock::SharedPtr clock_; + rclcpp::Logger logger_; std::vector output_tp_array_; autoware_auto_planning_msgs::msg::Trajectory::SharedPtr trajectory_resampled_; autoware_auto_planning_msgs::msg::Trajectory trajectory_; diff --git a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp index cb812202d8651..91b110b35b401 100644 --- a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp +++ b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp @@ -57,48 +57,43 @@ enum TYPE { namespace pure_pursuit { PurePursuitLateralController::PurePursuitLateralController(rclcpp::Node & node) -: node_{&node}, tf_buffer_(node_->get_clock()), tf_listener_(tf_buffer_) +: clock_(node.get_clock()), logger_(node.get_logger()), tf_buffer_(clock_), tf_listener_(tf_buffer_) { pure_pursuit_ = std::make_unique(); // Vehicle Parameters - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*node_).getVehicleInfo(); + const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); param_.wheel_base = vehicle_info.wheel_base_m; param_.max_steering_angle = vehicle_info.max_steer_angle_rad; // Algorithm Parameters - param_.ld_velocity_ratio = node_->declare_parameter("ld_velocity_ratio"); - param_.ld_lateral_error_ratio = node_->declare_parameter("ld_lateral_error_ratio"); - param_.ld_curvature_ratio = node_->declare_parameter("ld_curvature_ratio"); + param_.ld_velocity_ratio = node.declare_parameter("ld_velocity_ratio"); + param_.ld_lateral_error_ratio = node.declare_parameter("ld_lateral_error_ratio"); + param_.ld_curvature_ratio = node.declare_parameter("ld_curvature_ratio"); param_.long_ld_lateral_error_threshold = - node_->declare_parameter("long_ld_lateral_error_threshold"); - param_.min_lookahead_distance = node_->declare_parameter("min_lookahead_distance"); - param_.max_lookahead_distance = node_->declare_parameter("max_lookahead_distance"); + node.declare_parameter("long_ld_lateral_error_threshold"); + param_.min_lookahead_distance = node.declare_parameter("min_lookahead_distance"); + param_.max_lookahead_distance = node.declare_parameter("max_lookahead_distance"); param_.reverse_min_lookahead_distance = - node_->declare_parameter("reverse_min_lookahead_distance"); - param_.converged_steer_rad_ = node_->declare_parameter("converged_steer_rad"); - param_.prediction_ds = node_->declare_parameter("prediction_ds"); - param_.prediction_distance_length = - node_->declare_parameter("prediction_distance_length"); - param_.resampling_ds = node_->declare_parameter("resampling_ds"); + node.declare_parameter("reverse_min_lookahead_distance"); + param_.converged_steer_rad_ = node.declare_parameter("converged_steer_rad"); + param_.prediction_ds = node.declare_parameter("prediction_ds"); + param_.prediction_distance_length = node.declare_parameter("prediction_distance_length"); + param_.resampling_ds = node.declare_parameter("resampling_ds"); param_.curvature_calculation_distance = - node_->declare_parameter("curvature_calculation_distance"); - param_.enable_path_smoothing = node_->declare_parameter("enable_path_smoothing"); - param_.path_filter_moving_ave_num = - node_->declare_parameter("path_filter_moving_ave_num"); + node.declare_parameter("curvature_calculation_distance"); + param_.enable_path_smoothing = node.declare_parameter("enable_path_smoothing"); + param_.path_filter_moving_ave_num = node.declare_parameter("path_filter_moving_ave_num"); // Debug Publishers pub_debug_marker_ = - node_->create_publisher("~/debug/markers", 0); - pub_debug_values_ = node_->create_publisher( + node.create_publisher("~/debug/markers", 0); + pub_debug_values_ = node.create_publisher( "~/debug/ld_outputs", rclcpp::QoS{1}); // Publish predicted trajectory - pub_predicted_trajectory_ = node_->create_publisher( + pub_predicted_trajectory_ = node.create_publisher( "~/output/predicted_trajectory", 1); - - // Wait for first current pose - tf_utils::waitForTransform(tf_buffer_, "map", "base_link"); } double PurePursuitLateralController::calcLookaheadDistance( @@ -128,7 +123,7 @@ double PurePursuitLateralController::calcLookaheadDistance( debug_msg.data.at(TYPE::VELOCITY) = static_cast(velocity); debug_msg.data.at(TYPE::CURVATURE) = static_cast(curvature); debug_msg.data.at(TYPE::LATERAL_ERROR) = static_cast(lateral_error); - debug_msg.stamp = node_->now(); + debug_msg.stamp = clock_->now(); pub_debug_values_->publish(debug_msg); }; @@ -220,7 +215,7 @@ void PurePursuitLateralController::averageFilterTrajectory( autoware_auto_planning_msgs::msg::Trajectory & u) { if (static_cast(u.points.size()) <= 2 * param_.path_filter_moving_ave_num) { - RCLCPP_ERROR(node_->get_logger(), "Cannot smooth path! Trajectory size is too low!"); + RCLCPP_ERROR(logger_, "Cannot smooth path! Trajectory size is too low!"); return; } @@ -303,9 +298,7 @@ boost::optional PurePursuitLateralController::generatePredictedTraje tmp_msg = generateCtrlCmdMsg(pp_output->curvature); predicted_trajectory.points.at(i).longitudinal_velocity_mps = pp_output->velocity; } else { - RCLCPP_WARN_THROTTLE( - node_->get_logger(), *node_->get_clock(), 5000, - "failed to solve pure_pursuit for prediction"); + RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "failed to solve pure_pursuit for prediction"); tmp_msg = generateCtrlCmdMsg(0.0); } TrajectoryPoint p2; @@ -320,9 +313,7 @@ boost::optional PurePursuitLateralController::generatePredictedTraje tmp_msg = generateCtrlCmdMsg(pp_output->curvature); predicted_trajectory.points.at(i).longitudinal_velocity_mps = pp_output->velocity; } else { - RCLCPP_WARN_THROTTLE( - node_->get_logger(), *node_->get_clock(), 5000, - "failed to solve pure_pursuit for prediction"); + RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "failed to solve pure_pursuit for prediction"); tmp_msg = generateCtrlCmdMsg(0.0); } predicted_trajectory.points.push_back( @@ -363,7 +354,7 @@ LateralOutput PurePursuitLateralController::run(const InputData & input_data) // calculate predicted trajectory with iterative calculation const auto predicted_trajectory = generatePredictedTrajectory(); if (!predicted_trajectory) { - RCLCPP_ERROR(node_->get_logger(), "Failed to generate predicted trajectory."); + RCLCPP_ERROR(logger_, "Failed to generate predicted trajectory."); } else { pub_predicted_trajectory_->publish(*predicted_trajectory); } @@ -389,8 +380,7 @@ AckermannLateralCommand PurePursuitLateralController::generateOutputControlCmd() publishDebugMarker(); } else { RCLCPP_WARN_THROTTLE( - node_->get_logger(), *node_->get_clock(), 5000, - "failed to solve pure_pursuit for control command calculation"); + logger_, *clock_, 5000, "failed to solve pure_pursuit for control command calculation"); if (prev_cmd_) { output_cmd = *prev_cmd_; } else { @@ -406,7 +396,7 @@ AckermannLateralCommand PurePursuitLateralController::generateCtrlCmdMsg( const double tmp_steering = planning_utils::convertCurvatureToSteeringAngle(param_.wheel_base, target_curvature); AckermannLateralCommand cmd; - cmd.stamp = node_->get_clock()->now(); + cmd.stamp = clock_->now(); cmd.steering_tire_angle = static_cast( std::min(std::max(tmp_steering, -param_.max_steering_angle), param_.max_steering_angle)); @@ -428,8 +418,7 @@ boost::optional PurePursuitLateralController::calcTargetCurvature( { // Ignore invalid trajectory if (trajectory_resampled_->points.size() < 3) { - RCLCPP_WARN_THROTTLE( - node_->get_logger(), *node_->get_clock(), 5000, "received path size is < 3, ignored"); + RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "received path size is < 3, ignored"); return {}; } @@ -438,7 +427,7 @@ boost::optional PurePursuitLateralController::calcTargetCurvature( const auto closest_idx_result = motion_utils::findNearestIndex(output_tp_array_, pose, 3.0, M_PI_4); if (!closest_idx_result) { - RCLCPP_ERROR(node_->get_logger(), "cannot find closest waypoint"); + RCLCPP_ERROR(logger_, "cannot find closest waypoint"); return {}; } diff --git a/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp b/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp index 4868fa7a6f51d..489709d1f5c4c 100644 --- a/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp +++ b/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp @@ -36,6 +36,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include "tf2_msgs/msg/tf_message.hpp" +#include "visualization_msgs/msg/marker_array.hpp" #include #include diff --git a/control/trajectory_follower_node/package.xml b/control/trajectory_follower_node/package.xml index 8673634e24058..34e9fb45424a7 100644 --- a/control/trajectory_follower_node/package.xml +++ b/control/trajectory_follower_node/package.xml @@ -32,6 +32,7 @@ rclcpp_components trajectory_follower_base vehicle_info_util + visualization_msgs ros2launch diff --git a/control/trajectory_follower_node/src/controller_node.cpp b/control/trajectory_follower_node/src/controller_node.cpp index 5ee9f7f4f1e71..675ca9dffe3ae 100644 --- a/control/trajectory_follower_node/src/controller_node.cpp +++ b/control/trajectory_follower_node/src/controller_node.cpp @@ -17,6 +17,7 @@ #include "mpc_lateral_controller/mpc_lateral_controller.hpp" #include "pid_longitudinal_controller/pid_longitudinal_controller.hpp" #include "pure_pursuit/pure_pursuit_lateral_controller.hpp" +#include "tier4_autoware_utils/ros/marker_helper.hpp" #include #include diff --git a/control/trajectory_follower_node/src/simple_trajectory_follower.cpp b/control/trajectory_follower_node/src/simple_trajectory_follower.cpp index f073574cc229f..5e9488bf2ca93 100644 --- a/control/trajectory_follower_node/src/simple_trajectory_follower.cpp +++ b/control/trajectory_follower_node/src/simple_trajectory_follower.cpp @@ -14,8 +14,8 @@ #include "trajectory_follower_node/simple_trajectory_follower.hpp" -#include -#include +#include +#include #include diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp index 8dec06c455868..6a33855fb4d0a 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp @@ -104,25 +104,28 @@ void VehicleCmdFilter::VehicleCmdFilter::limitLongitudinalWithJerk( std::clamp(static_cast(input.longitudinal.jerk), -lon_jerk_lim, lon_jerk_lim); } +// Use ego vehicle speed (not speed command) for the lateral acceleration calculation, otherwise the +// filtered steering angle oscillates if the input velocity oscillates. void VehicleCmdFilter::limitLateralWithLatAcc( [[maybe_unused]] const double dt, AckermannControlCommand & input) const { const auto lat_acc_lim = getLatAccLim(); - double latacc = calcLatAcc(input); + double latacc = calcLatAcc(input, current_speed_); if (std::fabs(latacc) > lat_acc_lim) { - double v_sq = - std::max(static_cast(input.longitudinal.speed * input.longitudinal.speed), 0.001); + double v_sq = std::max(static_cast(current_speed_ * current_speed_), 0.001); double steer_lim = std::atan(lat_acc_lim * param_.wheel_base / v_sq); input.lateral.steering_tire_angle = latacc > 0.0 ? steer_lim : -steer_lim; } } +// Use ego vehicle speed (not speed command) for the lateral acceleration calculation, otherwise the +// filtered steering angle oscillates if the input velocity oscillates. void VehicleCmdFilter::limitLateralWithLatJerk( const double dt, AckermannControlCommand & input) const { - double curr_latacc = calcLatAcc(input); - double prev_latacc = calcLatAcc(prev_cmd_); + double curr_latacc = calcLatAcc(input, current_speed_); + double prev_latacc = calcLatAcc(prev_cmd_, current_speed_); const auto lat_jerk_lim = getLatJerkLim(); @@ -130,9 +133,9 @@ void VehicleCmdFilter::limitLateralWithLatJerk( const double latacc_min = prev_latacc - lat_jerk_lim * dt; if (curr_latacc > latacc_max) { - input.lateral.steering_tire_angle = calcSteerFromLatacc(input.longitudinal.speed, latacc_max); + input.lateral.steering_tire_angle = calcSteerFromLatacc(current_speed_, latacc_max); } else if (curr_latacc < latacc_min) { - input.lateral.steering_tire_angle = calcSteerFromLatacc(input.longitudinal.speed, latacc_min); + input.lateral.steering_tire_angle = calcSteerFromLatacc(current_speed_, latacc_min); } } @@ -205,6 +208,11 @@ double VehicleCmdFilter::calcLatAcc(const AckermannControlCommand & cmd) const return v * v * std::tan(cmd.lateral.steering_tire_angle) / param_.wheel_base; } +double VehicleCmdFilter::calcLatAcc(const AckermannControlCommand & cmd, const double v) const +{ + return v * v * std::tan(cmd.lateral.steering_tire_angle) / param_.wheel_base; +} + double VehicleCmdFilter::limitDiff( const double curr, const double prev, const double diff_lim) const { diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp index eb85fcbeb8606..a79b8a38bae8d 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp @@ -81,6 +81,7 @@ class VehicleCmdFilter bool setParameterWithValidation(const VehicleCmdFilterParam & p); double calcLatAcc(const AckermannControlCommand & cmd) const; + double calcLatAcc(const AckermannControlCommand & cmd, const double v) const; double calcSteerFromLatacc(const double v, const double latacc) const; double limitDiff(const double curr, const double prev, const double diff_lim) const; diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index aeace3234d6b9..78d91e266c516 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -20,7 +20,7 @@ #include "vehicle_cmd_filter.hpp" #include -#include +#include #include #include #include diff --git a/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp b/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp index 6384fbfb22f60..1156656385117 100644 --- a/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp +++ b/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp @@ -150,7 +150,6 @@ class PubSubNode : public rclcpp::Node if (!cmd_history_.empty()) { // ego moves as commanded. msg.twist.twist.linear.x = cmd_history_.back()->longitudinal.speed; // ego moves as commanded. - } else { } pub_odom_->publish(msg); } @@ -238,16 +237,19 @@ class PubSubNode : public rclcpp::Node const auto lon_jerk = (lon_acc - cmd_prev->longitudinal.acceleration) / dt; const auto lat_acc = lon_vel * lon_vel * std::tan(cmd_curr->lateral.steering_tire_angle) / wheelbase; - const auto prev_lon_vel = cmd_prev->longitudinal.speed; + + // TODO(Horibe): prev_lat_acc should use the previous velocity, but use the current velocity + // since the current filtering logic uses the current velocity. const auto prev_lat_acc = - prev_lon_vel * prev_lon_vel * std::tan(cmd_prev->lateral.steering_tire_angle) / wheelbase; + lon_vel * lon_vel * std::tan(cmd_prev->lateral.steering_tire_angle) / wheelbase; const auto lat_jerk = (lat_acc - prev_lat_acc) / dt; /* debug print */ // const auto steer = cmd_curr->lateral.steering_tire_angle; // PRINT_VALUES( - // dt, lon_vel, lon_acc, lon_jerk, lat_acc, lat_jerk, steer, max_lon_acc_lim, - // max_lon_jerk_lim, max_lat_acc_lim, max_lat_jerk_lim); + // dt, i_curr, i_prev, steer, lon_vel, prev_lon_vel, lon_acc, lon_jerk, lat_acc, prev_lat_acc, + // prev_lat_acc2, lat_jerk, max_lon_acc_lim, max_lon_jerk_lim, max_lat_acc_lim, + // max_lat_jerk_lim); // Output command must be smaller than maximum limit. // TODO(Horibe): check for each velocity range. @@ -368,6 +370,16 @@ TEST_P(TestFixture, CheckFilterForSinCmd) [[maybe_unused]] auto b = std::system("ros2 node info /test_vehicle_cmd_gate_filter_pubsub"); [[maybe_unused]] auto c = std::system("ros2 node info /vehicle_cmd_gate"); + // std::cerr << "speed signal: " << cmd_generator_.p_.velocity.max << " * sin(2pi * " + // << cmd_generator_.p_.velocity.freq << " * dt + " << cmd_generator_.p_.velocity.bias + // << ")" << std::endl; + // std::cerr << "accel signal: " << cmd_generator_.p_.acceleration.max << " * sin(2pi * " + // << cmd_generator_.p_.acceleration.freq << " * dt + " + // << cmd_generator_.p_.acceleration.bias << ")" << std::endl; + // std::cerr << "steer signal: " << cmd_generator_.p_.steering.max << " * sin(2pi * " + // << cmd_generator_.p_.steering.freq << " * dt + " << cmd_generator_.p_.steering.bias + // << ")" << std::endl; + for (size_t i = 0; i < 100; ++i) { const bool reset_clock = (i == 0); const auto cmd = cmd_generator_.calcSinWaveCommand(reset_clock); @@ -384,17 +396,23 @@ TEST_P(TestFixture, CheckFilterForSinCmd) }; // High frequency, large value -CmdParams p1 = {/*steer*/ {10, 1, 0}, /*velocity*/ {10, 1.2, 0}, /*acc*/ {5, 1.5, 2}}; +CmdParams p1 = {/*steer*/ {0.5, 1, 0}, /*velocity*/ {10, 0.0, 0}, /*acc*/ {5, 1.5, 2}}; INSTANTIATE_TEST_SUITE_P(TestParam1, TestFixture, ::testing::Values(p1)); // High frequency, normal value -CmdParams p2 = {/*steer*/ {1.5, 2, 1}, /*velocity*/ {5, 1.0, 0}, /*acc*/ {2.0, 3.0, 2}}; +CmdParams p2 = {/*steer*/ {0.5, 2, 1}, /*velocity*/ {5, 1.0, 0}, /*acc*/ {2.0, 3.0, 2}}; INSTANTIATE_TEST_SUITE_P(TestParam2, TestFixture, ::testing::Values(p2)); // High frequency, small value -CmdParams p3 = {/*steer*/ {1.5, 3, 2}, /*velocity*/ {2, 3, 0}, /*acc*/ {0.5, 3, 2}}; +CmdParams p3 = {/*steer*/ {0.5, 3, 2}, /*velocity*/ {2, 3, 0}, /*acc*/ {0.5, 3, 2}}; INSTANTIATE_TEST_SUITE_P(TestParam3, TestFixture, ::testing::Values(p3)); // Low frequency -CmdParams p4 = {/*steer*/ {10, 0.1, 0.5}, /*velocity*/ {10, 0.2, 0}, /*acc*/ {5, 0.1, 2}}; +CmdParams p4 = {/*steer*/ {0.5, 0.1, 0.5}, /*velocity*/ {10, 0.2, 0}, /*acc*/ {5, 0.1, 2}}; INSTANTIATE_TEST_SUITE_P(TestParam4, TestFixture, ::testing::Values(p4)); + +// Large steer, large velocity -> this test fails. +// Lateral acceleration and lateral jerk affect both steer and velocity, and if both steer and +// velocity changes significantly, the current logic cannot adequately handle the situation. +// CmdParams p5 = {/*steer*/ {10.0, 1.0, 0.5}, /*velocity*/ {10, 0.2, 0}, /*acc*/ {5, 0.1, 2}}; +// INSTANTIATE_TEST_SUITE_P(TestParam5, TestFixture, ::testing::Values(p5)); diff --git a/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp b/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp index 7ce8580120652..1791493aeb17b 100644 --- a/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp +++ b/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp @@ -56,12 +56,20 @@ AckermannControlCommand genCmd(double s, double sr, double v, double a) return cmd; } +// calc from ego velocity +double calcLatAcc(const AckermannControlCommand & cmd, const double wheelbase, const double ego_v) +{ + return ego_v * ego_v * std::tan(cmd.lateral.steering_tire_angle) / wheelbase; +} + +// calc from command velocity double calcLatAcc(const AckermannControlCommand & cmd, const double wheelbase) { double v = cmd.longitudinal.speed; return v * v * std::tan(cmd.lateral.steering_tire_angle) / wheelbase; } +// calc from command velocity double calcLatJerk( const AckermannControlCommand & cmd, const AckermannControlCommand & prev_cmd, const double wheelbase, const double dt) @@ -75,14 +83,28 @@ double calcLatJerk( return (curr - prev) / dt; } +// calc from ego velocity +double calcLatJerk( + const AckermannControlCommand & cmd, const AckermannControlCommand & prev_cmd, + const double wheelbase, const double dt, const double ego_v) +{ + const auto prev = ego_v * ego_v * std::tan(prev_cmd.lateral.steering_tire_angle) / wheelbase; + + const auto curr = ego_v * ego_v * std::tan(cmd.lateral.steering_tire_angle) / wheelbase; + + return (curr - prev) / dt; +} + void test_1d_limit( - double V_LIM, double A_LIM, double J_LIM, double LAT_A_LIM, double LAT_J_LIM, double STEER_DIFF, - const AckermannControlCommand & prev_cmd, const AckermannControlCommand & raw_cmd) + double ego_v, double V_LIM, double A_LIM, double J_LIM, double LAT_A_LIM, double LAT_J_LIM, + double STEER_DIFF, const AckermannControlCommand & prev_cmd, + const AckermannControlCommand & raw_cmd) { const double WHEELBASE = 3.0; const double DT = 0.1; // [s] vehicle_cmd_gate::VehicleCmdFilter filter; + filter.setCurrentSpeed(ego_v); setFilterParams( filter, V_LIM, {0.0}, {A_LIM}, {J_LIM}, {LAT_A_LIM}, {LAT_J_LIM}, {STEER_DIFF}, WHEELBASE); filter.setPrevCmd(prev_cmd); @@ -153,8 +175,8 @@ void test_1d_limit( { auto filtered_cmd = raw_cmd; filter.limitLateralWithLatAcc(DT, filtered_cmd); - const double filtered_latacc = calcLatAcc(filtered_cmd, WHEELBASE); - const double raw_latacc = calcLatAcc(raw_cmd, WHEELBASE); + const double filtered_latacc = calcLatAcc(filtered_cmd, WHEELBASE, ego_v); + const double raw_latacc = calcLatAcc(raw_cmd, WHEELBASE, ego_v); // check if the filtered value does not exceed the limit. ASSERT_LT_NEAR(std::abs(filtered_latacc), LAT_A_LIM); @@ -169,9 +191,9 @@ void test_1d_limit( { auto filtered_cmd = raw_cmd; filter.limitLateralWithLatJerk(DT, filtered_cmd); - const double prev_lat_acc = calcLatAcc(prev_cmd, WHEELBASE); - const double filtered_lat_acc = calcLatAcc(filtered_cmd, WHEELBASE); - const double raw_lat_acc = calcLatAcc(raw_cmd, WHEELBASE); + const double prev_lat_acc = calcLatAcc(prev_cmd, WHEELBASE, ego_v); + const double filtered_lat_acc = calcLatAcc(filtered_cmd, WHEELBASE, ego_v); + const double raw_lat_acc = calcLatAcc(raw_cmd, WHEELBASE, ego_v); const double filtered_lateral_jerk = (filtered_lat_acc - prev_lat_acc) / DT; // check if the filtered value does not exceed the limit. @@ -211,6 +233,7 @@ TEST(VehicleCmdFilter, VehicleCmdFilter) const std::vector lat_a_arr = {0.01, 1.0, 100.0}; const std::vector lat_j_arr = {0.01, 1.0, 100.0}; const std::vector steer_diff_arr = {0.01, 1.0, 100.0}; + const std::vector ego_v_arr = {0.0, 0.1, 1.0, 3.0, 15.0}; const std::vector prev_cmd_arr = { genCmd(0.0, 0.0, 0.0, 0.0), genCmd(1.0, 1.0, 1.0, 1.0)}; @@ -226,7 +249,9 @@ TEST(VehicleCmdFilter, VehicleCmdFilter) for (const auto & prev_cmd : prev_cmd_arr) { for (const auto & raw_cmd : raw_cmd_arr) { for (const auto & steer_diff : steer_diff_arr) { - test_1d_limit(v, a, j, la, lj, steer_diff, prev_cmd, raw_cmd); + for (const auto & ego_v : ego_v_arr) { + test_1d_limit(ego_v, v, a, j, la, lj, steer_diff, prev_cmd, raw_cmd); + } } } } @@ -371,66 +396,72 @@ TEST(VehicleCmdFilter, VehicleCmdFilterInterpolate) // lateral acc lim // p.reference_speed_points = std::vector{2.0, 4.0, 10.0}; // p.lat_acc_lim = std::vector{0.1, 0.2, 0.3}; - const auto _calcLatAcc = [&](const auto & cmd) { return calcLatAcc(cmd, WHEELBASE); }; + const auto _calcLatAcc = [&](const auto & cmd, const double ego_v) { + return calcLatAcc(cmd, WHEELBASE, ego_v); + }; { + // since the lateral acceleration is 0 when the velocity is 0, the target value is 0 only in + // this case set_speed_and_reset_prev(0.0); - EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd)), 0.1, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd), 0.0), 0.0, ep); set_speed_and_reset_prev(2.0); - EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd)), 0.1, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd), 2.0), 0.1, ep); set_speed_and_reset_prev(3.0); - EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd)), 0.15, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd), 3.0), 0.15, ep); set_speed_and_reset_prev(5.0); - EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd)), 0.2 + 0.1 / 6.0, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd), 5.0), 0.2 + 0.1 / 6.0, ep); set_speed_and_reset_prev(8.0); - EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd)), 0.2 + 0.4 / 6.0, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd), 8.0), 0.2 + 0.4 / 6.0, ep); set_speed_and_reset_prev(10.0); - EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd)), 0.3, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd), 10.0), 0.3, ep); set_speed_and_reset_prev(15.0); - EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd)), 0.3, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd), 15.0), 0.3, ep); } // lateral jerk lim // p.reference_speed_points = std::vector{2.0, 4.0, 10.0}; // p.lat_jerk_lim = std::vector{0.9, 0.7, 0.1}; - const auto _calcLatJerk = [&](const auto & cmd) { - return calcLatJerk(cmd, AckermannControlCommand{}, WHEELBASE, DT); + const auto _calcLatJerk = [&](const auto & cmd, const double ego_v) { + return calcLatJerk(cmd, AckermannControlCommand{}, WHEELBASE, DT, ego_v); }; { + // since the lateral acceleration and jerk is 0 when the velocity is 0, the target value is 0 + // only in this case set_speed_and_reset_prev(0.0); - EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd)), 0.9, ep); - EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd)), DT * 0.9, ep); + EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd), 0.0), 0.0, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd), 0.0), DT * 0.0, ep); set_speed_and_reset_prev(2.0); - EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd)), 0.9, ep); - EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd)), DT * 0.9, ep); + EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd), 2.0), 0.9, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd), 2.0), DT * 0.9, ep); set_speed_and_reset_prev(3.0); - EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd)), 0.8, ep); - EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd)), DT * 0.8, ep); + EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd), 3.0), 0.8, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd), 3.0), DT * 0.8, ep); set_speed_and_reset_prev(5.0); const auto expect_v5 = 0.7 - 0.6 * (1.0 / 6.0); - EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd)), expect_v5, ep); - EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd)), DT * expect_v5, ep); + EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd), 5.0), expect_v5, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd), 5.0), DT * expect_v5, ep); set_speed_and_reset_prev(8.0); const auto expect_v8 = 0.7 - 0.6 * (4.0 / 6.0); - EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd)), expect_v8, ep); - EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd)), DT * expect_v8, ep); + EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd), 8.0), expect_v8, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd), 8.0), DT * expect_v8, ep); set_speed_and_reset_prev(10.0); - EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd)), 0.1, ep); - EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd)), DT * 0.1, ep); + EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd), 10.0), 0.1, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd), 10.0), DT * 0.1, ep); set_speed_and_reset_prev(15.0); - EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd)), 0.1, ep); - EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd)), DT * 0.1, ep); + EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd), 15.0), 0.1, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd), 15.0), DT * 0.1, ep); } // steering diff lim diff --git a/evaluator/kinematic_evaluator/src/metrics/kinematic_metrics.cpp b/evaluator/kinematic_evaluator/src/metrics/kinematic_metrics.cpp index 2a22692dc5ac9..2bb0dad86bf71 100644 --- a/evaluator/kinematic_evaluator/src/metrics/kinematic_metrics.cpp +++ b/evaluator/kinematic_evaluator/src/metrics/kinematic_metrics.cpp @@ -14,8 +14,6 @@ #include "kinematic_evaluator/metrics/kinematic_metrics.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" - namespace kinematic_diagnostics { namespace metrics diff --git a/evaluator/localization_evaluator/src/metrics/localization_metrics.cpp b/evaluator/localization_evaluator/src/metrics/localization_metrics.cpp index 240a7dd91e9b2..fab3377913bd4 100644 --- a/evaluator/localization_evaluator/src/metrics/localization_metrics.cpp +++ b/evaluator/localization_evaluator/src/metrics/localization_metrics.cpp @@ -14,7 +14,7 @@ #include "localization_evaluator/metrics/localization_metrics.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include namespace localization_diagnostics { diff --git a/evaluator/planning_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/planning_evaluator/src/metrics/deviation_metrics.cpp index 5146fcd3ec0f2..e1ad9c1a2eeec 100644 --- a/evaluator/planning_evaluator/src/metrics/deviation_metrics.cpp +++ b/evaluator/planning_evaluator/src/metrics/deviation_metrics.cpp @@ -15,7 +15,8 @@ #include "planning_evaluator/metrics/deviation_metrics.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/geometry/pose_deviation.hpp" namespace planning_diagnostics { diff --git a/evaluator/planning_evaluator/src/metrics/metrics_utils.cpp b/evaluator/planning_evaluator/src/metrics/metrics_utils.cpp index e2fd04a1d7a6e..6134a100f31a4 100644 --- a/evaluator/planning_evaluator/src/metrics/metrics_utils.cpp +++ b/evaluator/planning_evaluator/src/metrics/metrics_utils.cpp @@ -13,8 +13,7 @@ // limitations under the License. #include "planning_evaluator/metrics/trajectory_metrics.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" - +#include "tier4_autoware_utils/geometry/geometry.hpp" namespace planning_diagnostics { namespace metrics diff --git a/evaluator/planning_evaluator/src/metrics/obstacle_metrics.cpp b/evaluator/planning_evaluator/src/metrics/obstacle_metrics.cpp index 4d97757c3c33e..82d6dcc51097e 100644 --- a/evaluator/planning_evaluator/src/metrics/obstacle_metrics.cpp +++ b/evaluator/planning_evaluator/src/metrics/obstacle_metrics.cpp @@ -14,7 +14,7 @@ #include "planning_evaluator/metrics/obstacle_metrics.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include diff --git a/evaluator/planning_evaluator/src/metrics/stability_metrics.cpp b/evaluator/planning_evaluator/src/metrics/stability_metrics.cpp index abd3ca5584951..7a3418da881c6 100644 --- a/evaluator/planning_evaluator/src/metrics/stability_metrics.cpp +++ b/evaluator/planning_evaluator/src/metrics/stability_metrics.cpp @@ -15,7 +15,7 @@ #include "planning_evaluator/metrics/stability_metrics.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include diff --git a/evaluator/planning_evaluator/src/metrics/trajectory_metrics.cpp b/evaluator/planning_evaluator/src/metrics/trajectory_metrics.cpp index cba03e8b2d90e..d0d8d61c4c231 100644 --- a/evaluator/planning_evaluator/src/metrics/trajectory_metrics.cpp +++ b/evaluator/planning_evaluator/src/metrics/trajectory_metrics.cpp @@ -15,8 +15,7 @@ #include "planning_evaluator/metrics/trajectory_metrics.hpp" #include "planning_evaluator/metrics/metrics_utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" - +#include "tier4_autoware_utils/geometry/geometry.hpp" namespace planning_diagnostics { namespace metrics diff --git a/evaluator/planning_evaluator/src/metrics_calculator.cpp b/evaluator/planning_evaluator/src/metrics_calculator.cpp index da04d6fcd2bfa..38113513dbad2 100644 --- a/evaluator/planning_evaluator/src/metrics_calculator.cpp +++ b/evaluator/planning_evaluator/src/metrics_calculator.cpp @@ -14,13 +14,12 @@ #include "planning_evaluator/metrics_calculator.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "planning_evaluator/metrics/deviation_metrics.hpp" #include "planning_evaluator/metrics/obstacle_metrics.hpp" #include "planning_evaluator/metrics/stability_metrics.hpp" #include "planning_evaluator/metrics/trajectory_metrics.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" - +#include "tier4_autoware_utils/geometry/geometry.hpp" namespace planning_diagnostics { std::optional> MetricsCalculator::calculate( diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml index 4de9aec81bb38..3b32d080ad51c 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml @@ -4,8 +4,8 @@ - - + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index c74c0fdcc63b7..eab2a56607dee 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -79,12 +79,54 @@ - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -96,6 +138,8 @@ + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml index 99f09cfe6abbd..463340efdecfe 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml @@ -143,12 +143,54 @@ - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml index a9301f31d87aa..58e9e231e4aa0 100644 --- a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml +++ b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml @@ -15,6 +15,7 @@ + @@ -143,6 +144,7 @@ + diff --git a/launch/tier4_perception_launch/package.xml b/launch/tier4_perception_launch/package.xml index c1723c1fa07e8..336588891d9b2 100644 --- a/launch/tier4_perception_launch/package.xml +++ b/launch/tier4_perception_launch/package.xml @@ -12,6 +12,7 @@ ament_cmake_auto autoware_cmake + cluster_merger compare_map_segmentation crosswalk_traffic_light_estimator detected_object_feature_remover diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py index 59e8038e49188..62d4c5b7188ee 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py @@ -177,10 +177,6 @@ def launch_setup(context, *args, **kwargs): "~/input/traffic_signals", "/perception/traffic_light_recognition/traffic_signals", ), - ( - "~/input/external_traffic_signals", - "/external/traffic_light_recognition/traffic_signals", - ), ( "~/input/external_velocity_limit_mps", "/planning/scenario_planning/max_velocity_default", diff --git a/localization/ar_tag_based_localizer/README.md b/localization/ar_tag_based_localizer/README.md index e471378773cbb..c7b970ebbcdcf 100644 --- a/localization/ar_tag_based_localizer/README.md +++ b/localization/ar_tag_based_localizer/README.md @@ -55,7 +55,26 @@ ros2 launch autoware_launch ... \ ... ``` -[Sample rosbag and map](https://drive.google.com/file/d/1wiCQjyjRnYbb0dg8G6mRecdSGh8tv3zR/view) +### Rosbag + +#### [Sample rosbag and map (AWSIM data)](https://drive.google.com/file/d/1uMVwQQFcfs8JOqfoA1FqfH_fLPwQ71jK/view) + +This data is simulated data created by [AWSIM](https://tier4.github.io/AWSIM/). +Essentially, AR tag-based self-localization is not intended for such public road driving, but for driving in a smaller area, so the max driving speed is set at 15 km/h. + +It is a known problem that the timing of when each AR tag begins to be detected can cause significant changes in estimation. + +![sample_result_in_awsim](./doc_image/sample_result_in_awsim.png) + +#### [Sample rosbag and map (Real world data)](https://drive.google.com/file/d/1wiCQjyjRnYbb0dg8G6mRecdSGh8tv3zR/view) + +Please remap the topic names and play it. + +```bash +ros2 bag play /path/to/ar_tag_based_localizer_sample_bag/ -r 0.5 -s sqlite3 \ + --remap /sensing/camera/front/image:=/sensing/camera/traffic_light/image_raw \ + /sensing/camera/front/image/info:=/sensing/camera/traffic_light/camera_info +``` This dataset contains issues such as missing IMU data, and overall the accuracy is low. Even when running AR tag-based self-localization, significant difference from the true trajectory can be observed. diff --git a/localization/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml b/localization/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml index 385e86498c58c..d0eef44229e0d 100644 --- a/localization/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml +++ b/localization/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml @@ -4,21 +4,19 @@ marker_size: 0.6 # target_tag_ids - target_tag_ids: ['0','4','5'] + target_tag_ids: ['0','1','2','3','4','5','6'] - # covariance - covariance: [0.1, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.01] + # base_covariance + # This value is dynamically scaled according to the distance at which AR tags are detected. + base_covariance: [0.2, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.02] # Detect AR-Tags within this range and publish the pose of ego vehicle - distance_threshold: 6.0 # [m] - - # Camera frame id - camera_frame: "camera" + distance_threshold: 13.0 # [m] # Detector parameters # See https://github.com/pal-robotics/aruco_ros/blob/7787a6794d30c248bc546d1582e65dd47bc40c12/aruco/include/aruco/markerdetector.h#L106-L126 diff --git a/localization/ar_tag_based_localizer/doc_image/sample_result_in_awsim.png b/localization/ar_tag_based_localizer/doc_image/sample_result_in_awsim.png new file mode 100644 index 0000000000000..875005214bf5e Binary files /dev/null and b/localization/ar_tag_based_localizer/doc_image/sample_result_in_awsim.png differ diff --git a/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp b/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp index 39373d92add25..b6a513b981e14 100644 --- a/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp +++ b/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp @@ -69,14 +69,14 @@ class ArTagBasedLocalizer : public rclcpp::Node private: void image_callback(const sensor_msgs::msg::Image::ConstSharedPtr & msg); void cam_info_callback(const sensor_msgs::msg::CameraInfo & msg); - void publish_pose_as_base_link(const geometry_msgs::msg::PoseStamped & msg); + void publish_pose_as_base_link( + const geometry_msgs::msg::PoseStamped & msg, const std::string & camera_frame_id); // Parameters float marker_size_{}; std::vector target_tag_ids_; - std::vector covariance_; + std::vector base_covariance_; double distance_threshold_squared_{}; - std::string camera_frame_; // tf std::unique_ptr tf_buffer_; @@ -87,7 +87,7 @@ class ArTagBasedLocalizer : public rclcpp::Node std::unique_ptr it_; // Subscribers - image_transport::Subscriber image_sub_; + rclcpp::Subscription::SharedPtr image_sub_; rclcpp::Subscription::SharedPtr cam_info_sub_; // Publishers diff --git a/localization/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp b/localization/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp index ca3bfe2e4a6b0..c97e1171949e7 100644 --- a/localization/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp +++ b/localization/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp @@ -69,9 +69,8 @@ bool ArTagBasedLocalizer::setup() */ marker_size_ = static_cast(this->declare_parameter("marker_size")); target_tag_ids_ = this->declare_parameter>("target_tag_ids"); - covariance_ = this->declare_parameter>("covariance"); + base_covariance_ = this->declare_parameter>("base_covariance"); distance_threshold_squared_ = std::pow(this->declare_parameter("distance_threshold"), 2); - camera_frame_ = this->declare_parameter("camera_frame"); std::string detection_mode = this->declare_parameter("detection_mode"); float min_marker_size = static_cast(this->declare_parameter("min_marker_size")); if (detection_mode == "DM_NORMAL") { @@ -109,20 +108,22 @@ bool ArTagBasedLocalizer::setup() /* Subscribers */ - image_sub_ = it_->subscribe("~/input/image", 1, &ArTagBasedLocalizer::image_callback, this); + rclcpp::QoS qos_sub(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); + qos_sub.best_effort(); + image_sub_ = this->create_subscription( + "~/input/image", qos_sub, + std::bind(&ArTagBasedLocalizer::image_callback, this, std::placeholders::_1)); cam_info_sub_ = this->create_subscription( - "~/input/camera_info", 1, + "~/input/camera_info", qos_sub, std::bind(&ArTagBasedLocalizer::cam_info_callback, this, std::placeholders::_1)); /* Publishers */ - auto qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); - qos.reliable(); - qos.transient_local(); + rclcpp::QoS qos_pub(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); image_pub_ = it_->advertise("~/debug/result", 1); pose_pub_ = this->create_publisher( - "~/output/pose_with_covariance", qos); + "~/output/pose_with_covariance", qos_pub); RCLCPP_INFO(this->get_logger(), "Setup of ar_tag_based_localizer node is successful!"); return true; @@ -164,7 +165,7 @@ void ArTagBasedLocalizer::image_callback(const sensor_msgs::msg::Image::ConstSha geometry_msgs::msg::TransformStamped tf_cam_to_marker_stamped; tf2::toMsg(tf_cam_to_marker, tf_cam_to_marker_stamped.transform); tf_cam_to_marker_stamped.header.stamp = curr_stamp; - tf_cam_to_marker_stamped.header.frame_id = camera_frame_; + tf_cam_to_marker_stamped.header.frame_id = msg->header.frame_id; tf_cam_to_marker_stamped.child_frame_id = "detected_marker_" + std::to_string(marker.id); tf_broadcaster_->sendTransform(tf_cam_to_marker_stamped); @@ -172,7 +173,7 @@ void ArTagBasedLocalizer::image_callback(const sensor_msgs::msg::Image::ConstSha tf2::toMsg(tf_cam_to_marker, pose_cam_to_marker.pose); pose_cam_to_marker.header.stamp = curr_stamp; pose_cam_to_marker.header.frame_id = std::to_string(marker.id); - publish_pose_as_base_link(pose_cam_to_marker); + publish_pose_as_base_link(pose_cam_to_marker, msg->header.frame_id); // drawing the detected markers marker.draw(in_image, cv::Scalar(0, 0, 255), 2); @@ -206,7 +207,8 @@ void ArTagBasedLocalizer::cam_info_callback(const sensor_msgs::msg::CameraInfo & cam_info_received_ = true; } -void ArTagBasedLocalizer::publish_pose_as_base_link(const geometry_msgs::msg::PoseStamped & msg) +void ArTagBasedLocalizer::publish_pose_as_base_link( + const geometry_msgs::msg::PoseStamped & msg, const std::string & camera_frame_id) { // Check if frame_id is in target_tag_ids if ( @@ -241,7 +243,7 @@ void ArTagBasedLocalizer::publish_pose_as_base_link(const geometry_msgs::msg::Po geometry_msgs::msg::TransformStamped camera_to_base_link_tf; try { camera_to_base_link_tf = - tf_buffer_->lookupTransform(camera_frame_, "base_link", tf2::TimePointZero); + tf_buffer_->lookupTransform(camera_frame_id, "base_link", tf2::TimePointZero); } catch (tf2::TransformException & ex) { RCLCPP_INFO(this->get_logger(), "Could not transform base_link to camera: %s", ex.what()); return; @@ -265,8 +267,15 @@ void ArTagBasedLocalizer::publish_pose_as_base_link(const geometry_msgs::msg::Po pose_with_covariance_stamped.header.stamp = msg.header.stamp; pose_with_covariance_stamped.header.frame_id = "map"; pose_with_covariance_stamped.pose.pose = tf2::toMsg(map_to_base_link); - std::copy( - covariance_.begin(), covariance_.end(), pose_with_covariance_stamped.pose.covariance.begin()); + + // ~5[m]: base_covariance + // 5~[m]: scaling base_covariance by std::pow(distance/5, 3) + const double distance = std::sqrt(distance_squared); + const double scale = distance / 5; + const double coeff = std::max(1.0, std::pow(scale, 3)); + for (int i = 0; i < 36; i++) { + pose_with_covariance_stamped.pose.covariance[i] = coeff * base_covariance_[i]; + } pose_pub_->publish(pose_with_covariance_stamped); } diff --git a/localization/ar_tag_based_localizer/src/tag_tf_caster_core.cpp b/localization/ar_tag_based_localizer/src/tag_tf_caster_core.cpp index d0279caf88dda..427bf513058d4 100644 --- a/localization/ar_tag_based_localizer/src/tag_tf_caster_core.cpp +++ b/localization/ar_tag_based_localizer/src/tag_tf_caster_core.cpp @@ -19,6 +19,7 @@ #include #include +#include #ifdef ROS_DISTRO_GALACTIC #include #else diff --git a/localization/ekf_localizer/CMakeLists.txt b/localization/ekf_localizer/CMakeLists.txt index 6e7e194f7cf72..d5cd85aa2ed85 100644 --- a/localization/ekf_localizer/CMakeLists.txt +++ b/localization/ekf_localizer/CMakeLists.txt @@ -51,14 +51,7 @@ if(BUILD_TESTING) ) find_package(ament_cmake_gtest REQUIRED) - set(TEST_FILES - test/test_aged_object_queue.cpp - test/test_mahalanobis.cpp - test/test_measurement.cpp - test/test_numeric.cpp - test/test_state_transition.cpp - test/test_string.cpp - test/test_warning_message.cpp) + file(GLOB_RECURSE TEST_FILES test/*.cpp) foreach(filepath ${TEST_FILES}) add_testcase(${filepath}) diff --git a/localization/ekf_localizer/test/src/test_ekf_localizer.cpp b/localization/ekf_localizer/test/src/test_ekf_localizer.cpp deleted file mode 100644 index f249dffef6034..0000000000000 --- a/localization/ekf_localizer/test/src/test_ekf_localizer.cpp +++ /dev/null @@ -1,305 +0,0 @@ -// Copyright 2018-2019 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 "ekf_localizer/ekf_localizer.hpp" - -#include -#include -#include - -#include - -#include -#include - -#include -#include -#include -#include - -using std::placeholders::_1; - -class EKFLocalizerTestSuite : public ::testing::Test -{ -protected: - void SetUp() { rclcpp::init(0, nullptr); } - void TearDown() { (void)rclcpp::shutdown(); } -}; // sanity_check - -class TestEKFLocalizerNode : public EKFLocalizer -{ -public: - TestEKFLocalizerNode(const std::string & node_name, const rclcpp::NodeOptions & node_options) - : EKFLocalizer(node_name, node_options) - { - sub_twist = this->create_subscription( - "/ekf_twist", 1, std::bind(&TestEKFLocalizerNode::testCallbackTwist, this, _1)); - sub_pose = this->create_subscription( - "/ekf_pose", 1, std::bind(&TestEKFLocalizerNode::testCallbackPose, this, _1)); - - using std::chrono_literals::operator""ms; - test_timer_ = rclcpp::create_timer( - this, get_clock(), 100ms, std::bind(&TestEKFLocalizerNode::testTimerCallback, this)); - } - ~TestEKFLocalizerNode() {} - - std::string frame_id_a_ = "world"; - std::string frame_id_b_ = "base_link"; - - rclcpp::Subscription::SharedPtr sub_twist; - rclcpp::Subscription::SharedPtr sub_pose; - - rclcpp::TimerBase::SharedPtr test_timer_; - - geometry_msgs::msg::PoseStamped::SharedPtr test_current_pose_ptr_; - geometry_msgs::msg::TwistStamped::SharedPtr test_current_twist_ptr_; - - void testTimerCallback() - { - /* !!! this should be defined before sendTransform() !!! */ - static std::shared_ptr br = - std::make_shared(shared_from_this()); - geometry_msgs::msg::TransformStamped sent; - - rclcpp::Time current_time = this->now(); - - sent.header.stamp = current_time; - sent.header.frame_id = frame_id_a_; - sent.child_frame_id = frame_id_b_; - sent.transform.translation.x = -7.11; - sent.transform.translation.y = 0.0; - sent.transform.translation.z = 0.0; - tf2::Quaternion q; - q.setRPY(0, 0, 0.5); - sent.transform.rotation.x = q.x(); - sent.transform.rotation.y = q.y(); - sent.transform.rotation.z = q.z(); - sent.transform.rotation.w = q.w(); - - br->sendTransform(sent); - } - - void testCallbackPose(geometry_msgs::msg::PoseStamped::SharedPtr pose) - { - test_current_pose_ptr_ = std::make_shared(*pose); - } - - void testCallbackTwist(geometry_msgs::msg::TwistStamped::SharedPtr twist) - { - test_current_twist_ptr_ = std::make_shared(*twist); - } - - void resetCurrentPoseAndTwist() - { - test_current_pose_ptr_ = nullptr; - test_current_twist_ptr_ = nullptr; - } -}; - -TEST_F(EKFLocalizerTestSuite, measurementUpdatePose) -{ - rclcpp::NodeOptions node_options; - auto ekf = std::make_shared("EKFLocalizerTestSuite", node_options); - - auto pub_pose = ekf->create_publisher("/in_pose", 1); - - geometry_msgs::msg::PoseStamped in_pose; - in_pose.header.frame_id = "world"; - in_pose.pose.position.x = 1.0; - in_pose.pose.position.y = 2.0; - in_pose.pose.position.z = 3.0; - in_pose.pose.orientation.x = 0.0; - in_pose.pose.orientation.y = 0.0; - in_pose.pose.orientation.z = 0.0; - in_pose.pose.orientation.w = 1.0; - - /* test for valid value */ - const double pos_x = 12.3; - in_pose.pose.position.x = pos_x; // for valid value - - for (int i = 0; i < 20; ++i) { - in_pose.header.stamp = ekf->now(); - pub_pose->publish(in_pose); - rclcpp::spin_some(ekf); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - } - - ASSERT_NE(ekf->test_current_pose_ptr_, nullptr); - ASSERT_NE(ekf->test_current_twist_ptr_, nullptr); - - double ekf_x = ekf->test_current_pose_ptr_->pose.position.x; - bool is_succeeded = !(std::isnan(ekf_x) || std::isinf(ekf_x)); - ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; - - ASSERT_TRUE(std::fabs(ekf_x - pos_x) < 0.1) - << "ekf pos x: " << ekf_x << " should be close to " << pos_x; - - /* test for invalid value */ - in_pose.pose.position.x = NAN; // check for invalid values - for (int i = 0; i < 10; ++i) { - in_pose.header.stamp = ekf->now(); - pub_pose->publish(in_pose); - rclcpp::spin_some(ekf); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - } - is_succeeded = !(std::isnan(ekf_x) || std::isinf(ekf_x)); - ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; - - ekf->resetCurrentPoseAndTwist(); -} - -TEST_F(EKFLocalizerTestSuite, measurementUpdateTwist) -{ - rclcpp::NodeOptions node_options; - auto ekf = std::make_shared("EKFLocalizerTestSuite", node_options); - - auto pub_twist = ekf->create_publisher("/in_twist", 1); - geometry_msgs::msg::TwistStamped in_twist; - in_twist.header.frame_id = "base_link"; - - /* test for valid value */ - const double vx = 12.3; - in_twist.twist.linear.x = vx; // for valid value - for (int i = 0; i < 20; ++i) { - in_twist.header.stamp = ekf->now(); - pub_twist->publish(in_twist); - rclcpp::spin_some(ekf); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - } - - ASSERT_FALSE(ekf->test_current_pose_ptr_ == nullptr); - ASSERT_FALSE(ekf->test_current_twist_ptr_ == nullptr); - - double ekf_vx = ekf->test_current_twist_ptr_->twist.linear.x; - bool is_succeeded = !(std::isnan(ekf_vx) || std::isinf(ekf_vx)); - ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; - ASSERT_TRUE(std::fabs(ekf_vx - vx) < 0.1) - << "ekf vel x: " << ekf_vx << ", should be close to " << vx; - - /* test for invalid value */ - in_twist.twist.linear.x = NAN; // check for invalid values - for (int i = 0; i < 10; ++i) { - in_twist.header.stamp = ekf->now(); - pub_twist->publish(in_twist); - rclcpp::spin_some(ekf); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - } - - ekf_vx = ekf->test_current_twist_ptr_->twist.linear.x; - is_succeeded = !(std::isnan(ekf_vx) || std::isinf(ekf_vx)); - ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; - - ekf->resetCurrentPoseAndTwist(); -} - -TEST_F(EKFLocalizerTestSuite, measurementUpdatePoseWithCovariance) -{ - rclcpp::NodeOptions node_options; - node_options.append_parameter_override("use_pose_with_covariance", true); - rclcpp::sleep_for(std::chrono::milliseconds(200)); - auto ekf = std::make_shared("EKFLocalizerTestSuite", node_options); - - auto pub_pose = ekf->create_publisher( - "/in_pose_with_covariance", 1); - geometry_msgs::msg::PoseWithCovarianceStamped in_pose; - in_pose.header.frame_id = "world"; - in_pose.pose.pose.position.x = 1.0; - in_pose.pose.pose.position.y = 2.0; - in_pose.pose.pose.position.z = 3.0; - in_pose.pose.pose.orientation.x = 0.0; - in_pose.pose.pose.orientation.y = 0.0; - in_pose.pose.pose.orientation.z = 0.0; - in_pose.pose.pose.orientation.w = 1.0; - for (int i = 0; i < 36; ++i) { - in_pose.pose.covariance[i] = 0.1; - } - - /* test for valid value */ - const double pos_x = 99.3; - in_pose.pose.pose.position.x = pos_x; // for valid value - - for (int i = 0; i < 20; ++i) { - in_pose.header.stamp = ekf->now(); - pub_pose->publish(in_pose); - rclcpp::spin_some(ekf); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - } - - ASSERT_FALSE(ekf->test_current_pose_ptr_ == nullptr); - ASSERT_FALSE(ekf->test_current_twist_ptr_ == nullptr); - - double ekf_x = ekf->test_current_pose_ptr_->pose.position.x; - bool is_succeeded = !(std::isnan(ekf_x) || std::isinf(ekf_x)); - ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; - ASSERT_TRUE(std::fabs(ekf_x - pos_x) < 0.1) - << "ekf pos x: " << ekf_x << " should be close to " << pos_x; - - /* test for invalid value */ - in_pose.pose.pose.position.x = NAN; // check for invalid values - for (int i = 0; i < 10; ++i) { - in_pose.header.stamp = ekf->now(); - pub_pose->publish(in_pose); - rclcpp::spin_some(ekf); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - } - is_succeeded = !(std::isnan(ekf_x) || std::isinf(ekf_x)); - ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; - - ekf->resetCurrentPoseAndTwist(); -} - -TEST_F(EKFLocalizerTestSuite, measurementUpdateTwistWithCovariance) -{ - rclcpp::NodeOptions node_options; - auto ekf = std::make_shared("EKFLocalizerTestSuite", node_options); - - auto pub_twist = ekf->create_publisher( - "/in_twist_with_covariance", 1); - geometry_msgs::msg::TwistWithCovarianceStamped in_twist; - in_twist.header.frame_id = "base_link"; - - /* test for valid value */ - const double vx = 12.3; - in_twist.twist.twist.linear.x = vx; // for valid value - for (int i = 0; i < 36; ++i) { - in_twist.twist.covariance[i] = 0.1; - } - for (int i = 0; i < 10; ++i) { - in_twist.header.stamp = ekf->now(); - pub_twist->publish(in_twist); - rclcpp::spin_some(ekf); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - } - - ASSERT_FALSE(ekf->test_current_pose_ptr_ == nullptr); - ASSERT_FALSE(ekf->test_current_twist_ptr_ == nullptr); - - double ekf_vx = ekf->test_current_twist_ptr_->twist.linear.x; - bool is_succeeded = !(std::isnan(ekf_vx) || std::isinf(ekf_vx)); - ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; - ASSERT_TRUE((ekf_vx - vx) < 0.1) << "vel x should be close to " << vx; - - /* test for invalid value */ - in_twist.twist.twist.linear.x = NAN; // check for invalid values - for (int i = 0; i < 10; ++i) { - in_twist.header.stamp = ekf->now(); - pub_twist->publish(in_twist); - rclcpp::spin_some(ekf); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - } - - ekf_vx = ekf->test_current_twist_ptr_->twist.linear.x; - is_succeeded = !(std::isnan(ekf_vx) || std::isinf(ekf_vx)); - ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; -} diff --git a/localization/ekf_localizer/test/test_covariance.cpp b/localization/ekf_localizer/test/test_covariance.cpp index fdc732ea6d34c..23458fb18a838 100644 --- a/localization/ekf_localizer/test/test_covariance.cpp +++ b/localization/ekf_localizer/test/test_covariance.cpp @@ -31,15 +31,15 @@ TEST(EKFCovarianceToPoseMessageCovariance, SmokeTest) P(2, 2) = 9.; std::array covariance = ekfCovarianceToPoseMessageCovariance(P); - EXPECT_EQ(covariance(0), 1.); - EXPECT_EQ(covariance(1), 2.); - EXPECT_EQ(covariance(5), 3.); - EXPECT_EQ(covariance(6), 4.); - EXPECT_EQ(covariance(7), 5.); - EXPECT_EQ(covariance(11), 6.); - EXPECT_EQ(covariance(30), 7.); - EXPECT_EQ(covariance(31), 8.); - EXPECT_EQ(covariance(35), 9.); + EXPECT_EQ(covariance[0], 1.); + EXPECT_EQ(covariance[1], 2.); + EXPECT_EQ(covariance[5], 3.); + EXPECT_EQ(covariance[6], 4.); + EXPECT_EQ(covariance[7], 5.); + EXPECT_EQ(covariance[11], 6.); + EXPECT_EQ(covariance[30], 7.); + EXPECT_EQ(covariance[31], 8.); + EXPECT_EQ(covariance[35], 9.); } // ensure other elements are zero @@ -62,10 +62,10 @@ TEST(EKFCovarianceToTwistMessageCovariance, SmokeTest) P(5, 5) = 4.; std::array covariance = ekfCovarianceToTwistMessageCovariance(P); - EXPECT_EQ(covariance(0), 1.); - EXPECT_EQ(covariance(5), 2.); - EXPECT_EQ(covariance(30), 3.); - EXPECT_EQ(covariance(35), 4.); + EXPECT_EQ(covariance[0], 1.); + EXPECT_EQ(covariance[5], 2.); + EXPECT_EQ(covariance[30], 3.); + EXPECT_EQ(covariance[35], 4.); } // ensure other elements are zero diff --git a/localization/ekf_localizer/test/test_ekf_localizer.test b/localization/ekf_localizer/test/test_ekf_localizer.test deleted file mode 100644 index 4eecfa6e094b7..0000000000000 --- a/localization/ekf_localizer/test/test_ekf_localizer.test +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp index b0c7a5c4471c6..97d4a02472f2d 100644 --- a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp +++ b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp @@ -15,8 +15,8 @@ #ifndef GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ #define GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ +#include "tier4_autoware_utils/ros/msg_covariance.hpp" #include "tier4_autoware_utils/ros/transform_listener.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index b30cbaca1e87a..fcbe4804ca7d2 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -64,6 +64,7 @@ One optional function is regularization. Please see the regularization chapter i | `max_iterations` | int | The number of iterations required to calculate alignment | | `converged_param_type` | int | The type of indicators for scan matching score (0: TP, 1: NVTL) | | `converged_param_transform_probability` | double | Threshold for deciding whether to trust the estimation result | +| `lidar_topic_timeout_sec` | double | Tolerance of timestamp difference between current time and sensor pointcloud | | `num_threads` | int | Number of threads used for parallel computing | (TP: Transform Probability, NVTL: Nearest Voxel Transform Probability) diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml index a5d8142b6616e..b5affd72b2158 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -37,6 +37,9 @@ # The number of particles to estimate initial pose initial_estimate_particles_num: 100 + # Tolerance of timestamp difference between current time and sensor pointcloud. [sec] + lidar_topic_timeout_sec: 1.0 + # Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec] initial_pose_timeout_sec: 1.0 diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index 8657e354c728a..ed3b99019f7d9 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -179,6 +179,7 @@ class NDTScanMatcher : public rclcpp::Node double converged_param_nearest_voxel_transformation_likelihood_; int initial_estimate_particles_num_; + double lidar_topic_timeout_sec_; double initial_pose_timeout_sec_; double initial_pose_distance_tolerance_m_; float inversion_vector_threshold_; diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 667fd59196df3..103df19c2b5db 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -91,6 +91,7 @@ NDTScanMatcher::NDTScanMatcher() converged_param_transform_probability_(4.5), converged_param_nearest_voxel_transformation_likelihood_(2.3), initial_estimate_particles_num_(100), + lidar_topic_timeout_sec_(1.0), initial_pose_timeout_sec_(1.0), initial_pose_distance_tolerance_m_(10.0), inversion_vector_threshold_(-0.9), @@ -141,6 +142,9 @@ NDTScanMatcher::NDTScanMatcher() "converged_param_nearest_voxel_transformation_likelihood", converged_param_nearest_voxel_transformation_likelihood_); + lidar_topic_timeout_sec_ = + this->declare_parameter("lidar_topic_timeout_sec", lidar_topic_timeout_sec_); + initial_pose_timeout_sec_ = this->declare_parameter("initial_pose_timeout_sec", initial_pose_timeout_sec_); @@ -268,9 +272,16 @@ void NDTScanMatcher::timer_diagnostic() diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; diag_status_msg.message += "Initializing State. "; } + if ( + state_ptr_->count("lidar_topic_delay_time_sec") && + std::stod((*state_ptr_)["lidar_topic_delay_time_sec"]) > lidar_topic_timeout_sec_) { + diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + diag_status_msg.message += "lidar_topic_delay_time_sec exceed limit. "; + } if ( state_ptr_->count("skipping_publish_num") && - std::stoi((*state_ptr_)["skipping_publish_num"]) > 1) { + std::stoi((*state_ptr_)["skipping_publish_num"]) > 1 && + std::stoi((*state_ptr_)["skipping_publish_num"]) < 5) { diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; diag_status_msg.message += "skipping_publish_num > 1. "; } @@ -280,6 +291,13 @@ void NDTScanMatcher::timer_diagnostic() diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; diag_status_msg.message += "skipping_publish_num exceed limit. "; } + if ( + state_ptr_->count("nearest_voxel_transformation_likelihood") && + std::stod((*state_ptr_)["nearest_voxel_transformation_likelihood"]) < + converged_param_nearest_voxel_transformation_likelihood_) { + diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + diag_status_msg.message += "NDT score is unreliably low. "; + } // Ignore local optimal solution if ( state_ptr_->count("is_local_optimal_solution_oscillation") && @@ -346,11 +364,29 @@ void NDTScanMatcher::callback_sensor_points( return; } + const rclcpp::Time sensor_ros_time = sensor_points_msg_in_sensor_frame->header.stamp; + const double lidar_topic_delay_time_sec = (this->now() - sensor_ros_time).seconds(); + (*state_ptr_)["lidar_topic_delay_time_sec"] = std::to_string(lidar_topic_delay_time_sec); + + if (lidar_topic_delay_time_sec > lidar_topic_timeout_sec_) { + RCLCPP_WARN( + this->get_logger(), + "The LiDAR topic is experiencing latency. The delay time is %lf[sec] (the tolerance is " + "%lf[sec])", + lidar_topic_delay_time_sec, lidar_topic_timeout_sec_); + + // If the delay time of the LiDAR topic exceeds the delay compensation time of ekf_localizer, + // even if further processing continues, the estimated result will be rejected by ekf_localizer. + // Therefore, it would be acceptable to exit the function here. + // However, for now, we will continue the processing as it is. + + // return; + } + // mutex ndt_ptr_ std::lock_guard lock(ndt_ptr_mtx_); const auto exe_start_time = std::chrono::system_clock::now(); - const rclcpp::Time sensor_ros_time = sensor_points_msg_in_sensor_frame->header.stamp; // preprocess input pointcloud pcl::shared_ptr> sensor_points_in_sensor_frame( diff --git a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp index d2e9f66ad047a..0adc7612e9f61 100644 --- a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp +++ b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp @@ -36,8 +36,8 @@ class Lanelet2MapLoaderNode : public rclcpp::Node explicit Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options); static lanelet::LaneletMapPtr load_map( - const std::string & lanelet2_filename, const std::string & lanelet2_map_projector_type, - const double & map_origin_lat = 0.0, const double & map_origin_lon = 0.0); + const std::string & lanelet2_filename, + const tier4_map_msgs::msg::MapProjectorInfo & projector_info); static HADMapBin create_map_bin_msg( const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now); diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index fbc2572d0bc6d..e663d19d516ac 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -19,6 +19,7 @@ component_interface_specs component_interface_utils fmt + geography_utils geometry_msgs lanelet2_extension libpcl-all-dev diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp index 328808e04d9e0..259c168edcc5c 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp @@ -34,6 +34,7 @@ #include "map_loader/lanelet2_map_loader_node.hpp" #include +#include #include #include #include @@ -69,8 +70,7 @@ void Lanelet2MapLoaderNode::on_map_projector_info( const auto center_line_resolution = get_parameter("center_line_resolution").as_double(); // load map from file - const auto map = load_map( - lanelet2_filename, msg->projector_type, msg->map_origin.latitude, msg->map_origin.longitude); + const auto map = load_map(lanelet2_filename, *msg); if (!map) { return; } @@ -88,26 +88,18 @@ void Lanelet2MapLoaderNode::on_map_projector_info( } lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( - const std::string & lanelet2_filename, const std::string & lanelet2_map_projector_type, - const double & map_origin_lat, const double & map_origin_lon) + const std::string & lanelet2_filename, + const tier4_map_msgs::msg::MapProjectorInfo & projector_info) { lanelet::ErrorMessages errors{}; - if (lanelet2_map_projector_type == tier4_map_msgs::msg::MapProjectorInfo::MGRS) { - lanelet::projection::MGRSProjector projector{}; - const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); - if (errors.empty()) { - return map; - } - } else if ( - lanelet2_map_projector_type == tier4_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN_UTM) { - lanelet::GPSPoint position{map_origin_lat, map_origin_lon}; - lanelet::Origin origin{position}; - lanelet::projection::UtmProjector projector{origin}; - const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); + if (projector_info.projector_type != tier4_map_msgs::msg::MapProjectorInfo::LOCAL) { + std::unique_ptr projector = + geography_utils::get_lanelet2_projector(projector_info); + const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, *projector, &errors); if (errors.empty()) { return map; } - } else if (lanelet2_map_projector_type == tier4_map_msgs::msg::MapProjectorInfo::LOCAL) { + } else { // Use MGRSProjector as parser lanelet::projection::MGRSProjector projector{}; const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); @@ -132,9 +124,6 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( } return map; - } else { - RCLCPP_ERROR(rclcpp::get_logger("map_loader"), "lanelet2_map_projector_type is not supported"); - return nullptr; } for (const auto & error : errors) { diff --git a/map/map_projection_loader/src/map_projection_loader.cpp b/map/map_projection_loader/src/map_projection_loader.cpp index 79083b755fb26..74c340ca53557 100644 --- a/map/map_projection_loader/src/map_projection_loader.cpp +++ b/map/map_projection_loader/src/map_projection_loader.cpp @@ -35,6 +35,7 @@ tier4_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & fi msg.vertical_datum = data["vertical_datum"].as(); msg.map_origin.latitude = data["map_origin"]["latitude"].as(); msg.map_origin.longitude = data["map_origin"]["longitude"].as(); + msg.map_origin.altitude = data["map_origin"]["altitude"].as(); } else if (msg.projector_type == tier4_map_msgs::msg::MapProjectorInfo::LOCAL) { ; // do nothing } else { diff --git a/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp b/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp index 601e8dfa0117f..8900381060472 100644 --- a/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp +++ b/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include diff --git a/mkdocs.yaml b/mkdocs.yaml index a72529fbe959a..2ca8b3c86ad43 100644 --- a/mkdocs.yaml +++ b/mkdocs.yaml @@ -55,7 +55,8 @@ plugins: regex: - ^(?!(.*/)?assets/).*\.(?!(.*\.)?md|(.*\.)?svg|(.*\.)?png|(.*\.)?gif|(.*\.)?jpg).*$ - ^(.*/)?[^.]*$ - - macros + - macros: + module_name: mkdocs_macros - mkdocs-video - same-dir - search diff --git a/mkdocs_macros.py b/mkdocs_macros.py new file mode 100644 index 0000000000000..56292a815d80d --- /dev/null +++ b/mkdocs_macros.py @@ -0,0 +1,70 @@ +import json + +from tabulate import tabulate + +# This file is for defining macros for mkdocs-macros plugin +# Check https://mkdocs-macros-plugin.readthedocs.io/en/latest/macros/ for the details + + +def format_param_type(param_type): + if param_type == "number": + return "float" + else: + return param_type + + +def format_param_range(param): + list_of_range = [] + if "enum" in param.keys(): + list_of_range.append(param["enum"]) + if "minimum" in param.keys(): + list_of_range.append("≥" + str(param["minimum"])) + if "exclusiveMinimum" in param.keys(): + list_of_range.append(">" + str(param["exclusiveMinimum"])) + if "maximum" in param.keys(): + list_of_range.append("≤" + str(param["maximum"])) + if "exclusiveMaximum" in param.keys(): + list_of_range.append("<" + str(param["exclusiveMaximum"])) + if "exclusive" in param.keys(): + list_of_range.append("≠" + str(param["exclusive"])) + + if len(list_of_range) == 0: + return "N/A" + else: + range_in_text = "" + for item in list_of_range: + if range_in_text != "": + range_in_text += "
" + range_in_text += str(item) + return range_in_text + + +def extract_parameter_info(parameters, namespace=""): + params = [] + for k, v in parameters.items(): + if v["type"] != "object": + param = {} + param["Name"] = namespace + k + param["Type"] = format_param_type(v["type"]) + param["Description"] = v["description"] + param["Default"] = v["default"] + param["Range"] = format_param_range(v) + params.append(param) + else: # if the object is namespace, then dive deeper in to json value + params.extend(extract_parameter_info(v["properties"], k + ".")) + return params + + +def format_json(json_data): + parameters = list(json_data["definitions"].values())[0]["properties"] + # cspell: ignore tablefmt + markdown_table = tabulate(extract_parameter_info(parameters), headers="keys", tablefmt="github") + return markdown_table + + +def define_env(env): + @env.macro + def json_to_markdown(json_schema_file_path): + with open(json_schema_file_path) as f: + data = json.load(f) + return format_json(data) diff --git a/perception/cluster_merger/CMakeLists.txt b/perception/cluster_merger/CMakeLists.txt new file mode 100644 index 0000000000000..49506f4b439fb --- /dev/null +++ b/perception/cluster_merger/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 3.8) +project(cluster_merger) + +# find dependencies +find_package(autoware_cmake REQUIRED) +autoware_package() + +# Targets +ament_auto_add_library(cluster_merger_node_component SHARED + src/cluster_merger/node.cpp +) + +rclcpp_components_register_node(cluster_merger_node_component + PLUGIN "cluster_merger::ClusterMergerNode" + EXECUTABLE cluster_merger_node) + + +if(BUILD_TESTING) + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch +) diff --git a/perception/cluster_merger/README.md b/perception/cluster_merger/README.md new file mode 100644 index 0000000000000..b46f7401b70ec --- /dev/null +++ b/perception/cluster_merger/README.md @@ -0,0 +1,72 @@ +# cluster merger + +## Purpose + +cluster_merger is a package for merging pointcloud clusters as detected objects with feature type. + +## Inner-working / Algorithms + +The clusters of merged topics are simply concatenated from clusters of input topics. + +## Input / Output + +### Input + +| Name | Type | Description | +| ---------------- | -------------------------------------------------------- | ------------------- | +| `input/cluster0` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | pointcloud clusters | +| `input/cluster1` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | pointcloud clusters | + +### Output + +| Name | Type | Description | +| ----------------- | ----------------------------------------------------- | --------------- | +| `output/clusters` | `autoware_auto_perception_msgs::msg::DetectedObjects` | merged clusters | + +## Parameters + +| Name | Type | Description | Default value | +| :---------------- | :----- | :----------------------------------- | :------------ | +| `output_frame_id` | string | The header frame_id of output topic. | base_link | + +## Assumptions / Known limits + + + +## (Optional) Error detection and handling + + + +## (Optional) Performance characterization + + + +## (Optional) References/External links + + + +## (Optional) Future extensions / Unimplemented parts diff --git a/perception/cluster_merger/include/cluster_merger/node.hpp b/perception/cluster_merger/include/cluster_merger/node.hpp new file mode 100644 index 0000000000000..8da5999f00384 --- /dev/null +++ b/perception/cluster_merger/include/cluster_merger/node.hpp @@ -0,0 +1,73 @@ +// Copyright 2023 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 CLUSTER_MERGER__NODE_HPP_ +#define CLUSTER_MERGER__NODE_HPP_ + +#include "message_filters/subscriber.h" +#include "message_filters/sync_policies/approximate_time.h" +#include "message_filters/synchronizer.h" +#include "rclcpp/rclcpp.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#include "tier4_perception_msgs/msg/detected_objects_with_feature.hpp" + +#include +#include + +#include +#include +#include +#include + +namespace cluster_merger +{ +using tier4_perception_msgs::msg::DetectedObjectsWithFeature; +using tier4_perception_msgs::msg::DetectedObjectWithFeature; + +class ClusterMergerNode : public rclcpp::Node +{ +public: + explicit ClusterMergerNode(const rclcpp::NodeOptions & node_options); + +private: + // Subscriber + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + rclcpp::Subscription::SharedPtr sub_objects_{}; + message_filters::Subscriber objects0_sub_; + message_filters::Subscriber objects1_sub_; + typedef message_filters::sync_policies::ApproximateTime< + DetectedObjectsWithFeature, DetectedObjectsWithFeature> + SyncPolicy; + typedef message_filters::Synchronizer Sync; + Sync sync_; + + std::string output_frame_id_; + + std::vector::SharedPtr> sub_objects_array{}; + std::shared_ptr transform_listener_; + + void objectsCallback( + const DetectedObjectsWithFeature::ConstSharedPtr & input_objects0_msg, + const DetectedObjectsWithFeature::ConstSharedPtr & input_objects1_msg); + // Publisher + rclcpp::Publisher::SharedPtr pub_objects_; +}; + +} // namespace cluster_merger + +#endif // CLUSTER_MERGER__NODE_HPP_ diff --git a/perception/cluster_merger/launch/cluster_merger.launch.xml b/perception/cluster_merger/launch/cluster_merger.launch.xml new file mode 100644 index 0000000000000..1bbd0ebd91e12 --- /dev/null +++ b/perception/cluster_merger/launch/cluster_merger.launch.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/perception/cluster_merger/package.xml b/perception/cluster_merger/package.xml new file mode 100644 index 0000000000000..14826ad07e098 --- /dev/null +++ b/perception/cluster_merger/package.xml @@ -0,0 +1,28 @@ + + + + cluster_merger + 0.1.0 + The ROS 2 cluster merger package + Yukihiro Saito + Shunsuke Miura + autoware + Apache License 2.0 + + ament_cmake_auto + + geometry_msgs + message_filters + object_recognition_utils + rclcpp + rclcpp_components + tier4_autoware_utils + tier4_perception_msgs + autoware_cmake + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/perception/cluster_merger/src/cluster_merger/node.cpp b/perception/cluster_merger/src/cluster_merger/node.cpp new file mode 100644 index 0000000000000..48bf953027510 --- /dev/null +++ b/perception/cluster_merger/src/cluster_merger/node.cpp @@ -0,0 +1,73 @@ +// Copyright 2023 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 "cluster_merger/node.hpp" + +#include "object_recognition_utils/object_recognition_utils.hpp" + +#include +#include +#include +namespace cluster_merger +{ + +ClusterMergerNode::ClusterMergerNode(const rclcpp::NodeOptions & node_options) +: rclcpp::Node("cluster_merger_node", node_options), + tf_buffer_(get_clock()), + tf_listener_(tf_buffer_), + objects0_sub_(this, "input/cluster0", rclcpp::QoS{1}.get_rmw_qos_profile()), + objects1_sub_(this, "input/cluster1", rclcpp::QoS{1}.get_rmw_qos_profile()), + sync_(SyncPolicy(10), objects0_sub_, objects1_sub_) +{ + output_frame_id_ = declare_parameter("output_frame_id"); + using std::placeholders::_1; + using std::placeholders::_2; + sync_.registerCallback(std::bind(&ClusterMergerNode::objectsCallback, this, _1, _2)); + + // Publisher + pub_objects_ = create_publisher("~/output/clusters", rclcpp::QoS{1}); +} + +void ClusterMergerNode::objectsCallback( + const DetectedObjectsWithFeature::ConstSharedPtr & input_objects0_msg, + const DetectedObjectsWithFeature::ConstSharedPtr & input_objects1_msg) +{ + if (pub_objects_->get_subscription_count() < 1) { + return; + } + // TODO(badai-nguyen): transform input topics to desired frame before concatenating + /* transform to the same with cluster0 frame id*/ + DetectedObjectsWithFeature transformed_objects0; + DetectedObjectsWithFeature transformed_objects1; + if ( + !object_recognition_utils::transformObjectsWithFeature( + *input_objects0_msg, output_frame_id_, tf_buffer_, transformed_objects0) || + !object_recognition_utils::transformObjectsWithFeature( + *input_objects1_msg, output_frame_id_, tf_buffer_, transformed_objects1)) { + return; + } + + DetectedObjectsWithFeature output_objects; + output_objects.header = input_objects0_msg->header; + // add check frame id and transform if they are different + output_objects.feature_objects = transformed_objects0.feature_objects; + output_objects.feature_objects.insert( + output_objects.feature_objects.end(), transformed_objects1.feature_objects.begin(), + transformed_objects1.feature_objects.end()); + pub_objects_->publish(output_objects); +} +} // namespace cluster_merger + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(cluster_merger::ClusterMergerNode) diff --git a/perception/crosswalk_traffic_light_estimator/CMakeLists.txt b/perception/crosswalk_traffic_light_estimator/CMakeLists.txt index 11a6b232caf90..2947070eca01e 100644 --- a/perception/crosswalk_traffic_light_estimator/CMakeLists.txt +++ b/perception/crosswalk_traffic_light_estimator/CMakeLists.txt @@ -28,6 +28,8 @@ endif() ## Install ## ############# -ament_auto_package(INSTALL_TO_SHARE - launch +ament_auto_package( + INSTALL_TO_SHARE + launch + config ) diff --git a/perception/crosswalk_traffic_light_estimator/README.md b/perception/crosswalk_traffic_light_estimator/README.md index e23b454905081..73c7151997c54 100644 --- a/perception/crosswalk_traffic_light_estimator/README.md +++ b/perception/crosswalk_traffic_light_estimator/README.md @@ -22,9 +22,10 @@ ## Parameters -| Name | Type | Description | Default value | -| :---------------------- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| `use_last_detect_color` | `bool` | If this parameter is `true`, this module estimates pedestrian's traffic signal as RED not only when vehicle's traffic signal is detected as GREEN/AMBER but also when detection results change GREEN/AMBER to UNKNOWN. (If detection results change RED or AMBER to UNKNOWN, this module estimates pedestrian's traffic signal as UNKNOWN.) If this parameter is `false`, this module use only latest detection results for estimation. (Only when the detection result is GREEN/AMBER, this module estimates pedestrian's traffic signal as RED.) | `true` | +| Name | Type | Description | Default value | +| :---------------------------- | :------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| `use_last_detect_color` | `bool` | If this parameter is `true`, this module estimates pedestrian's traffic signal as RED not only when vehicle's traffic signal is detected as GREEN/AMBER but also when detection results change GREEN/AMBER to UNKNOWN. (If detection results change RED or AMBER to UNKNOWN, this module estimates pedestrian's traffic signal as UNKNOWN.) If this parameter is `false`, this module use only latest detection results for estimation. (Only when the detection result is GREEN/AMBER, this module estimates pedestrian's traffic signal as RED.) | `true` | +| `last_detect_color_hold_time` | `double` | The time threshold to hold for last detect color. | `2.0` | ## Inner-workings / Algorithms diff --git a/perception/crosswalk_traffic_light_estimator/config/crosswalk_traffic_light_estimator.param.yaml b/perception/crosswalk_traffic_light_estimator/config/crosswalk_traffic_light_estimator.param.yaml new file mode 100644 index 0000000000000..33168c87de7eb --- /dev/null +++ b/perception/crosswalk_traffic_light_estimator/config/crosswalk_traffic_light_estimator.param.yaml @@ -0,0 +1,18 @@ +# Copyright 2023 UCI SORA Lab +# +# 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. + +/**: + ros__parameters: + use_last_detect_color: true + last_detect_color_hold_time: 2.0 diff --git a/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp b/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp index 3b4855f1f38e9..161a4d19b96ef 100644 --- a/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp +++ b/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp @@ -19,7 +19,8 @@ #include #include #include -#include +#include +#include #include #include diff --git a/perception/crosswalk_traffic_light_estimator/launch/crosswalk_traffic_light_estimator.launch.xml b/perception/crosswalk_traffic_light_estimator/launch/crosswalk_traffic_light_estimator.launch.xml index 91bec60c90aee..21d718c3439cd 100644 --- a/perception/crosswalk_traffic_light_estimator/launch/crosswalk_traffic_light_estimator.launch.xml +++ b/perception/crosswalk_traffic_light_estimator/launch/crosswalk_traffic_light_estimator.launch.xml @@ -1,9 +1,23 @@ - + + + + diff --git a/perception/crosswalk_traffic_light_estimator/package.xml b/perception/crosswalk_traffic_light_estimator/package.xml index f029972f7124f..bec5b666683c4 100644 --- a/perception/crosswalk_traffic_light_estimator/package.xml +++ b/perception/crosswalk_traffic_light_estimator/package.xml @@ -5,6 +5,8 @@ The crosswalk_traffic_light_estimator package Satoshi Ota + Shunsuke Miura + Tao Zhong Apache License 2.0 ament_cmake_auto diff --git a/perception/crosswalk_traffic_light_estimator/schema/crosswalk_traffic_light_estimator.schema.json b/perception/crosswalk_traffic_light_estimator/schema/crosswalk_traffic_light_estimator.schema.json new file mode 100644 index 0000000000000..ecc273263236a --- /dev/null +++ b/perception/crosswalk_traffic_light_estimator/schema/crosswalk_traffic_light_estimator.schema.json @@ -0,0 +1,36 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Crosswalk Traffic Light Estimator Node", + "type": "object", + "definitions": { + "crosswalk_traffic_light_estimator": { + "type": "object", + "properties": { + "use_last_detect_color": { + "type": "boolean", + "default": true, + "description": "If this parameter is true, this module estimates pedestrian's traffic signal as RED not only when vehicle's traffic signal is detected as GREEN/AMBER but also when detection results change GREEN/AMBER to UNKNOWN. (If detection results change RED or AMBER to UNKNOWN, this module estimates pedestrian's traffic signal as UNKNOWN.) If this parameter is false, this module use only latest detection results for estimation. (Only when the detection result is GREEN/AMBER, this module estimates pedestrian's traffic signal as RED.)." + }, + "last_detect_color_hold_time": { + "type": "number", + "default": 2.0, + "exclusiveMinimum": 0.0, + "description": "The time threshold to hold for last detect color." + } + }, + "required": ["use_last_detect_color", "last_detect_color_hold_time"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/crosswalk_traffic_light_estimator" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/crosswalk_traffic_light_estimator/src/node.cpp b/perception/crosswalk_traffic_light_estimator/src/node.cpp index dd7a9ea04ae38..62c8ac1cb7551 100644 --- a/perception/crosswalk_traffic_light_estimator/src/node.cpp +++ b/perception/crosswalk_traffic_light_estimator/src/node.cpp @@ -1,4 +1,4 @@ -// Copyright 2022 TIER IV, Inc. +// Copyright 2022-2023 UCI SORA Lab, 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. @@ -81,8 +81,8 @@ CrosswalkTrafficLightEstimatorNode::CrosswalkTrafficLightEstimatorNode( { using std::placeholders::_1; - use_last_detect_color_ = this->declare_parameter("use_last_detect_color", true); - last_detect_color_hold_time_ = this->declare_parameter("last_detect_color_hold_time", 2.0); + use_last_detect_color_ = declare_parameter("use_last_detect_color"); + last_detect_color_hold_time_ = declare_parameter("last_detect_color_hold_time"); sub_map_ = create_subscription( "~/input/vector_map", rclcpp::QoS{1}.transient_local(), diff --git a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/debugger.hpp b/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/debugger.hpp index 97c4387122301..9ef1f75427b65 100644 --- a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/debugger.hpp +++ b/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/debugger.hpp @@ -16,7 +16,6 @@ #define OBSTACLE_POINTCLOUD_BASED_VALIDATOR__DEBUGGER_HPP_ #include -#include #include #include diff --git a/perception/detected_object_validation/src/object_lanelet_filter.cpp b/perception/detected_object_validation/src/object_lanelet_filter.cpp index d22a05fc7162d..2e393f27c37a7 100644 --- a/perception/detected_object_validation/src/object_lanelet_filter.cpp +++ b/perception/detected_object_validation/src/object_lanelet_filter.cpp @@ -15,7 +15,7 @@ #include "detected_object_filter/object_lanelet_filter.hpp" #include -#include +#include #include #include diff --git a/perception/detected_object_validation/src/object_position_filter.cpp b/perception/detected_object_validation/src/object_position_filter.cpp index 6980f5fd3c17f..0f59e60d57d55 100644 --- a/perception/detected_object_validation/src/object_position_filter.cpp +++ b/perception/detected_object_validation/src/object_position_filter.cpp @@ -14,8 +14,6 @@ #include "detected_object_filter/object_position_filter.hpp" -#include - namespace object_position_filter { ObjectPositionFilterNode::ObjectPositionFilterNode(const rclcpp::NodeOptions & node_options) diff --git a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp index bad20d8da577f..00e53e9de9a9c 100644 --- a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp @@ -15,7 +15,7 @@ #include "obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp" #include -#include +#include #include diff --git a/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp b/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp index 50e3f3b540929..948f040d7ebde 100644 --- a/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp +++ b/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp @@ -16,7 +16,7 @@ #include #include -#include +#include #include diff --git a/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp b/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp index 09ae73a7fa03c..fb7642356e8d9 100644 --- a/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp +++ b/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp @@ -20,7 +20,6 @@ #include #include #include -#include #include #include diff --git a/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp b/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp index 577e0ff12c367..0eacfa527750b 100644 --- a/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp +++ b/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp @@ -22,7 +22,6 @@ #include #include #include -#include #include #include diff --git a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp index de71f94151486..7b436e1edd64c 100644 --- a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp +++ b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp @@ -16,6 +16,9 @@ #include "object_recognition_utils/object_recognition_utils.hpp" +#include +#include + #include #include #include diff --git a/perception/front_vehicle_velocity_estimator/include/front_vehicle_velocity_estimator/front_vehicle_velocity_estimator.hpp b/perception/front_vehicle_velocity_estimator/include/front_vehicle_velocity_estimator/front_vehicle_velocity_estimator.hpp index 5141bcad04990..01f738fc6beaa 100644 --- a/perception/front_vehicle_velocity_estimator/include/front_vehicle_velocity_estimator/front_vehicle_velocity_estimator.hpp +++ b/perception/front_vehicle_velocity_estimator/include/front_vehicle_velocity_estimator/front_vehicle_velocity_estimator.hpp @@ -19,7 +19,7 @@ #include "pcl/point_types.h" #include "pcl_conversions/pcl_conversions.h" #include "rclcpp/logger.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/boost_geometry.hpp" #include "autoware_auto_perception_msgs/msg/detected_objects.hpp" #include "nav_msgs/msg/odometry.hpp" diff --git a/perception/front_vehicle_velocity_estimator/src/front_vehicle_velocity_estimator_node/front_vehicle_velocity_estimator.cpp b/perception/front_vehicle_velocity_estimator/src/front_vehicle_velocity_estimator_node/front_vehicle_velocity_estimator.cpp index f1b8be19c3223..6f59823615c04 100644 --- a/perception/front_vehicle_velocity_estimator/src/front_vehicle_velocity_estimator_node/front_vehicle_velocity_estimator.cpp +++ b/perception/front_vehicle_velocity_estimator/src/front_vehicle_velocity_estimator_node/front_vehicle_velocity_estimator.cpp @@ -14,6 +14,8 @@ #include "front_vehicle_velocity_estimator/front_vehicle_velocity_estimator.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" + #include #include diff --git a/perception/heatmap_visualizer/src/utils.cpp b/perception/heatmap_visualizer/src/utils.cpp index 2c1be762b2cc9..c4b1c3d6a22c6 100644 --- a/perception/heatmap_visualizer/src/utils.cpp +++ b/perception/heatmap_visualizer/src/utils.cpp @@ -15,7 +15,8 @@ #include "heatmap_visualizer/utils.hpp" #include -#include +#include +#include #include diff --git a/perception/image_projection_based_fusion/CMakeLists.txt b/perception/image_projection_based_fusion/CMakeLists.txt index 906ad71d21732..ce7c3b5ea12a9 100644 --- a/perception/image_projection_based_fusion/CMakeLists.txt +++ b/perception/image_projection_based_fusion/CMakeLists.txt @@ -22,6 +22,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/utils/utils.cpp src/roi_cluster_fusion/node.cpp src/roi_detected_object_fusion/node.cpp + src/roi_pointcloud_fusion/node.cpp ) target_link_libraries(${PROJECT_NAME} @@ -39,6 +40,11 @@ rclcpp_components_register_node(${PROJECT_NAME} EXECUTABLE roi_cluster_fusion_node ) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "image_projection_based_fusion::RoiPointCloudFusionNode" + EXECUTABLE roi_pointcloud_fusion_node +) + set(CUDA_VERBOSE OFF) # set flags for CUDA availability diff --git a/perception/image_projection_based_fusion/README.md b/perception/image_projection_based_fusion/README.md index 1bbee35ab44f8..207989d8a6f25 100644 --- a/perception/image_projection_based_fusion/README.md +++ b/perception/image_projection_based_fusion/README.md @@ -58,3 +58,4 @@ The rclcpp::TimerBase timer could not break a for loop, therefore even if time i | roi_cluster_fusion | Overwrite a classification label of clusters by that of ROIs from a 2D object detector. | [link](./docs/roi-cluster-fusion.md) | | roi_detected_object_fusion | Overwrite a classification label of detected objects by that of ROIs from a 2D object detector. | [link](./docs/roi-detected-object-fusion.md) | | pointpainting_fusion | Paint the point cloud with the ROIs from a 2D object detector and feed to a 3D object detector. | [link](./docs/pointpainting-fusion.md) | +| roi_pointcloud_fusion | Matching pointcloud with ROIs from a 2D object detector to detect unknown-labeled objects | [link](./docs/roi-pointcloud-fusion.md) | diff --git a/perception/image_projection_based_fusion/docs/images/roi_pointcloud_fusion.png b/perception/image_projection_based_fusion/docs/images/roi_pointcloud_fusion.png new file mode 100644 index 0000000000000..c529de7c728fb Binary files /dev/null and b/perception/image_projection_based_fusion/docs/images/roi_pointcloud_fusion.png differ diff --git a/perception/image_projection_based_fusion/docs/roi-pointcloud-fusion.md b/perception/image_projection_based_fusion/docs/roi-pointcloud-fusion.md new file mode 100644 index 0000000000000..5d0b241a578d6 --- /dev/null +++ b/perception/image_projection_based_fusion/docs/roi-pointcloud-fusion.md @@ -0,0 +1,93 @@ +# roi_pointcloud_fusion + +## Purpose + +The node `roi_pointcloud_fusion` is to cluster the pointcloud based on Region Of Interests (ROIs) detected by a 2D object detector, specific for unknown labeled ROI. + +## Inner-workings / Algorithms + +- The pointclouds are projected onto image planes and extracted as cluster if they are inside the unknown labeled ROIs. +- Since the cluster might contain unrelated points from background, then a refinement step is added to filter the background pointcloud by distance to camera. + +![roi_pointcloud_fusion_image](./images/roi_pointcloud_fusion.png) + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ------------------------ | -------------------------------------------------------- | --------------------------------------------------------- | +| `input` | `sensor_msgs::msg::PointCloud2` | input pointcloud | +| `input/camera_info[0-7]` | `sensor_msgs::msg::CameraInfo` | camera information to project 3d points onto image planes | +| `input/rois[0-7]` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | ROIs from each image | +| `input/image_raw[0-7]` | `sensor_msgs::msg::Image` | images for visualization | + +### Output + +| Name | Type | Description | +| ----------------- | -------------------------------------------------------- | -------------------------------------------- | +| `output` | `sensor_msgs::msg::PointCloud2` | output pointcloud as default of interface | +| `output_clusters` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | output clusters | +| `debug/clusters` | `sensor_msgs/msg/PointCloud2` | colored cluster pointcloud for visualization | + +## Parameters + +### Core Parameters + +| Name | Type | Description | +| ---------------------- | ------ | -------------------------------------------------------------------------------------------- | +| `min_cluster_size` | int | the minimum number of points that a cluster needs to contain in order to be considered valid | +| `cluster_2d_tolerance` | double | cluster tolerance measured in radial direction | +| `rois_number` | int | the number of input rois | + +## Assumptions / Known limits + +- Currently, the refinement is only based on distance to camera, the roi based clustering is supposed to work well with small object ROIs. Others criteria for refinement might needed in the future. + + + +## (Optional) Error detection and handling + + + +## (Optional) Performance characterization + + + +## (Optional) References/External links + + + +## (Optional) Future extensions / Unimplemented parts + + diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp index 016633fa4ef92..5174aebe069a8 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp @@ -29,6 +29,9 @@ #include #include #include +#include +#include +#include #include #include @@ -49,6 +52,8 @@ using autoware_auto_perception_msgs::msg::DetectedObjects; using sensor_msgs::msg::PointCloud2; using tier4_perception_msgs::msg::DetectedObjectsWithFeature; using tier4_perception_msgs::msg::DetectedObjectWithFeature; +using PointCloud = pcl::PointCloud; +using autoware_auto_perception_msgs::msg::ObjectClassification; template class FusionNode : public rclcpp::Node diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/voxel_generator.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/voxel_generator.hpp index 3e223dd3101df..5e7a5087efb84 100755 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/voxel_generator.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/voxel_generator.hpp @@ -17,6 +17,7 @@ #include +#include #include namespace image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp index 38f9e71eea02d..70a7866e79b87 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp @@ -43,13 +43,13 @@ class RoiClusterFusionNode bool use_iou_{false}; bool use_cluster_semantic_type_{false}; bool only_allow_inside_cluster_{false}; - float roi_scale_factor_{1.1f}; - float iou_threshold_{0.0f}; - float unknown_iou_threshold_{0.0f}; + double roi_scale_factor_{1.1}; + double iou_threshold_{0.0}; + double unknown_iou_threshold_{0.0}; const float min_roi_existence_prob_ = 0.1; // keep small value to lessen affect on merger object stage bool remove_unknown_; - float trust_distance_; + double trust_distance_; bool filter_by_distance(const DetectedObjectWithFeature & obj); bool out_of_scope(const DetectedObjectWithFeature & obj); diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp new file mode 100644 index 0000000000000..2b4eb822a9edb --- /dev/null +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp @@ -0,0 +1,53 @@ +// Copyright 2023 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 IMAGE_PROJECTION_BASED_FUSION__ROI_POINTCLOUD_FUSION__NODE_HPP_ +#define IMAGE_PROJECTION_BASED_FUSION__ROI_POINTCLOUD_FUSION__NODE_HPP_ + +#include "image_projection_based_fusion/fusion_node.hpp" + +#include +#include +namespace image_projection_based_fusion +{ +class RoiPointCloudFusionNode +: public FusionNode +{ +private: + int min_cluster_size_{1}; + bool fuse_unknown_only_{true}; + double cluster_2d_tolerance_; + + rclcpp::Publisher::SharedPtr pub_objects_ptr_; + std::vector output_fused_objects_; + rclcpp::Publisher::SharedPtr cluster_debug_pub_; + + /* data */ +public: + explicit RoiPointCloudFusionNode(const rclcpp::NodeOptions & options); + +protected: + void preprocess(sensor_msgs::msg::PointCloud2 & pointcloud_msg) override; + + void postprocess(sensor_msgs::msg::PointCloud2 & pointcloud_msg) override; + + void fuseOnSingleImage( + const PointCloud2 & input_pointcloud_msg, const std::size_t image_id, + const DetectedObjectsWithFeature & input_roi_msg, + const sensor_msgs::msg::CameraInfo & camera_info, PointCloud2 & output_pointcloud_msg) override; + bool out_of_scope(const DetectedObjectWithFeature & obj); +}; + +} // namespace image_projection_based_fusion +#endif // IMAGE_PROJECTION_BASED_FUSION__ROI_POINTCLOUD_FUSION__NODE_HPP_ diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp index 541cf997412c2..d7fd3c3882046 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp @@ -32,18 +32,54 @@ #include #endif +#include "image_projection_based_fusion/fusion_node.hpp" + +#include +#include +#include +#include + +#include "autoware_auto_perception_msgs/msg/shape.hpp" +#include "tier4_perception_msgs/msg/detected_object_with_feature.hpp" + +#include +#include +#include + #include #include +#include namespace image_projection_based_fusion { +using PointCloud = pcl::PointCloud; std::optional getTransformStamped( const tf2_ros::Buffer & tf_buffer, const std::string & target_frame_id, const std::string & source_frame_id, const rclcpp::Time & time); Eigen::Affine3d transformToEigen(const geometry_msgs::msg::Transform & t); +void convertCluster2FeatureObject( + const std_msgs::msg::Header & header, const PointCloud & cluster, + DetectedObjectWithFeature & feature_obj); +PointCloud closest_cluster( + PointCloud & cluster, const double cluster_2d_tolerance, const int min_cluster_size, + const pcl::PointXYZ & center); + +void updateOutputFusedObjects( + std::vector & output_objs, const std::vector & clusters, + const std_msgs::msg::Header & in_cloud_header, const std_msgs::msg::Header & in_roi_header, + const tf2_ros::Buffer & tf_buffer, const int min_cluster_size, const float cluster_2d_tolerance, + std::vector & output_fused_objects); + +geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud); + +pcl::PointXYZ getClosestPoint(const pcl::PointCloud & cluster); +void addShapeAndKinematic( + const pcl::PointCloud & cluster, + tier4_perception_msgs::msg::DetectedObjectWithFeature & feature_obj); + } // namespace image_projection_based_fusion #endif // IMAGE_PROJECTION_BASED_FUSION__UTILS__UTILS_HPP_ diff --git a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml index 302b567942e84..b2b9bae80f071 100644 --- a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml @@ -49,9 +49,10 @@ + - + diff --git a/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml new file mode 100644 index 0000000000000..181f4ccb88320 --- /dev/null +++ b/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml @@ -0,0 +1,77 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/image_projection_based_fusion/package.xml b/perception/image_projection_based_fusion/package.xml index 32d7a5633b811..5ff99af2ebb21 100644 --- a/perception/image_projection_based_fusion/package.xml +++ b/perception/image_projection_based_fusion/package.xml @@ -14,6 +14,7 @@ autoware_auto_perception_msgs autoware_point_types cv_bridge + euclidean_cluster image_transport lidar_centerpoint message_filters diff --git a/perception/image_projection_based_fusion/src/fusion_node.cpp b/perception/image_projection_based_fusion/src/fusion_node.cpp index 1bc351b08c01b..a540688f7e751 100644 --- a/perception/image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/image_projection_based_fusion/src/fusion_node.cpp @@ -422,4 +422,5 @@ void FusionNode::publish(const Msg & output_msg) template class FusionNode; template class FusionNode; template class FusionNode; +template class FusionNode; } // namespace image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp index 5253ac192c786..9f8ef7b94a123 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -133,7 +133,7 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt auto it = find(paint_class_names.begin(), paint_class_names.end(), cls); if (it != paint_class_names.end()) { int index = it - paint_class_names.begin(); - class_index_[cls] = index + 1; + class_index_[cls] = pow(2, index); // regard each class as a bit in binary } else { isClassTable_.erase(cls); } @@ -345,7 +345,8 @@ dc | dc dc dc dc ||zc| data = &painted_pointcloud_msg.data[0]; auto p_class = reinterpret_cast(&output[stride + class_offset]); for (const auto & cls : isClassTable_) { - *p_class = cls.second(label2d) ? class_index_[cls.first] : *p_class; + // add up the class values if the point belongs to multiple classes + *p_class = cls.second(label2d) ? (class_index_[cls.first] + *p_class) : *p_class; } } #if 0 diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp index bd49d9e446f1f..f462b9b0ba17a 100755 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp @@ -59,8 +59,10 @@ std::size_t VoxelGenerator::pointsToVoxels( point[1] = point_current.y(); point[2] = point_current.z(); point[3] = time_lag; + // decode the class value back to one-hot binary and assign it to point for (std::size_t i = 1; i <= config_.class_size_; i++) { - point[3 + i] = (*class_iter == i) ? 1 : 0; + auto decode = std::bitset<8>(*class_iter).to_string(); + point[3 + i] = decode[-i] == 1 ? 1 : 0; } out_of_range = false; diff --git a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp index 9180b18adeed8..e9cb76b050427 100644 --- a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp @@ -36,16 +36,16 @@ namespace image_projection_based_fusion RoiClusterFusionNode::RoiClusterFusionNode(const rclcpp::NodeOptions & options) : FusionNode("roi_cluster_fusion", options) { - use_iou_x_ = declare_parameter("use_iou_x", true); - use_iou_y_ = declare_parameter("use_iou_y", false); - use_iou_ = declare_parameter("use_iou", false); - use_cluster_semantic_type_ = declare_parameter("use_cluster_semantic_type", false); - only_allow_inside_cluster_ = declare_parameter("only_allow_inside_cluster_", true); - roi_scale_factor_ = declare_parameter("roi_scale_factor", 1.1); - iou_threshold_ = declare_parameter("iou_threshold", 0.1); - unknown_iou_threshold_ = declare_parameter("unknown_iou_threshold", 0.1); - remove_unknown_ = declare_parameter("remove_unknown", false); - trust_distance_ = declare_parameter("trust_distance", 100.0); + use_iou_x_ = declare_parameter("use_iou_x"); + use_iou_y_ = declare_parameter("use_iou_y"); + use_iou_ = declare_parameter("use_iou"); + use_cluster_semantic_type_ = declare_parameter("use_cluster_semantic_type"); + only_allow_inside_cluster_ = declare_parameter("only_allow_inside_cluster"); + roi_scale_factor_ = declare_parameter("roi_scale_factor"); + iou_threshold_ = declare_parameter("iou_threshold"); + unknown_iou_threshold_ = declare_parameter("unknown_iou_threshold"); + remove_unknown_ = declare_parameter("remove_unknown"); + trust_distance_ = declare_parameter("trust_distance"); } void RoiClusterFusionNode::preprocess(DetectedObjectsWithFeature & output_cluster_msg) @@ -171,12 +171,16 @@ void RoiClusterFusionNode::fuseOnSingleImage( for (const auto & feature_obj : input_roi_msg.feature_objects) { int index = 0; double max_iou = 0.0; + bool is_roi_label_known = + feature_obj.object.classification.front().label != ObjectClassification::UNKNOWN; for (const auto & cluster_map : m_cluster_roi) { double iou(0.0), iou_x(0.0), iou_y(0.0); if (use_iou_) { iou = calcIoU(cluster_map.second, feature_obj.feature.roi); } - if (use_iou_x_) { + // use for unknown roi to improve small objects like traffic cone detect + // TODO(badai-nguyen): add option to disable roi_cluster mode + if (use_iou_x_ || !is_roi_label_known) { iou_x = calcIoUX(cluster_map.second, feature_obj.feature.roi); } if (use_iou_y_) { @@ -192,8 +196,6 @@ void RoiClusterFusionNode::fuseOnSingleImage( } } if (!output_cluster_msg.feature_objects.empty()) { - bool is_roi_label_known = feature_obj.object.classification.front().label != - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; bool is_roi_existence_prob_higher = output_cluster_msg.feature_objects.at(index).object.existence_probability <= feature_obj.object.existence_probability; diff --git a/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp new file mode 100644 index 0000000000000..233e568ebee0b --- /dev/null +++ b/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp @@ -0,0 +1,165 @@ +// Copyright 2023 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 "image_projection_based_fusion/roi_pointcloud_fusion/node.hpp" + +#include "image_projection_based_fusion/utils/geometry.hpp" +#include "image_projection_based_fusion/utils/utils.hpp" + +#ifdef ROS_DISTRO_GALACTIC +#include +#include +#else +#include +#include +#endif + +#include "euclidean_cluster/utils.hpp" + +namespace image_projection_based_fusion +{ +RoiPointCloudFusionNode::RoiPointCloudFusionNode(const rclcpp::NodeOptions & options) +: FusionNode("roi_pointcloud_fusion", options) +{ + fuse_unknown_only_ = declare_parameter("fuse_unknown_only"); + min_cluster_size_ = declare_parameter("min_cluster_size"); + cluster_2d_tolerance_ = declare_parameter("cluster_2d_tolerance"); + pub_objects_ptr_ = + this->create_publisher("output_clusters", rclcpp::QoS{1}); + cluster_debug_pub_ = this->create_publisher("debug/clusters", 1); +} + +void RoiPointCloudFusionNode::preprocess(__attribute__((unused)) + sensor_msgs::msg::PointCloud2 & pointcloud_msg) +{ + return; +} + +void RoiPointCloudFusionNode::postprocess(__attribute__((unused)) + sensor_msgs::msg::PointCloud2 & pointcloud_msg) +{ + const auto objects_sub_count = pub_objects_ptr_->get_subscription_count() + + pub_objects_ptr_->get_intra_process_subscription_count(); + if (objects_sub_count < 1) { + return; + } + + DetectedObjectsWithFeature output_msg; + output_msg.header = pointcloud_msg.header; + output_msg.feature_objects = output_fused_objects_; + + if (objects_sub_count > 0) { + pub_objects_ptr_->publish(output_msg); + } + output_fused_objects_.clear(); + // publish debug cluster + if (cluster_debug_pub_->get_subscription_count() > 0) { + sensor_msgs::msg::PointCloud2 debug_cluster_msg; + euclidean_cluster::convertObjectMsg2SensorMsg(output_msg, debug_cluster_msg); + cluster_debug_pub_->publish(debug_cluster_msg); + } +} +void RoiPointCloudFusionNode::fuseOnSingleImage( + const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, + __attribute__((unused)) const std::size_t image_id, + const DetectedObjectsWithFeature & input_roi_msg, + const sensor_msgs::msg::CameraInfo & camera_info, + __attribute__((unused)) sensor_msgs::msg::PointCloud2 & output_pointcloud_msg) +{ + if (input_pointcloud_msg.data.empty()) { + return; + } + std::vector output_objs; + // select ROIs for fusion + for (const auto & feature_obj : input_roi_msg.feature_objects) { + if (fuse_unknown_only_) { + bool is_roi_label_unknown = feature_obj.object.classification.front().label == + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; + if (is_roi_label_unknown) { + output_objs.push_back(feature_obj); + } + } else { + // TODO(badai-nguyen): selected class from a list + output_objs.push_back(feature_obj); + } + } + if (output_objs.empty()) { + return; + } + + // transform pointcloud to camera optical frame id + Eigen::Matrix4d projection; + projection << camera_info.p.at(0), camera_info.p.at(1), camera_info.p.at(2), camera_info.p.at(3), + camera_info.p.at(4), camera_info.p.at(5), camera_info.p.at(6), camera_info.p.at(7), + camera_info.p.at(8), camera_info.p.at(9), camera_info.p.at(10), camera_info.p.at(11); + geometry_msgs::msg::TransformStamped transform_stamped; + { + const auto transform_stamped_optional = getTransformStamped( + tf_buffer_, input_roi_msg.header.frame_id, input_pointcloud_msg.header.frame_id, + input_roi_msg.header.stamp); + if (!transform_stamped_optional) { + return; + } + transform_stamped = transform_stamped_optional.value(); + } + + sensor_msgs::msg::PointCloud2 transformed_cloud; + tf2::doTransform(input_pointcloud_msg, transformed_cloud, transform_stamped); + + std::vector clusters; + clusters.resize(output_objs.size()); + + for (sensor_msgs::PointCloud2ConstIterator iter_x(transformed_cloud, "x"), + iter_y(transformed_cloud, "y"), iter_z(transformed_cloud, "z"), + iter_orig_x(input_pointcloud_msg, "x"), iter_orig_y(input_pointcloud_msg, "y"), + iter_orig_z(input_pointcloud_msg, "z"); + iter_x != iter_x.end(); + ++iter_x, ++iter_y, ++iter_z, ++iter_orig_x, ++iter_orig_y, ++iter_orig_z) { + if (*iter_z <= 0.0) { + continue; + } + Eigen::Vector4d projected_point = projection * Eigen::Vector4d(*iter_x, *iter_y, *iter_z, 1.0); + Eigen::Vector2d normalized_projected_point = Eigen::Vector2d( + projected_point.x() / projected_point.z(), projected_point.y() / projected_point.z()); + + for (std::size_t i = 0; i < output_objs.size(); ++i) { + auto & feature_obj = output_objs.at(i); + const auto & check_roi = feature_obj.feature.roi; + auto & cluster = clusters.at(i); + + if ( + check_roi.x_offset <= normalized_projected_point.x() && + check_roi.y_offset <= normalized_projected_point.y() && + check_roi.x_offset + check_roi.width >= normalized_projected_point.x() && + check_roi.y_offset + check_roi.height >= normalized_projected_point.y()) { + cluster.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y, *iter_orig_z)); + } + } + } + + // refine and update output_fused_objects_ + updateOutputFusedObjects( + output_objs, clusters, input_pointcloud_msg.header, input_roi_msg.header, tf_buffer_, + min_cluster_size_, cluster_2d_tolerance_, output_fused_objects_); +} + +bool RoiPointCloudFusionNode::out_of_scope(__attribute__((unused)) + const DetectedObjectWithFeature & obj) +{ + return false; +} +} // namespace image_projection_based_fusion + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(image_projection_based_fusion::RoiPointCloudFusionNode) diff --git a/perception/image_projection_based_fusion/src/utils/utils.cpp b/perception/image_projection_based_fusion/src/utils/utils.cpp index 670c5596001fb..76cd1e3c61e82 100644 --- a/perception/image_projection_based_fusion/src/utils/utils.cpp +++ b/perception/image_projection_based_fusion/src/utils/utils.cpp @@ -13,7 +13,6 @@ // limitations under the License. #include "image_projection_based_fusion/utils/utils.hpp" - namespace image_projection_based_fusion { @@ -39,4 +38,197 @@ Eigen::Affine3d transformToEigen(const geometry_msgs::msg::Transform & t) return a; } +void convertCluster2FeatureObject( + const std_msgs::msg::Header & header, const PointCloud & cluster, + DetectedObjectWithFeature & feature_obj) +{ + PointCloud2 ros_cluster; + pcl::toROSMsg(cluster, ros_cluster); + ros_cluster.header = header; + feature_obj.feature.cluster = ros_cluster; + feature_obj.object.kinematics.pose_with_covariance.pose.position = getCentroid(ros_cluster); + feature_obj.object.existence_probability = 1.0f; +} + +PointCloud closest_cluster( + PointCloud & cluster, const double cluster_2d_tolerance, const int min_cluster_size, + const pcl::PointXYZ & center) +{ + // sort point by distance to camera origin + + auto func = [center](const pcl::PointXYZ & p1, const pcl::PointXYZ & p2) { + return tier4_autoware_utils::calcDistance2d(center, p1) < + tier4_autoware_utils::calcDistance2d(center, p2); + }; + std::sort(cluster.begin(), cluster.end(), func); + PointCloud out_cluster; + for (auto & point : cluster) { + if (out_cluster.empty()) { + out_cluster.push_back(point); + continue; + } + if (tier4_autoware_utils::calcDistance2d(out_cluster.back(), point) < cluster_2d_tolerance) { + out_cluster.push_back(point); + continue; + } + if (out_cluster.size() >= static_cast(min_cluster_size)) { + return out_cluster; + } + out_cluster.clear(); + out_cluster.push_back(point); + } + return out_cluster; +} + +void updateOutputFusedObjects( + std::vector & output_objs, const std::vector & clusters, + const std_msgs::msg::Header & in_cloud_header, const std_msgs::msg::Header & in_roi_header, + const tf2_ros::Buffer & tf_buffer, const int min_cluster_size, const float cluster_2d_tolerance, + std::vector & output_fused_objects) +{ + if (output_objs.size() != clusters.size()) { + return; + } + Eigen::Vector3d orig_camera_frame, orig_point_frame; + Eigen::Affine3d camera2lidar_affine; + orig_camera_frame << 0.0, 0.0, 0.0; + { + const auto transform_stamped_optional = getTransformStamped( + tf_buffer, in_cloud_header.frame_id, in_roi_header.frame_id, in_roi_header.stamp); + if (!transform_stamped_optional) { + return; + } + camera2lidar_affine = transformToEigen(transform_stamped_optional.value().transform); + } + orig_point_frame = camera2lidar_affine * orig_camera_frame; + pcl::PointXYZ camera_orig_point_frame = + pcl::PointXYZ(orig_point_frame.x(), orig_point_frame.y(), orig_point_frame.z()); + + for (std::size_t i = 0; i < output_objs.size(); ++i) { + PointCloud cluster = clusters.at(i); + auto & feature_obj = output_objs.at(i); + if (cluster.points.size() < std::size_t(min_cluster_size)) { + continue; + } + + // TODO(badai-nguyen): change to interface to select refine criteria like closest, largest + // to output refine cluster and centroid + auto refine_cluster = + closest_cluster(cluster, cluster_2d_tolerance, min_cluster_size, camera_orig_point_frame); + if (refine_cluster.points.size() < std::size_t(min_cluster_size)) { + continue; + } + + refine_cluster.width = refine_cluster.points.size(); + refine_cluster.height = 1; + refine_cluster.is_dense = false; + // convert cluster to object + convertCluster2FeatureObject(in_cloud_header, refine_cluster, feature_obj); + output_fused_objects.push_back(feature_obj); + } +} + +void addShapeAndKinematic( + const pcl::PointCloud & cluster, + tier4_perception_msgs::msg::DetectedObjectWithFeature & feature_obj) +{ + if (cluster.empty()) { + return; + } + pcl::PointXYZ centroid = pcl::PointXYZ(0.0, 0.0, 0.0); + float max_z = -1e6; + float min_z = 1e6; + for (const auto & point : cluster) { + centroid.x += point.x; + centroid.y += point.y; + centroid.z += point.z; + max_z = max_z < point.z ? point.z : max_z; + min_z = min_z > point.z ? point.z : min_z; + } + centroid.x = centroid.x / static_cast(cluster.size()); + centroid.y = centroid.y / static_cast(cluster.size()); + centroid.z = centroid.z / static_cast(cluster.size()); + + std::vector cluster2d; + std::vector cluster2d_convex; + + for (size_t i = 0; i < cluster.size(); ++i) { + cluster2d.push_back( + cv::Point((cluster.at(i).x - centroid.x) * 1000.0, (cluster.at(i).y - centroid.y) * 1000.)); + } + cv::convexHull(cluster2d, cluster2d_convex); + if (cluster2d_convex.empty()) { + return; + } + pcl::PointXYZ polygon_centroid = pcl::PointXYZ(0.0, 0.0, 0.0); + for (size_t i = 0; i < cluster2d_convex.size(); ++i) { + polygon_centroid.x += static_cast(cluster2d_convex.at(i).x) / 1000.0; + polygon_centroid.y += static_cast(cluster2d_convex.at(i).y) / 1000.0; + } + polygon_centroid.x = polygon_centroid.x / static_cast(cluster2d_convex.size()); + polygon_centroid.y = polygon_centroid.y / static_cast(cluster2d_convex.size()); + + autoware_auto_perception_msgs::msg::Shape shape; + for (size_t i = 0; i < cluster2d_convex.size(); ++i) { + geometry_msgs::msg::Point32 point; + point.x = cluster2d_convex.at(i).x / 1000.0; + point.y = cluster2d_convex.at(i).y / 1000.0; + point.z = 0.0; + shape.footprint.points.push_back(point); + } + shape.type = autoware_auto_perception_msgs::msg::Shape::POLYGON; + constexpr float eps = 0.01; + shape.dimensions.x = 0; + shape.dimensions.y = 0; + shape.dimensions.z = std::max((max_z - min_z), eps); + feature_obj.object.shape = shape; + feature_obj.object.kinematics.pose_with_covariance.pose.position.x = + centroid.x + polygon_centroid.x; + feature_obj.object.kinematics.pose_with_covariance.pose.position.y = + centroid.y + polygon_centroid.y; + feature_obj.object.kinematics.pose_with_covariance.pose.position.z = + min_z + shape.dimensions.z * 0.5; + feature_obj.object.existence_probability = 1.0; + feature_obj.object.kinematics.pose_with_covariance.pose.orientation.x = 0; + feature_obj.object.kinematics.pose_with_covariance.pose.orientation.y = 0; + feature_obj.object.kinematics.pose_with_covariance.pose.orientation.z = 0; + feature_obj.object.kinematics.pose_with_covariance.pose.orientation.w = 1; +} + +geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud) +{ + geometry_msgs::msg::Point centroid; + centroid.x = 0.0f; + centroid.y = 0.0f; + centroid.z = 0.0f; + for (sensor_msgs::PointCloud2ConstIterator iter_x(pointcloud, "x"), + iter_y(pointcloud, "y"), iter_z(pointcloud, "z"); + iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { + centroid.x += *iter_x; + centroid.y += *iter_y; + centroid.z += *iter_z; + } + const size_t size = pointcloud.width * pointcloud.height; + centroid.x = centroid.x / static_cast(size); + centroid.y = centroid.y / static_cast(size); + centroid.z = centroid.z / static_cast(size); + return centroid; +} + +pcl::PointXYZ getClosestPoint(const pcl::PointCloud & cluster) +{ + pcl::PointXYZ closest_point; + double min_dist = 1e6; + pcl::PointXYZ orig_point = pcl::PointXYZ(0.0, 0.0, 0.0); + for (std::size_t i = 0; i < cluster.points.size(); ++i) { + pcl::PointXYZ point = cluster.points.at(i); + double dist_closest_point = tier4_autoware_utils::calcDistance2d(point, orig_point); + if (min_dist > dist_closest_point) { + min_dist = dist_closest_point; + closest_point = pcl::PointXYZ(point.x, point.y, point.z); + } + } + return closest_point; +} + } // namespace image_projection_based_fusion diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/README.md b/perception/lidar_apollo_segmentation_tvm_nodes/README.md index 626048b2416a0..488eea4f92168 100644 --- a/perception/lidar_apollo_segmentation_tvm_nodes/README.md +++ b/perception/lidar_apollo_segmentation_tvm_nodes/README.md @@ -49,6 +49,10 @@ The input are non-ground points as a PointCloud2 message from the sensor_msgs pa The output is a [DetectedObjectsWithFeature](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_perception_msgs/msg/object_recognition/DetectedObjectsWithFeature.msg). +#### Parameters + +{{ json_to_markdown("perception/lidar_apollo_segmentation_tvm_nodes/schema/lidar_apollo_segmentation_tvm_nodes.schema.json") }} + ### Error detection and handling Abort and warn when the input frame can't be converted to `base_link`. diff --git a/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp b/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp index 90129b837eb1b..d4d7b1379776a 100644 --- a/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp +++ b/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp @@ -16,8 +16,7 @@ #include "object_recognition_utils/geometry.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" - +#include "tier4_autoware_utils/geometry/geometry.hpp" namespace centerpoint { diff --git a/perception/map_based_prediction/README.md b/perception/map_based_prediction/README.md index 256a1d0ae899f..7631b47772862 100644 --- a/perception/map_based_prediction/README.md +++ b/perception/map_based_prediction/README.md @@ -143,30 +143,29 @@ If the target object is inside the road or crosswalk, this module outputs one or ### Output -| Name | Type | Description | -| ------------------------ | ------------------------------------------------------ | ------------------------------------- | -| `~/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | tracking objects with predicted path. | -| `~/objects_path_markers` | `visualization_msgs::msg::MarkerArray` | marker for visualization. | +| Name | Type | Description | +| ------------------------ | ------------------------------------------------------ | ------------------------------------------------------------------------------------- | +| `~/input/objects` | `autoware_auto_perception_msgs::msg::TrackedObjects` | tracking objects. Default is set to `/perception/object_recognition/tracking/objects` | +| `~/output/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | tracking objects with predicted path. | +| `~/objects_path_markers` | `visualization_msgs::msg::MarkerArray` | marker for visualization. | ## Parameters -| Parameter | Type | Description | -| ------------------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------ | -| `enable_delay_compensation` | bool | flag to enable the time delay compensation for the position of the object | -| `prediction_time_horizon` | double | predict time duration for predicted path [s] | -| `prediction_sampling_delta_time` | double | sampling time for points in predicted path [s] | -| `min_velocity_for_map_based_prediction` | double | apply map-based prediction to the objects with higher velocity than this value | -| `min_crosswalk_user_velocity` | double | minimum velocity use in path prediction for crosswalk users | -| `dist_threshold_for_searching_lanelet` | double | The threshold of the angle used when searching for the lane to which the object belongs [rad] | -| `delta_yaw_threshold_for_searching_lanelet` | double | The threshold of the distance used when searching for the lane to which the object belongs [m] | -| `sigma_lateral_offset` | double | Standard deviation for lateral position of objects [m] | -| `sigma_yaw_angle` | double | Standard deviation yaw angle of objects [rad] | -| `object_buffer_time_length` | double | Time span of object history to store the information [s] | -| `history_time_length` | double | Time span of object information used for prediction [s] | -| `dist_ratio_threshold_to_left_bound` | double | Conditions for using lane change detection of objects. Distance to the left bound of lanelet. | -| `dist_ratio_threshold_to_right_bound` | double | Conditions for using lane change detection of objects. Distance to the right bound of lanelet. | -| `diff_dist_threshold_to_left_bound` | double | Conditions for using lane change detection of objects. Differential value of horizontal position of objects. | -| `diff_dist_threshold_to_right_bound` | double | Conditions for using lane change detection of objects. Differential value of horizontal position of objects. | +| Parameter | Unit | Type | Description | +| ---------------------------------------------------------------- | ----- | ------ | ------------------------------------------------------------------------------------------------------------------------------------- | +| `enable_delay_compensation` | [-] | bool | flag to enable the time delay compensation for the position of the object | +| `prediction_time_horizon` | [s] | double | predict time duration for predicted path | +| `prediction_sampling_delta_time` | [s] | double | sampling time for points in predicted path | +| `min_velocity_for_map_based_prediction` | [m/s] | double | apply map-based prediction to the objects with higher velocity than this value | +| `min_crosswalk_user_velocity` | [m/s] | double | minimum velocity used when crosswalk user's velocity is calculated | +| `max_crosswalk_user_delta_yaw_threshold_for_lanelet` | [rad] | double | maximum yaw difference between crosswalk user and lanelet to use in path prediction for crosswalk users | +| `dist_threshold_for_searching_lanelet` | [m] | double | The threshold of the angle used when searching for the lane to which the object belongs | +| `delta_yaw_threshold_for_searching_lanelet` | [rad] | double | The threshold of the angle used when searching for the lane to which the object belongs | +| `sigma_lateral_offset` | [m] | double | Standard deviation for lateral position of objects | +| `sigma_yaw_angle_deg` | [deg] | double | Standard deviation yaw angle of objects | +| `object_buffer_time_length` | [s] | double | Time span of object history to store the information | +| `history_time_length` | [s] | double | Time span of object information used for prediction | +| `prediction_time_horizon_rate_for_validate_shoulder_lane_length` | [-] | double | prediction path will disabled when the estimated path length exceeds lanelet length. This parameter control the estimated path length | ## Assumptions / Known limits diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index 7364e3d11b650..f694892541f4a 100644 --- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -20,11 +20,9 @@ #include #include #include -#include #include #include #include -#include #include #include diff --git a/perception/map_based_prediction/launch/map_based_prediction.launch.xml b/perception/map_based_prediction/launch/map_based_prediction.launch.xml index f7ae53cd1d588..619c0c9507e0b 100644 --- a/perception/map_based_prediction/launch/map_based_prediction.launch.xml +++ b/perception/map_based_prediction/launch/map_based_prediction.launch.xml @@ -4,10 +4,12 @@ + - + + diff --git a/perception/map_based_prediction/package.xml b/perception/map_based_prediction/package.xml index a76310972c738..d7ff85adc9193 100644 --- a/perception/map_based_prediction/package.xml +++ b/perception/map_based_prediction/package.xml @@ -9,6 +9,8 @@ Shunsuke Miura Yoshi Ri Takayuki Murooka + Kyoichi Sugahara + Kotaro Uetake Apache License 2.0 ament_cmake diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index 259fbae1cef91..c1cb9d9dd3910 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -16,7 +16,10 @@ #include #include -#include +#include +#include +#include +#include #include @@ -735,13 +738,13 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ prediction_time_horizon_, prediction_sampling_time_interval_, min_crosswalk_user_velocity_); sub_objects_ = this->create_subscription( - "/perception/object_recognition/tracking/objects", 1, + "~/input/objects", 1, std::bind(&MapBasedPredictionNode::objectsCallback, this, std::placeholders::_1)); sub_map_ = this->create_subscription( "/vector_map", rclcpp::QoS{1}.transient_local(), std::bind(&MapBasedPredictionNode::mapCallback, this, std::placeholders::_1)); - pub_objects_ = this->create_publisher("objects", rclcpp::QoS{1}); + pub_objects_ = this->create_publisher("~/output/objects", rclcpp::QoS{1}); pub_debug_markers_ = this->create_publisher("maneuver", rclcpp::QoS{1}); pub_calculation_time_ = create_publisher("~/debug/calculation_time", 1); diff --git a/perception/map_based_prediction/src/path_generator.cpp b/perception/map_based_prediction/src/path_generator.cpp index b1f6319801d4a..547132410badf 100644 --- a/perception/map_based_prediction/src/path_generator.cpp +++ b/perception/map_based_prediction/src/path_generator.cpp @@ -15,8 +15,8 @@ #include "map_based_prediction/path_generator.hpp" #include -#include -#include +#include +#include #include diff --git a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp index f115d8c07f72f..d2b6ee5de475e 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp @@ -21,14 +21,16 @@ #include #include -#include #include #include #include #include +#include #include +#include + #include #include #include diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index 006a87430965f..6571e70b8c123 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -20,7 +20,9 @@ #include "multi_object_tracker/utils/utils.hpp" -#include +#include +#include +#include #include #include diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 50f582d6a2501..97d6d48c35d1b 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -34,7 +34,9 @@ #include #include -#include +#include +#include +#include using Label = autoware_auto_perception_msgs::msg::ObjectClassification; diff --git a/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp index d9137bc67ef55..51adca7e69b56 100644 --- a/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp @@ -18,8 +18,6 @@ #include "multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp" -#include - using Label = autoware_auto_perception_msgs::msg::ObjectClassification; MultipleVehicleTracker::MultipleVehicleTracker( diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 978b5e346efb2..aa3f7b1c30d01 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -34,7 +34,9 @@ #include #include -#include +#include +#include +#include using Label = autoware_auto_perception_msgs::msg::ObjectClassification; diff --git a/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp index 00822a2776ec8..2084ac28e70f0 100644 --- a/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp @@ -34,7 +34,6 @@ #include #include -#include PassThroughTracker::PassThroughTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp index 87036c9b7665a..eed9d05359b77 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp @@ -18,8 +18,6 @@ #include "multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp" -#include - using Label = autoware_auto_perception_msgs::msg::ObjectClassification; PedestrianAndBicycleTracker::PedestrianAndBicycleTracker( diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index d2f144a25f103..b168717042db3 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -21,7 +21,9 @@ #include "multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" -#include +#include +#include +#include #include #include diff --git a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp index 474191f8926ec..897b858a6aabe 100644 --- a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp @@ -30,7 +30,9 @@ #include #include -#include +#include +#include +#include UnknownTracker::UnknownTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, diff --git a/perception/object_merger/include/object_association_merger/utils/utils.hpp b/perception/object_merger/include/object_association_merger/utils/utils.hpp index dce5a48c5d2ce..bb4466bd4944d 100644 --- a/perception/object_merger/include/object_association_merger/utils/utils.hpp +++ b/perception/object_merger/include/object_association_merger/utils/utils.hpp @@ -19,8 +19,6 @@ #ifndef OBJECT_ASSOCIATION_MERGER__UTILS__UTILS_HPP_ #define OBJECT_ASSOCIATION_MERGER__UTILS__UTILS_HPP_ -#include - #include #include #include diff --git a/perception/object_merger/src/object_association_merger/node.cpp b/perception/object_merger/src/object_association_merger/node.cpp index 4f600ce8a4948..0ad2a76f74754 100644 --- a/perception/object_merger/src/object_association_merger/node.cpp +++ b/perception/object_merger/src/object_association_merger/node.cpp @@ -16,7 +16,7 @@ #include "object_association_merger/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include diff --git a/perception/object_velocity_splitter/src/object_velocity_splitter_node/object_velocity_splitter_node.cpp b/perception/object_velocity_splitter/src/object_velocity_splitter_node/object_velocity_splitter_node.cpp index 775f612b8caeb..a5d54b63b3311 100644 --- a/perception/object_velocity_splitter/src/object_velocity_splitter_node/object_velocity_splitter_node.cpp +++ b/perception/object_velocity_splitter/src/object_velocity_splitter_node/object_velocity_splitter_node.cpp @@ -14,7 +14,7 @@ #include "object_velocity_splitter/object_velocity_splitter_node.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include #include diff --git a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp index 72136c20fddb8..691c13a6d4701 100644 --- a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp +++ b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp @@ -15,7 +15,9 @@ #include "occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp" #include -#include +#include +#include +#include #include diff --git a/perception/probabilistic_occupancy_grid_map/include/utils/utils.hpp b/perception/probabilistic_occupancy_grid_map/include/utils/utils.hpp index 4af7ef950272d..047b747c2861f 100644 --- a/perception/probabilistic_occupancy_grid_map/include/utils/utils.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/utils/utils.hpp @@ -18,7 +18,6 @@ #include #include #include -#include #include #include diff --git a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp index e6f8ca17757c2..b4505eedddd21 100644 --- a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp @@ -18,7 +18,6 @@ #include "utils/utils.hpp" #include -#include #include diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp index 4e71c4bda3f21..8652cfa34d96c 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp @@ -56,7 +56,6 @@ #include #include -#include #include diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp index b07076de8342a..56aeea30e0773 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp index 9556b0a93cc93..20a5770e37fdb 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp index e8b53b91d81f7..f6369602b8890 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp @@ -20,7 +20,8 @@ #include "utils/utils.hpp" #include -#include +#include +#include #include diff --git a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp b/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp index f2cd6203b3685..0060754cd875c 100644 --- a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp @@ -14,6 +14,8 @@ #include "utils/utils.hpp" +#include + #include namespace utils diff --git a/perception/radar_fusion_to_detected_object/README.md b/perception/radar_fusion_to_detected_object/README.md index b22938cbf0e5a..defc716a744d9 100644 --- a/perception/radar_fusion_to_detected_object/README.md +++ b/perception/radar_fusion_to_detected_object/README.md @@ -13,11 +13,11 @@ The document of core algorithm is [here](docs/algorithm.md) ### Parameters for sensor fusion -| Name | Type | Description | Default value | -| :----------------------- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| bounding_box_margin | double | The distance to extend the 2D bird's-eye view Bounding Box on each side. This distance is used as a threshold to find radar centroids falling inside the extended box. [m] | 2.0 | -| split_threshold_velocity | double | The object's velocity threshold to decide to split for two objects from radar information (currently not implemented) [m/s] | 5.0 | -| threshold_yaw_diff | double | The yaw orientation threshold. If $ \vert \theta _{ob} - \theta_ {ra} \vert < threshold*yaw_diff $ attached to radar information include estimated velocity, where $ \theta*{ob} $ is yaw angle from 3d detected object, $ \theta\_ {ra} $ is yaw angle from radar object. [rad] | 0.35 | +| Name | Type | Description | Default value | +| :----------------------- | :----- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| bounding_box_margin | double | The distance to extend the 2D bird's-eye view Bounding Box on each side. This distance is used as a threshold to find radar centroids falling inside the extended box. [m] | 2.0 | +| split_threshold_velocity | double | The object's velocity threshold to decide to split for two objects from radar information (currently not implemented) [m/s] | 5.0 | +| threshold_yaw_diff | double | The yaw orientation threshold. If ∣ θ_ob − θ_ra ∣ < threshold × yaw_diff attached to radar information include estimated velocity, where*θob*is yaw angle from 3d detected object,\*θ_ra is yaw angle from radar object. [rad] | 0.35 | ### Weight parameters for velocity estimation diff --git a/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object.hpp b/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object.hpp index e51024ba4e205..b5f2005a84baf 100644 --- a/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object.hpp +++ b/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object.hpp @@ -16,8 +16,7 @@ #define RADAR_FUSION_TO_DETECTED_OBJECT_HPP_ #include "rclcpp/logger.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" - +#include "tier4_autoware_utils/geometry/boost_geometry.hpp" #define EIGEN_MPL2_ONLY #include #include diff --git a/perception/radar_fusion_to_detected_object/schema/radar_fusion_to_detected_object.schema.json b/perception/radar_fusion_to_detected_object/schema/radar_fusion_to_detected_object.schema.json new file mode 100644 index 0000000000000..688414df56c1e --- /dev/null +++ b/perception/radar_fusion_to_detected_object/schema/radar_fusion_to_detected_object.schema.json @@ -0,0 +1,114 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Radar Fusion to Detected Object Node", + "type": "object", + "definitions": { + "radar_fusion_to_detected_object": { + "type": "object", + "properties": { + "node_params": { + "type": "object", + "properties": { + "update_rate_hz": { + "type": "number", + "description": "Update rate for parameters. [hz]", + "default": "10.0", + "minimum": 0.0 + } + }, + "required": ["update_rate_hz"] + }, + "core_params": { + "type": "object", + "properties": { + "bounding_box_margin": { + "type": "number", + "description": "The distance to extend the 2D bird's-eye view Bounding Box on each side. This distance is used as a threshold to find radar centroids falling inside the extended box. [m]", + "default": "2.0", + "minimum": 0.0 + }, + "split_threshold_velocity": { + "type": "number", + "description": "The object's velocity threshold to decide to split for two objects from radar information (currently not implemented) [m/s]", + "default": "5.0", + "minimum": 0.0 + }, + "threshold_yaw_diff": { + "type": "number", + "description": "The yaw orientation threshold. If ∣θob−θra∣ +#include + #include #include @@ -40,7 +43,7 @@ void RadarFusionToDetectedObject::setParam(const Param & param) // Radar fusion param param_.bounding_box_margin = param.bounding_box_margin; param_.split_threshold_velocity = param.split_threshold_velocity; - param_.threshold_yaw_diff = param.split_threshold_velocity; + param_.threshold_yaw_diff = param.threshold_yaw_diff; // Normalize weight param double sum_weight = param.velocity_weight_median + param.velocity_weight_min_distance + diff --git a/perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp b/perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp index a65302aea866d..b233bdf31adc6 100644 --- a/perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp +++ b/perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp @@ -61,31 +61,29 @@ RadarObjectFusionToDetectedObjectNode::RadarObjectFusionToDetectedObjectNode( std::bind(&RadarObjectFusionToDetectedObjectNode::onSetParam, this, _1)); // Node Parameter - node_param_.update_rate_hz = declare_parameter("node_params.update_rate_hz", 10.0); + node_param_.update_rate_hz = declare_parameter("node_params.update_rate_hz"); // Core Parameter - core_param_.bounding_box_margin = - declare_parameter("core_params.bounding_box_margin", 0.5); + core_param_.bounding_box_margin = declare_parameter("core_params.bounding_box_margin"); core_param_.split_threshold_velocity = - declare_parameter("core_params.split_threshold_velocity", 0.0); - core_param_.threshold_yaw_diff = - declare_parameter("core_params.threshold_yaw_diff", 0.35); + declare_parameter("core_params.split_threshold_velocity"); + core_param_.threshold_yaw_diff = declare_parameter("core_params.threshold_yaw_diff"); core_param_.velocity_weight_min_distance = - declare_parameter("core_params.velocity_weight_min_distance", 1.0); + declare_parameter("core_params.velocity_weight_min_distance"); core_param_.velocity_weight_average = - declare_parameter("core_params.velocity_weight_average", 0.0); + declare_parameter("core_params.velocity_weight_average"); core_param_.velocity_weight_median = - declare_parameter("core_params.velocity_weight_median", 0.0); + declare_parameter("core_params.velocity_weight_median"); core_param_.velocity_weight_target_value_average = - declare_parameter("core_params.velocity_weight_target_value_average", 0.0); + declare_parameter("core_params.velocity_weight_target_value_average"); core_param_.velocity_weight_target_value_top = - declare_parameter("core_params.velocity_weight_target_value_top", 1.0); + declare_parameter("core_params.velocity_weight_target_value_top"); core_param_.convert_doppler_to_twist = - declare_parameter("core_params.convert_doppler_to_twist", false); + declare_parameter("core_params.convert_doppler_to_twist"); core_param_.threshold_probability = - declare_parameter("core_params.threshold_probability", 0.0); + declare_parameter("core_params.threshold_probability"); core_param_.compensate_probability = - declare_parameter("core_params.compensate_probability", false); + declare_parameter("core_params.compensate_probability"); // Core radar_fusion_to_detected_object_ = std::make_unique(get_logger()); diff --git a/perception/radar_object_clustering/include/radar_object_clustering/radar_object_clustering_node.hpp b/perception/radar_object_clustering/include/radar_object_clustering/radar_object_clustering_node.hpp index 0e82c5388d297..c762996f8e0a7 100644 --- a/perception/radar_object_clustering/include/radar_object_clustering/radar_object_clustering_node.hpp +++ b/perception/radar_object_clustering/include/radar_object_clustering/radar_object_clustering_node.hpp @@ -16,7 +16,6 @@ #define RADAR_OBJECT_CLUSTERING__RADAR_OBJECT_CLUSTERING_NODE_HPP_ #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include "autoware_auto_perception_msgs/msg/detected_objects.hpp" diff --git a/perception/radar_object_clustering/src/radar_object_clustering_node/radar_object_clustering_node.cpp b/perception/radar_object_clustering/src/radar_object_clustering_node/radar_object_clustering_node.cpp index 7f9879fc357a5..74e85bde21385 100644 --- a/perception/radar_object_clustering/src/radar_object_clustering_node/radar_object_clustering_node.cpp +++ b/perception/radar_object_clustering/src/radar_object_clustering_node/radar_object_clustering_node.cpp @@ -15,6 +15,8 @@ #include "radar_object_clustering/radar_object_clustering_node.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/math/unit_conversion.hpp" #include diff --git a/perception/radar_object_tracker/README.md b/perception/radar_object_tracker/README.md index 9f11d50ce2b3f..739a4a745ff6c 100644 --- a/perception/radar_object_tracker/README.md +++ b/perception/radar_object_tracker/README.md @@ -26,9 +26,10 @@ See more details in the [models.md](models.md). ### Input -| Name | Type | Description | -| --------- | ----------------------------------------------------- | ---------------- | -| `~/input` | `autoware_auto_perception_msgs::msg::DetectedObjects` | Detected objects | +| Name | Type | Description | +| ------------- | ----------------------------------------------------- | ---------------- | +| `~/input` | `autoware_auto_perception_msgs::msg::DetectedObjects` | Detected objects | +| `/vector/map` | `autoware_auto_msgs::msg::HADMapBin` | Map data | ### Output @@ -40,20 +41,29 @@ See more details in the [models.md](models.md). ### Node Parameters -| Name | Type | Default Value | Description | -| --------------------------- | ------ | ----------------------------- | ----------------------------------------------------------- | -| `publish_rate` | double | 30.0 | The rate at which to publish the output messages | -| `world_frame_id` | string | "world" | The frame ID of the world coordinate system | -| `enable_delay_compensation` | bool | false | Whether to enable delay compensation | -| `tracking_config_directory` | string | "" | The directory containing the tracking configuration files | -| `enable_logging` | bool | false | Whether to enable logging | -| `logging_file_path` | string | "~/.ros/association_log.json" | The path to the file where logs should be written | -| `can_assign_matrix` | array | | An array of integers used in the data association algorithm | -| `max_dist_matrix` | array | | An array of doubles used in the data association algorithm | -| `max_area_matrix` | array | | An array of doubles used in the data association algorithm | -| `min_area_matrix` | array | | An array of doubles used in the data association algorithm | -| `max_rad_matrix` | array | | An array of doubles used in the data association algorithm | -| `min_iou_matrix` | array | | An array of doubles used in the data association algorithm | +| Name | Type | Default Value | Description | +| ------------------------------------ | ------ | --------------------------- | --------------------------------------------------------------------------------------------------------------- | +| `publish_rate` | double | 10.0 | The rate at which to publish the output messages | +| `world_frame_id` | string | "map" | The frame ID of the world coordinate system | +| `enable_delay_compensation` | bool | false | Whether to enable delay compensation. If set to `true`, output topic is published by timer with `publish_rate`. | +| `tracking_config_directory` | string | "" | The directory containing the tracking configuration files | +| `enable_logging` | bool | false | Whether to enable logging | +| `logging_file_path` | string | "/tmp/association_log.json" | The path to the file where logs should be written | +| `tracker_lifetime` | double | 1.0 | The lifetime of the tracker in seconds | +| `use_distance_based_noise_filtering` | bool | true | Whether to use distance based filtering | +| `minimum_range_threshold` | double | 70.0 | Minimum distance threshold for filtering in meters | +| `use_map_based_noise_filtering` | bool | true | Whether to use map based filtering | +| `max_distance_from_lane` | double | 5.0 | Maximum distance from lane for filtering in meters | +| `max_angle_diff_from_lane` | double | 0.785398 | Maximum angle difference from lane for filtering in radians | +| `max_lateral_velocity` | double | 5.0 | Maximum lateral velocity for filtering in m/s | +| `can_assign_matrix` | array | | An array of integers used in the data association algorithm | +| `max_dist_matrix` | array | | An array of doubles used in the data association algorithm | +| `max_area_matrix` | array | | An array of doubles used in the data association algorithm | +| `min_area_matrix` | array | | An array of doubles used in the data association algorithm | +| `max_rad_matrix` | array | | An array of doubles used in the data association algorithm | +| `min_iou_matrix` | array | | An array of doubles used in the data association algorithm | + +See more details in the [models.md](models.md). ## Assumptions / Known limits diff --git a/perception/radar_object_tracker/config/radar_object_tracker.param.yaml b/perception/radar_object_tracker/config/radar_object_tracker.param.yaml new file mode 100644 index 0000000000000..f80adffb41090 --- /dev/null +++ b/perception/radar_object_tracker/config/radar_object_tracker.param.yaml @@ -0,0 +1,27 @@ +# general parameters for radar_object_tracker node +/**: + ros__parameters: + # basic settings + world_frame_id: "map" + tracker_lifetime: 1.0 # [sec] + # if empty, use default config declared in this package + tracking_config_directory: "" + + # delay compensate parameters + publish_rate: 10.0 + enable_delay_compensation: false + + # logging + enable_logging: false + logging_file_path: "/tmp/association_log.json" + + # filtering + ## 1. distance based filtering: remove closer objects than this threshold + use_distance_based_noise_filtering: true + minimum_range_threshold: 70.0 # [m] + + ## 2. lanelet map based filtering + use_map_based_noise_filtering: true + max_distance_from_lane: 5.0 # [m] + max_angle_diff_from_lane: 0.785398 # [rad] (45 deg) + max_lateral_velocity: 5.0 # [m/s] diff --git a/perception/radar_object_tracker/image/noise_filtering.drawio.svg b/perception/radar_object_tracker/image/noise_filtering.drawio.svg new file mode 100644 index 0000000000000..cd044cdfa1379 --- /dev/null +++ b/perception/radar_object_tracker/image/noise_filtering.drawio.svg @@ -0,0 +1,165 @@ + + + + + + + + + + + +
+
+
self vehicle
+
+
+ + self vehicle + + + + + + + +
+
+
removed
+
+
+ + removed + + + + + + + +
+
+
distance threshold
+
+
+ + distance threshold + + + + + + + +
+
+
+ filtered object +
+
+
+ + filtered object + + + + + + + + + + + +
+
+
removed
+
+
+ + removed + + + + + + + + + +
+
+
+ lane distance +
+ threshold +
+
+
+ + lane distance... + + + + + + + + + + + +
+
+
+ removed +
+ with +
+ angle threshold +
+
+
+ + removed... + + + + + + + + + +
+
+
+ removed by +
+ high +
+ lateral velocity +
+
+
+ + removed by... + + + + + + + Text is not SVG - cannot display + + + diff --git a/perception/radar_object_tracker/include/radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp b/perception/radar_object_tracker/include/radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp index 898a7855aabae..415ff24b34cc3 100644 --- a/perception/radar_object_tracker/include/radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp +++ b/perception/radar_object_tracker/include/radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp @@ -33,6 +33,16 @@ #include #endif +#include +#include +#include + +#include +#include +#include +#include +#include +#include #include #include @@ -41,8 +51,11 @@ #include #include +using autoware_auto_mapping_msgs::msg::HADMapBin; using autoware_auto_perception_msgs::msg::DetectedObject; using autoware_auto_perception_msgs::msg::DetectedObjects; +using autoware_auto_perception_msgs::msg::TrackedObject; +using autoware_auto_perception_msgs::msg::TrackedObjects; class RadarObjectTrackerNode : public rclcpp::Node { @@ -50,20 +63,23 @@ class RadarObjectTrackerNode : public rclcpp::Node explicit RadarObjectTrackerNode(const rclcpp::NodeOptions & node_options); private: + // pub-sub rclcpp::Publisher::SharedPtr tracked_objects_pub_; rclcpp::Subscription::SharedPtr detected_object_sub_; - rclcpp::TimerBase::SharedPtr publish_timer_; // publish timer + rclcpp::TimerBase::SharedPtr publish_timer_; // publish timer + rclcpp::Subscription::SharedPtr sub_map_; // map subscriber tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; - + float tracker_lifetime_; std::map tracker_map_; void onMeasurement( const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg); void onTimer(); + void onMap(const HADMapBin::ConstSharedPtr map_msg); std::string world_frame_id_; // tracking frame std::string tracker_config_directory_; @@ -77,11 +93,36 @@ class RadarObjectTrackerNode : public rclcpp::Node std::string path; } logging_; + // noise reduction + bool use_distance_based_noise_filtering_; + bool use_map_based_noise_filtering_; + + // distance based noise reduction + double minimum_range_threshold_; + std::string sensor_frame_ = "base_link"; + + // map based noise reduction + bool map_is_loaded_ = false; + double max_distance_from_lane_; + double max_lateral_velocity_; + double max_angle_diff_from_lane_; + // Lanelet Map Pointers + std::shared_ptr lanelet_map_ptr_; + std::shared_ptr routing_graph_ptr_; + std::shared_ptr traffic_rules_ptr_; + // Crosswalk Entry Points + // lanelet::ConstLanelets crosswalks_; + void checkTrackerLifeCycle( std::list> & list_tracker, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform); void sanitizeTracker( std::list> & list_tracker, const rclcpp::Time & time); + void mapBasedNoiseFilter( + std::list> & list_tracker, const rclcpp::Time & time); + void distanceBasedNoiseFilter( + std::list> & list_tracker, const rclcpp::Time & time, + const geometry_msgs::msg::Transform & self_transform); std::shared_ptr createNewTracker( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) const; diff --git a/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml b/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml index 7ef3e58a64161..e2d30c1b69d19 100644 --- a/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml +++ b/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml @@ -7,16 +7,18 @@ + + + + + + - - - - diff --git a/perception/radar_object_tracker/models.md b/perception/radar_object_tracker/models.md index a4c53db156f0f..f4e4c6a02edaf 100644 --- a/perception/radar_object_tracker/models.md +++ b/perception/radar_object_tracker/models.md @@ -74,3 +74,33 @@ v_{k+1} &= v_k \\ \omega_{k+1} &= \omega_k \end{align} $$ + +## Noise filtering + +Radar sensors often have noisy measurement. So we use the following filter to reduce the false positive objects. + +The figure below shows the current noise filtering process. + +![noise_filter](image/noise_filtering.drawio.svg) + +### minimum range filter + +In most cases, Radar sensors are used with other sensors such as LiDAR and Camera, and Radar sensors are used to detect objects far away. So we can filter out objects that are too close to the sensor. + +`use_distance_based_noise_filtering` parameter is used to enable/disable this filter, and `minimum_range_threshold` parameter is used to set the threshold. + +### lanelet based filter + +With lanelet map information, We can filter out false positive objects that are not likely important obstacles. + +We filter out objects that satisfy the following conditions: + +- too large lateral distance from lane +- velocity direction is too different from lane direction +- too large lateral velocity + +Each condition can be set by the following parameters: + +- `max_distance_from_lane` +- `max_angle_diff_from_lane` +- `max_lateral_velocity` diff --git a/perception/radar_object_tracker/package.xml b/perception/radar_object_tracker/package.xml index 3ff6feb617baa..ad71e613c1a18 100644 --- a/perception/radar_object_tracker/package.xml +++ b/perception/radar_object_tracker/package.xml @@ -16,6 +16,7 @@ autoware_auto_perception_msgs eigen kalman_filter + lanelet2_extension mussp nlohmann-json-dev object_recognition_utils diff --git a/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp b/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp index f32084493f34e..6b02836e1f933 100644 --- a/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp +++ b/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp @@ -60,6 +60,131 @@ boost::optional getTransformAnonymous( } } +// check if lanelet is close enough to the target lanelet +bool isDuplicated( + const std::pair & target_lanelet, + const lanelet::ConstLanelets & lanelets) +{ + const double CLOSE_LANELET_THRESHOLD = 0.1; + for (const auto & lanelet : lanelets) { + const auto target_lanelet_end_p = target_lanelet.second.centerline2d().back(); + const auto lanelet_end_p = lanelet.centerline2d().back(); + const double dist = std::hypot( + target_lanelet_end_p.x() - lanelet_end_p.x(), target_lanelet_end_p.y() - lanelet_end_p.y()); + if (dist < CLOSE_LANELET_THRESHOLD) { + return true; + } + } + + return false; +} + +// check if the lanelet is valid for object tracking +bool checkCloseLaneletCondition( + const std::pair & lanelet, const TrackedObject & object, + const double max_distance_from_lane, const double max_angle_diff_from_lane) +{ + // Step1. If we only have one point in the centerline, we will ignore the lanelet + if (lanelet.second.centerline().size() <= 1) { + return false; + } + + // Step2. Check if the obstacle is inside or close enough to the lanelet + lanelet::BasicPoint2d search_point( + object.kinematics.pose_with_covariance.pose.position.x, + object.kinematics.pose_with_covariance.pose.position.y); + if (!lanelet::geometry::inside(lanelet.second, search_point)) { + const auto distance = lanelet.first; + if (distance > max_distance_from_lane) { + return false; + } + } + + // Step3. Calculate the angle difference between the lane angle and obstacle angle + const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const double lane_yaw = lanelet::utils::getLaneletAngle( + lanelet.second, object.kinematics.pose_with_covariance.pose.position); + const double delta_yaw = object_yaw - lane_yaw; + const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw); + const double abs_norm_delta = std::fabs(normalized_delta_yaw); + + // Step4. Check if the closest lanelet is valid, and add all + // of the lanelets that are below max_dist and max_delta_yaw + const double object_vel = object.kinematics.twist_with_covariance.twist.linear.x; + const bool is_yaw_reversed = M_PI - max_angle_diff_from_lane < abs_norm_delta && object_vel < 0.0; + if (is_yaw_reversed || abs_norm_delta < max_angle_diff_from_lane) { + return true; + } + + return false; +} + +lanelet::ConstLanelets getClosestValidLanelets( + const TrackedObject & object, const lanelet::LaneletMapPtr & lanelet_map_ptr, + const double max_distance_from_lane, const double max_angle_diff_from_lane) +{ + // obstacle point + lanelet::BasicPoint2d search_point( + object.kinematics.pose_with_covariance.pose.position.x, + object.kinematics.pose_with_covariance.pose.position.y); + + // nearest lanelet + std::vector> surrounding_lanelets = + lanelet::geometry::findNearest(lanelet_map_ptr->laneletLayer, search_point, 10); + + // No Closest Lanelets + if (surrounding_lanelets.empty()) { + return {}; + } + + lanelet::ConstLanelets closest_lanelets; + for (const auto & lanelet : surrounding_lanelets) { + // Check if the close lanelets meet the necessary condition for start lanelets and + // Check if similar lanelet is inside the closest lanelet + if ( + !checkCloseLaneletCondition( + lanelet, object, max_distance_from_lane, max_angle_diff_from_lane) || + isDuplicated(lanelet, closest_lanelets)) { + continue; + } + + closest_lanelets.push_back(lanelet.second); + } + + return closest_lanelets; +} + +bool hasValidVelocityDirectionToLanelet( + const TrackedObject & object, const lanelet::ConstLanelets & lanelets, + const double max_lateral_velocity) +{ + // get object velocity direction + const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const double object_vel_x = object.kinematics.twist_with_covariance.twist.linear.x; + const double object_vel_y = object.kinematics.twist_with_covariance.twist.linear.y; + const double object_vel_yaw = std::atan2(object_vel_y, object_vel_x); // local frame + const double object_vel_yaw_global = + tier4_autoware_utils::normalizeRadian(object_yaw + object_vel_yaw); + const double object_vel = std::hypot(object_vel_x, object_vel_y); + + for (const auto & lanelet : lanelets) { + // get lanelet angle + const double lane_yaw = lanelet::utils::getLaneletAngle( + lanelet, object.kinematics.pose_with_covariance.pose.position); + const double delta_yaw = object_vel_yaw_global - lane_yaw; + const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw); + + // get lateral velocity + const double lane_vel = object_vel * std::sin(normalized_delta_yaw); + // std::cout << "lane_vel: " << lane_vel << " , delta yaw:" << normalized_delta_yaw << " , + // object_vel " << object_vel <("output", rclcpp::QoS{1}); + sub_map_ = this->create_subscription( + "/vector_map", rclcpp::QoS{1}.transient_local(), + std::bind(&RadarObjectTrackerNode::onMap, this, std::placeholders::_1)); // Parameters - double publish_rate = declare_parameter("publish_rate", 30.0); - world_frame_id_ = declare_parameter("world_frame_id", "world"); - bool enable_delay_compensation{declare_parameter("enable_delay_compensation", false)}; - tracker_config_directory_ = declare_parameter("tracking_config_directory", ""); - logging_.enable = declare_parameter("enable_logging", false); - logging_.path = - declare_parameter("logging_file_path", "~/.ros/association_log.json"); + tracker_lifetime_ = declare_parameter("tracker_lifetime"); + double publish_rate = declare_parameter("publish_rate"); + world_frame_id_ = declare_parameter("world_frame_id"); + bool enable_delay_compensation{declare_parameter("enable_delay_compensation")}; + tracker_config_directory_ = declare_parameter("tracking_config_directory"); + logging_.enable = declare_parameter("enable_logging"); + logging_.path = declare_parameter("logging_file_path"); + + // noise filter + use_distance_based_noise_filtering_ = + declare_parameter("use_distance_based_noise_filtering"); + use_map_based_noise_filtering_ = declare_parameter("use_map_based_noise_filtering"); + minimum_range_threshold_ = declare_parameter("minimum_range_threshold"); + max_distance_from_lane_ = declare_parameter("max_distance_from_lane"); + max_angle_diff_from_lane_ = declare_parameter("max_angle_diff_from_lane"); + max_lateral_velocity_ = declare_parameter("max_lateral_velocity"); // Load tracking config file if (tracker_config_directory_.empty()) { @@ -132,6 +269,17 @@ RadarObjectTrackerNode::RadarObjectTrackerNode(const rclcpp::NodeOptions & node_ std::make_pair(Label::MOTORCYCLE, this->declare_parameter("motorcycle_tracker"))); } +// load map information to node parameter +void RadarObjectTrackerNode::onMap(const HADMapBin::ConstSharedPtr msg) +{ + RCLCPP_INFO(get_logger(), "[Radar Object Tracker]: Start loading lanelet"); + lanelet_map_ptr_ = std::make_shared(); + lanelet::utils::conversion::fromBinMsg( + *msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_); + RCLCPP_INFO(get_logger(), "[Radar Object Tracker]: Map is loaded"); + map_is_loaded_ = true; +} + void RadarObjectTrackerNode::onMeasurement( const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg) { @@ -176,6 +324,14 @@ void RadarObjectTrackerNode::onMeasurement( /* life cycle check */ checkTrackerLifeCycle(list_tracker_, measurement_time, *self_transform); + /* map based noise filter */ + if (map_is_loaded_ && use_map_based_noise_filtering_) { + mapBasedNoiseFilter(list_tracker_, measurement_time); + } + /* distance based noise filter */ + if (use_distance_based_noise_filtering_) { + distanceBasedNoiseFilter(list_tracker_, measurement_time, *self_transform); + } /* sanitize trackers */ sanitizeTracker(list_tracker_, measurement_time); @@ -227,6 +383,16 @@ void RadarObjectTrackerNode::onTimer() /* life cycle check */ checkTrackerLifeCycle(list_tracker_, current_time, *self_transform); + + /* map based noise filter */ + if (map_is_loaded_ && use_map_based_noise_filtering_) { + mapBasedNoiseFilter(list_tracker_, current_time); + } + /* distance based noise filter */ + if (use_distance_based_noise_filtering_) { + distanceBasedNoiseFilter(list_tracker_, current_time, *self_transform); + } + /* sanitize trackers */ sanitizeTracker(list_tracker_, current_time); @@ -238,12 +404,9 @@ void RadarObjectTrackerNode::checkTrackerLifeCycle( std::list> & list_tracker, const rclcpp::Time & time, [[maybe_unused]] const geometry_msgs::msg::Transform & self_transform) { - /* params */ - constexpr float max_elapsed_time = 1.0; - /* delete tracker */ for (auto itr = list_tracker.begin(); itr != list_tracker.end(); ++itr) { - const bool is_old = max_elapsed_time < (*itr)->getElapsedTimeFromLastUpdate(time); + const bool is_old = tracker_lifetime_ < (*itr)->getElapsedTimeFromLastUpdate(time); if (is_old) { auto erase_itr = itr; --itr; @@ -252,6 +415,51 @@ void RadarObjectTrackerNode::checkTrackerLifeCycle( } } +// remove objects by lanelet information +void RadarObjectTrackerNode::mapBasedNoiseFilter( + std::list> & list_tracker, const rclcpp::Time & time) +{ + for (auto itr = list_tracker.begin(); itr != list_tracker.end(); ++itr) { + autoware_auto_perception_msgs::msg::TrackedObject object; + (*itr)->getTrackedObject(time, object); + const auto closest_lanelets = getClosestValidLanelets( + object, lanelet_map_ptr_, max_distance_from_lane_, max_angle_diff_from_lane_); + + // 1. If the object is not close to any lanelet, delete the tracker + const bool no_closest_lanelet = closest_lanelets.empty(); + // 2. If the object velocity direction is not close to the lanelet direction, delete the tracker + const bool is_velocity_direction_close_to_lanelet = + hasValidVelocityDirectionToLanelet(object, closest_lanelets, max_lateral_velocity_); + if (no_closest_lanelet || !is_velocity_direction_close_to_lanelet) { + // std::cout << "object removed due to map based noise filter" << " no close lanelet: " << + // no_closest_lanelet << " velocity direction flag:" << is_velocity_direction_close_to_lanelet + // << std::endl; + itr = list_tracker.erase(itr); + --itr; + } + } +} + +// remove objects by distance +void RadarObjectTrackerNode::distanceBasedNoiseFilter( + std::list> & list_tracker, const rclcpp::Time & time, + const geometry_msgs::msg::Transform & self_transform) +{ + // remove objects that are too close + for (auto itr = list_tracker.begin(); itr != list_tracker.end(); ++itr) { + autoware_auto_perception_msgs::msg::TrackedObject object; + (*itr)->getTrackedObject(time, object); + const double distance = std::hypot( + object.kinematics.pose_with_covariance.pose.position.x - self_transform.translation.x, + object.kinematics.pose_with_covariance.pose.position.y - self_transform.translation.y); + if (distance < minimum_range_threshold_) { + // std::cout << "object removed due to small distance. distance: " << distance << std::endl; + itr = list_tracker.erase(itr); + --itr; + } + } +} + void RadarObjectTrackerNode::sanitizeTracker( std::list> & list_tracker, const rclcpp::Time & time) { diff --git a/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp b/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp index 62ebb2ec0146b..351410161d8b2 100644 --- a/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp +++ b/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp @@ -16,7 +16,7 @@ #define RADAR_TRACKS_MSGS_CONVERTER__RADAR_TRACKS_MSGS_CONVERTER_NODE_HPP_ #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/ros/transform_listener.hpp" #include "autoware_auto_perception_msgs/msg/detected_objects.hpp" #include "autoware_auto_perception_msgs/msg/object_classification.hpp" diff --git a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp index ab5e4eb5abe90..75556fbb0c372 100644 --- a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp +++ b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp @@ -14,7 +14,9 @@ #include "radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/math/unit_conversion.hpp" +#include "tier4_autoware_utils/ros/msg_covariance.hpp" #include diff --git a/perception/shape_estimation/include/shape_estimation/corrector/utils.hpp b/perception/shape_estimation/include/shape_estimation/corrector/utils.hpp index b932c67133850..60ccb0b4ef02c 100644 --- a/perception/shape_estimation/include/shape_estimation/corrector/utils.hpp +++ b/perception/shape_estimation/include/shape_estimation/corrector/utils.hpp @@ -17,8 +17,6 @@ #include "shape_estimation/shape_estimator.hpp" -#include - #include #include diff --git a/perception/shape_estimation/include/shape_estimation/model/model_interface.hpp b/perception/shape_estimation/include/shape_estimation/model/model_interface.hpp index 7f9baa9a0f5e0..ccd04bae40698 100644 --- a/perception/shape_estimation/include/shape_estimation/model/model_interface.hpp +++ b/perception/shape_estimation/include/shape_estimation/model/model_interface.hpp @@ -15,8 +15,6 @@ #ifndef SHAPE_ESTIMATION__MODEL__MODEL_INTERFACE_HPP_ #define SHAPE_ESTIMATION__MODEL__MODEL_INTERFACE_HPP_ -#include - #include #include diff --git a/perception/shape_estimation/lib/corrector/utils.cpp b/perception/shape_estimation/lib/corrector/utils.cpp index e75dcc25f521a..e16f8bed8d36b 100644 --- a/perception/shape_estimation/lib/corrector/utils.cpp +++ b/perception/shape_estimation/lib/corrector/utils.cpp @@ -14,10 +14,11 @@ #include "shape_estimation/corrector/utils.hpp" +#include + #include #include #include - #ifdef ROS_DISTRO_GALACTIC #include #include diff --git a/perception/shape_estimation/src/node.cpp b/perception/shape_estimation/src/node.cpp index 9be64112bc32d..987cf8106c99e 100644 --- a/perception/shape_estimation/src/node.cpp +++ b/perception/shape_estimation/src/node.cpp @@ -15,7 +15,7 @@ #include "shape_estimation/shape_estimator.hpp" #include -#include +#include #include diff --git a/perception/simple_object_merger/include/simple_object_merger/simple_object_merger_node.hpp b/perception/simple_object_merger/include/simple_object_merger/simple_object_merger_node.hpp index 6e2daf9266c9c..0d041439e6092 100644 --- a/perception/simple_object_merger/include/simple_object_merger/simple_object_merger_node.hpp +++ b/perception/simple_object_merger/include/simple_object_merger/simple_object_merger_node.hpp @@ -16,7 +16,7 @@ #define SIMPLE_OBJECT_MERGER__SIMPLE_OBJECT_MERGER_NODE_HPP_ #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/ros/transform_listener.hpp" #include "autoware_auto_perception_msgs/msg/detected_objects.hpp" diff --git a/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp b/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp index 506a40e672fb6..73a96da454833 100644 --- a/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp +++ b/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp @@ -14,6 +14,8 @@ #include "simple_object_merger/simple_object_merger_node.hpp" +#include + #include #include #include diff --git a/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp b/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp index 3baefaa43163a..3838e93113066 100644 --- a/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp +++ b/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp @@ -16,6 +16,7 @@ #include #include +#include #include #include @@ -111,7 +112,8 @@ void TrafficLightArbiter::arbitrateAndPublish(const builtin_interfaces::msg::Tim std::unordered_map> regulatory_element_signals_map; if (map_regulatory_elements_set_ == nullptr) { - RCLCPP_WARN(get_logger(), "Received traffic signal messages before a map"); + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 5000, "Received traffic signal messages before a map"); return; } @@ -126,8 +128,9 @@ void TrafficLightArbiter::arbitrateAndPublish(const builtin_interfaces::msg::Tim auto add_signal_function = [&](const auto & signal, bool priority) { const auto id = signal.traffic_signal_id; if (!map_regulatory_elements_set_->count(id)) { - RCLCPP_WARN( - get_logger(), "Received a traffic signal not present in the current map (%lu)", id); + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 5000, + "Received a traffic signal not present in the current map (%lu)", id); return; } diff --git a/perception/traffic_light_fine_detector/package.xml b/perception/traffic_light_fine_detector/package.xml index c54912e256c02..8ff9f2d133b52 100644 --- a/perception/traffic_light_fine_detector/package.xml +++ b/perception/traffic_light_fine_detector/package.xml @@ -4,7 +4,7 @@ traffic_light_fine_detector 0.1.0 The traffic_light_fine_detector package - Mingyu Li + Tao Zhong Shunsuke Miura Apache License 2.0 diff --git a/perception/traffic_light_map_based_detector/package.xml b/perception/traffic_light_map_based_detector/package.xml index 1021757e99a07..c2d53869fa3d5 100644 --- a/perception/traffic_light_map_based_detector/package.xml +++ b/perception/traffic_light_map_based_detector/package.xml @@ -22,6 +22,7 @@ sensor_msgs tf2 tf2_eigen + tf2_geometry_msgs tf2_ros tier4_autoware_utils tier4_perception_msgs diff --git a/perception/traffic_light_map_based_detector/src/node.cpp b/perception/traffic_light_map_based_detector/src/node.cpp index fcf13e3900793..f0a5d7b7b1fde 100644 --- a/perception/traffic_light_map_based_detector/src/node.cpp +++ b/perception/traffic_light_map_based_detector/src/node.cpp @@ -17,7 +17,8 @@ #include #include #include -#include +#include +#include #include #include @@ -30,6 +31,12 @@ #include #include +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + namespace { cv::Point2d calcRawImagePointFromPoint3D( diff --git a/perception/traffic_light_multi_camera_fusion/package.xml b/perception/traffic_light_multi_camera_fusion/package.xml index eb0858152e69e..b08ccfe668085 100644 --- a/perception/traffic_light_multi_camera_fusion/package.xml +++ b/perception/traffic_light_multi_camera_fusion/package.xml @@ -4,7 +4,7 @@ traffic_light_multi_camera_fusion 0.1.0 The traffic_light_multi_camera_fusion package - Mingyu Li + Tao Zhong Shunsuke Miura Apache License 2.0 diff --git a/perception/traffic_light_occlusion_predictor/package.xml b/perception/traffic_light_occlusion_predictor/package.xml index 0ae390c90b9a9..49a67d68057d9 100644 --- a/perception/traffic_light_occlusion_predictor/package.xml +++ b/perception/traffic_light_occlusion_predictor/package.xml @@ -4,7 +4,7 @@ traffic_light_occlusion_predictor 0.1.0 The traffic_light_occlusion_predictor package - Mingyu Li + Tao Zhong Shunsuke Miura Apache License 2.0 diff --git a/perception/traffic_light_occlusion_predictor/src/nodelet.cpp b/perception/traffic_light_occlusion_predictor/src/nodelet.cpp index 366820a725018..c460f6a623bc4 100644 --- a/perception/traffic_light_occlusion_predictor/src/nodelet.cpp +++ b/perception/traffic_light_occlusion_predictor/src/nodelet.cpp @@ -18,7 +18,6 @@ #include #include #include -#include #include #include diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index 0352096d02b2b..f62e418371401 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -32,6 +32,7 @@ ament_auto_add_library(behavior_path_planner_node SHARED src/utils/path_utils.cpp src/utils/path_safety_checker/safety_check.cpp src/utils/path_safety_checker/objects_filtering.cpp + src/utils/start_goal_planner_common/utils.cpp src/utils/avoidance/utils.cpp src/utils/lane_change/utils.cpp src/utils/side_shift/util.cpp diff --git a/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml b/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml index 1609cdbc60b7a..160ebdc180020 100644 --- a/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml +++ b/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml @@ -21,12 +21,14 @@ rear: 0.5 # [m] extra length to add to the rear of the dynamic object footprint left: 0.5 # [m] extra length to add to the left of the dynamic object footprint right: 0.5 # [m] extra length to add to the rear of the dynamic object footprint + path_preprocessing: + max_arc_length: 50.0 # [m] maximum arc length along the path where the ego footprint is projected (0.0 means no limit) + resample_interval: 2.0 # [m] fixed interval between resampled path points (0.0 means path points are directly used) expansion: method: polygon # method used to expand the drivable area. Either 'lanelet' or 'polygon'. # 'lanelet': add lanelets overlapped by the ego footprints # 'polygon': add polygons built around sections of the ego footprint that go out of the drivable area max_distance: 0.0 # [m] maximum distance by which the drivable area can be expended (0.0 means no limit) - max_path_arc_length: 50.0 # [m] maximum arc length along the path where the ego footprint is projected (0.0 means no limit) extra_arc_length: 0.5 # [m] extra expansion arc length around an ego footprint avoid_linestring: types: # linestring types in the lanelet maps that will not be crossed when expanding the drivable area diff --git a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml index 39d05a7fb761b..e77f7726015af 100644 --- a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml +++ b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml @@ -2,7 +2,6 @@ ros__parameters: goal_planner: # general params - minimum_request_length: 100.0 th_arrived_distance: 1.0 th_stopped_velocity: 0.01 th_stopped_time: 2.0 # It must be greater than the state_machine's. @@ -17,7 +16,7 @@ longitudinal_margin: 3.0 max_lateral_offset: 0.5 lateral_offset_interval: 0.25 - ignore_distance_from_lane_start: 15.0 + ignore_distance_from_lane_start: 0.0 margin_from_boundary: 0.5 # occupancy grid map @@ -36,6 +35,7 @@ # pull over pull_over: + minimum_request_length: 100.0 pull_over_velocity: 3.0 pull_over_minimum_velocity: 1.38 decide_path_distance: 10.0 @@ -104,6 +104,66 @@ neighbor_radius: 8.0 margin: 1.0 + stop_condition: + maximum_deceleration_for_stop: 1.0 + maximum_jerk_for_stop: 1.0 + path_safety_check: + # EgoPredictedPath + ego_predicted_path: + acceleration: 1.0 + time_horizon: 10.0 + time_resolution: 0.5 + min_slow_speed: 0.0 + delay_until_departure: 1.0 + target_velocity: 1.0 + # For target object filtering + target_filtering: + safety_check_time_horizon: 5.0 + safety_check_time_resolution: 1.0 + # detection range + object_check_forward_distance: 10.0 + object_check_backward_distance: 100.0 + ignore_object_velocity_threshold: 0.0 + # ObjectTypesToCheck + object_types_to_check: + check_car: true + check_truck: true + check_bus: true + check_trailer: true + check_bicycle: true + check_motorcycle: true + check_pedestrian: true + check_unknown: false + # ObjectLaneConfiguration + object_lane_configuration: + check_current_lane: true + check_right_side_lane: true + check_left_side_lane: true + check_shoulder_lane: true + check_other_lane: false + include_opposite_lane: false + invert_opposite_lane: false + check_all_predicted_path: true + use_all_predicted_path: true + use_predicted_path_outside_lanelet: false + + # For safety check + safety_check_params: + # safety check configuration + enable_safety_check: true + # collision check parameters + check_all_predicted_path: true + publish_debug_marker: false + rss_params: + rear_vehicle_reaction_time: 1.0 + rear_vehicle_safety_time_margin: 1.0 + lateral_distance_max_threshold: 1.0 + longitudinal_distance_min_threshold: 1.0 + longitudinal_velocity_delta_time: 1.0 + # temporary + backward_path_length: 30.0 + forward_path_length: 100.0 + # debug debug: print_debug_info: false diff --git a/planning/behavior_path_planner/config/scene_module_manager.param.yaml b/planning/behavior_path_planner/config/scene_module_manager.param.yaml index e89c5539a0965..9895d9b5e473f 100644 --- a/planning/behavior_path_planner/config/scene_module_manager.param.yaml +++ b/planning/behavior_path_planner/config/scene_module_manager.param.yaml @@ -8,6 +8,7 @@ enable_rtc: true enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: true + keep_last: false priority: 6 max_module_size: 1 @@ -16,6 +17,7 @@ enable_rtc: true enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: true + keep_last: false priority: 6 max_module_size: 1 @@ -24,6 +26,7 @@ enable_rtc: true enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: true + keep_last: false priority: 5 max_module_size: 1 @@ -32,6 +35,7 @@ enable_rtc: true enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: true + keep_last: false priority: 5 max_module_size: 1 @@ -40,6 +44,7 @@ enable_rtc: true enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false + keep_last: false priority: 0 max_module_size: 1 @@ -48,6 +53,7 @@ enable_rtc: true enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false + keep_last: false priority: 2 max_module_size: 1 @@ -56,6 +62,7 @@ enable_rtc: true enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false + keep_last: true priority: 1 max_module_size: 1 @@ -64,6 +71,7 @@ enable_rtc: true enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: false + keep_last: false priority: 4 max_module_size: 1 @@ -72,6 +80,7 @@ enable_rtc: true enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false + keep_last: false priority: 3 max_module_size: 1 @@ -80,5 +89,6 @@ enable_rtc: true enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: true + keep_last: false priority: 7 max_module_size: 1 diff --git a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml index bea78664c65a3..b62262423fa72 100644 --- a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml +++ b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml @@ -72,3 +72,63 @@ max_planning_time: 150.0 neighbor_radius: 8.0 margin: 1.0 + + stop_condition: + maximum_deceleration_for_stop: 1.0 + maximum_jerk_for_stop: 1.0 + path_safety_check: + # EgoPredictedPath + ego_predicted_path: + acceleration: 1.0 + time_horizon: 10.0 + time_resolution: 0.5 + min_slow_speed: 0.0 + delay_until_departure: 1.0 + target_velocity: 1.0 + # For target object filtering + target_filtering: + safety_check_time_horizon: 5.0 + safety_check_time_resolution: 1.0 + # detection range + object_check_forward_distance: 10.0 + object_check_backward_distance: 100.0 + ignore_object_velocity_threshold: 0.0 + # ObjectTypesToCheck + object_types_to_check: + check_car: true + check_truck: true + check_bus: true + check_trailer: true + check_bicycle: true + check_motorcycle: true + check_pedestrian: true + check_unknown: false + # ObjectLaneConfiguration + object_lane_configuration: + check_current_lane: true + check_right_side_lane: true + check_left_side_lane: true + check_shoulder_lane: true + check_other_lane: false + include_opposite_lane: false + invert_opposite_lane: false + check_all_predicted_path: true + use_all_predicted_path: true + use_predicted_path_outside_lanelet: false + + # For safety check + safety_check_params: + # safety check configuration + enable_safety_check: true + # collision check parameters + check_all_predicted_path: true + publish_debug_marker: false + rss_params: + rear_vehicle_reaction_time: 1.0 + rear_vehicle_safety_time_margin: 1.0 + lateral_distance_max_threshold: 1.0 + longitudinal_distance_min_threshold: 1.0 + longitudinal_velocity_delta_time: 1.0 + # temporary + backward_path_length: 30.0 + forward_path_length: 100.0 diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md index 693d610951bbb..da96567ed542a 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md @@ -192,7 +192,7 @@ The avoidance target should be limited to stationary objects (you should not avo Not only the length from the centerline, but also the length from the road shoulder is calculated and used for the filtering process. It calculates the ratio of _the actual length between the the object's center and the center line_ `shift_length` and _the maximum length the object can shift_ `shiftable_length`. $$ -l_D = l_a - \frac{width}{2} \\ +l_D = l_a - \frac{width}{2}, \\ ratio = \frac{l_d}{l_D} $$ @@ -201,7 +201,7 @@ $$ - $l_a$ : distance between centerline and most left boundary. - $width$ : object width -The closer the object is to the shoulder, the larger the value of $ratio$ (theoretical max value is 1.0), and it compares the value and `object_check_shiftable_ratio` to determine whether the object is a parked-car. +The closer the object is to the shoulder, the larger the value of $ratio$ (theoretical max value is 1.0), and it compares the value and `object_check_shiftable_ratio` to determine whether the object is a parked-car. If the road has no road shoulders, it uses `object_check_min_road_shoulder_width` as a road shoulder width virtually. ![fig2](../image/avoidance/parked-car-detection.svg) @@ -692,7 +692,7 @@ namespace: `avoidance.target_filtering.` | object_check_backward_distance | [m] | double | Backward distance to search the avoidance target. | 2.0 | | object_check_goal_distance | [m] | double | Backward distance to search the avoidance target. | 20.0 | | object_check_shiftable_ratio | [m] | double | Vehicles around the center line within this distance will be excluded from avoidance target. | 0.6 | -| object_check_min_road_shoulder_width | [m] | double | Vehicles around the center line within this distance will be excluded from avoidance target. | 0.5 | +| object_check_min_road_shoulder_width | [m] | double | Width considered as a road shoulder if the lane does not have a road shoulder target. | 0.5 | | object_last_seen_threshold | [s] | double | For the compensation of the detection lost. The object is registered once it is observed as an avoidance target. When the detection loses, the timer will start and the object will be un-registered when the time exceeds this limit. | 2.0 | ### Safety check parameters diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md index 99dadc959f5a5..4cc8910acf82b 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md @@ -86,8 +86,8 @@ Either one is activated when all conditions are met. ### fixed_goal_planner -- The distance between the goal and ego-vehicle is shorter than `minimum_request_length`. - Route is set with `allow_goal_modification=false` by default. +- ego-vehicle is in the same lane as the goal. @@ -95,7 +95,7 @@ Either one is activated when all conditions are met. #### pull over on road lane -- The distance between the goal and ego-vehicle is shorter than `minimum_request_length`. +- The distance between the goal and ego-vehicle is shorter than `pull_over_minimum_request_length`. - Route is set with `allow_goal_modification=true` . - We can set this option with [SetRoute](https://github.com/autowarefoundation/autoware_adapi_msgs/blob/main/autoware_adapi_v1_msgs/routing/srv/SetRoute.srv#L2) api service. - We support `2D Rough Goal Pose` with the key bind `r` in RViz, but in the future there will be a panel of tools to manipulate various Route API from RViz. @@ -105,7 +105,7 @@ Either one is activated when all conditions are met. #### pull over on shoulder lane -- The distance between the goal and ego-vehicle is shorter than `minimum_request_length`. +- The distance between the goal and ego-vehicle is shorter than `pull_over_minimum_request_length`. - Goal is set in the `road_shoulder`. @@ -118,17 +118,11 @@ Either one is activated when all conditions are met. ## General parameters for goal_planner -| Name | Unit | Type | Description | Default value | -| :------------------------- | :----- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| minimum_request_length | [m] | double | when the ego-vehicle approaches the goal by this distance or a safe distance to stop, the module is activated. | 100.0 | -| th_arrived_distance | [m] | double | distance threshold for arrival of path termination | 1.0 | -| th_stopped_velocity | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | -| th_stopped_time | [s] | double | time threshold for arrival of path termination | 2.0 | -| pull_over_velocity | [m/s] | double | decelerate to this speed by the goal search area | 3.0 | -| pull_over_minimum_velocity | [m/s] | double | speed of pull_over after stopping once. this prevents excessive acceleration. | 1.38 | -| margin_from_boundary | [m] | double | distance margin from edge of the shoulder lane | 0.5 | -| decide_path_distance | [m] | double | decide path if it approaches this distance relative to the parking position. after that, no path planning and goal search are performed | 10.0 | -| maximum_deceleration | [m/s2] | double | maximum deceleration. it prevents sudden deceleration when a parking path cannot be found suddenly | 1.0 | +| Name | Unit | Type | Description | Default value | +| :------------------ | :---- | :----- | :------------------------------------------------- | :------------ | +| th_arrived_distance | [m] | double | distance threshold for arrival of path termination | 1.0 | +| th_stopped_velocity | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | +| th_stopped_time | [s] | double | time threshold for arrival of path termination | 2.0 | ## **collision check** @@ -150,11 +144,11 @@ Generate footprints from ego-vehicle path points and determine obstacle collisio #### Parameters for object recognition based collision check -| Name | Unit | Type | Description | Default value | -| :----------------------------------------------------------- | :--- | :----- | :--------------------------------------------------------- | :------------ | ---------------------------------------------------------------------------------------------------------- | -| use_object_recognition | [-] | bool | flag whether to use object recognition for collision check | true | -| object_recognition_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 0.6 | -| object_recognition_collision_check_max_extra_stopping_margin | [m] | double | | 1.0 | 　maximum value when adding longitudinal distance margin for collision check considering stopping distance | +| Name | Unit | Type | Description | Default value | +| :----------------------------------------------------------- | :--- | :----- | :------------------------------------------------------------------------------------------------------- | :------------ | +| use_object_recognition | [-] | bool | flag whether to use object recognition for collision check | true | +| object_recognition_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 0.6 | +| object_recognition_collision_check_max_extra_stopping_margin | [m] | double | maximum value when adding longitudinal distance margin for collision check considering stopping distance | 1.0 | ## **Goal Search** @@ -174,12 +168,22 @@ searched for in certain range of the shoulder lane. | longitudinal_margin | [m] | double | margin between ego-vehicle at the goal position and obstacles | 3.0 | | max_lateral_offset | [m] | double | maximum offset of goal search in the lateral direction | 0.5 | | lateral_offset_interval | [m] | double | distance interval of goal search in the lateral direction | 0.25 | -| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 15.0 | +| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | +| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | +| margin_from_boundary | [m] | double | distance margin from edge of the shoulder lane | 0.5 | -## **Path Generation** +## **Pull Over** There are three path generation methods. -The path is generated with a certain margin (default: `0.5 m`) from left boundary of shoulder lane. +The path is generated with a certain margin (default: `0.5 m`) from the boundary of shoulder lane. + +| Name | Unit | Type | Description | Default value | +| :------------------------------- | :----- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| pull_over_minimum_request_length | [m] | double | when the ego-vehicle approaches the goal by this distance or a safe distance to stop, pull over is activated. | 100.0 | +| pull_over_velocity | [m/s] | double | decelerate to this speed by the goal search area | 3.0 | +| pull_over_minimum_velocity | [m/s] | double | speed of pull_over after stopping once. this prevents excessive acceleration. | 1.38 | +| decide_path_distance | [m] | double | decide path if it approaches this distance relative to the parking position. after that, no path planning and goal search are performed | 10.0 | +| maximum_deceleration | [m/s2] | double | maximum deceleration. it prevents sudden deceleration when a parking path cannot be found suddenly | 1.0 | ### **shift parking** diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md index fddd535931d94..2fd1ff1cfd376 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md @@ -55,12 +55,12 @@ where `common_param` is vehicle common parameter, which defines vehicle common m The `longitudinal_acceleration_resolution` is determine by the following ```C++ -longitudinal_acceleration_resolution = (maximum_longitudinal_acceleration - minimum_longitudinal_deceleration) / longitudinal_acceleration_sampling_num +longitudinal_acceleration_resolution = (maximum_longitudinal_acceleration - minimum_longitudinal_acceleration) / longitudinal_acceleration_sampling_num ``` Note that when the `current_velocity` is lower than `minimum_lane_changing_velocity`, the vehicle needs to accelerate its velocity to `minimum_lane_changing_velocity`. Therefore, longitudinal acceleration becomes positive value (not decelerate). -The following figure illustrates when `lane_change_sampling_num = 4`. Assuming that `maximum_deceleration = 1.0` then `a0 == 0.0 == no deceleration`, `a1 == 0.25`, `a2 == 0.5`, `a3 == 0.75` and `a4 == 1.0 == maximum_deceleration`. `a0` is the expected lane change trajectories should ego vehicle do not decelerate, and `a1`'s path is the expected lane change trajectories should ego vehicle decelerate at `0.25 m/s^2`. +The following figure illustrates when `longitudinal_acceleration_sampling_num = 4`. Assuming that `maximum_deceleration = 1.0` then `a0 == 0.0 == no deceleration`, `a1 == 0.25`, `a2 == 0.5`, `a3 == 0.75` and `a4 == 1.0 == maximum_deceleration`. `a0` is the expected lane change trajectories should ego vehicle do not decelerate, and `a1`'s path is the expected lane change trajectories should ego vehicle decelerate at `0.25 m/s^2`. ![path_samples](../image/lane_change/lane_change-candidate_path_samples.png) @@ -79,12 +79,12 @@ The maximum and minimum lateral accelerations are defined in the lane change par | 4.0 | 0.3 | 0.4 | | 6.0 | 0.3 | 0.5 | -In this case, when the current velocity of the ego vehicle is 3.0, the minimum and maximum lateral accelerations are 0.2 and 0.35 respectively. These values are obtained by linearly interpolating the second and third rows of the map, which provide the minimum and maximum lateral acceleration values. +In this case, when the current velocity of the ego vehicle is 3.0, the minimum and maximum lateral accelerations are 0.25 and 0.4 respectively. These values are obtained by linearly interpolating the second and third rows of the map, which provide the minimum and maximum lateral acceleration values. Within this range, we sample the lateral acceleration for the ego vehicle. Similar to the method used for sampling longitudinal acceleration, the resolution of lateral acceleration (lateral_acceleration_resolution) is determined by the following: ```C++ -lateral_acceleration_resolution = (maximum_lateral_acceleration - minimum_lateral_deceleration) / lateral_acceleration_sampling_num +lateral_acceleration_resolution = (maximum_lateral_acceleration - minimum_lateral_acceleration) / lateral_acceleration_sampling_num ``` #### Candidate Path's validity check @@ -163,7 +163,7 @@ First, we divide the target objects into obstacles in the target lane, obstacles ![object lanes](../image/lane_change/lane_objects.drawio.svg) -Furthermore, to change lanes behind a vehicle waiting at a traffic light, we skip the safety check for the stopping vehicles near the traffic light. +Furthermore, to change lanes behind a vehicle waiting at a traffic light, we skip the safety check for the stopping vehicles near the traffic light.　The explanation for parked car detection is written in [documentation for avoidance module](../docs/behavior_path_planner_avoidance_design.md). ##### Collision check in prepare phase @@ -276,11 +276,10 @@ The following parameters are configurable in `lane_change.param.yaml`. | `prediction_time_resolution` | [s] | double | Time resolution for object's path interpolation and collision check. | 0.5 | | `longitudinal_acceleration_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by longitudinal acceleration | 5 | | `lateral_acceleration_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by lateral acceleration | 3 | -| `object_check_min_road_shoulder_width` | [m] | double | Vehicles around the center line within this distance will be excluded from parking objects | 0.5 | +| `object_check_min_road_shoulder_width` | [m] | double | Width considered as a road shoulder if the lane does not have a road shoulder | 0.5 | | `object_shiftable_ratio_threshold` | [-] | double | Vehicles around the center line within this distance ratio will be excluded from parking objects | 0.6 | | `min_length_for_turn_signal_activation` | [m] | double | Turn signal will be activated if the ego vehicle approaches to this length from minimum lane change length | 10.0 | | `length_ratio_for_turn_signal_deactivation` | [-] | double | Turn signal will be deactivated if the ego vehicle approaches to this length ratio for lane change finish point | 0.8 | -| `object_shiftable_ratio_threshold` | [-] | double | Vehicles around the center line within this distance ratio will be excluded from parking objects | 0.6 | | `max_longitudinal_acc` | [-] | double | maximum longitudinal acceleration for lane change | 1.0 | | `min_longitudinal_acc` | [-] | double | maximum longitudinal deceleration for lane change | -1.0 | | `lateral_acceleration.velocity` | [m/s] | double | Reference velocity for lateral acceleration calculation (look up table) | [0.0, 4.0, 10.0] | diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp index 5b252cef92714..78172c6fc0750 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp @@ -18,6 +18,7 @@ #include "behavior_path_planner/parameters.hpp" #include "behavior_path_planner/turn_signal_decider.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include #include @@ -29,6 +30,7 @@ #include #include #include +#include #include #include #include @@ -148,6 +150,7 @@ struct PlannerData BehaviorPathPlannerParameters parameters{}; drivable_area_expansion::DrivableAreaExpansionParameters drivable_area_expansion_parameters{}; + mutable std::optional drivable_area_expansion_prev_crop_pose; mutable TurnSignalDecider turn_signal_decider; TurnIndicatorsCommand getTurnSignal( diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp index ac24591a0a283..d308f55799ce0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp @@ -59,8 +59,6 @@ MarkerArray createTargetObjectsMarkerArray(const ObjectDataArray & objects, cons MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const std::string & ns); -MarkerArray createUnsafeObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns); - MarkerArray makeOverhangToRoadShoulderMarkerArray( const behavior_path_planner::ObjectDataArray & objects, std::string && ns); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp index 808e76a140761..d337bae17ca6c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp @@ -26,12 +26,8 @@ namespace marker_utils::lane_change_markers { using behavior_path_planner::LaneChangePath; using visualization_msgs::msg::MarkerArray; -MarkerArray showObjectInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); MarkerArray showAllValidLaneChangePath( const std::vector & lanes, std::string && ns); -MarkerArray showLerpedPose(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); -MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); -MarkerArray showPolygonPose(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); MarkerArray createLaneChangingVirtualWallMarker( const geometry_msgs::msg::Pose & lane_changing_pose, const std::string & module_name, const rclcpp::Time & now, const std::string & ns); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp index 657437615f809..c53735486efb0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp @@ -17,9 +17,8 @@ #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" -#include +#include #include #include @@ -104,6 +103,11 @@ MarkerArray createPredictedPathMarkerArray( const PredictedPath & ego_predicted_path, const vehicle_info_util::VehicleInfo & vehicle_info, std::string && ns, const int32_t & id, const float & r, const float & g, const float & b); +MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); + +MarkerArray showPredictedPath(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); + +MarkerArray showSafetyCheckInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); } // namespace marker_utils #endif // BEHAVIOR_PATH_PLANNER__MARKER_UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index d6b99dca70618..e3a3784c5e928 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -28,6 +28,7 @@ struct ModuleConfigParameters bool enable_rtc{false}; bool enable_simultaneous_execution_as_approved_module{false}; bool enable_simultaneous_execution_as_candidate_module{false}; + bool keep_last{false}; uint8_t priority{0}; uint8_t max_module_size{0}; }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp index 29857590d4eda..4bdfc807aa7c4 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp @@ -19,6 +19,7 @@ #include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp" #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/lane_following/module_data.hpp" +#include "tier4_autoware_utils/system/stop_watch.hpp" #include #include @@ -364,16 +365,10 @@ class PlannerManager * @param planner data. * @return root lanelet. */ - lanelet::ConstLanelet updateRootLanelet( - const std::shared_ptr & data, bool success_lane_change = false) const + lanelet::ConstLanelet updateRootLanelet(const std::shared_ptr & data) const { lanelet::ConstLanelet ret{}; - if (success_lane_change) { - data->route_handler->getClosestPreferredLaneletWithinRoute( - data->self_odometry->pose.pose, &ret); - } else { - data->route_handler->getClosestLaneletWithinRoute(data->self_odometry->pose.pose, &ret); - } + data->route_handler->getClosestLaneletWithinRoute(data->self_odometry->pose.pose, &ret); RCLCPP_DEBUG(logger_, "update start lanelet. id:%ld", ret.id()); return ret; } diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp index 3c302d934735c..fccc27051380e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp @@ -74,7 +74,6 @@ struct DynamicAvoidanceParameters double max_oncoming_crossing_object_angle{0.0}; // drivable area generation - std::string polygon_generation_method{}; double min_obj_path_based_lon_polygon_margin{0.0}; double lat_offset_from_obstacle{0.0}; double max_lat_offset_to_avoid{0.0}; @@ -94,6 +93,10 @@ struct DynamicAvoidanceParameters class DynamicAvoidanceModule : public SceneModuleInterface { public: + enum class PolygonGenerationMethod { + EGO_PATH_BASE = 0, + OBJECT_PATH_BASE, + }; struct DynamicAvoidanceObject { DynamicAvoidanceObject( @@ -128,15 +131,18 @@ class DynamicAvoidanceModule : public SceneModuleInterface std::optional lat_offset_to_avoid{std::nullopt}; bool is_collision_left; bool should_be_avoided{false}; + PolygonGenerationMethod polygon_generation_method{PolygonGenerationMethod::OBJECT_PATH_BASE}; void update( const MinMaxValue & arg_lon_offset_to_avoid, const MinMaxValue & arg_lat_offset_to_avoid, - const bool arg_is_collision_left, const bool arg_should_be_avoided) + const bool arg_is_collision_left, const bool arg_should_be_avoided, + const PolygonGenerationMethod & arg_polygon_generation_method) { lon_offset_to_avoid = arg_lon_offset_to_avoid; lat_offset_to_avoid = arg_lat_offset_to_avoid; is_collision_left = arg_is_collision_left; should_be_avoided = arg_should_be_avoided; + polygon_generation_method = arg_polygon_generation_method; } }; @@ -230,11 +236,12 @@ class DynamicAvoidanceModule : public SceneModuleInterface void updateObject( const std::string & uuid, const MinMaxValue & lon_offset_to_avoid, const MinMaxValue & lat_offset_to_avoid, const bool is_collision_left, - const bool should_be_avoided) + const bool should_be_avoided, const PolygonGenerationMethod & polygon_generation_method) { if (object_map_.count(uuid) != 0) { object_map_.at(uuid).update( - lon_offset_to_avoid, lat_offset_to_avoid, is_collision_left, should_be_avoided); + lon_offset_to_avoid, lat_offset_to_avoid, is_collision_left, should_be_avoided, + polygon_generation_method); } } @@ -312,7 +319,8 @@ class DynamicAvoidanceModule : public SceneModuleInterface void updateTargetObjects(); bool willObjectCutIn( const std::vector & ego_path, const PredictedPath & predicted_path, - const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const; + const double obj_tangent_vel, const LatLonOffset & lat_lon_offset, + PolygonGenerationMethod & polygon_generation_method) const; DecisionWithReason willObjectCutOut( const double obj_tangent_vel, const double obj_normal_vel, const bool is_object_left, const std::optional & prev_object) const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index 8d9c8e147f532..a6f9565c08aae 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -24,6 +24,9 @@ #include "behavior_path_planner/utils/goal_planner/goal_searcher.hpp" #include "behavior_path_planner/utils/goal_planner/shift_pull_over.hpp" #include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp" +#include "behavior_path_planner/utils/utils.hpp" #include #include @@ -57,6 +60,12 @@ using freespace_planning_algorithms::PlannerCommonParam; using freespace_planning_algorithms::RRTStar; using freespace_planning_algorithms::RRTStarParam; +using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; +using behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; +using behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; +using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; + enum class PathType { NONE = 0, SHIFT, @@ -65,19 +74,23 @@ enum class PathType { FREESPACE, }; -struct PUllOverStatus +struct PullOverStatus { std::shared_ptr pull_over_path{}; std::shared_ptr lane_parking_pull_over_path{}; size_t current_path_idx{0}; bool require_increment_{true}; // if false, keep current path idx. std::shared_ptr prev_stop_path{nullptr}; - lanelet::ConstLanelets current_lanes{}; - lanelet::ConstLanelets pull_over_lanes{}; - std::vector lanes{}; // current + pull_over - bool has_decided_path{false}; - bool is_safe{false}; + std::shared_ptr prev_stop_path_after_approval{nullptr}; + // stop path after approval, stop path is not updated until safety is confirmed + lanelet::ConstLanelets current_lanes{}; // TODO(someone): explain + lanelet::ConstLanelets pull_over_lanes{}; // TODO(someone): explain + std::vector lanes{}; // current + pull_over + bool has_decided_path{false}; // if true, the path has is decided and safe against static objects + bool is_safe_static_objects{false}; // current path is safe against *static* objects + bool is_safe_dynamic_objects{false}; // current path is safe against *dynamic* objects bool prev_is_safe{false}; + bool prev_is_safe_dynamic_objects{false}; bool has_decided_velocity{false}; bool has_requested_approval{false}; bool is_ready{false}; @@ -106,6 +119,13 @@ class GoalPlannerModule : public SceneModuleInterface void updateModuleParams(const std::any & parameters) override { parameters_ = std::any_cast>(parameters); + if (parameters_->safety_check_params.enable_safety_check) { + ego_predicted_path_params_ = + std::make_shared(parameters_->ego_predicted_path_params); + objects_filtering_params_ = + std::make_shared(parameters_->objects_filtering_params); + safety_check_params_ = std::make_shared(parameters_->safety_check_params); + } } // TODO(someone): remove this, and use base class function @@ -134,10 +154,18 @@ class GoalPlannerModule : public SceneModuleInterface bool canTransitIdleToRunningState() override { return false; } - PUllOverStatus status_; + PullOverStatus status_; + + mutable StartGoalPlannerData goal_planner_data_; std::shared_ptr parameters_; + mutable std::shared_ptr ego_predicted_path_params_; + mutable std::shared_ptr objects_filtering_params_; + mutable std::shared_ptr safety_check_params_; + + vehicle_info_util::VehicleInfo vehicle_info_; + // planner std::vector> pull_over_planners_; std::unique_ptr freespace_planner_; @@ -177,7 +205,6 @@ class GoalPlannerModule : public SceneModuleInterface // for parking policy bool left_side_parking_{true}; - mutable bool allow_goal_modification_{false}; // need to be set in isExecutionRequested // pre-generate lane parking paths in a separate thread rclcpp::TimerBase::SharedPtr lane_parking_timer_; @@ -206,7 +233,18 @@ class GoalPlannerModule : public SceneModuleInterface const Pose & search_start_offset_pose, PathWithLaneId & path) const; PathWithLaneId generateStopPath(); PathWithLaneId generateFeasibleStopPath(); - boost::optional calcFeasibleDecelDistance(const double target_velocity) const; + + /** + * @brief Generate a stop point in the current path based on the vehicle's pose and constraints. + * + * This function generates a stop point in the current path. If no lanes are available or if + * stopping is not feasible due to constraints (maximum deceleration, maximum jerk), no stop point + * is inserted into the path. + * + * @return the modified path with the stop point inserted. If no feasible stop point can be + * determined, returns an empty optional. + */ + std::optional generateStopInsertedCurrentPath(); void keepStoppedWithCurrentPath(PathWithLaneId & path); double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const; @@ -254,6 +292,17 @@ class GoalPlannerModule : public SceneModuleInterface // output setter void setOutput(BehaviorModuleOutput & output); void setStopPath(BehaviorModuleOutput & output); + + /** + * @brief Sets a stop path in the current path based on safety conditions and previous paths. + * + * This function sets a stop path in the current path. Depending on whether the previous safety + * judgement against dynamic objects were safe or if a previous stop path existed, it either + * generates a new stop path or uses the previous stop path. + * + * @param output BehaviorModuleOutput + */ + void setStopPathFromCurrentPath(BehaviorModuleOutput & output); void setModifiedGoal(BehaviorModuleOutput & output) const; void setTurnSignalInfo(BehaviorModuleOutput & output) const; @@ -275,6 +324,14 @@ class GoalPlannerModule : public SceneModuleInterface // rtc std::pair calcDistanceToPathChange() const; + // safety check + void initializeSafetyCheckParameters(); + SafetyCheckParams createSafetyCheckParams() const; + void updateSafetyCheckTargetObjectsData( + const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane, + const std::vector & ego_predicted_path) const; + bool isSafePath() const; + // debug void setDebugData(); void printParkingPositionError() const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp index c410999b2aaa9..4a7eecfc68caf 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp @@ -40,14 +40,14 @@ class GoalPlannerModuleManager : public SceneModuleManagerInterface void updateModuleParams(const std::vector & parameters) override; + bool isAlwaysExecutableModule() const override; + bool isSimultaneousExecutableAsApprovedModule() const override; bool isSimultaneousExecutableAsCandidateModule() const override; private: std::shared_ptr parameters_; - - bool left_side_parking_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp index f30bf31d4e010..1a7350d82aa4e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp @@ -209,6 +209,10 @@ class LaneChangeBase return direction_; } + boost::optional getStopPose() const { return lane_change_stop_pose_; } + + void resetStopPose() { lane_change_stop_pose_ = boost::none; } + protected: virtual lanelet::ConstLanelets getCurrentLanes() const = 0; @@ -244,6 +248,7 @@ class LaneChangeBase PathWithLaneId prev_module_path_{}; DrivableAreaInfo prev_drivable_area_info_{}; TurnSignalInfo prev_turn_signal_info_{}; + boost::optional lane_change_stop_pose_{boost::none}; PathWithLaneId prev_approved_path_{}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index d1476abb63e27..ef57c31c28b5e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -147,6 +147,8 @@ class NormalLaneChange : public LaneChangeBase const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const lanelet::ConstLanelets & target_backward_lanes) const; + void setStopPose(const Pose & stop_pose); + rclcpp::Logger logger_ = rclcpp::get_logger("lane_change").get_child(getModuleTypeStr()); }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp index 3405ae446615d..077f7a3a763d5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp @@ -22,10 +22,15 @@ #include #include #include +#include +#include +#include #include #include #include +#include #include +#include #include #include @@ -50,12 +55,8 @@ namespace behavior_path_planner { using autoware_adapi_v1_msgs::msg::SteeringFactor; using autoware_auto_planning_msgs::msg::PathWithLaneId; -using motion_utils::createDeadLineVirtualWallMarker; -using motion_utils::createSlowDownVirtualWallMarker; -using motion_utils::createStopVirtualWallMarker; using rtc_interface::RTCInterface; using steering_factor_interface::SteeringFactorInterface; -using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::generateUUID; using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; @@ -79,16 +80,16 @@ class SceneModuleInterface public: SceneModuleInterface( const std::string & name, rclcpp::Node & node, - const std::unordered_map> & rtc_interface_ptr_map) + std::unordered_map> rtc_interface_ptr_map) : name_{name}, logger_{node.get_logger().get_child(name)}, clock_{node.get_clock()}, - rtc_interface_ptr_map_(rtc_interface_ptr_map), + rtc_interface_ptr_map_(std::move(rtc_interface_ptr_map)), steering_factor_interface_ptr_( std::make_unique(&node, utils::convertToSnakeCase(name))) { - for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) { - uuid_map_.emplace(itr->first, generateUUID()); + for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { + uuid_map_.emplace(module_name, generateUUID()); } } @@ -176,9 +177,9 @@ class SceneModuleInterface */ void publishRTCStatus() { - for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) { - if (itr->second) { - itr->second->publishCooperateStatus(clock_->now()); + for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { + if (ptr) { + ptr->publishCooperateStatus(clock_->now()); } } } @@ -193,18 +194,18 @@ class SceneModuleInterface void lockRTCCommand() { - for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) { - if (itr->second) { - itr->second->lockCommandUpdate(); + for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { + if (ptr) { + ptr->lockCommandUpdate(); } } } void unlockRTCCommand() { - for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) { - if (itr->second) { - itr->second->unlockCommandUpdate(); + for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { + if (ptr) { + ptr->unlockCommandUpdate(); } } } @@ -398,10 +399,10 @@ class SceneModuleInterface virtual void updateRTCStatus(const double start_distance, const double finish_distance) { - for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) { - if (itr->second) { - itr->second->updateCooperateStatus( - uuid_map_.at(itr->first), isExecutionReady(), start_distance, finish_distance, + for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { + if (ptr) { + ptr->updateCooperateStatus( + uuid_map_.at(module_name), isExecutionReady(), start_distance, finish_distance, clock_->now()); } } @@ -483,9 +484,9 @@ class SceneModuleInterface void removeRTCStatus() { - for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) { - if (itr->second) { - itr->second->clearCooperateStatus(); + for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { + if (ptr) { + ptr->clearCooperateStatus(); } } } diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp index cb47c33b6901c..38101416a80b3 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp @@ -55,6 +55,7 @@ class SceneModuleManagerInterface enable_simultaneous_execution_as_candidate_module_( config.enable_simultaneous_execution_as_candidate_module), enable_rtc_(config.enable_rtc), + keep_last_(config.keep_last), max_module_num_(config.max_module_size), priority_(config.priority) { @@ -214,16 +215,37 @@ class SceneModuleManagerInterface bool canLaunchNewModule() const { return observers_.size() < max_module_num_; } + /** + * Determine if the module is always executable, regardless of the state of other modules. + * + * When this returns true: + * - The module can be executed even if other modules are not marked as 'simultaneously + * executable'. + * - Conversely, the presence of this module will not prevent other modules + * from executing, even if they are not marked as 'simultaneously executable'. + */ + virtual bool isAlwaysExecutableModule() const { return false; } + virtual bool isSimultaneousExecutableAsApprovedModule() const { + if (isAlwaysExecutableModule()) { + return true; + } + return enable_simultaneous_execution_as_approved_module_; } virtual bool isSimultaneousExecutableAsCandidateModule() const { + if (isAlwaysExecutableModule()) { + return true; + } + return enable_simultaneous_execution_as_candidate_module_; } + bool isKeepLast() const { return keep_last_; } + void setData(const std::shared_ptr & planner_data) { planner_data_ = planner_data; } void reset() @@ -290,6 +312,8 @@ class SceneModuleManagerInterface private: bool enable_rtc_; + bool keep_last_; + size_t max_module_num_; size_t priority_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index 2e8c9e57f8842..e9a94126d9f0e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -17,12 +17,15 @@ #include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp" #include "behavior_path_planner/utils/start_planner/freespace_pull_out.hpp" #include "behavior_path_planner/utils/start_planner/geometric_pull_out.hpp" #include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" #include "behavior_path_planner/utils/start_planner/shift_pull_out.hpp" #include "behavior_path_planner/utils/start_planner/start_planner_parameters.hpp" +#include "behavior_path_planner/utils/utils.hpp" #include #include @@ -43,6 +46,11 @@ namespace behavior_path_planner { +using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; +using behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; +using behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; +using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; using geometry_msgs::msg::PoseArray; using lane_departure_checker::LaneDepartureChecker; @@ -55,7 +63,8 @@ struct PullOutStatus lanelet::ConstLanelets pull_out_lanes{}; bool is_safe_static_objects{false}; // current path is safe against static objects bool is_safe_dynamic_objects{false}; // current path is safe against dynamic objects - bool back_finished{false}; + bool back_finished{false}; // if backward driving is not required, this is also set to true + // todo: rename to clear variable name. Pose pull_out_start_pose{}; PullOutStatus() {} @@ -84,12 +93,19 @@ class StartPlannerModule : public SceneModuleInterface BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; CandidateOutput planCandidate() const override; - + void processOnEntry() override; void processOnExit() override; void setParameters(const std::shared_ptr & parameters) { parameters_ = parameters; + if (parameters->safety_check_params.enable_safety_check) { + ego_predicted_path_params_ = + std::make_shared(parameters_->ego_predicted_path_params); + objects_filtering_params_ = + std::make_shared(parameters_->objects_filtering_params); + safety_check_params_ = std::make_shared(parameters_->safety_check_params); + } } void resetStatus(); @@ -109,11 +125,17 @@ class StartPlannerModule : public SceneModuleInterface bool canTransitIdleToRunningState() override { return false; } + void initializeSafetyCheckParameters(); + std::shared_ptr parameters_; + mutable std::shared_ptr ego_predicted_path_params_; + mutable std::shared_ptr objects_filtering_params_; + mutable std::shared_ptr safety_check_params_; vehicle_info_util::VehicleInfo vehicle_info_; std::vector> start_planners_; PullOutStatus status_; + mutable StartGoalPlannerData start_planner_data_; std::deque odometry_buffer_; @@ -131,7 +153,8 @@ class StartPlannerModule : public SceneModuleInterface std::mutex mutex_; PathWithLaneId getFullPath() const; - std::vector searchPullOutStartPoses(); + PathWithLaneId calcStartPoseCandidatesBackwardPath() const; + std::vector searchPullOutStartPoses(const PathWithLaneId & start_pose_candidates) const; std::shared_ptr lane_departure_checker_; @@ -141,8 +164,8 @@ class StartPlannerModule : public SceneModuleInterface void incrementPathIndex(); PathWithLaneId getCurrentPath() const; void planWithPriority( - const std::vector & start_pose_candidates, const Pose & goal_pose, - const std::string search_priority); + const std::vector & start_pose_candidates, const Pose & refined_start_pose, + const Pose & goal_pose, const std::string search_priority); PathWithLaneId generateStopPath() const; lanelet::ConstLanelets getPathRoadLanes(const PathWithLaneId & path) const; std::vector generateDrivableLanes(const PathWithLaneId & path) const; @@ -155,6 +178,10 @@ class StartPlannerModule : public SceneModuleInterface bool isStopped(); bool isStuck(); bool hasFinishedCurrentPath(); + void updateSafetyCheckTargetObjectsData( + const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane, + const std::vector & ego_predicted_path) const; + bool isSafePath() const; void setDrivableAreaInfo(BehaviorModuleOutput & output) const; // check if the goal is located behind the ego in the same route segment. @@ -163,6 +190,7 @@ class StartPlannerModule : public SceneModuleInterface // generate BehaviorPathOutput with stopping path and update status BehaviorModuleOutput generateStopOutput(); + SafetyCheckParams createSafetyCheckParams() const; // freespace planner void onFreespacePlannerTimer(); bool planFreespacePath(); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index 0278b73b0acb4..215a8bfd97cc0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -20,10 +20,11 @@ #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include -#include +#include #include #include +#include #include #include @@ -556,9 +557,6 @@ struct DebugData // shift path std::vector proposed_spline_shift; - // future pose - PathWithLaneId path_with_planned_velocity; - // avoidance require objects ObjectDataArray unavoidable_objects; @@ -568,6 +566,10 @@ struct DebugData // tmp for plot PathWithLaneId center_line; + // collision check debug map + utils::path_safety_checker::CollisionCheckDebugMap collision_check; + + // debug msg array AvoidanceDebugMsgArray avoidance_debug_msg_array; }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp index e2934a02b63d8..0dc07f0950716 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp @@ -70,7 +70,7 @@ void fillLongitudinalAndLengthByClosestEnvelopeFootprint( const PathWithLaneId & path, const Point & ego_pos, ObjectData & obj); double calcEnvelopeOverhangDistance( - const ObjectData & object_data, const Pose & base_pose, Point & overhang_pose); + const ObjectData & object_data, const PathWithLaneId & path, Point & overhang_pose); void setEndData( AvoidLine & al, const double length, const geometry_msgs::msg::Pose & end, const size_t end_idx, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp index 53ef91473fe24..8293e0a1d10a9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp @@ -15,6 +15,7 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_ +#include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" @@ -22,17 +23,17 @@ #include +#include + namespace drivable_area_expansion { /// @brief Expand the drivable area based on the projected ego footprint along the path -/// @param[in] path path whose drivable area will be expanded -/// @param[in] params expansion parameters -/// @param[in] dynamic_objects dynamic objects -/// @param[in] route_handler route handler +/// @param[inout] path path whose drivable area will be expanded +/// @param[inout] planner_data planning data (params, dynamic objects, route handler, ...) /// @param[in] path_lanes lanelets of the path void expandDrivableArea( - PathWithLaneId & path, const DrivableAreaExpansionParameters & params, - const PredictedObjects & dynamic_objects, const route_handler::RouteHandler & route_handler, + PathWithLaneId & path, + const std::shared_ptr planner_data, const lanelet::ConstLanelets & path_lanes); /// @brief Create a polygon combining the drivable area of a path and some expansion polygons diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp index e5e1c76f2521f..8fc0157710dc8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp @@ -64,6 +64,6 @@ multi_polygon_t createObjectFootprints( /// @param[in] params expansion parameters defining how to create the footprint /// @return footprint polygons of the path multi_polygon_t createPathFootprints( - const PathWithLaneId & path, const DrivableAreaExpansionParameters & params); + const std::vector & path, const DrivableAreaExpansionParameters & params); } // namespace drivable_area_expansion #endif // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp index a4c11c68d13ec..81eab9560b736 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp @@ -48,8 +48,10 @@ struct DrivableAreaExpansionParameters "dynamic_expansion.dynamic_objects.extra_footprint_offset.right"; static constexpr auto EXPANSION_METHOD_PARAM = "dynamic_expansion.expansion.method"; static constexpr auto MAX_EXP_DIST_PARAM = "dynamic_expansion.expansion.max_distance"; + static constexpr auto RESAMPLE_INTERVAL_PARAM = + "dynamic_expansion.path_preprocessing.resample_interval"; static constexpr auto MAX_PATH_ARC_LENGTH_PARAM = - "dynamic_expansion.expansion.max_path_arc_length"; + "dynamic_expansion.path_preprocessing.max_arc_length"; static constexpr auto EXTRA_ARC_LENGTH_PARAM = "dynamic_expansion.expansion.extra_arc_length"; static constexpr auto AVOID_DYN_OBJECTS_PARAM = "dynamic_expansion.dynamic_objects.avoid"; static constexpr auto AVOID_LINESTRING_TYPES_PARAM = "dynamic_expansion.avoid_linestring.types"; @@ -78,6 +80,7 @@ struct DrivableAreaExpansionParameters double dynamic_objects_extra_front_offset{}; double max_expansion_distance{}; double max_path_arc_length{}; + double resample_interval{}; double extra_arc_length{}; bool avoid_dynamic_objects{}; std::vector avoid_linestring_types{}; @@ -98,6 +101,7 @@ struct DrivableAreaExpansionParameters enabled = node.declare_parameter(ENABLED_PARAM); max_expansion_distance = node.declare_parameter(MAX_EXP_DIST_PARAM); max_path_arc_length = node.declare_parameter(MAX_PATH_ARC_LENGTH_PARAM); + resample_interval = node.declare_parameter(RESAMPLE_INTERVAL_PARAM); ego_extra_front_offset = node.declare_parameter(EGO_EXTRA_OFFSET_FRONT); ego_extra_rear_offset = node.declare_parameter(EGO_EXTRA_OFFSET_REAR); ego_extra_left_offset = node.declare_parameter(EGO_EXTRA_OFFSET_LEFT); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp index daabfa97598fd..e300a347e47a8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp @@ -26,6 +26,7 @@ namespace drivable_area_expansion { using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::PathPoint; +using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp index cb769a1f2884c..9cae5e18f1352 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp @@ -94,6 +94,10 @@ class GeometricParallelParking std::vector getArcPaths() const { return arc_paths_; } std::vector getPaths() const { return paths_; } + std::vector> getPairsTerminalVelocityAndAccel() const + { + return pairs_terminal_velocity_and_accel_; + } PathWithLaneId getPathByIdx(size_t const idx) const; PathWithLaneId getCurrentPath() const; PathWithLaneId getFullPath() const; @@ -112,6 +116,7 @@ class GeometricParallelParking std::vector arc_paths_; std::vector paths_; + std::vector> pairs_terminal_velocity_and_accel_; size_t current_path_idx_ = 0; void clearPaths(); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp index f87944641f59b..c458d017f518c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp @@ -17,6 +17,7 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_PLANNER_PARAMETERS_HPP_ #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include #include @@ -40,7 +41,6 @@ enum class ParkingPolicy { struct GoalPlannerParameters { // general params - double minimum_request_length; double th_arrived_distance; double th_stopped_velocity; double th_stopped_time; @@ -71,6 +71,7 @@ struct GoalPlannerParameters double object_recognition_collision_check_max_extra_stopping_margin; // pull over general params + double pull_over_minimum_request_length; double pull_over_velocity; double pull_over_minimum_velocity; double decide_path_distance; @@ -99,6 +100,18 @@ struct GoalPlannerParameters AstarParam astar_parameters; RRTStarParam rrt_star_parameters; + // stop condition + double maximum_deceleration_for_stop; + double maximum_jerk_for_stop; + + // hysteresis parameter + double hysteresis_factor_expand_rate; + + // path safety checker + utils::path_safety_checker::EgoPredictedPathParams ego_predicted_path_params; + utils::path_safety_checker::ObjectsFilteringParams objects_filtering_params; + utils::path_safety_checker::SafetyCheckParams safety_check_params; + // debug bool print_debug_info; }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp index 4b800917d4aec..20788e53309ec 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp @@ -26,6 +26,7 @@ #include #include +#include #include using autoware_auto_planning_msgs::msg::PathWithLaneId; @@ -46,6 +47,8 @@ struct PullOverPath { PullOverPlannerType type{PullOverPlannerType::NONE}; std::vector partial_paths{}; + // accelerate with constant acceleration to the target velocity + std::vector> pairs_terminal_velocity_and_accel{}; Pose start_pose{}; Pose end_pose{}; std::vector debug_poses{}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp index 3dde96e813aad..0adda77a2ad72 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp @@ -46,15 +46,15 @@ using visualization_msgs::msg::MarkerArray; // TODO(sugahara) move to util PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2); -lanelet::ConstLanelets getPullOverLanes(const RouteHandler & route_handler, const bool left_side); +lanelet::ConstLanelets getPullOverLanes( + const RouteHandler & route_handler, const bool left_side, const double backward_distance, + const double forward_distance); PredictedObjects filterObjectsByLateralDistance( const Pose & ego_pose, const double vehicle_width, const PredictedObjects & objects, const double distance_thresh, const bool filter_inside); -bool isAllowedGoalModification( - const std::shared_ptr & route_handler, const bool left_side_parking); -bool checkOriginalGoalIsInShoulder( - const std::shared_ptr & route_handler, const bool left_side_parking); +bool isAllowedGoalModification(const std::shared_ptr & route_handler); +bool checkOriginalGoalIsInShoulder(const std::shared_ptr & route_handler); // debug MarkerArray createPullOverAreaMarkerArray( diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp index 87c93f49901f0..475b38f5bb824 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -170,7 +170,7 @@ struct ObjectsFilteringParams struct SafetyCheckParams { bool enable_safety_check; ///< Enable safety checks. - double backward_lane_length; ///< Length of the backward lane for path generation. + double backward_path_length; ///< Length of the backward lane for path generation. double forward_path_length; ///< Length of the forward path lane for path generation. RSSparams rss_params; ///< Parameters related to the RSS model. bool publish_debug_marker{false}; ///< Option to publish debug markers. @@ -179,10 +179,10 @@ struct SafetyCheckParams struct CollisionCheckDebug { std::string unsafe_reason; ///< Reason indicating unsafe situation. - Pose current_pose{}; ///< Ego vehicle's current pose. Twist current_twist{}; ///< Ego vehicle's current velocity and rotation. - Twist object_twist{}; ///< Detected object's velocity and rotation. Pose expected_ego_pose{}; ///< Predicted future pose of ego vehicle. + Pose current_obj_pose{}; ///< Detected object's current pose. + Twist object_twist{}; ///< Detected object's velocity and rotation. Pose expected_obj_pose{}; ///< Predicted future pose of object. double rss_longitudinal{0.0}; ///< Longitudinal RSS measure. double inter_vehicle_distance{0.0}; ///< Distance between ego vehicle and object. @@ -190,9 +190,10 @@ struct CollisionCheckDebug double extended_polygon_lat_offset{0.0}; ///< Lateral offset for extended polygon. bool is_front{false}; ///< True if object is in front of ego vehicle. bool is_safe{false}; ///< True if situation is deemed safe. - std::vector lerped_path; ///< Interpolated ego vehicle path. - Polygon2d extended_ego_polygon{}; ///< Ego vehicle's extended collision polygon. - Polygon2d extended_obj_polygon{}; ///< Detected object's extended collision polygon. + std::vector ego_predicted_path; ///< ego vehicle's predicted path. + std::vector obj_predicted_path; ///< object's predicted path. + Polygon2d extended_ego_polygon{}; ///< Ego vehicle's extended collision polygon. + Polygon2d extended_obj_polygon{}; ///< Detected object's extended collision polygon. }; using CollisionCheckDebugPair = std::pair; using CollisionCheckDebugMap = diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp new file mode 100644 index 0000000000000..55b8bdf777692 --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp @@ -0,0 +1,48 @@ +// Copyright 2023 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 BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__COMMON_MODULE_DATA_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__COMMON_MODULE_DATA_HPP_ + +#include "behavior_path_planner/marker_utils/utils.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" + +#include + +#include + +#include + +namespace behavior_path_planner +{ +using autoware_auto_perception_msgs::msg::PredictedObjects; +using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; +using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; +/* + * Common data for start/goal_planner module + */ +struct StartGoalPlannerData +{ + // filtered objects + PredictedObjects filtered_objects; + TargetObjectsOnLane target_objects_on_lane; + std::vector ego_predicted_path; + // collision check debug map + CollisionCheckDebugMap collision_check; +}; + +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__COMMON_MODULE_DATA_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp new file mode 100644 index 0000000000000..217b3748fa250 --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp @@ -0,0 +1,118 @@ +// Copyright 2023 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 BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__UTILS_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__UTILS_HPP_ + +#include "behavior_path_planner/data_manager.hpp" +#include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp" +#include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" +#include "behavior_path_planner/utils/start_planner/start_planner_parameters.hpp" + +#include + +#include +#include +#include + +namespace behavior_path_planner::utils::start_goal_planner_common +{ + +using behavior_path_planner::StartPlannerParameters; +using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; +using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; +using behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; +using behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; + +boost::optional calcFeasibleDecelDistance( + std::shared_ptr planner_data, const double acc_lim, const double jerk_lim, + const double target_velocity); + +/** + * @brief Update path velocities based on driving direction. + * + * This function updates the longitudinal velocity of each point in the provided paths, + * based on whether the vehicle is driving forward or backward. It also sets the terminal + * velocity and acceleration for each path. + * + * @param paths A vector of paths with lane IDs to be updated. + * @param terminal_vel_acc_pairs A vector of pairs, where each pair contains the terminal + * velocity and acceleration for a corresponding path. + * @param target_velocity The target velocity for ego vehicle predicted path. + * @param acceleration The acceleration for ego vehicle predicted path. + */ +void modifyVelocityByDirection( + std::vector & paths, + std::vector> & terminal_vel_acc_pairs, const double target_velocity, + const double acceleration); + +void updateEgoPredictedPathParams( + std::shared_ptr & ego_predicted_path_params, + const std::shared_ptr & start_planner_params); + +void updateEgoPredictedPathParams( + std::shared_ptr & ego_predicted_path_params, + const std::shared_ptr & goal_planner_params); + +void updateSafetyCheckParams( + std::shared_ptr & safety_check_params, + const std::shared_ptr & start_planner_params); + +void updateSafetyCheckParams( + std::shared_ptr & safety_check_params, + const std::shared_ptr & goal_planner_params); + +void updateObjectsFilteringParams( + std::shared_ptr & objects_filtering_params, + const std::shared_ptr & start_planner_params); + +void updateObjectsFilteringParams( + std::shared_ptr & objects_filtering_params, + const std::shared_ptr & goal_planner_params); + +void updatePathProperty( + std::shared_ptr & ego_predicted_path_params, + const std::pair & pairs_terminal_velocity_and_accel); + +void initializeCollisionCheckDebugMap(CollisionCheckDebugMap & collision_check_debug_map); + +void updateSafetyCheckTargetObjectsData( + StartGoalPlannerData & data, const PredictedObjects & filtered_objects, + const TargetObjectsOnLane & target_objects_on_lane, + const std::vector & ego_predicted_path); + +std::pair getPairsTerminalVelocityAndAccel( + const std::vector> & pairs_terminal_velocity_and_accel, + const size_t current_path_idx); + +/** + * @brief removeInverseOrderPathPoints function + * + * This function is designed to handle a situation that can arise when shifting paths on a curve, + * where the positions of the path points may become inverted (i.e., a point further along the path + * comes to be located before an earlier point). It corrects for this by using the function + * tier4_autoware_utils::isDrivingForward(p1, p2) to check if each pair of adjacent points is in + * the correct order (with the second point being 'forward' of the first). Any points which fail + * this test are omitted from the returned PathWithLaneId. + * + * @param path A path with points possibly in incorrect order. + * @return Returns a new PathWithLaneId that has all points in the correct order. + */ +PathWithLaneId removeInverseOrderPathPoints(const PathWithLaneId & path); + +} // namespace behavior_path_planner::utils::start_goal_planner_common + +#endif // BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp index abdf17c9c6cfe..2b2de183b2dac 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp @@ -41,7 +41,7 @@ class FreespacePullOut : public PullOutPlannerBase PlannerType getPlannerType() override { return PlannerType::FREESPACE; } - boost::optional plan(Pose start_pose, Pose end_pose) override; + boost::optional plan(const Pose & start_pose, const Pose & end_pose) override; protected: std::unique_ptr planner_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/geometric_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/geometric_pull_out.hpp index 7ace770dc7ff1..a5ff52e74038f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/geometric_pull_out.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/geometric_pull_out.hpp @@ -29,7 +29,7 @@ class GeometricPullOut : public PullOutPlannerBase explicit GeometricPullOut(rclcpp::Node & node, const StartPlannerParameters & parameters); PlannerType getPlannerType() override { return PlannerType::GEOMETRIC; }; - boost::optional plan(Pose start_pose, Pose goal_pose) override; + boost::optional plan(const Pose & start_pose, const Pose & goal_pose) override; GeometricParallelParking planner_; ParallelParkingParameters parallel_parking_parameters_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp index 6e6e5111e5dd9..cb662efd767bf 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp @@ -60,7 +60,7 @@ class PullOutPlannerBase } virtual PlannerType getPlannerType() = 0; - virtual boost::optional plan(Pose start_pose, Pose goal_pose) = 0; + virtual boost::optional plan(const Pose & start_pose, const Pose & goal_pose) = 0; protected: std::shared_ptr planner_data_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp index 2042593064c51..0842d0a17bd97 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp @@ -37,7 +37,7 @@ class ShiftPullOut : public PullOutPlannerBase std::shared_ptr & lane_departure_checker); PlannerType getPlannerType() override { return PlannerType::SHIFT; }; - boost::optional plan(Pose start_pose, Pose goal_pose) override; + boost::optional plan(const Pose & start_pose, const Pose & goal_pose) override; std::vector calcPullOutPaths( const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp index 1e26bef0c85be..4c095a38e7f73 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp @@ -17,6 +17,7 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__START_PLANNER_PARAMETERS_HPP_ #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include #include @@ -76,6 +77,18 @@ struct StartPlannerParameters PlannerCommonParam freespace_planner_common_parameters; AstarParam astar_parameters; RRTStarParam rrt_star_parameters; + + // stop condition + double maximum_deceleration_for_stop; + double maximum_jerk_for_stop; + + // hysteresis parameter + double hysteresis_factor_expand_rate = 0.0; + + // path safety checker + utils::path_safety_checker::EgoPredictedPathParams ego_predicted_path_params; + utils::path_safety_checker::ObjectsFilteringParams objects_filtering_params; + utils::path_safety_checker::SafetyCheckParams safety_check_params; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp index 63e374e074a5a..d85e6181764d5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp @@ -16,6 +16,9 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__UTIL_HPP_ #include "behavior_path_planner/data_manager.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" +#include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" #include @@ -36,6 +39,7 @@ namespace behavior_path_planner::start_planner_utils using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_perception_msgs::msg::PredictedPath; using autoware_auto_planning_msgs::msg::PathWithLaneId; +using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using route_handler::RouteHandler; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index be227f4f7e93e..d5e1d22fdcc88 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -22,11 +22,11 @@ #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" #include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" #include -#include +#include #include #include @@ -323,8 +323,8 @@ PathPointWithLaneId insertStopPoint(const double length, PathWithLaneId & path); double getSignedDistanceFromBoundary( const lanelet::ConstLanelets & shoulder_lanelets, const Pose & pose, const bool left_side); std::optional getSignedDistanceFromBoundary( - const lanelet::ConstLanelets & shoulder_lanelets, const LinearRing2d & footprint, - const Pose & vehicle_pose, const bool left_side); + const lanelet::ConstLanelets & lanelets, const double vehicle_width, const double base_link2front, + const double base_link2rear, const Pose & vehicle_pose, const bool left_side); // misc diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 3023357bd0a96..41df95378c52f 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -19,7 +19,7 @@ #include "behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp" #include "behavior_path_planner/utils/path_utils.hpp" -#include +#include #include #include @@ -284,6 +284,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() declare_parameter(ns + "enable_simultaneous_execution_as_approved_module"); config.enable_simultaneous_execution_as_candidate_module = declare_parameter(ns + "enable_simultaneous_execution_as_candidate_module"); + config.keep_last = declare_parameter(ns + "keep_last"); config.priority = declare_parameter(ns + "priority"); config.max_module_size = declare_parameter(ns + "max_module_size"); return config; @@ -1043,6 +1044,9 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam( updateParam( parameters, DrivableAreaExpansionParameters::MAX_PATH_ARC_LENGTH_PARAM, planner_data_->drivable_area_expansion_parameters.max_path_arc_length); + updateParam( + parameters, DrivableAreaExpansionParameters::RESAMPLE_INTERVAL_PARAM, + planner_data_->drivable_area_expansion_parameters.resample_interval); updateParam( parameters, DrivableAreaExpansionParameters::EXTRA_ARC_LENGTH_PARAM, planner_data_->drivable_area_expansion_parameters.extra_arc_length); diff --git a/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp b/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp index 94073ad467326..b2c326fda7a69 100644 --- a/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp +++ b/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp @@ -16,6 +16,7 @@ #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "motion_utils/trajectory/path_with_lane_id.hpp" #include @@ -416,12 +417,6 @@ MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const return msg; } -MarkerArray createUnsafeObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns) -{ - return createObjectsCubeMarkerArray( - objects, ns + "_cube", createMarkerScale(3.2, 1.7, 2.0), createMarkerColor(0.0, 0.0, 1.0, 0.8)); -} - MarkerArray makeOverhangToRoadShoulderMarkerArray( const ObjectDataArray & objects, std::string && ns) { diff --git a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp b/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp index aab2be6f3054e..7493982db78b1 100644 --- a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp +++ b/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp @@ -36,40 +36,6 @@ using geometry_msgs::msg::Point; using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerScale; -MarkerArray showObjectInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) -{ - Marker obj_marker = createDefaultMarker( - "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING, - createMarkerScale(0.5, 0.5, 0.5), colors::aqua()); - - MarkerArray marker_array; - int32_t id{0}; - - marker_array.markers.reserve(obj_debug_vec.size()); - - int idx{0}; - - for (const auto & [uuid, info] : obj_debug_vec) { - obj_marker.id = ++id; - obj_marker.pose = info.current_pose; - - std::ostringstream ss; - - ss << "Idx: " << ++idx << "\nReason: " << info.unsafe_reason - << "\nRSS dist: " << std::setprecision(4) << info.rss_longitudinal - << "\nEgo to obj: " << info.inter_vehicle_distance - << "\nExtended polygon lateral offset: " << info.extended_polygon_lat_offset - << "\nExtended polygon longitudinal offset: " << info.extended_polygon_lon_offset - << "\nPosition: " << (info.is_front ? "front" : "back") - << "\nSafe: " << (info.is_safe ? "Yes" : "No"); - - obj_marker.text = ss.str(); - - marker_array.markers.push_back(obj_marker); - } - return marker_array; -} - MarkerArray showAllValidLaneChangePath(const std::vector & lanes, std::string && ns) { if (lanes.empty()) { @@ -103,121 +69,6 @@ MarkerArray showAllValidLaneChangePath(const std::vector & lanes return marker_array; } -MarkerArray showLerpedPose(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) -{ - MarkerArray marker_array; - int32_t id{0}; - const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()}; - marker_array.markers.reserve(obj_debug_vec.size()); - - for (const auto & [uuid, info] : obj_debug_vec) { - Marker marker = createDefaultMarker( - "map", current_time, ns, ++id, Marker::POINTS, createMarkerScale(0.3, 0.3, 0.3), - colors::magenta()); - marker.points.reserve(info.lerped_path.size()); - - for (const auto & point : info.lerped_path) { - marker.points.push_back(point.position); - } - - marker_array.markers.push_back(marker); - } - return marker_array; -} - -MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) -{ - if (obj_debug_vec.empty()) { - return MarkerArray{}; - } - - constexpr float scale_val{0.2}; - int32_t id{0}; - const auto now = rclcpp::Clock{RCL_ROS_TIME}.now(); - Marker ego_marker = createDefaultMarker( - "map", now, ns, id, Marker::LINE_STRIP, createMarkerScale(scale_val, scale_val, scale_val), - colors::green()); - Marker obj_marker = ego_marker; - - auto text_marker = createDefaultMarker( - "map", now, ns + "_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - createMarkerScale(1.5, 1.5, 1.5), colors::white()); - - MarkerArray marker_array; - - const auto reserve_size = obj_debug_vec.size(); - - marker_array.markers.reserve(reserve_size * 4); - - int32_t idx = {0}; - - for (const auto & [uuid, info] : obj_debug_vec) { - const auto color = info.is_safe ? colors::green() : colors::red(); - const auto & ego_polygon = info.extended_ego_polygon.outer(); - const auto poly_z = info.current_pose.position.z; // temporally - ego_marker.id = ++id; - ego_marker.color = color; - ego_marker.points.reserve(ego_polygon.size()); - for (const auto & p : ego_polygon) { - ego_marker.points.push_back(tier4_autoware_utils::createPoint(p.x(), p.y(), poly_z)); - } - marker_array.markers.push_back(ego_marker); - - std::ostringstream ss; - text_marker.id = ego_marker.id; - ss << ++idx; - text_marker.text = ss.str(); - text_marker.pose = info.expected_ego_pose; - - marker_array.markers.push_back(text_marker); - - const auto & obj_polygon = info.extended_obj_polygon.outer(); - obj_marker.id = ++id; - obj_marker.color = color; - obj_marker.points.reserve(obj_polygon.size()); - for (const auto & p : obj_polygon) { - obj_marker.points.push_back(tier4_autoware_utils::createPoint(p.x(), p.y(), poly_z)); - } - marker_array.markers.push_back(obj_marker); - - text_marker.id = obj_marker.id; - text_marker.pose = info.expected_obj_pose; - marker_array.markers.push_back(text_marker); - - ego_marker.points.clear(); - obj_marker.points.clear(); - } - - return marker_array; -} - -MarkerArray showPolygonPose(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) -{ - const auto colors = colors::colors_list(); - const auto loop_size = std::min(colors.size(), obj_debug_vec.size()); - MarkerArray marker_array; - int32_t id{0}; - size_t idx{0}; - const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()}; - marker_array.markers.reserve(obj_debug_vec.size()); - - for (const auto & [uuid, info] : obj_debug_vec) { - const auto & color = colors.at(idx); - Marker marker = createDefaultMarker( - "map", current_time, ns, ++id, Marker::POINTS, createMarkerScale(0.2, 0.2, 0.2), color); - marker.points.reserve(2); - marker.points.push_back(info.expected_ego_pose.position); - marker.points.push_back(info.expected_obj_pose.position); - marker_array.markers.push_back(marker); - ++idx; - if (idx >= loop_size) { - break; - } - } - - return marker_array; -} - MarkerArray createLaneChangingVirtualWallMarker( const geometry_msgs::msg::Pose & lane_changing_pose, const std::string & module_name, const rclcpp::Time & now, const std::string & ns) diff --git a/planning/behavior_path_planner/src/marker_utils/utils.cpp b/planning/behavior_path_planner/src/marker_utils/utils.cpp index f24e772d198c1..b51fc585ce935 100644 --- a/planning/behavior_path_planner/src/marker_utils/utils.cpp +++ b/planning/behavior_path_planner/src/marker_utils/utils.cpp @@ -14,12 +14,14 @@ #include "behavior_path_planner/marker_utils/utils.hpp" +#include "behavior_path_planner/marker_utils/colors.hpp" #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include #include - +#include namespace marker_utils { using behavior_path_planner::ShiftLine; @@ -37,7 +39,7 @@ using visualization_msgs::msg::Marker; CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj) { CollisionCheckDebug debug; - debug.current_pose = obj.initial_pose.pose; + debug.current_obj_pose = obj.initial_pose.pose; debug.current_twist = obj.initial_twist.twist; return {tier4_autoware_utils::toHexString(obj.uuid), debug}; } @@ -486,4 +488,162 @@ MarkerArray createPredictedPathMarkerArray( return msg; } +MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) +{ + if (obj_debug_vec.empty()) { + return MarkerArray{}; + } + + int32_t id{0}; + const auto now = rclcpp::Clock{RCL_ROS_TIME}.now(); + + constexpr float line_scale_val{0.2}; + const auto line_marker_scale = createMarkerScale(line_scale_val, line_scale_val, line_scale_val); + + auto default_line_marker = [&](const auto & color = colors::green()) { + return createDefaultMarker("map", now, ns, ++id, Marker::LINE_STRIP, line_marker_scale, color); + }; + + constexpr float text_scale_val{1.5}; + const auto text_marker_scale = createMarkerScale(text_scale_val, text_scale_val, text_scale_val); + + auto default_text_marker = [&]() { + return createDefaultMarker( + "map", now, ns + "_text", ++id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, + text_marker_scale, colors::white()); + }; + + auto default_cube_marker = + [&](const auto & width, const auto & depth, const auto & color = colors::green()) { + return createDefaultMarker( + "map", now, ns + "_cube", ++id, visualization_msgs::msg::Marker::CUBE, + createMarkerScale(width, depth, 1.0), color); + }; + + MarkerArray marker_array; + marker_array.markers.reserve( + obj_debug_vec.size() * 5); // poly ego, text ego, poly obj, text obj, cube obj + + int32_t idx = {0}; + for (const auto & [uuid, info] : obj_debug_vec) { + const auto color = info.is_safe ? colors::green() : colors::red(); + const auto poly_z = info.current_obj_pose.position.z; // temporally + + const auto insert_polygon_marker = [&](const auto & polygon) { + marker_array.markers.emplace_back(); + auto & polygon_marker = marker_array.markers.back(); + polygon_marker = default_line_marker(color); + polygon_marker.points.reserve(polygon.outer().size()); + for (const auto & p : polygon.outer()) { + polygon_marker.points.push_back(createPoint(p.x(), p.y(), poly_z)); + } + }; + + insert_polygon_marker(info.extended_ego_polygon); + insert_polygon_marker(info.extended_obj_polygon); + + const auto str_idx = std::to_string(++idx); + const auto insert_text_marker = [&](const auto & pose) { + marker_array.markers.emplace_back(); + auto & text_marker = marker_array.markers.back(); + text_marker = default_text_marker(); + text_marker.text = str_idx; + text_marker.pose = pose; + }; + + insert_text_marker(info.expected_ego_pose); + insert_text_marker(info.expected_obj_pose); + + const auto insert_cube_marker = [&](const auto & pose) { + marker_array.markers.emplace_back(); + auto & cube_marker = marker_array.markers.back(); + cube_marker = default_cube_marker(1.0, 1.0, color); + cube_marker.pose = pose; + }; + insert_cube_marker(info.current_obj_pose); + } + return marker_array; +} + +MarkerArray showPredictedPath(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) +{ + int32_t id{0}; + const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()}; + const auto arrow_marker_scale = createMarkerScale(1.0, 0.3, 0.3); + const auto default_arrow_marker = [&](const auto & color) { + return createDefaultMarker( + "map", current_time, ns, ++id, Marker::ARROW, arrow_marker_scale, color); + }; + + MarkerArray marker_array; + marker_array.markers.reserve(std::accumulate( + obj_debug_vec.cbegin(), obj_debug_vec.cend(), 0UL, + [&](const auto current_sum, const auto & obj_debug) { + const auto & [uuid, info] = obj_debug; + return current_sum + info.ego_predicted_path.size() + info.obj_predicted_path.size() + 2; + })); + + for (const auto & [uuid, info] : obj_debug_vec) { + const auto insert_marker = [&](const auto & path, const auto & color) { + for (const auto & pose : path) { + marker_array.markers.emplace_back(); + auto & marker = marker_array.markers.back(); + marker = default_arrow_marker(color); + marker.pose = pose.pose; + } + }; + + insert_marker(info.ego_predicted_path, colors::aqua()); + insert_marker(info.obj_predicted_path, colors::yellow()); + const auto insert_expected_pose_marker = [&](const auto & pose, const auto & color) { + // instead of checking for distance, inserting a new marker might be more efficient + marker_array.markers.emplace_back(); + auto & marker = marker_array.markers.back(); + marker = default_arrow_marker(color); + marker.pose = pose; + marker.pose.position.z += 0.05; + }; + + insert_expected_pose_marker(info.expected_ego_pose, colors::red()); + insert_expected_pose_marker(info.expected_obj_pose, colors::red()); + } + return marker_array; +} + +MarkerArray showSafetyCheckInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) +{ + int32_t id{0}; + auto default_text_marker = [&]() { + return createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, ++id, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.5, 0.5, 0.5), colors::aqua()); + }; + + MarkerArray marker_array; + + marker_array.markers.reserve(obj_debug_vec.size()); + + int idx{0}; + + for (const auto & [uuid, info] : obj_debug_vec) { + auto safety_check_info_text = default_text_marker(); + safety_check_info_text.pose = info.current_obj_pose; + + std::ostringstream ss; + + ss << "Idx: " << ++idx << "\nUnsafe reason: " << info.unsafe_reason + << "\nRSS dist: " << std::setprecision(4) << info.rss_longitudinal + << "\nEgo to obj: " << info.inter_vehicle_distance + << "\nExtended polygon: " << (info.is_front ? "ego" : "object") + << "\nExtended polygon lateral offset: " << info.extended_polygon_lat_offset + << "\nExtended polygon longitudinal offset: " << info.extended_polygon_lon_offset + << "\nLast checked position: " << (info.is_front ? "obj in front ego" : "obj at back ego") + << "\nSafe: " << (info.is_safe ? "Yes" : "No"); + + safety_check_info_text.text = ss.str(); + marker_array.markers.push_back(safety_check_info_text); + } + return marker_array; +} + } // namespace marker_utils diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index c09df4151ca35..ee9c445e9fd2f 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -16,6 +16,7 @@ #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "tier4_autoware_utils/system/stop_watch.hpp" #include #include @@ -194,22 +195,6 @@ std::vector PlannerManager::getRequestModules( std::vector request_modules{}; - /** - * check whether it is possible to push back more modules to approved modules. - */ - { - const auto find_block_module = [this](const auto & m) { - return !getManager(m)->isSimultaneousExecutableAsApprovedModule(); - }; - - const auto itr = - std::find_if(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), find_block_module); - - if (itr != approved_module_ptrs_.end()) { - return {}; - } - } - const auto toc = [this](const auto & name) { processing_time_.at(name) += stop_watch_.toc(name, true); }; @@ -218,14 +203,62 @@ std::vector PlannerManager::getRequestModules( stop_watch_.tic(manager_ptr->name()); /** - * don't launch candidate module if approved modules already exist. + * determine the execution capability of modules based on existing approved modules. */ - if (!approved_module_ptrs_.empty()) { - if (!manager_ptr->isSimultaneousExecutableAsApprovedModule()) { - toc(manager_ptr->name()); + // Condition 1: always executable module can be added regardless of the existence of other + // modules, so skip checking the existence of other modules. + // in other cases, need to check the existence of other modules and which module can be added. + const bool has_non_always_executable_module = std::any_of( + approved_module_ptrs_.begin(), approved_module_ptrs_.end(), + [this](const auto & m) { return !getManager(m)->isAlwaysExecutableModule(); }); + if (!manager_ptr->isAlwaysExecutableModule() && has_non_always_executable_module) { + // pairs of find_block_module and is_executable + std::vector, std::function>> + conditions; + + // Condition 2: do not add modules that are neither always nor simultaneous executable + // if there exists at least one approved module that is simultaneous but not always + // executable. (only modules that are either always executable or simultaneous executable can + // be added) + conditions.push_back( + {[&](const SceneModulePtr & m) { + return !getManager(m)->isAlwaysExecutableModule() && + getManager(m)->isSimultaneousExecutableAsApprovedModule(); + }, + [&]() { return manager_ptr->isSimultaneousExecutableAsApprovedModule(); }}); + + // Condition 3: do not add modules that are not always executable if there exists + // at least one approved module that is neither always nor simultaneous executable. + // (only modules that are always executable can be added) + conditions.push_back( + {[&](const SceneModulePtr & m) { + return !getManager(m)->isAlwaysExecutableModule() && + !getManager(m)->isSimultaneousExecutableAsApprovedModule(); + }, + [&]() { return false; }}); + + bool skip_module = false; + for (const auto & condition : conditions) { + const auto & find_block_module = condition.first; + const auto & is_executable = condition.second; + + const auto itr = std::find_if( + approved_module_ptrs_.begin(), approved_module_ptrs_.end(), find_block_module); + + if (itr != approved_module_ptrs_.end() && !is_executable()) { + toc(manager_ptr->name()); + skip_module = true; + continue; + } + } + if (skip_module) { continue; } } + // else{ + // Condition 4: if none of the above conditions are met, any module can be added. + // (when the approved modules are either empty or consist only of always executable modules.) + // } /** * launch new candidate module. @@ -347,20 +380,58 @@ std::pair PlannerManager::runRequestModule * remove non-executable modules. */ for (const auto & module_ptr : sorted_request_modules) { - if (!getManager(module_ptr)->isSimultaneousExecutableAsCandidateModule()) { - if (executable_modules.empty()) { - executable_modules.push_back(module_ptr); - break; - } + // Condition 1: always executable module can be added regardless of the existence of other + // modules. + if (getManager(module_ptr)->isAlwaysExecutableModule()) { + executable_modules.push_back(module_ptr); + continue; } - const auto itr = - std::find_if(executable_modules.begin(), executable_modules.end(), [this](const auto & m) { - return !getManager(m)->isSimultaneousExecutableAsCandidateModule(); - }); - - if (itr == executable_modules.end()) { + // Condition 4: If the executable modules are either empty or consist only of always executable + // modules, any module can be added. + const bool has_non_always_executable_module = std::any_of( + executable_modules.begin(), executable_modules.end(), + [this](const auto & m) { return !getManager(m)->isAlwaysExecutableModule(); }); + if (!has_non_always_executable_module) { executable_modules.push_back(module_ptr); + continue; + } + + // pairs of find_block_module and is_executable + std::vector, std::function>> + conditions; + + // Condition 3: Only modules that are always executable can be added + // if there exists at least one executable module that is neither always nor simultaneous + // executable. + conditions.push_back( + {[this](const SceneModulePtr & m) { + return !getManager(m)->isAlwaysExecutableModule() && + !getManager(m)->isSimultaneousExecutableAsCandidateModule(); + }, + [&]() { return false; }}); + + // Condition 2: Only modules that are either always executable or simultaneous executable can be + // added if there exists at least one executable module that is simultaneous but not always + // executable. + conditions.push_back( + {[this](const SceneModulePtr & m) { + return !getManager(m)->isAlwaysExecutableModule() && + getManager(m)->isSimultaneousExecutableAsCandidateModule(); + }, + [&]() { return getManager(module_ptr)->isSimultaneousExecutableAsCandidateModule(); }}); + + for (const auto & condition : conditions) { + const auto & find_block_module = condition.first; + const auto & is_executable = condition.second; + + const auto itr = + std::find_if(executable_modules.begin(), executable_modules.end(), find_block_module); + + if (itr != executable_modules.end() && is_executable()) { + executable_modules.push_back(module_ptr); + break; + } } } @@ -456,11 +527,49 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrisKeepLast(); + }; + move_to_end(approved_module_ptrs_, keep_last_module_cond); + } + // lock approved modules besides last one std::for_each(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [&](const auto & m) { m->lockOutputPath(); }); - approved_module_ptrs_.back()->unlockOutputPath(); + + // unlock only last approved module except keep last module. + { + const auto not_keep_last_modules = std::find_if( + approved_module_ptrs_.rbegin(), approved_module_ptrs_.rend(), + [this](const auto & m) { return !getManager(m)->isKeepLast(); }); + + if (not_keep_last_modules != approved_module_ptrs_.rend()) { + (*not_keep_last_modules)->unlockOutputPath(); + } + } /** * execute all approved modules. @@ -478,21 +587,31 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrisWaitingApproval(); }; + const auto not_keep_last_module = std::find_if( + approved_module_ptrs_.rbegin(), approved_module_ptrs_.rend(), + [this](const auto & m) { return !getManager(m)->isKeepLast(); }); - const auto itr = std::find_if( - approved_module_ptrs_.rbegin(), approved_module_ptrs_.rend(), waiting_approval_modules); + // convert reverse iterator -> iterator + const auto begin_itr = not_keep_last_module != approved_module_ptrs_.rend() + ? std::next(not_keep_last_module).base() + : approved_module_ptrs_.begin(); - if (itr != approved_module_ptrs_.rend()) { - const auto is_last_module = std::distance(approved_module_ptrs_.rbegin(), itr) == 0; - if (is_last_module) { - clearCandidateModules(); - candidate_module_ptrs_.push_back(*itr); + const auto waiting_approval_modules_itr = std::find_if( + begin_itr, approved_module_ptrs_.end(), + [](const auto & m) { return m->isWaitingApproval(); }); + + if (waiting_approval_modules_itr != approved_module_ptrs_.end()) { + clearCandidateModules(); + candidate_module_ptrs_.push_back(*waiting_approval_modules_itr); - debug_info_.emplace_back(*itr, Action::MOVE, "Back To Waiting Approval"); + debug_info_.emplace_back( + *waiting_approval_modules_itr, Action::MOVE, "Back To Waiting Approval"); - approved_module_ptrs_.pop_back(); - } + std::for_each( + waiting_approval_modules_itr, approved_module_ptrs_.end(), + [&results](const auto & m) { results.erase(m->name()); }); + + approved_module_ptrs_.erase(waiting_approval_modules_itr); std::for_each( manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); }); @@ -529,36 +648,43 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrname(); - const auto approved_modules_output = [&output_module_name, &results]() { - if (results.count(output_module_name) == 0) { - return results.at("root"); + const auto approved_modules_output = [&results, this]() { + const auto itr = std::find_if( + approved_module_ptrs_.rbegin(), approved_module_ptrs_.rend(), + [&results](const auto & m) { return results.count(m->name()) != 0; }); + + if (itr != approved_module_ptrs_.rend()) { + return results.at((*itr)->name()); } - return results.at(output_module_name); + return results.at("root"); }(); /** * remove success module immediately. if lane change module has succeeded, update root lanelet. */ { - const auto success_module_itr = std::partition( - approved_module_ptrs_.begin(), approved_module_ptrs_.end(), - [](const auto & m) { return m->getCurrentStatus() != ModuleStatus::SUCCESS; }); + const auto success_module_cond = [](const auto & m) { + return m->getCurrentStatus() == ModuleStatus::SUCCESS; + }; + move_to_end(approved_module_ptrs_, success_module_cond); + + const auto itr = + std::find_if(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), success_module_cond); const auto success_lane_change = std::any_of( - success_module_itr, approved_module_ptrs_.end(), + itr, approved_module_ptrs_.end(), [](const auto & m) { return m->name().find("lane_change") != std::string::npos; }); if (success_lane_change) { - root_lanelet_ = updateRootLanelet(data, true); + root_lanelet_ = updateRootLanelet(data); } - std::for_each(success_module_itr, approved_module_ptrs_.end(), [&](auto & m) { + std::for_each(itr, approved_module_ptrs_.end(), [&](auto & m) { debug_info_.emplace_back(m, Action::DELETE, "From Approved"); deleteExpiredModules(m); }); - approved_module_ptrs_.erase(success_module_itr, approved_module_ptrs_.end()); + approved_module_ptrs_.erase(itr, approved_module_ptrs_.end()); std::for_each( manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); }); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 543f02abe730f..51871b9f6c4f0 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -23,7 +23,10 @@ #include #include -#include +#include +#include +#include +#include #include #include @@ -50,6 +53,7 @@ using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; using motion_utils::findNearestIndex; using motion_utils::findNearestSegmentIndex; +using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::calcInterpolatedPose; using tier4_autoware_utils::calcLateralDeviation; @@ -331,7 +335,7 @@ ObjectData AvoidanceModule::createObjectData( // Find the footprint point closest to the path, set to object_data.overhang_distance. object_data.overhang_dist = utils::avoidance::calcEnvelopeOverhangDistance( - object_data, object_closest_pose, object_data.overhang_pose.position); + object_data, data.reference_path, object_data.overhang_pose.position); // Check whether the the ego should avoid the object. const auto & vehicle_width = planner_data_->parameters.vehicle_width; @@ -880,17 +884,35 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( } // output avoidance path under lateral jerk constraints. - const auto feasible_shift_length = PathShifter::calcLateralDistFromJerk( + const auto feasible_relative_shift_length = PathShifter::calcLateralDistFromJerk( remaining_distance, helper_.getLateralMaxJerkLimit(), helper_.getAvoidanceEgoSpeed()); - RCLCPP_WARN_THROTTLE( - getLogger(), *clock_, 1000, - "original shift length is not feasible. generate avoidance path under the constraints. " - "[original: (%.2f) actual: (%.2f)]", - std::abs(avoiding_shift), feasible_shift_length); + if (std::abs(feasible_relative_shift_length) < parameters_->lateral_execution_threshold) { + object.reason = "LessThanExecutionThreshold"; + return boost::none; + } + + const auto feasible_shift_length = + desire_shift_length > 0.0 ? feasible_relative_shift_length + current_ego_shift + : -1.0 * feasible_relative_shift_length + current_ego_shift; + + const auto feasible = + std::abs(feasible_shift_length - object.overhang_dist) < + 0.5 * planner_data_->parameters.vehicle_width + object_parameter.safety_buffer_lateral; + if (feasible) { + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 1000, "feasible shift length is not enough to avoid. "); + object.reason = AvoidanceDebugFactor::INSUFFICIENT_LATERAL_MARGIN; + return boost::none; + } - return desire_shift_length > 0.0 ? feasible_shift_length + current_ego_shift - : -1.0 * feasible_shift_length + current_ego_shift; + { + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 1000, "use feasible shift length. [original: (%.2f) actual: (%.2f)]", + std::abs(avoiding_shift), feasible_relative_shift_length); + } + + return feasible_shift_length; }; const auto is_forward_object = [](const auto & object) { return object.longitudinal > 0.0; }; @@ -982,6 +1004,9 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( if (is_valid_shift_line(al_avoid) && is_valid_shift_line(al_return)) { avoid_lines.push_back(al_avoid); avoid_lines.push_back(al_return); + } else { + o.reason = "InvalidShiftLine"; + continue; } o.is_avoidable = true; @@ -1852,27 +1877,36 @@ bool AvoidanceModule::isSafePath( avoid_data_, planner_data_, parameters_, is_right_shift.value()); for (const auto & object : safety_check_target_objects) { + auto current_debug_data = marker_utils::createObjectDebug(object); + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object.initial_pose.pose, object.shape); const auto is_object_front = utils::path_safety_checker::isTargetObjectFront(getEgoPose(), obj_polygon, p.vehicle_info); + const auto is_object_incoming = + std::abs(calcYawDeviation(getEgoPose(), object.initial_pose.pose)) > M_PI_2; + const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( object, parameters_->check_all_predicted_path); - const auto & ego_predicted_path = - is_object_front ? ego_predicted_path_for_front_object : ego_predicted_path_for_rear_object; + const auto & ego_predicted_path = is_object_front && !is_object_incoming + ? ego_predicted_path_for_front_object + : ego_predicted_path_for_rear_object; for (const auto & obj_path : obj_predicted_paths) { - CollisionCheckDebug collision{}; if (!utils::path_safety_checker::checkCollision( shifted_path.path, ego_predicted_path, object, obj_path, p, parameters_->rss_params, - hysteresis_factor, collision)) { + hysteresis_factor, current_debug_data.second)) { + marker_utils::updateCollisionCheckDebugMap( + debug.collision_check, current_debug_data, false); + safe_count_ = 0; return false; } } + marker_utils::updateCollisionCheckDebugMap(debug.collision_check, current_debug_data, true); } safe_count_++; @@ -2504,6 +2538,7 @@ void AvoidanceModule::updateData() void AvoidanceModule::processOnEntry() { initVariables(); + removeRTCStatus(); } void AvoidanceModule::processOnExit() @@ -2682,12 +2717,14 @@ void AvoidanceModule::updateDebugMarker( using marker_utils::createShiftGradMarkerArray; using marker_utils::createShiftLengthMarkerArray; using marker_utils::createShiftLineMarkerArray; + using marker_utils::showPolygon; + using marker_utils::showPredictedPath; + using marker_utils::showSafetyCheckInfo; using marker_utils::avoidance_marker::createAvoidLineMarkerArray; using marker_utils::avoidance_marker::createEgoStatusMarkerArray; using marker_utils::avoidance_marker::createOtherObjectsMarkerArray; using marker_utils::avoidance_marker::createOverhangFurthestLineStringMarkerArray; using marker_utils::avoidance_marker::createPredictedVehiclePositions; - using marker_utils::avoidance_marker::createUnsafeObjectsMarkerArray; using marker_utils::avoidance_marker::makeOverhangToRoadShoulderMarkerArray; using tier4_autoware_utils::appendMarkerArray; @@ -2711,9 +2748,11 @@ void AvoidanceModule::updateDebugMarker( add(createShiftLineMarkerArray(sl_arr, shifter.getBaseOffset(), ns, r, g, b, w)); }; + const auto addObjects = [&](const ObjectDataArray & objects, const auto & ns) { + add(createOtherObjectsMarkerArray(objects, ns)); + }; + add(createEgoStatusMarkerArray(data, getEgoPose(), "ego_status")); - add(createPredictedVehiclePositions( - debug.path_with_planned_velocity, "predicted_vehicle_positions")); const auto & path = data.reference_path; add(createPathMarkerArray(debug.center_line, "centerline", 0, 0.0, 0.5, 0.9)); @@ -2725,31 +2764,37 @@ void AvoidanceModule::updateDebugMarker( add(createLaneletsAreaMarkerArray(*debug.current_lanelets, "current_lanelet", 0.0, 1.0, 0.0)); add(createPolygonMarkerArray(debug.detection_area, "detection_area", 0L, 0.16, 1.0, 0.69, 0.1)); - add(createOtherObjectsMarkerArray( - data.other_objects, AvoidanceDebugFactor::OBJECT_IS_BEHIND_THRESHOLD)); - add(createOtherObjectsMarkerArray( - data.other_objects, AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD)); - add(createOtherObjectsMarkerArray( - data.other_objects, AvoidanceDebugFactor::OBJECT_BEHIND_PATH_GOAL)); - add(createOtherObjectsMarkerArray( - data.other_objects, AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE)); - add(createOtherObjectsMarkerArray(data.other_objects, AvoidanceDebugFactor::OBJECT_IS_NOT_TYPE)); - add(createOtherObjectsMarkerArray(data.other_objects, AvoidanceDebugFactor::NOT_PARKING_OBJECT)); - add(createOtherObjectsMarkerArray(data.other_objects, std::string("MovingObject"))); - add(createOtherObjectsMarkerArray(data.other_objects, std::string("CrosswalkUser"))); - add(createOtherObjectsMarkerArray(data.other_objects, std::string("OutOfTargetArea"))); - add(createOtherObjectsMarkerArray(data.other_objects, std::string("NotNeedAvoidance"))); - add(createOtherObjectsMarkerArray(data.other_objects, std::string("LessThanExecutionThreshold"))); - add(makeOverhangToRoadShoulderMarkerArray(data.target_objects, "overhang")); add(createOverhangFurthestLineStringMarkerArray(debug.bounds, "bounds", 1.0, 0.0, 1.0)); - add(createUnsafeObjectsMarkerArray(debug.unsafe_objects, "unsafe_objects")); + // ignore objects + { + addObjects(data.other_objects, AvoidanceDebugFactor::OBJECT_IS_BEHIND_THRESHOLD); + addObjects(data.other_objects, AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD); + addObjects(data.other_objects, AvoidanceDebugFactor::OBJECT_IS_NOT_TYPE); + addObjects(data.other_objects, AvoidanceDebugFactor::OBJECT_BEHIND_PATH_GOAL); + addObjects(data.other_objects, AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE); + addObjects(data.other_objects, AvoidanceDebugFactor::NOT_PARKING_OBJECT); + addObjects(data.other_objects, std::string("MovingObject")); + addObjects(data.other_objects, std::string("CrosswalkUser")); + addObjects(data.other_objects, std::string("OutOfTargetArea")); + addObjects(data.other_objects, std::string("NotNeedAvoidance")); + addObjects(data.other_objects, std::string("LessThanExecutionThreshold")); + } // parent object info - addAvoidLine(debug.registered_raw_shift, "p_registered_shift", 0.8, 0.8, 0.0); - addAvoidLine(debug.current_raw_shift, "p_current_raw_shift", 0.5, 0.2, 0.2); - addAvoidLine(debug.extra_return_shift, "p_extra_return_shift", 0.0, 0.5, 0.8); + { + addAvoidLine(debug.registered_raw_shift, "p_registered_shift", 0.8, 0.8, 0.0); + addAvoidLine(debug.current_raw_shift, "p_current_raw_shift", 0.5, 0.2, 0.2); + addAvoidLine(debug.extra_return_shift, "p_extra_return_shift", 0.0, 0.5, 0.8); + } + + // safety check + { + add(showSafetyCheckInfo(debug.collision_check, "object_debug_info")); + add(showPredictedPath(debug.collision_check, "ego_predicted_path")); + add(showPolygon(debug.collision_check, "ego_and_target_polygon_relation")); + } // shift length { diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index 8900e2b3e827c..912f867fd5701 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -14,6 +14,9 @@ #include "behavior_path_planner/scene_module/avoidance/manager.hpp" +#include "tier4_autoware_utils/ros/parameter.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" + #include #include diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp index 0bfda71a09079..b5ecffbcc9791 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include @@ -63,7 +63,7 @@ void appendObjectMarker(MarkerArray & marker_array, const geometry_msgs::msg::Po } void appendExtractedPolygonMarker( - MarkerArray & marker_array, const tier4_autoware_utils::Polygon2d & obj_poly) + MarkerArray & marker_array, const tier4_autoware_utils::Polygon2d & obj_poly, const double obj_z) { auto marker = tier4_autoware_utils::createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "extracted_polygons", marker_array.markers.size(), @@ -78,6 +78,7 @@ void appendExtractedPolygonMarker( geometry_msgs::msg::Point bound_geom_point; bound_geom_point.x = bound_point.x(); bound_geom_point.y = bound_point.y(); + bound_geom_point.z = obj_z; marker.points.push_back(bound_geom_point); } @@ -151,6 +152,25 @@ double calcDiffAngleAgainstPath( return diff_yaw; } +double calcDiffAngleBetweenPaths( + const std::vector & path_points, const PredictedPath & predicted_path) +{ + const size_t nearest_idx = + motion_utils::findNearestSegmentIndex(path_points, predicted_path.path.front().position); + const double ego_yaw = tf2::getYaw(path_points.at(nearest_idx).point.pose.orientation); + + constexpr size_t max_predicted_path_size = 5; + double signed_max_angle{0.0}; + for (size_t i = 0; i < std::min(max_predicted_path_size, predicted_path.path.size()); ++i) { + const double obj_yaw = tf2::getYaw(predicted_path.path.at(i).orientation); + const double diff_yaw = tier4_autoware_utils::normalizeRadian(obj_yaw - ego_yaw); + if (std::abs(signed_max_angle) < std::abs(diff_yaw)) { + signed_max_angle = diff_yaw; + } + } + return signed_max_angle; +} + double calcDistanceToPath( const std::vector & path_points, const geometry_msgs::msg::Point & target_pos) @@ -281,10 +301,10 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan() std::vector obstacles_for_drivable_area; for (const auto & object : target_objects_) { const auto obstacle_poly = [&]() { - if (parameters_->polygon_generation_method == "ego_path_base") { + if (object.polygon_generation_method == PolygonGenerationMethod::EGO_PATH_BASE) { return calcEgoPathBasedDynamicObstaclePolygon(object); } - if (parameters_->polygon_generation_method == "object_path_base") { + if (object.polygon_generation_method == PolygonGenerationMethod::OBJECT_PATH_BASE) { return calcObjectPathBasedDynamicObstaclePolygon(object); } throw std::logic_error("The polygon_generation_method's string is invalid."); @@ -294,7 +314,7 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan() {object.pose, obstacle_poly.value(), object.is_collision_left}); appendObjectMarker(info_marker_, object.pose); - appendExtractedPolygonMarker(debug_marker_, obstacle_poly.value()); + appendExtractedPolygonMarker(debug_marker_, obstacle_poly.value(), object.pose.position.z); } } @@ -375,6 +395,10 @@ void DynamicAvoidanceModule::updateTargetObjects() predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x, predicted_object.kinematics.initial_twist_with_covariance.twist.linear.y); const auto prev_object = getObstacleFromUuid(prev_objects, obj_uuid); + const auto obj_path = *std::max_element( + predicted_object.kinematics.predicted_paths.begin(), + predicted_object.kinematics.predicted_paths.end(), + [](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; }); // 1.a. check label const bool is_label_target_obstacle = @@ -391,7 +415,7 @@ void DynamicAvoidanceModule::updateTargetObjects() } // 1.c. check if object is not crossing ego's path - const double obj_angle = calcDiffAngleAgainstPath(prev_module_path->points, obj_pose); + const double obj_angle = calcDiffAngleBetweenPaths(prev_module_path->points, obj_path); const double max_crossing_object_angle = 0.0 <= obj_tangent_vel ? parameters_->max_overtaking_crossing_object_angle : parameters_->max_oncoming_crossing_object_angle; @@ -448,6 +472,7 @@ void DynamicAvoidanceModule::updateTargetObjects() // 2. Precise filtering of target objects and check if they should be avoided for (const auto & object : target_objects_manager_.getValidObjects()) { + PolygonGenerationMethod polygon_generation_method{PolygonGenerationMethod::EGO_PATH_BASE}; const auto obj_uuid = object.uuid; const auto prev_object = getObstacleFromUuid(prev_objects, obj_uuid); const auto obj_path = *std::max_element( @@ -472,8 +497,8 @@ void DynamicAvoidanceModule::updateTargetObjects() getLateralLongitudinalOffset(prev_module_path->points, object.pose, object.shape); // 2.c. check if object will not cut in - const bool will_object_cut_in = - willObjectCutIn(prev_module_path->points, obj_path, object.vel, lat_lon_offset); + const bool will_object_cut_in = willObjectCutIn( + prev_module_path->points, obj_path, object.vel, lat_lon_offset, polygon_generation_method); if (will_object_cut_in) { RCLCPP_INFO_EXPRESSION( getLogger(), parameters_->enable_debug_info, @@ -551,7 +576,8 @@ void DynamicAvoidanceModule::updateTargetObjects() const bool should_be_avoided = true; target_objects_manager_.updateObject( - obj_uuid, lon_offset_to_avoid, lat_offset_to_avoid, is_collision_left, should_be_avoided); + obj_uuid, lon_offset_to_avoid, lat_offset_to_avoid, is_collision_left, should_be_avoided, + polygon_generation_method); } } @@ -627,16 +653,26 @@ bool DynamicAvoidanceModule::isObjectFarFromPath( bool DynamicAvoidanceModule::willObjectCutIn( const std::vector & ego_path, const PredictedPath & predicted_path, - const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const + const double obj_tangent_vel, const LatLonOffset & lat_lon_offset, + PolygonGenerationMethod & polygon_generation_method) const { - constexpr double epsilon_path_lat_diff = 0.3; - - // Ignore oncoming object - if (obj_tangent_vel < 0) { + // Check if ego's path and object's path are close. + const bool will_object_cut_in = [&]() { + for (const auto & predicted_path_point : predicted_path.path) { + const double paths_lat_diff = + motion_utils::calcLateralOffset(ego_path, predicted_path_point.position); + if (std::abs(paths_lat_diff) < planner_data_->parameters.vehicle_width / 2.0) { + return true; + } + } + return false; + }(); + if (!will_object_cut_in) { + // The object's path will not cut in return false; } - // Ignore object close to the ego + // Ignore object longitudinally close to the ego const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(ego_path); const double relative_velocity = getEgoSpeed() - obj_tangent_vel; const double lon_offset_ego_to_obj = @@ -647,17 +683,11 @@ bool DynamicAvoidanceModule::willObjectCutIn( lon_offset_ego_to_obj < std::max( parameters_->min_lon_offset_ego_to_cut_in_object, relative_velocity * parameters_->min_time_to_start_cut_in)) { + polygon_generation_method = PolygonGenerationMethod::EGO_PATH_BASE; return false; } - for (const auto & predicted_path_point : predicted_path.path) { - const double paths_lat_diff = - motion_utils::calcLateralOffset(ego_path, predicted_path_point.position); - if (std::abs(paths_lat_diff) < epsilon_path_lat_diff) { - return true; - } - } - return false; + return true; } DynamicAvoidanceModule::DecisionWithReason DynamicAvoidanceModule::willObjectCutOut( diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp index c77d81457c15d..616194ea66ac7 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp @@ -14,6 +14,8 @@ #include "behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" + #include #include @@ -81,8 +83,6 @@ DynamicAvoidanceModuleManager::DynamicAvoidanceModuleManager( { // drivable_area_generation std::string ns = "dynamic_avoidance.drivable_area_generation."; - p.polygon_generation_method = - node->declare_parameter(ns + "polygon_generation_method"); p.min_obj_path_based_lon_polygon_margin = node->declare_parameter(ns + "object_path_base.min_longitudinal_polygon_margin"); p.lat_offset_from_obstacle = node->declare_parameter(ns + "lat_offset_from_obstacle"); @@ -181,9 +181,6 @@ void DynamicAvoidanceModuleManager::updateModuleParams( { // drivable_area_generation const std::string ns = "dynamic_avoidance.drivable_area_generation."; - - updateParam( - parameters, ns + "polygon_generation_method", p->polygon_generation_method); updateParam( parameters, ns + "object_path_base.min_longitudinal_polygon_margin", p->min_obj_path_based_lon_polygon_margin); diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index f37149f706436..12f6f243e9a9c 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -16,16 +16,17 @@ #include "behavior_path_planner/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner/utils/goal_planner/util.hpp" +#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "tier4_autoware_utils/math/unit_conversion.hpp" #include #include #include -#include #include -#include #include #include @@ -34,6 +35,7 @@ #include #include +using behavior_path_planner::utils::start_goal_planner_common::calcFeasibleDecelDistance; using motion_utils::calcDecelDistWithJerkAndAccConstraints; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; @@ -51,10 +53,12 @@ GoalPlannerModule::GoalPlannerModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, const std::unordered_map > & rtc_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{parameters} +: SceneModuleInterface{name, node, rtc_interface_ptr_map}, + parameters_{parameters}, + vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()} { LaneDepartureChecker lane_departure_checker{}; - lane_departure_checker.setVehicleInfo(vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()); + lane_departure_checker.setVehicleInfo(vehicle_info_); occupancy_grid_map_ = std::make_shared(); @@ -117,7 +121,7 @@ GoalPlannerModule::GoalPlannerModule( void GoalPlannerModule::resetStatus() { - PUllOverStatus initial_status{}; + PullOverStatus initial_status{}; status_ = initial_status; pull_over_path_candidates_.clear(); closest_start_pose_.reset(); @@ -248,12 +252,27 @@ void GoalPlannerModule::initializeOccupancyGridMap() occupancy_grid_map_->setParam(occupancy_grid_map_param); } +void GoalPlannerModule::initializeSafetyCheckParameters() +{ + utils::start_goal_planner_common::updateEgoPredictedPathParams( + ego_predicted_path_params_, parameters_); + utils::start_goal_planner_common::updateSafetyCheckParams(safety_check_params_, parameters_); + utils::start_goal_planner_common::updateObjectsFilteringParams( + objects_filtering_params_, parameters_); +} + void GoalPlannerModule::processOnEntry() { // Initialize occupancy grid map if (parameters_->use_occupancy_grid) { initializeOccupancyGridMap(); } + // Initialize safety checker + if (parameters_->safety_check_params.enable_safety_check) { + initializeSafetyCheckParameters(); + utils::start_goal_planner_common::initializeCollisionCheckDebugMap( + goal_planner_data_.collision_check); + } } void GoalPlannerModule::processOnExit() @@ -271,11 +290,7 @@ bool GoalPlannerModule::isExecutionRequested() const const auto & route_handler = planner_data_->route_handler; const Pose & current_pose = planner_data_->self_odometry->pose.pose; - const Pose & goal_pose = route_handler->getGoalPose(); - - // if goal is shoulder lane, allow goal modification - allow_goal_modification_ = - route_handler->isAllowedGoalModification() || checkOriginalGoalIsInShoulder(); + const Pose goal_pose = route_handler->getOriginalGoalPose(); // check if goal_pose is in current_lanes. lanelet::ConstLanelet current_lane{}; @@ -306,13 +321,19 @@ bool GoalPlannerModule::isExecutionRequested() const return false; } + // if goal modification is not allowed + // 1) goal_pose is in current_lanes, plan path to the original fixed goal + // 2) goal_pose is NOT in current_lanes, do not execute goal_planner + if (!goal_planner_utils::isAllowedGoalModification(route_handler)) { + return goal_is_in_current_lanes; + } + // if goal arc coordinates can be calculated, check if goal is in request_length const double self_to_goal_arc_length = utils::getSignedDistance(current_pose, goal_pose, current_lanes); - const double request_length = - goal_planner_utils::isAllowedGoalModification(route_handler, left_side_parking_) - ? calcModuleRequestLength() - : parameters_->minimum_request_length; + const double request_length = goal_planner_utils::isAllowedGoalModification(route_handler) + ? calcModuleRequestLength() + : parameters_->pull_over_minimum_request_length; if (self_to_goal_arc_length < 0.0 || self_to_goal_arc_length > request_length) { // if current position is far from goal or behind goal, do not execute goal_planner return false; @@ -321,15 +342,16 @@ bool GoalPlannerModule::isExecutionRequested() const // if goal modification is not allowed // 1) goal_pose is in current_lanes, plan path to the original fixed goal // 2) goal_pose is NOT in current_lanes, do not execute goal_planner - if (!goal_planner_utils::isAllowedGoalModification(route_handler, left_side_parking_)) { + if (!goal_planner_utils::isAllowedGoalModification(route_handler)) { return goal_is_in_current_lanes; } // if (A) or (B) is met execute pull over // (A) target lane is `road` and same to the current lanes // (B) target lane is `road_shoulder` and neighboring to the current lanes - const lanelet::ConstLanelets pull_over_lanes = - goal_planner_utils::getPullOverLanes(*(route_handler), left_side_parking_); + const lanelet::ConstLanelets pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(route_handler), left_side_parking_, parameters_->backward_goal_search_length, + parameters_->forward_goal_search_length); lanelet::ConstLanelet target_lane{}; lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &target_lane); if (!isCrossingPossible(current_lane, target_lane)) { @@ -341,26 +363,43 @@ bool GoalPlannerModule::isExecutionRequested() const bool GoalPlannerModule::isExecutionReady() const { + // TODO(Sugahara): should check safe or not but in the current flow, it is not possible. + if (status_.pull_over_path == nullptr) { + return true; + } + + if (status_.is_safe_static_objects && parameters_->safety_check_params.enable_safety_check) { + if (!isSafePath()) { + RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against dynamic objects"); + return false; + } + } return true; } double GoalPlannerModule::calcModuleRequestLength() const { - const auto min_stop_distance = calcFeasibleDecelDistance(0.0); + const auto min_stop_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); if (!min_stop_distance) { - return parameters_->minimum_request_length; + return parameters_->pull_over_minimum_request_length; } const double minimum_request_length = *min_stop_distance + parameters_->backward_goal_search_length + approximate_pull_over_distance_; - return std::max(minimum_request_length, parameters_->minimum_request_length); + return std::max(minimum_request_length, parameters_->pull_over_minimum_request_length); } Pose GoalPlannerModule::calcRefinedGoal(const Pose & goal_pose) const { - const lanelet::ConstLanelets pull_over_lanes = - goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_); + const double vehicle_width = planner_data_->parameters.vehicle_width; + const double base_link2front = planner_data_->parameters.base_link2front; + const double base_link2rear = planner_data_->parameters.base_link2rear; + + const lanelet::ConstLanelets pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, + parameters_->forward_goal_search_length); lanelet::Lanelet closest_pull_over_lanelet{}; lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &closest_pull_over_lanelet); @@ -390,15 +429,18 @@ Pose GoalPlannerModule::calcRefinedGoal(const Pose & goal_pose) const center_pose.orientation = tf2::toMsg(tf_quat); } - const auto distance_from_left_bound = utils::getSignedDistanceFromBoundary( - pull_over_lanes, vehicle_footprint_, center_pose, left_side_parking_); - if (!distance_from_left_bound) { + const auto distance_from_bound = utils::getSignedDistanceFromBoundary( + pull_over_lanes, vehicle_width, base_link2front, base_link2rear, center_pose, + left_side_parking_); + if (!distance_from_bound) { RCLCPP_ERROR(getLogger(), "fail to calculate refined goal"); return goal_pose; } + const double sign = left_side_parking_ ? -1.0 : 1.0; const double offset_from_center_line = - distance_from_left_bound.value() + parameters_->margin_from_boundary; + sign * (distance_from_bound.value() + parameters_->margin_from_boundary); + const auto refined_goal_pose = calcOffsetPose(center_pose, 0, -offset_from_center_line, 0); return refined_goal_pose; @@ -408,8 +450,7 @@ ModuleStatus GoalPlannerModule::updateState() { // finish module only when the goal is fixed if ( - !goal_planner_utils::isAllowedGoalModification( - planner_data_->route_handler, left_side_parking_) && + !goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler) && hasFinishedGoalPlanner()) { return ModuleStatus::SUCCESS; } @@ -446,7 +487,7 @@ bool GoalPlannerModule::planFreespacePath() mutex_.lock(); status_.pull_over_path = std::make_shared(*freespace_path); status_.current_path_idx = 0; - status_.is_safe = true; + status_.is_safe_static_objects = true; modified_goal_pose_ = goal_candidate; last_path_update_time_ = std::make_unique(clock_->now()); debug_data_.freespace_planner.is_planning = false; @@ -483,7 +524,7 @@ void GoalPlannerModule::returnToLaneParking() } mutex_.lock(); - status_.is_safe = true; + status_.is_safe_static_objects = true; status_.has_decided_path = false; status_.pull_over_path = status_.lane_parking_pull_over_path; status_.current_path_idx = 0; @@ -505,9 +546,9 @@ void GoalPlannerModule::generateGoalCandidates() } // calculate goal candidates - const Pose goal_pose = route_handler->getGoalPose(); + const Pose goal_pose = route_handler->getOriginalGoalPose(); refined_goal_pose_ = calcRefinedGoal(goal_pose); - if (goal_planner_utils::isAllowedGoalModification(route_handler, left_side_parking_)) { + if (goal_planner_utils::isAllowedGoalModification(route_handler)) { goal_searcher_->setPlannerData(planner_data_); goal_candidates_ = goal_searcher_->search(refined_goal_pose_); } else { @@ -520,10 +561,14 @@ void GoalPlannerModule::generateGoalCandidates() BehaviorModuleOutput GoalPlannerModule::plan() { + resetPathCandidate(); + resetPathReference(); + generateGoalCandidates(); - if (goal_planner_utils::isAllowedGoalModification( - planner_data_->route_handler, left_side_parking_)) { + path_reference_ = getPreviousModuleOutput().reference_path; + + if (goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { return planWithGoalModification(); } else { fixed_goal_planner_->setPreviousModuleOutput(getPreviousModuleOutput()); @@ -540,7 +585,7 @@ void GoalPlannerModule::selectSafePullOverPath() const auto pull_over_path_candidates = pull_over_path_candidates_; const auto goal_candidates = goal_candidates_; mutex_.unlock(); - status_.is_safe = false; + status_.is_safe_static_objects = false; for (const auto & pull_over_path : pull_over_path_candidates) { // check if goal is safe const auto goal_candidate_it = std::find_if( @@ -559,7 +604,7 @@ void GoalPlannerModule::selectSafePullOverPath() continue; } - status_.is_safe = true; + status_.is_safe_static_objects = true; mutex_.lock(); status_.pull_over_path = std::make_shared(pull_over_path); status_.current_path_idx = 0; @@ -571,7 +616,7 @@ void GoalPlannerModule::selectSafePullOverPath() } // decelerate before the search area start - if (status_.is_safe) { + if (status_.is_safe_static_objects) { const auto search_start_offset_pose = calcLongitudinalOffsetPose( status_.pull_over_path->getFullPath().points, refined_goal_pose_.position, -parameters_->backward_goal_search_length - planner_data_->parameters.base_link2front - @@ -582,7 +627,9 @@ void GoalPlannerModule::selectSafePullOverPath() decelerateBeforeSearchStart(*search_start_offset_pose, first_path); } else { // if already passed the search start offset pose, set pull_over_velocity to first_path. - const auto min_decel_distance = calcFeasibleDecelDistance(parameters_->pull_over_velocity); + const auto min_decel_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, + parameters_->pull_over_velocity); for (auto & p : first_path.points) { const double distance_from_ego = calcSignedArcLengthFromEgo(first_path, p.point.pose); if (min_decel_distance && distance_from_ego < *min_decel_distance) { @@ -607,27 +654,35 @@ void GoalPlannerModule::setLanes() planner_data_, parameters_->backward_goal_search_length, parameters_->forward_goal_search_length, /*forward_only_in_route*/ false); - status_.pull_over_lanes = - goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_); + status_.pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, + parameters_->forward_goal_search_length); status_.lanes = utils::generateDrivableLanesWithShoulderLanes(status_.current_lanes, status_.pull_over_lanes); } void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) { - if (status_.is_safe) { - // clear stop pose when the path is safe and activated - if (isActivated()) { - resetWallPoses(); + if (status_.is_safe_static_objects) { + if (isSafePath()) { + // clear stop pose when the path is safe against static/dynamic objects and activated + if (isActivated()) { + resetWallPoses(); + } + // keep stop if not enough time passed, + // because it takes time for the trajectory to be reflected + auto current_path = getCurrentPath(); + keepStoppedWithCurrentPath(current_path); + + output.path = std::make_shared(current_path); + output.reference_path = getPreviousModuleOutput().reference_path; + } else if (status_.has_decided_path && isActivated()) { + // situation : not safe against dynamic objects after approval + // insert stop point in current path if ego is able to stop with acceleration and jerk + // constraints + setStopPathFromCurrentPath(output); } - // keep stop if not enough time passed, - // because it takes time for the trajectory to be reflected - auto current_path = getCurrentPath(); - keepStoppedWithCurrentPath(current_path); - - output.path = std::make_shared(current_path); - output.reference_path = getPreviousModuleOutput().reference_path; } else { // not safe: use stop_path setStopPath(output); @@ -644,7 +699,8 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) // for the next loop setOutput(). // this is used to determine whether to generate a new stop path or keep the current stop path. - status_.prev_is_safe = status_.is_safe; + status_.prev_is_safe = status_.is_safe_static_objects; + status_.prev_is_safe_dynamic_objects = status_.is_safe_dynamic_objects; } void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) @@ -652,7 +708,6 @@ void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) if (status_.prev_is_safe || status_.prev_stop_path == nullptr) { // safe -> not_safe or no prev_stop_path: generate new stop_path output.path = std::make_shared(generateStopPath()); - output.reference_path = getPreviousModuleOutput().reference_path; status_.prev_stop_path = output.path; // set stop path as pull over path mutex_.lock(); @@ -667,10 +722,34 @@ void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) } else { // not_safe -> not_safe: use previous stop path output.path = status_.prev_stop_path; - output.reference_path = getPreviousModuleOutput().reference_path; RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull_over path, use previous stop path"); } + output.reference_path = getPreviousModuleOutput().reference_path; +} + +void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output) +{ + // safe or not safe(no feasible stop_path found) -> not_safe: try to find new feasible stop_path + if (status_.prev_is_safe_dynamic_objects || status_.prev_stop_path_after_approval == nullptr) { + if (const auto stop_inserted_path = generateStopInsertedCurrentPath()) { + output.path = std::make_shared(*stop_inserted_path); + status_.prev_stop_path_after_approval = output.path; + RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, generate stop path"); + } else { + output.path = std::make_shared(getCurrentPath()); + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, + "Collision detected, no feasible stop path found, cannot stop."); + } + std::lock_guard lock(mutex_); + last_path_update_time_ = std::make_unique(clock_->now()); + } else { + // not_safe safe(no feasible stop path found) -> not_safe: use previous stop path + output.path = status_.prev_stop_path_after_approval; + RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, use previous stop path"); + } + output.reference_path = getPreviousModuleOutput().reference_path; } void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const @@ -693,7 +772,7 @@ void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const void GoalPlannerModule::setModifiedGoal(BehaviorModuleOutput & output) const { const auto & route_handler = planner_data_->route_handler; - if (status_.is_safe) { + if (status_.is_safe_static_objects) { PoseWithUuidStamped modified_goal{}; modified_goal.uuid = route_handler->getRouteUuid(); modified_goal.pose = modified_goal_pose_->goal_pose; @@ -741,7 +820,7 @@ bool GoalPlannerModule::hasDecidedPath() const } // if path is not safe, not decided - if (!status_.is_safe) { + if (!status_.is_safe_static_objects) { return false; } @@ -840,8 +919,12 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() { - if (goal_planner_utils::isAllowedGoalModification( - planner_data_->route_handler, left_side_parking_)) { + resetPathCandidate(); + resetPathReference(); + + path_reference_ = getPreviousModuleOutput().reference_path; + + if (goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { return planWaitingApprovalWithGoalModification(); } else { fixed_goal_planner_->setPreviousModuleOutput(getPreviousModuleOutput()); @@ -858,7 +941,7 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification( out.modified_goal = plan().modified_goal; // update status_ out.path = std::make_shared(generateStopPath()); out.reference_path = getPreviousModuleOutput().reference_path; - path_candidate_ = status_.is_safe + path_candidate_ = status_.is_safe_static_objects ? std::make_shared(status_.pull_over_path->getFullPath()) : out.path; path_reference_ = getPreviousModuleOutput().reference_path; @@ -953,17 +1036,19 @@ PathWithLaneId GoalPlannerModule::generateStopPath() reference_path.points, refined_goal_pose_.position, -parameters_->backward_goal_search_length - common_parameters.base_link2front - approximate_pull_over_distance_); - if (!status_.is_safe && !closest_start_pose_ && !search_start_offset_pose) { + if (!status_.is_safe_static_objects && !closest_start_pose_ && !search_start_offset_pose) { return generateFeasibleStopPath(); } - const Pose stop_pose = status_.is_safe ? status_.pull_over_path->start_pose - : (closest_start_pose_ ? closest_start_pose_.value() - : *search_start_offset_pose); + const Pose stop_pose = + status_.is_safe_static_objects + ? status_.pull_over_path->start_pose + : (closest_start_pose_ ? closest_start_pose_.value() : *search_start_offset_pose); // if stop pose is closer than min_stop_distance, stop as soon as possible const double ego_to_stop_distance = calcSignedArcLengthFromEgo(reference_path, stop_pose); - const auto min_stop_distance = calcFeasibleDecelDistance(0.0); + const auto min_stop_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); const double eps_vel = 0.01; const bool is_stopped = std::abs(current_vel) < eps_vel; const double buffer = is_stopped ? stop_distance_buffer_ : 0.0; @@ -980,7 +1065,9 @@ PathWithLaneId GoalPlannerModule::generateStopPath() decelerateBeforeSearchStart(*search_start_offset_pose, reference_path); } else { // if already passed the search start offset pose, set pull_over_velocity to reference_path. - const auto min_decel_distance = calcFeasibleDecelDistance(pull_over_velocity); + const auto min_decel_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, + pull_over_velocity); for (auto & p : reference_path.points) { const double distance_from_ego = calcSignedArcLengthFromEgo(reference_path, p.point.pose); if (min_decel_distance && distance_from_ego < *min_decel_distance) { @@ -1008,7 +1095,8 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() auto stop_path = route_handler->getCenterLinePath(status_.current_lanes, s_start, s_end, true); // calc minimum stop distance under maximum deceleration - const auto min_stop_distance = calcFeasibleDecelDistance(0.0); + const auto min_stop_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); if (!min_stop_distance) { return stop_path; } @@ -1023,6 +1111,34 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() return stop_path; } +std::optional GoalPlannerModule::generateStopInsertedCurrentPath() +{ + auto current_path = getCurrentPath(); + if (current_path.points.empty()) { + return {}; + } + + // try to insert stop point in current_path after approval + // but if can't stop with constraints(maximum deceleration, maximum jerk), don't insert stop point + const auto min_stop_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); + if (!min_stop_distance) { + return {}; + } + + // set stop point + const auto stop_idx = motion_utils::insertStopPoint( + planner_data_->self_odometry->pose.pose, *min_stop_distance, current_path.points); + + if (!stop_idx) { + return {}; + } else { + stop_pose_ = current_path.points.at(*stop_idx).point.pose; + } + + return current_path; +} + void GoalPlannerModule::transitionToNextPathIfFinishingCurrentPath() { if (isActivated() && last_approved_time_ != nullptr) { @@ -1050,6 +1166,10 @@ bool GoalPlannerModule::incrementPathIndex() PathWithLaneId GoalPlannerModule::getCurrentPath() const { + if (status_.pull_over_path == nullptr) { + return PathWithLaneId{}; + } + if (status_.pull_over_path->partial_paths.size() <= status_.current_path_idx) { return PathWithLaneId{}; } @@ -1093,7 +1213,7 @@ bool GoalPlannerModule::isStuck() } // not found safe path - if (!status_.is_safe) { + if (!status_.is_safe_static_objects) { return true; } @@ -1107,7 +1227,7 @@ bool GoalPlannerModule::isStuck() bool GoalPlannerModule::hasFinishedCurrentPath() { - const auto & current_path_end = getCurrentPath().points.back(); + const auto current_path_end = getCurrentPath().points.back(); const auto & self_pose = planner_data_->self_odometry->pose.pose; const bool is_near_target = tier4_autoware_utils::calcDistance2d(current_path_end, self_pose) < parameters_->th_arrived_distance; @@ -1215,7 +1335,8 @@ bool GoalPlannerModule::hasEnoughDistance(const PullOverPath & pull_over_path) c return false; } - const auto current_to_stop_distance = calcFeasibleDecelDistance(0.0); + const auto current_to_stop_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); if (!current_to_stop_distance) { return false; } @@ -1251,30 +1372,6 @@ void GoalPlannerModule::keepStoppedWithCurrentPath(PathWithLaneId & path) } } -boost::optional GoalPlannerModule::calcFeasibleDecelDistance( - const double target_velocity) const -{ - const auto v_now = planner_data_->self_odometry->twist.twist.linear.x; - const auto a_now = planner_data_->self_acceleration->accel.accel.linear.x; - const auto a_lim = parameters_->maximum_deceleration; // positive value - const auto j_lim = parameters_->maximum_jerk; - - if (v_now < target_velocity) { - return 0.0; - } - - auto min_stop_distance = calcDecelDistWithJerkAndAccConstraints( - v_now, target_velocity, a_now, -a_lim, j_lim, -1.0 * j_lim); - - if (!min_stop_distance) { - return {}; - } - - min_stop_distance = std::max(*min_stop_distance, 0.0); - - return min_stop_distance; -} - double GoalPlannerModule::calcSignedArcLengthFromEgo( const PathWithLaneId & path, const Pose & pose) const { @@ -1299,7 +1396,8 @@ void GoalPlannerModule::decelerateForTurnSignal(const Pose & stop_pose, PathWith const float decel_vel = std::min(point.point.longitudinal_velocity_mps, static_cast(distance_to_stop / time)); const double distance_from_ego = calcSignedArcLengthFromEgo(path, point.point.pose); - const auto min_decel_distance = calcFeasibleDecelDistance(decel_vel); + const auto min_decel_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, decel_vel); // when current velocity already lower than decel_vel, min_decel_distance will be 0.0, // and do not need to decelerate. @@ -1317,7 +1415,8 @@ void GoalPlannerModule::decelerateForTurnSignal(const Pose & stop_pose, PathWith } const double stop_point_length = calcSignedArcLength(path.points, 0, stop_pose.position); - const auto min_stop_distance = calcFeasibleDecelDistance(0.0); + const auto min_stop_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); if (min_stop_distance && *min_stop_distance < stop_point_length) { const auto stop_point = utils::insertStopPoint(stop_point_length, path); @@ -1331,7 +1430,9 @@ void GoalPlannerModule::decelerateBeforeSearchStart( const Pose & current_pose = planner_data_->self_odometry->pose.pose; // slow down before the search area. - const auto min_decel_distance = calcFeasibleDecelDistance(pull_over_velocity); + const auto min_decel_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, + pull_over_velocity); if (min_decel_distance) { const double distance_to_search_start = calcSignedArcLengthFromEgo(path, search_start_offset_pose); @@ -1439,12 +1540,100 @@ bool GoalPlannerModule::isCrossingPossible(const PullOverPath & pull_over_path) return isCrossingPossible(start_pose, end_pose, lanes); } +void GoalPlannerModule::updateSafetyCheckTargetObjectsData( + const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane, + const std::vector & ego_predicted_path) const +{ + goal_planner_data_.filtered_objects = filtered_objects; + goal_planner_data_.target_objects_on_lane = target_objects_on_lane; + goal_planner_data_.ego_predicted_path = ego_predicted_path; +} + +bool GoalPlannerModule::isSafePath() const +{ + const auto pull_over_path = getCurrentPath(); + const auto & current_pose = planner_data_->self_odometry->pose.pose; + const double current_velocity = std::hypot( + planner_data_->self_odometry->twist.twist.linear.x, + planner_data_->self_odometry->twist.twist.linear.y); + const auto & dynamic_object = planner_data_->dynamic_object; + const auto & route_handler = planner_data_->route_handler; + const double backward_path_length = planner_data_->parameters.backward_path_length; + const auto current_lanes = utils::getExtendedCurrentLanes( + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *route_handler, left_side_parking_, parameters_->backward_goal_search_length, + parameters_->forward_goal_search_length); + const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(pull_over_path.points); + const std::pair terminal_velocity_and_accel = + utils::start_goal_planner_common::getPairsTerminalVelocityAndAccel( + status_.pull_over_path->pairs_terminal_velocity_and_accel, status_.current_path_idx); + RCLCPP_DEBUG( + getLogger(), "pairs_terminal_velocity_and_accel for goal_planner: %f, %f", + terminal_velocity_and_accel.first, terminal_velocity_and_accel.second); + RCLCPP_DEBUG(getLogger(), "current_path_idx %ld", status_.current_path_idx); + utils::start_goal_planner_common::updatePathProperty( + ego_predicted_path_params_, terminal_velocity_and_accel); + const auto ego_predicted_path = + behavior_path_planner::utils::path_safety_checker::createPredictedPath( + ego_predicted_path_params_, pull_over_path.points, current_pose, current_velocity, + ego_seg_idx); + + // filtering objects with velocity, position and class + const auto & filtered_objects = utils::path_safety_checker::filterObjects( + dynamic_object, route_handler, pull_over_lanes, current_pose.position, + objects_filtering_params_); + + // filtering objects based on the current position's lane + const auto & target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane( + pull_over_lanes, route_handler, filtered_objects, objects_filtering_params_); + + const double hysteresis_factor = + status_.is_safe_dynamic_objects ? 1.0 : parameters_->hysteresis_factor_expand_rate; + + utils::start_goal_planner_common::updateSafetyCheckTargetObjectsData( + goal_planner_data_, filtered_objects, target_objects_on_lane, ego_predicted_path); + + bool is_safe_dynamic_objects = true; + // Check for collisions with each predicted path of the object + for (const auto & object : target_objects_on_lane.on_current_lane) { + auto current_debug_data = marker_utils::createObjectDebug(object); + + bool is_safe_dynamic_object = true; + + const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( + object, objects_filtering_params_->check_all_predicted_path); + + // If a collision is detected, mark the object as unsafe and break the loop + for (const auto & obj_path : obj_predicted_paths) { + if (!utils::path_safety_checker::checkCollision( + pull_over_path, ego_predicted_path, object, obj_path, planner_data_->parameters, + safety_check_params_->rss_params, hysteresis_factor, current_debug_data.second)) { + marker_utils::updateCollisionCheckDebugMap( + goal_planner_data_.collision_check, current_debug_data, false); + is_safe_dynamic_objects = false; + is_safe_dynamic_object = false; + break; + } + } + if (is_safe_dynamic_object) { + marker_utils::updateCollisionCheckDebugMap( + goal_planner_data_.collision_check, current_debug_data, is_safe_dynamic_object); + } + } + + return is_safe_dynamic_objects; +} + void GoalPlannerModule::setDebugData() { debug_marker_.markers.clear(); + using marker_utils::createObjectsMarkerArray; using marker_utils::createPathMarkerArray; using marker_utils::createPoseMarkerArray; + using marker_utils::createPredictedPathMarkerArray; using motion_utils::createStopVirtualWallMarker; using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerColor; @@ -1458,9 +1647,7 @@ void GoalPlannerModule::setDebugData() } tier4_autoware_utils::appendMarkerArray(added, &debug_marker_); }; - - if (goal_planner_utils::isAllowedGoalModification( - planner_data_->route_handler, left_side_parking_)) { + if (goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { // Visualize pull over areas const auto color = status_.has_decided_path ? createMarkerColor(1.0, 1.0, 0.0, 0.999) // yellow : createMarkerColor(0.0, 1.0, 0.0, 0.999); // green @@ -1473,7 +1660,7 @@ void GoalPlannerModule::setDebugData() } // Visualize path and related pose - if (status_.is_safe) { + if (status_.is_safe_static_objects) { add(createPoseMarkerArray( status_.pull_over_path->start_pose, "pull_over_start_pose", 0, 0.3, 0.3, 0.9)); add(createPoseMarkerArray( @@ -1488,13 +1675,24 @@ void GoalPlannerModule::setDebugData() add( createPathMarkerArray(partial_path, "partial_path_" + std::to_string(i), 0, 0.9, 0.5, 0.9)); } + if (goal_planner_data_.ego_predicted_path.size() > 0) { + const auto & ego_predicted_path = utils::path_safety_checker::convertToPredictedPath( + goal_planner_data_.ego_predicted_path, ego_predicted_path_params_->time_resolution); + add(createPredictedPathMarkerArray( + ego_predicted_path, vehicle_info_, "ego_predicted_path", 0, 0.0, 0.5, 0.9)); + } + + if (goal_planner_data_.filtered_objects.objects.size() > 0) { + add(createObjectsMarkerArray( + goal_planner_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9)); + } } // Visualize planner type text { visualization_msgs::msg::MarkerArray planner_type_marker_array{}; - const auto color = status_.is_safe ? createMarkerColor(1.0, 1.0, 1.0, 0.99) - : createMarkerColor(1.0, 0.0, 0.0, 0.99); + const auto color = status_.is_safe_static_objects ? createMarkerColor(1.0, 1.0, 1.0, 0.99) + : createMarkerColor(1.0, 0.0, 0.0, 0.99); auto marker = createDefaultMarker( header.frame_id, header.stamp, "planner_type", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); @@ -1548,10 +1746,11 @@ void GoalPlannerModule::printParkingPositionError() const bool GoalPlannerModule::checkOriginalGoalIsInShoulder() const { const auto & route_handler = planner_data_->route_handler; - const Pose & goal_pose = route_handler->getGoalPose(); + const Pose & goal_pose = route_handler->getOriginalGoalPose(); - const lanelet::ConstLanelets pull_over_lanes = - goal_planner_utils::getPullOverLanes(*(route_handler), left_side_parking_); + const lanelet::ConstLanelets pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(route_handler), left_side_parking_, parameters_->backward_goal_search_length, + parameters_->forward_goal_search_length); lanelet::ConstLanelet target_lane{}; lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &target_lane); diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp index 7df1749a0fe82..ba147a28173c3 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp @@ -15,8 +15,8 @@ #include "behavior_path_planner/scene_module/goal_planner/manager.hpp" #include "behavior_path_planner/utils/goal_planner/util.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" -#include #include #include @@ -32,18 +32,17 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( { GoalPlannerParameters p; + const std::string base_ns = "goal_planner."; // general params { - std::string ns = "goal_planner."; - p.minimum_request_length = node->declare_parameter(ns + "minimum_request_length"); - p.th_stopped_velocity = node->declare_parameter(ns + "th_stopped_velocity"); - p.th_arrived_distance = node->declare_parameter(ns + "th_arrived_distance"); - p.th_stopped_time = node->declare_parameter(ns + "th_stopped_time"); + p.th_stopped_velocity = node->declare_parameter(base_ns + "th_stopped_velocity"); + p.th_arrived_distance = node->declare_parameter(base_ns + "th_arrived_distance"); + p.th_stopped_time = node->declare_parameter(base_ns + "th_stopped_time"); } // goal search { - std::string ns = "goal_planner.goal_search."; + const std::string ns = base_ns + "goal_search."; p.search_priority = node->declare_parameter(ns + "search_priority"); p.forward_goal_search_length = node->declare_parameter(ns + "forward_goal_search_length"); @@ -72,7 +71,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // occupancy grid map { - std::string ns = "goal_planner.occupancy_grid."; + const std::string ns = base_ns + "occupancy_grid."; p.use_occupancy_grid = node->declare_parameter(ns + "use_occupancy_grid"); p.use_occupancy_grid_for_longitudinal_margin = node->declare_parameter(ns + "use_occupancy_grid_for_longitudinal_margin"); @@ -84,7 +83,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // object recognition { - std::string ns = "goal_planner.object_recognition."; + const std::string ns = base_ns + "object_recognition."; p.use_object_recognition = node->declare_parameter(ns + "use_object_recognition"); p.object_recognition_collision_check_margin = node->declare_parameter(ns + "object_recognition_collision_check_margin"); @@ -95,7 +94,9 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // pull over general params { - std::string ns = "goal_planner.pull_over."; + const std::string ns = base_ns + "pull_over."; + p.pull_over_minimum_request_length = + node->declare_parameter(ns + "minimum_request_length"); p.pull_over_velocity = node->declare_parameter(ns + "pull_over_velocity"); p.pull_over_minimum_velocity = node->declare_parameter(ns + "pull_over_minimum_velocity"); @@ -106,7 +107,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // shift parking { - std::string ns = "goal_planner.pull_over.shift_parking."; + const std::string ns = base_ns + "pull_over.shift_parking."; p.enable_shift_parking = node->declare_parameter(ns + "enable_shift_parking"); p.shift_sampling_num = node->declare_parameter(ns + "shift_sampling_num"); p.maximum_lateral_jerk = node->declare_parameter(ns + "maximum_lateral_jerk"); @@ -118,7 +119,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // forward parallel parking forward { - std::string ns = "goal_planner.pull_over.parallel_parking.forward."; + const std::string ns = base_ns + "pull_over.parallel_parking.forward."; p.enable_arc_forward_parking = node->declare_parameter(ns + "enable_arc_forward_parking"); p.parallel_parking_parameters.after_forward_parking_straight_distance = node->declare_parameter(ns + "after_forward_parking_straight_distance"); @@ -134,7 +135,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // forward parallel parking backward { - std::string ns = "goal_planner.pull_over.parallel_parking.backward."; + const std::string ns = base_ns + "pull_over.parallel_parking.backward."; p.enable_arc_backward_parking = node->declare_parameter(ns + "enable_arc_backward_parking"); p.parallel_parking_parameters.after_backward_parking_straight_distance = @@ -151,7 +152,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // freespace parking general params { - std::string ns = "goal_planner.pull_over.freespace_parking."; + const std::string ns = base_ns + "pull_over.freespace_parking."; p.enable_freespace_parking = node->declare_parameter(ns + "enable_freespace_parking"); p.freespace_parking_algorithm = node->declare_parameter(ns + "freespace_parking_algorithm"); @@ -174,7 +175,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // freespace parking search config { - std::string ns = "goal_planner.pull_over.freespace_parking.search_configs."; + const std::string ns = base_ns + "pull_over.freespace_parking.search_configs."; p.freespace_parking_common_parameters.theta_size = node->declare_parameter(ns + "theta_size"); p.freespace_parking_common_parameters.angle_goal_range = @@ -191,14 +192,14 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // freespace parking costmap configs { - std::string ns = "goal_planner.pull_over.freespace_parking.costmap_configs."; + const std::string ns = base_ns + "pull_over.freespace_parking.costmap_configs."; p.freespace_parking_common_parameters.obstacle_threshold = node->declare_parameter(ns + "obstacle_threshold"); } // freespace parking astar { - std::string ns = "goal_planner.pull_over.freespace_parking.astar."; + const std::string ns = base_ns + "pull_over.freespace_parking.astar."; p.astar_parameters.only_behind_solutions = node->declare_parameter(ns + "only_behind_solutions"); p.astar_parameters.use_back = node->declare_parameter(ns + "use_back"); @@ -208,7 +209,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // freespace parking rrtstar { - std::string ns = "goal_planner.pull_over.freespace_parking.rrtstar."; + const std::string ns = base_ns + "pull_over.freespace_parking.rrtstar."; p.rrt_star_parameters.enable_update = node->declare_parameter(ns + "enable_update"); p.rrt_star_parameters.use_informed_sampling = node->declare_parameter(ns + "use_informed_sampling"); @@ -218,9 +219,125 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( p.rrt_star_parameters.margin = node->declare_parameter(ns + "margin"); } + // stop condition + { + p.maximum_deceleration_for_stop = + node->declare_parameter(base_ns + "stop_condition.maximum_deceleration_for_stop"); + p.maximum_jerk_for_stop = + node->declare_parameter(base_ns + "stop_condition.maximum_jerk_for_stop"); + } + + std::string path_safety_check_ns = "goal_planner.path_safety_check."; + + // EgoPredictedPath + std::string ego_path_ns = path_safety_check_ns + "ego_predicted_path."; + { + p.ego_predicted_path_params.acceleration = + node->declare_parameter(ego_path_ns + "acceleration"); + p.ego_predicted_path_params.time_horizon = + node->declare_parameter(ego_path_ns + "time_horizon"); + p.ego_predicted_path_params.time_resolution = + node->declare_parameter(ego_path_ns + "time_resolution"); + p.ego_predicted_path_params.min_slow_speed = + node->declare_parameter(ego_path_ns + "min_slow_speed"); + p.ego_predicted_path_params.delay_until_departure = + node->declare_parameter(ego_path_ns + "delay_until_departure"); + p.ego_predicted_path_params.target_velocity = + node->declare_parameter(ego_path_ns + "target_velocity"); + } + + // ObjectFilteringParams + std::string obj_filter_ns = path_safety_check_ns + "target_filtering."; + { + p.objects_filtering_params.safety_check_time_horizon = + node->declare_parameter(obj_filter_ns + "safety_check_time_horizon"); + p.objects_filtering_params.safety_check_time_resolution = + node->declare_parameter(obj_filter_ns + "safety_check_time_resolution"); + p.objects_filtering_params.object_check_forward_distance = + node->declare_parameter(obj_filter_ns + "object_check_forward_distance"); + p.objects_filtering_params.object_check_backward_distance = + node->declare_parameter(obj_filter_ns + "object_check_backward_distance"); + p.objects_filtering_params.ignore_object_velocity_threshold = + node->declare_parameter(obj_filter_ns + "ignore_object_velocity_threshold"); + p.objects_filtering_params.include_opposite_lane = + node->declare_parameter(obj_filter_ns + "include_opposite_lane"); + p.objects_filtering_params.invert_opposite_lane = + node->declare_parameter(obj_filter_ns + "invert_opposite_lane"); + p.objects_filtering_params.check_all_predicted_path = + node->declare_parameter(obj_filter_ns + "check_all_predicted_path"); + p.objects_filtering_params.use_all_predicted_path = + node->declare_parameter(obj_filter_ns + "use_all_predicted_path"); + p.objects_filtering_params.use_predicted_path_outside_lanelet = + node->declare_parameter(obj_filter_ns + "use_predicted_path_outside_lanelet"); + } + + // ObjectTypesToCheck + std::string obj_types_ns = obj_filter_ns + "object_types_to_check."; + { + p.objects_filtering_params.object_types_to_check.check_car = + node->declare_parameter(obj_types_ns + "check_car"); + p.objects_filtering_params.object_types_to_check.check_truck = + node->declare_parameter(obj_types_ns + "check_truck"); + p.objects_filtering_params.object_types_to_check.check_bus = + node->declare_parameter(obj_types_ns + "check_bus"); + p.objects_filtering_params.object_types_to_check.check_trailer = + node->declare_parameter(obj_types_ns + "check_trailer"); + p.objects_filtering_params.object_types_to_check.check_unknown = + node->declare_parameter(obj_types_ns + "check_unknown"); + p.objects_filtering_params.object_types_to_check.check_bicycle = + node->declare_parameter(obj_types_ns + "check_bicycle"); + p.objects_filtering_params.object_types_to_check.check_motorcycle = + node->declare_parameter(obj_types_ns + "check_motorcycle"); + p.objects_filtering_params.object_types_to_check.check_pedestrian = + node->declare_parameter(obj_types_ns + "check_pedestrian"); + } + + // ObjectLaneConfiguration + std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration."; + { + p.objects_filtering_params.object_lane_configuration.check_current_lane = + node->declare_parameter(obj_lane_ns + "check_current_lane"); + p.objects_filtering_params.object_lane_configuration.check_right_lane = + node->declare_parameter(obj_lane_ns + "check_right_side_lane"); + p.objects_filtering_params.object_lane_configuration.check_left_lane = + node->declare_parameter(obj_lane_ns + "check_left_side_lane"); + p.objects_filtering_params.object_lane_configuration.check_shoulder_lane = + node->declare_parameter(obj_lane_ns + "check_shoulder_lane"); + p.objects_filtering_params.object_lane_configuration.check_other_lane = + node->declare_parameter(obj_lane_ns + "check_other_lane"); + } + + // SafetyCheckParams + std::string safety_check_ns = path_safety_check_ns + "safety_check_params."; + { + p.safety_check_params.enable_safety_check = + node->declare_parameter(safety_check_ns + "enable_safety_check"); + p.safety_check_params.backward_path_length = + node->declare_parameter(safety_check_ns + "backward_path_length"); + p.safety_check_params.forward_path_length = + node->declare_parameter(safety_check_ns + "forward_path_length"); + p.safety_check_params.publish_debug_marker = + node->declare_parameter(safety_check_ns + "publish_debug_marker"); + } + + // RSSparams + std::string rss_ns = safety_check_ns + "rss_params."; + { + p.safety_check_params.rss_params.rear_vehicle_reaction_time = + node->declare_parameter(rss_ns + "rear_vehicle_reaction_time"); + p.safety_check_params.rss_params.rear_vehicle_safety_time_margin = + node->declare_parameter(rss_ns + "rear_vehicle_safety_time_margin"); + p.safety_check_params.rss_params.lateral_distance_max_threshold = + node->declare_parameter(rss_ns + "lateral_distance_max_threshold"); + p.safety_check_params.rss_params.longitudinal_distance_min_threshold = + node->declare_parameter(rss_ns + "longitudinal_distance_min_threshold"); + p.safety_check_params.rss_params.longitudinal_velocity_delta_time = + node->declare_parameter(rss_ns + "longitudinal_velocity_delta_time"); + } + // debug { - std::string ns = "goal_planner.debug."; + const std::string ns = base_ns + "debug."; p.print_debug_info = node->declare_parameter(ns + "print_debug_info"); } @@ -241,8 +358,6 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( } parameters_ = std::make_shared(p); - - left_side_parking_ = parameters_->parking_policy == ParkingPolicy::LEFT_SIDE; } void GoalPlannerModuleManager::updateModuleParams( @@ -259,12 +374,26 @@ void GoalPlannerModuleManager::updateModuleParams( }); } +bool GoalPlannerModuleManager::isAlwaysExecutableModule() const +{ + // enable AlwaysExecutable whenever goal modification is not allowed + // because only minor path refinements are made for fixed goals + if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { + return true; + } + + return false; +} + bool GoalPlannerModuleManager::isSimultaneousExecutableAsApprovedModule() const { + if (isAlwaysExecutableModule()) { + return true; + } + // enable SimultaneousExecutable whenever goal modification is not allowed // because only minor path refinements are made for fixed goals - if (!goal_planner_utils::isAllowedGoalModification( - planner_data_->route_handler, left_side_parking_)) { + if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { return true; } @@ -273,10 +402,13 @@ bool GoalPlannerModuleManager::isSimultaneousExecutableAsApprovedModule() const bool GoalPlannerModuleManager::isSimultaneousExecutableAsCandidateModule() const { + if (isAlwaysExecutableModule()) { + return true; + } + // enable SimultaneousExecutable whenever goal modification is not allowed // because only minor path refinements are made for fixed goals - if (!goal_planner_utils::isAllowedGoalModification( - planner_data_->route_handler, left_side_parking_)) { + if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { return true; } diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp index 841fc9092e279..a8ff0c0bbcf02 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp @@ -216,7 +216,7 @@ ObjectData AvoidanceByLaneChange::createObjectData( // Find the footprint point closest to the path, set to object_data.overhang_distance. object_data.overhang_dist = utils::avoidance::calcEnvelopeOverhangDistance( - object_data, object_closest_pose, object_data.overhang_pose.position); + object_data, data.reference_path, object_data.overhang_pose.position); // Check whether the the ego should avoid the object. const auto & vehicle_width = planner_data_->parameters.vehicle_width; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index 072c1c62ae3d2..74af83c2db2a1 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -31,6 +31,7 @@ namespace behavior_path_planner { +using tier4_autoware_utils::appendMarkerArray; using utils::lane_change::assignToCandidate; LaneChangeInterface::LaneChangeInterface( @@ -87,14 +88,11 @@ ModuleStatus LaneChangeInterface::updateState() } if (!module_type_->isValidPath()) { - return ModuleStatus::RUNNING; + return ModuleStatus::SUCCESS; } if (module_type_->isAbortState()) { - if (module_type_->hasFinishedAbort()) { - resetLaneChangeModule(); - } - return ModuleStatus::RUNNING; + return module_type_->hasFinishedAbort() ? ModuleStatus::SUCCESS : ModuleStatus::RUNNING; } if (module_type_->hasFinishedLaneChange()) { @@ -153,10 +151,7 @@ ModuleStatus LaneChangeInterface::updateState() getLogger().get_child(module_type_->getModuleTypeStr()), *clock_, 5000, "Lane change path is unsafe. Cancel lane change."); module_type_->toCancelState(); - if (!isWaitingApproval()) { - resetLaneChangeModule(); - } - return ModuleStatus::RUNNING; + return isWaitingApproval() ? ModuleStatus::RUNNING : ModuleStatus::SUCCESS; } if (!module_type_->isAbortEnabled()) { @@ -191,18 +186,12 @@ ModuleStatus LaneChangeInterface::updateState() return ModuleStatus::RUNNING; } -void LaneChangeInterface::resetLaneChangeModule() -{ - processOnExit(); - removeRTCStatus(); - processOnEntry(); -} - void LaneChangeInterface::updateData() { module_type_->setPreviousModulePaths( getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); module_type_->updateSpecialData(); + module_type_->resetStopPose(); } BehaviorModuleOutput LaneChangeInterface::plan() @@ -221,6 +210,8 @@ BehaviorModuleOutput LaneChangeInterface::plan() *prev_approved_path_ = *getPreviousModuleOutput().path; module_type_->insertStopPoint(module_type_->getLaneChangeStatus().target_lanes, *output.path); + stop_pose_ = module_type_->getStopPose(); + updateSteeringFactorPtr(output); clearWaitingApproval(); @@ -249,6 +240,8 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() path_reference_ = getPreviousModuleOutput().reference_path; + stop_pose_ = module_type_->getStopPose(); + if (!module_type_->isValidPath()) { removeRTCStatus(); path_candidate_ = std::make_shared(); @@ -286,6 +279,7 @@ void LaneChangeInterface::updateModuleParams(const std::any & parameters) void LaneChangeInterface::setData(const std::shared_ptr & data) { + planner_data_ = data; module_type_->setData(data); } @@ -295,11 +289,10 @@ void LaneChangeInterface::setObjectDebugVisualization() const if (!parameters_->publish_debug_marker) { return; } + using marker_utils::showPolygon; + using marker_utils::showPredictedPath; + using marker_utils::showSafetyCheckInfo; using marker_utils::lane_change_markers::showAllValidLaneChangePath; - using marker_utils::lane_change_markers::showLerpedPose; - using marker_utils::lane_change_markers::showObjectInfo; - using marker_utils::lane_change_markers::showPolygon; - using marker_utils::lane_change_markers::showPolygonPose; const auto debug_data = module_type_->getDebugData(); const auto debug_after_approval = module_type_->getAfterApprovalDebugData(); @@ -312,14 +305,14 @@ void LaneChangeInterface::setObjectDebugVisualization() const add(showAllValidLaneChangePath(debug_valid_path, "lane_change_valid_paths")); if (!debug_data.empty()) { - add(showObjectInfo(debug_data, "object_debug_info")); - add(showLerpedPose(debug_data, "ego_predicted_path")); + add(showSafetyCheckInfo(debug_data, "object_debug_info")); + add(showPredictedPath(debug_data, "ego_predicted_path")); add(showPolygon(debug_data, "ego_and_target_polygon_relation")); } if (!debug_after_approval.empty()) { - add(showObjectInfo(debug_after_approval, "object_debug_info_after_approval")); - add(showLerpedPose(debug_after_approval, "ego_predicted_path_after_approval")); + add(showSafetyCheckInfo(debug_after_approval, "object_debug_info_after_approval")); + add(showPredictedPath(debug_after_approval, "ego_predicted_path_after_approval")); add(showPolygon(debug_after_approval, "ego_and_target_polygon_relation_after_approval")); } } diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index 27f44433c6d4e..f4a75daa6c764 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -14,6 +14,9 @@ #include "behavior_path_planner/scene_module/lane_change/manager.hpp" +#include "tier4_autoware_utils/ros/parameter.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" + #include #include diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index bf0175ccd92e4..a81f6227e4064 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -23,6 +23,7 @@ #include #include +#include #include #include @@ -138,6 +139,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() const auto stop_dist = -(current_velocity * current_velocity / (2.0 * planner_data_->parameters.min_acc)); const auto stop_point = utils::insertStopPoint(stop_dist + current_dist, *output.path); + setStopPose(stop_point.point.pose); } const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path->points); @@ -198,6 +200,7 @@ void NormalLaneChange::insertStopPoint( const double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer; if (stopping_distance > 0.0) { const auto stop_point = utils::insertStopPoint(stopping_distance, path); + setStopPose(stop_point.point.pose); } } @@ -642,7 +645,6 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( const lanelet::ConstLanelets & target_backward_lanes) const { const auto current_pose = getEgoPose(); - const auto current_vel = getEgoVelocity(); const auto & route_handler = *getRouteHandler(); const auto & common_parameters = planner_data_->parameters; const auto & objects = *planner_data_->dynamic_object; @@ -665,12 +667,6 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( const auto target_backward_polygon = utils::lane_change::createPolygon( target_backward_lanes, 0.0, std::numeric_limits::max()); - // minimum distance to lane changing start point - const double t_prepare = common_parameters.lane_change_prepare_duration; - const double a_min = lane_change_parameters_->min_longitudinal_acc; - const double min_dist_to_lane_changing_start = std::max( - current_vel * t_prepare, current_vel * t_prepare + 0.5 * a_min * t_prepare * t_prepare); - LaneChangeTargetObjectIndices filtered_obj_indices; for (size_t i = 0; i < objects.objects.size(); ++i) { const auto & object = objects.objects.at(i); @@ -705,12 +701,8 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( // check if the object intersects with target lanes if (target_polygon && boost::geometry::intersects(target_polygon.value(), obj_polygon)) { - // ignore static parked object that are in front of the ego vehicle in target lanes - bool is_parked_object = - utils::lane_change::isParkedObject(target_path, route_handler, extended_object, 0.0, 0.0); - if (is_parked_object && min_dist_ego_to_obj > min_dist_to_lane_changing_start) { - continue; - } + // TODO(watanabe): ignore static parked object that are in front of the ego vehicle in target + // lanes filtered_obj_indices.target_lane.push_back(i); continue; @@ -1379,4 +1371,10 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( return path_safety_status; } + +void NormalLaneChange::setStopPose(const Pose & stop_pose) +{ + lane_change_stop_pose_ = stop_pose; +} + } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp b/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp index 5a039a98f2e52..288d9d44aee54 100644 --- a/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp @@ -14,6 +14,8 @@ #include "behavior_path_planner/scene_module/side_shift/manager.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" + #include #include diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp index 9d2da0d13f56b..09166cd72a83e 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp @@ -14,6 +14,8 @@ #include "behavior_path_planner/scene_module/start_planner/manager.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" + #include #include @@ -148,6 +150,122 @@ StartPlannerModuleManager::StartPlannerModuleManager( p.rrt_star_parameters.margin = node->declare_parameter(ns + "margin"); } + // stop condition + { + p.maximum_deceleration_for_stop = + node->declare_parameter(ns + "stop_condition.maximum_deceleration_for_stop"); + p.maximum_jerk_for_stop = + node->declare_parameter(ns + "stop_condition.maximum_jerk_for_stop"); + } + + std::string base_ns = "start_planner.path_safety_check."; + + // EgoPredictedPath + std::string ego_path_ns = base_ns + "ego_predicted_path."; + { + p.ego_predicted_path_params.acceleration = + node->declare_parameter(ego_path_ns + "acceleration"); + p.ego_predicted_path_params.time_horizon = + node->declare_parameter(ego_path_ns + "time_horizon"); + p.ego_predicted_path_params.time_resolution = + node->declare_parameter(ego_path_ns + "time_resolution"); + p.ego_predicted_path_params.min_slow_speed = + node->declare_parameter(ego_path_ns + "min_slow_speed"); + p.ego_predicted_path_params.delay_until_departure = + node->declare_parameter(ego_path_ns + "delay_until_departure"); + p.ego_predicted_path_params.target_velocity = + node->declare_parameter(ego_path_ns + "target_velocity"); + } + + // ObjectFilteringParams + std::string obj_filter_ns = base_ns + "target_filtering."; + { + p.objects_filtering_params.safety_check_time_horizon = + node->declare_parameter(obj_filter_ns + "safety_check_time_horizon"); + p.objects_filtering_params.safety_check_time_resolution = + node->declare_parameter(obj_filter_ns + "safety_check_time_resolution"); + p.objects_filtering_params.object_check_forward_distance = + node->declare_parameter(obj_filter_ns + "object_check_forward_distance"); + p.objects_filtering_params.object_check_backward_distance = + node->declare_parameter(obj_filter_ns + "object_check_backward_distance"); + p.objects_filtering_params.ignore_object_velocity_threshold = + node->declare_parameter(obj_filter_ns + "ignore_object_velocity_threshold"); + p.objects_filtering_params.include_opposite_lane = + node->declare_parameter(obj_filter_ns + "include_opposite_lane"); + p.objects_filtering_params.invert_opposite_lane = + node->declare_parameter(obj_filter_ns + "invert_opposite_lane"); + p.objects_filtering_params.check_all_predicted_path = + node->declare_parameter(obj_filter_ns + "check_all_predicted_path"); + p.objects_filtering_params.use_all_predicted_path = + node->declare_parameter(obj_filter_ns + "use_all_predicted_path"); + p.objects_filtering_params.use_predicted_path_outside_lanelet = + node->declare_parameter(obj_filter_ns + "use_predicted_path_outside_lanelet"); + } + + // ObjectTypesToCheck + std::string obj_types_ns = obj_filter_ns + "object_types_to_check."; + { + p.objects_filtering_params.object_types_to_check.check_car = + node->declare_parameter(obj_types_ns + "check_car"); + p.objects_filtering_params.object_types_to_check.check_truck = + node->declare_parameter(obj_types_ns + "check_truck"); + p.objects_filtering_params.object_types_to_check.check_bus = + node->declare_parameter(obj_types_ns + "check_bus"); + p.objects_filtering_params.object_types_to_check.check_trailer = + node->declare_parameter(obj_types_ns + "check_trailer"); + p.objects_filtering_params.object_types_to_check.check_unknown = + node->declare_parameter(obj_types_ns + "check_unknown"); + p.objects_filtering_params.object_types_to_check.check_bicycle = + node->declare_parameter(obj_types_ns + "check_bicycle"); + p.objects_filtering_params.object_types_to_check.check_motorcycle = + node->declare_parameter(obj_types_ns + "check_motorcycle"); + p.objects_filtering_params.object_types_to_check.check_pedestrian = + node->declare_parameter(obj_types_ns + "check_pedestrian"); + } + + // ObjectLaneConfiguration + std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration."; + { + p.objects_filtering_params.object_lane_configuration.check_current_lane = + node->declare_parameter(obj_lane_ns + "check_current_lane"); + p.objects_filtering_params.object_lane_configuration.check_right_lane = + node->declare_parameter(obj_lane_ns + "check_right_side_lane"); + p.objects_filtering_params.object_lane_configuration.check_left_lane = + node->declare_parameter(obj_lane_ns + "check_left_side_lane"); + p.objects_filtering_params.object_lane_configuration.check_shoulder_lane = + node->declare_parameter(obj_lane_ns + "check_shoulder_lane"); + p.objects_filtering_params.object_lane_configuration.check_other_lane = + node->declare_parameter(obj_lane_ns + "check_other_lane"); + } + + // SafetyCheckParams + std::string safety_check_ns = base_ns + "safety_check_params."; + { + p.safety_check_params.enable_safety_check = + node->declare_parameter(safety_check_ns + "enable_safety_check"); + p.safety_check_params.backward_path_length = + node->declare_parameter(safety_check_ns + "backward_path_length"); + p.safety_check_params.forward_path_length = + node->declare_parameter(safety_check_ns + "forward_path_length"); + p.safety_check_params.publish_debug_marker = + node->declare_parameter(safety_check_ns + "publish_debug_marker"); + } + + // RSSparams + std::string rss_ns = safety_check_ns + "rss_params."; + { + p.safety_check_params.rss_params.rear_vehicle_reaction_time = + node->declare_parameter(rss_ns + "rear_vehicle_reaction_time"); + p.safety_check_params.rss_params.rear_vehicle_safety_time_margin = + node->declare_parameter(rss_ns + "rear_vehicle_safety_time_margin"); + p.safety_check_params.rss_params.lateral_distance_max_threshold = + node->declare_parameter(rss_ns + "lateral_distance_max_threshold"); + p.safety_check_params.rss_params.longitudinal_distance_min_threshold = + node->declare_parameter(rss_ns + "longitudinal_distance_min_threshold"); + p.safety_check_params.rss_params.longitudinal_velocity_delta_time = + node->declare_parameter(rss_ns + "longitudinal_velocity_delta_time"); + } + // validation of parameters if (p.lateral_acceleration_sampling_num < 1) { RCLCPP_FATAL_STREAM( diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 2dc2401571d9a..a285224404f12 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -17,12 +17,13 @@ #include "behavior_path_planner/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include #include #include -#include #include #include @@ -97,6 +98,16 @@ BehaviorModuleOutput StartPlannerModule::run() return plan(); } +void StartPlannerModule::processOnEntry() +{ + // Initialize safety checker + if (parameters_->safety_check_params.enable_safety_check) { + initializeSafetyCheckParameters(); + utils::start_goal_planner_common::initializeCollisionCheckDebugMap( + start_planner_data_.collision_check); + } +} + void StartPlannerModule::processOnExit() { resetPathCandidate(); @@ -108,7 +119,7 @@ bool StartPlannerModule::isExecutionRequested() const { // TODO(Sugahara): if required lateral shift distance is small, don't engage this module. // Execute when current pose is near route start pose - const Pose & start_pose = planner_data_->route_handler->getOriginalStartPose(); + const Pose start_pose = planner_data_->route_handler->getOriginalStartPose(); const Pose & current_pose = planner_data_->self_odometry->pose.pose; if ( tier4_autoware_utils::calcDistance2d(start_pose.position, current_pose.position) > @@ -117,7 +128,7 @@ bool StartPlannerModule::isExecutionRequested() const } // Check if ego arrives at goal - const Pose & goal_pose = planner_data_->route_handler->getGoalPose(); + const Pose goal_pose = planner_data_->route_handler->getGoalPose(); if ( tier4_autoware_utils::calcDistance2d(goal_pose.position, current_pose.position) < parameters_->th_arrived_distance) { @@ -134,41 +145,25 @@ bool StartPlannerModule::isExecutionRequested() const return false; } - // Create vehicle footprint - const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info_); - const auto vehicle_footprint = transformVector( - local_vehicle_footprint, - tier4_autoware_utils::pose2transform(planner_data_->self_odometry->pose.pose)); - - // Check if ego is not out of lanes - const double backward_path_length = - planner_data_->parameters.backward_path_length + parameters_->max_back_distance; - const auto current_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_path_length, std::numeric_limits::max(), - /*forward_only_in_route*/ true); - const auto pull_out_lanes = - start_planner_utils::getPullOutLanes(planner_data_, backward_path_length); - auto lanes = current_lanes; - for (const auto & pull_out_lane : pull_out_lanes) { - auto it = std::find_if( - lanes.begin(), lanes.end(), [&pull_out_lane](const lanelet::ConstLanelet & lane) { - return lane.id() == pull_out_lane.id(); - }); - - if (it == lanes.end()) { - lanes.push_back(pull_out_lane); - } - } - - if (LaneDepartureChecker::isOutOfLane(lanes, vehicle_footprint)) { - return false; - } - return true; } bool StartPlannerModule::isExecutionReady() const { + if (!status_.is_safe_static_objects) { + return false; + } + + if (status_.pull_out_path.partial_paths.empty()) { + return true; + } + + if (status_.is_safe_static_objects && parameters_->safety_check_params.enable_safety_check) { + if (!isSafePath()) { + RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against dynamic objects"); + return false; + } + } return true; } @@ -198,6 +193,7 @@ BehaviorModuleOutput StartPlannerModule::plan() getLogger(), *clock_, 5000, "Start plan for a backward goal is not supported now"); const auto output = generateStopOutput(); setDebugData(); // use status updated in generateStopOutput() + updateRTCStatus(0, 0); return output; } @@ -215,6 +211,7 @@ BehaviorModuleOutput StartPlannerModule::plan() getLogger(), *clock_, 5000, "Not found safe pull out path, publish stop path"); const auto output = generateStopOutput(); setDebugData(); // use status updated in generateStopOutput() + updateRTCStatus(0, 0); return output; } @@ -280,6 +277,15 @@ CandidateOutput StartPlannerModule::planCandidate() const return CandidateOutput{}; } +void StartPlannerModule::initializeSafetyCheckParameters() +{ + utils::start_goal_planner_common::updateEgoPredictedPathParams( + ego_predicted_path_params_, parameters_); + utils::start_goal_planner_common::updateSafetyCheckParams(safety_check_params_, parameters_); + utils::start_goal_planner_common::updateObjectsFilteringParams( + objects_filtering_params_, parameters_); +} + PathWithLaneId StartPlannerModule::getFullPath() const { // combine partial pull out path @@ -311,6 +317,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() clearWaitingApproval(); const auto output = generateStopOutput(); setDebugData(); // use status updated in generateStopOutput() + updateRTCStatus(0, 0); return output; } @@ -321,6 +328,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() clearWaitingApproval(); const auto output = generateStopOutput(); setDebugData(); // use status updated in generateStopOutput() + updateRTCStatus(0, 0); return output; } @@ -407,8 +415,8 @@ PathWithLaneId StartPlannerModule::getCurrentPath() const } void StartPlannerModule::planWithPriority( - const std::vector & start_pose_candidates, const Pose & goal_pose, - const std::string search_priority) + const std::vector & start_pose_candidates, const Pose & refined_start_pose, + const Pose & goal_pose, const std::string search_priority) { // check if start pose candidates are valid if (start_pose_candidates.empty()) { @@ -419,6 +427,10 @@ void StartPlannerModule::planWithPriority( // Get the pull_out_start_pose for the current index const auto & pull_out_start_pose = start_pose_candidates.at(i); + // Set back_finished to true if the current start pose is same to refined_start_pose + status_.back_finished = + tier4_autoware_utils::calcDistance2d(pull_out_start_pose, refined_start_pose) < 0.01; + planner->setPlannerData(planner_data_); const auto pull_out_path = planner->plan(pull_out_start_pose, goal_pose); // not found safe path @@ -494,9 +506,6 @@ void StartPlannerModule::planWithPriority( for (const auto & p : order_priority) { if (is_safe_with_pose_planner(p.first, p.second)) { - const std::lock_guard lock(mutex_); - // Set back_finished flag based on the current index - status_.back_finished = p.first == 0; return; } } @@ -565,10 +574,15 @@ std::vector StartPlannerModule::generateDrivableLanes( const PathWithLaneId & path) const { const auto path_road_lanes = getPathRoadLanes(path); - if (!path_road_lanes.empty()) { - return utils::generateDrivableLanesWithShoulderLanes( - getPathRoadLanes(path), status_.pull_out_lanes); + lanelet::ConstLanelets shoulder_lanes; + const auto & rh = planner_data_->route_handler; + std::copy_if( + status_.pull_out_lanes.begin(), status_.pull_out_lanes.end(), + std::back_inserter(shoulder_lanes), + [&rh](const auto & pull_out_lane) { return rh->isShoulderLanelet(pull_out_lane); }); + + return utils::generateDrivableLanesWithShoulderLanes(path_road_lanes, shoulder_lanes); } // if path_road_lanes is empty, use only pull_out_lanes as drivable lanes @@ -599,7 +613,7 @@ void StartPlannerModule::updatePullOutStatus() // skip updating if enough time has not passed for preventing chattering between back and // start_planner - if (!has_received_new_route && !last_pull_out_start_update_time_ && !status_.back_finished) { + if (!has_received_new_route) { if (!last_pull_out_start_update_time_) { last_pull_out_start_update_time_ = std::make_unique(clock_->now()); } @@ -614,9 +628,24 @@ void StartPlannerModule::updatePullOutStatus() const auto & current_pose = planner_data_->self_odometry->pose.pose; const auto & goal_pose = planner_data_->route_handler->getGoalPose(); + // refine start pose with pull out lanes. + // 1) backward driving is not allowed: use refined pose just as start pose. + // 2) backward driving is allowed: use refined pose to check if backward driving is needed. + const PathWithLaneId start_pose_candidates_path = calcStartPoseCandidatesBackwardPath(); + const auto refined_start_pose = calcLongitudinalOffsetPose( + start_pose_candidates_path.points, planner_data_->self_odometry->pose.pose.position, 0.0); + if (!refined_start_pose) return; + // search pull out start candidates backward - std::vector start_pose_candidates = searchPullOutStartPoses(); - planWithPriority(start_pose_candidates, goal_pose, parameters_->search_priority); + const std::vector start_pose_candidates = std::invoke([&]() -> std::vector { + if (parameters_->enable_back) { + return searchPullOutStartPoses(start_pose_candidates_path); + } + return {*refined_start_pose}; + }); + + planWithPriority( + start_pose_candidates, *refined_start_pose, goal_pose, parameters_->search_priority); checkBackFinished(); if (!status_.back_finished) { @@ -626,20 +655,34 @@ void StartPlannerModule::updatePullOutStatus() } } -std::vector StartPlannerModule::searchPullOutStartPoses() +PathWithLaneId StartPlannerModule::calcStartPoseCandidatesBackwardPath() const { - std::vector pull_out_start_pose{}; - const Pose & current_pose = planner_data_->self_odometry->pose.pose; // get backward shoulder path const auto arc_position_pose = lanelet::utils::getArcCoordinates(status_.pull_out_lanes, current_pose); const double check_distance = parameters_->max_back_distance + 30.0; // buffer - auto backward_shoulder_path = planner_data_->route_handler->getCenterLinePath( + auto path = planner_data_->route_handler->getCenterLinePath( status_.pull_out_lanes, arc_position_pose.length - check_distance, arc_position_pose.length + check_distance); + // lateral shift to current_pose + const double distance_from_center_line = arc_position_pose.distance; + for (auto & p : path.points) { + p.point.pose = calcOffsetPose(p.point.pose, 0, distance_from_center_line, 0); + } + + return path; +} + +std::vector StartPlannerModule::searchPullOutStartPoses( + const PathWithLaneId & start_pose_candidates) const +{ + const Pose & current_pose = planner_data_->self_odometry->pose.pose; + + std::vector pull_out_start_pose{}; + // filter pull out lanes stop objects const auto [pull_out_lane_objects, others] = utils::path_safety_checker::separateObjectsByLanelets( @@ -647,28 +690,19 @@ std::vector StartPlannerModule::searchPullOutStartPoses() const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( pull_out_lane_objects, parameters_->th_moving_object_velocity); - // lateral shift to current_pose - const double distance_from_center_line = arc_position_pose.distance; - for (auto & p : backward_shoulder_path.points) { - p.point.pose = calcOffsetPose(p.point.pose, 0, distance_from_center_line, 0); - } - - // if backward driving is disable, just refine current pose to the lanes - if (!parameters_->enable_back) { - const auto refined_pose = - calcLongitudinalOffsetPose(backward_shoulder_path.points, current_pose.position, 0); - if (refined_pose) { - pull_out_start_pose.push_back(*refined_pose); - } - return pull_out_start_pose; - } + // Set the maximum backward distance less than the distance from the vehicle's base_link to the + // lane's rearmost point to prevent lane departure. + const double s_current = + lanelet::utils::getArcCoordinates(status_.pull_out_lanes, current_pose).length; + const double max_back_distance = std::clamp( + s_current - planner_data_->parameters.base_link2rear, 0.0, parameters_->max_back_distance); // check collision between footprint and object at the backed pose const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info_); - for (double back_distance = 0.0; back_distance <= parameters_->max_back_distance; + for (double back_distance = 0.0; back_distance <= max_back_distance; back_distance += parameters_->backward_search_resolution) { const auto backed_pose = calcLongitudinalOffsetPose( - backward_shoulder_path.points, current_pose.position, -back_distance); + start_pose_candidates.points, current_pose.position, -back_distance); if (!backed_pose) { continue; } @@ -676,10 +710,10 @@ std::vector StartPlannerModule::searchPullOutStartPoses() // check the back pose is near the lane end const double length_to_backed_pose = lanelet::utils::getArcCoordinates(status_.pull_out_lanes, *backed_pose).length; - double length_to_lane_end = 0.0; - for (const auto & lane : status_.pull_out_lanes) { - length_to_lane_end += lanelet::utils::getLaneletLength2d(lane); - } + + const double length_to_lane_end = std::accumulate( + std::begin(status_.pull_out_lanes), std::end(status_.pull_out_lanes), 0.0, + [](double acc, const auto & lane) { return acc + lanelet::utils::getLaneletLength2d(lane); }); const double distance_from_lane_end = length_to_lane_end - length_to_backed_pose; if (distance_from_lane_end < parameters_->ignore_distance_from_lane_end) { RCLCPP_WARN_THROTTLE( @@ -922,6 +956,85 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const return turn_signal; } +bool StartPlannerModule::isSafePath() const +{ + // TODO(Sugahara): should safety check for backward path + + const auto pull_out_path = getCurrentPath(); + const auto & current_pose = planner_data_->self_odometry->pose.pose; + const double current_velocity = std::hypot( + planner_data_->self_odometry->twist.twist.linear.x, + planner_data_->self_odometry->twist.twist.linear.y); + const auto & dynamic_object = planner_data_->dynamic_object; + const auto & route_handler = planner_data_->route_handler; + const double backward_path_length = + planner_data_->parameters.backward_path_length + parameters_->max_back_distance; + + const auto current_lanes = utils::getExtendedCurrentLanes( + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); + + // for ego predicted path + const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(pull_out_path.points); + const std::pair terminal_velocity_and_accel = + utils::start_goal_planner_common::getPairsTerminalVelocityAndAccel( + status_.pull_out_path.pairs_terminal_velocity_and_accel, status_.current_path_idx); + RCLCPP_DEBUG( + getLogger(), "pairs_terminal_velocity_and_accel for start_planner: %f, %f", + terminal_velocity_and_accel.first, terminal_velocity_and_accel.second); + RCLCPP_DEBUG(getLogger(), "current_path_idx %ld", status_.current_path_idx); + utils::start_goal_planner_common::updatePathProperty( + ego_predicted_path_params_, terminal_velocity_and_accel); + const auto ego_predicted_path = + behavior_path_planner::utils::path_safety_checker::createPredictedPath( + ego_predicted_path_params_, pull_out_path.points, current_pose, current_velocity, + ego_seg_idx); + + // filtering objects with velocity, position and class + const auto & filtered_objects = utils::path_safety_checker::filterObjects( + dynamic_object, route_handler, current_lanes, current_pose.position, objects_filtering_params_); + + // filtering objects based on the current position's lane + const auto & target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane( + current_lanes, route_handler, filtered_objects, objects_filtering_params_); + + const double hysteresis_factor = + status_.is_safe_dynamic_objects ? 1.0 : parameters_->hysteresis_factor_expand_rate; + + utils::start_goal_planner_common::updateSafetyCheckTargetObjectsData( + start_planner_data_, filtered_objects, target_objects_on_lane, ego_predicted_path); + + bool is_safe_dynamic_objects = true; + // Check for collisions with each predicted path of the object + for (const auto & object : target_objects_on_lane.on_current_lane) { + auto current_debug_data = marker_utils::createObjectDebug(object); + + bool is_safe_dynamic_object = true; + + const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( + object, objects_filtering_params_->check_all_predicted_path); + + // If a collision is detected, mark the object as unsafe and break the loop + for (const auto & obj_path : obj_predicted_paths) { + if (!utils::path_safety_checker::checkCollision( + pull_out_path, ego_predicted_path, object, obj_path, planner_data_->parameters, + safety_check_params_->rss_params, hysteresis_factor, current_debug_data.second)) { + marker_utils::updateCollisionCheckDebugMap( + start_planner_data_.collision_check, current_debug_data, false); + is_safe_dynamic_objects = false; + is_safe_dynamic_object = false; + break; + } + } + if (is_safe_dynamic_object) { + marker_utils::updateCollisionCheckDebugMap( + start_planner_data_.collision_check, current_debug_data, is_safe_dynamic_object); + } + } + + return is_safe_dynamic_objects; +} + bool StartPlannerModule::IsGoalBehindOfEgoInSameRouteSegment() const { const auto & rh = planner_data_->route_handler; @@ -1040,8 +1153,13 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons void StartPlannerModule::setDebugData() const { + using marker_utils::createObjectsMarkerArray; using marker_utils::createPathMarkerArray; using marker_utils::createPoseMarkerArray; + using marker_utils::createPredictedPathMarkerArray; + using marker_utils::showPolygon; + using marker_utils::showPredictedPath; + using marker_utils::showSafetyCheckInfo; using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerScale; @@ -1059,6 +1177,36 @@ void StartPlannerModule::setDebugData() const add(createPoseMarkerArray(status_.pull_out_path.start_pose, "start_pose", 0, 0.3, 0.9, 0.3)); add(createPoseMarkerArray(status_.pull_out_path.end_pose, "end_pose", 0, 0.9, 0.9, 0.3)); add(createPathMarkerArray(getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); + if (start_planner_data_.ego_predicted_path.size() > 0) { + const auto & ego_predicted_path = utils::path_safety_checker::convertToPredictedPath( + start_planner_data_.ego_predicted_path, ego_predicted_path_params_->time_resolution); + add(createPredictedPathMarkerArray( + ego_predicted_path, vehicle_info_, "ego_predicted_path", 0, 0.0, 0.5, 0.9)); + } + + if (start_planner_data_.filtered_objects.objects.size() > 0) { + add(createObjectsMarkerArray( + start_planner_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9)); + } + + // safety check + { + add(showSafetyCheckInfo(start_planner_data_.collision_check, "object_debug_info")); + add(showPredictedPath(start_planner_data_.collision_check, "ego_predicted_path")); + add(showPolygon(start_planner_data_.collision_check, "ego_and_target_polygon_relation")); + } + + if (start_planner_data_.ego_predicted_path.size() > 0) { + const auto & ego_predicted_path = utils::path_safety_checker::convertToPredictedPath( + start_planner_data_.ego_predicted_path, ego_predicted_path_params_->time_resolution); + add(createPredictedPathMarkerArray( + ego_predicted_path, vehicle_info_, "ego_predicted_path", 0, 0.0, 0.5, 0.9)); + } + + if (start_planner_data_.filtered_objects.objects.size() > 0) { + add(createObjectsMarkerArray( + start_planner_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9)); + } // Visualize planner type text const auto header = planner_data_->route_handler->getRouteHeader(); diff --git a/planning/behavior_path_planner/src/turn_signal_decider.cpp b/planning/behavior_path_planner/src/turn_signal_decider.cpp index fa661aef06681..65e3e8343f49c 100644 --- a/planning/behavior_path_planner/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner/src/turn_signal_decider.cpp @@ -18,9 +18,13 @@ #include #include +#include #include +#include #include -#include +#include +#include +#include #include #include diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 856c3220165e7..4afe5381daa64 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -17,10 +17,14 @@ #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" #include "behavior_path_planner/utils/avoidance/utils.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "motion_utils/trajectory/path_with_lane_id.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include #include -#include +#include +#include +#include #include @@ -396,7 +400,8 @@ void fillLongitudinalAndLengthByClosestEnvelopeFootprint( double max_distance = std::numeric_limits::lowest(); for (const auto & p : obj.envelope_poly.outer()) { const auto point = tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0); - const double arc_length = calcSignedArcLengthToFirstNearestPoint(path.points, ego_pos, point); + // TODO(someone): search around first position where the ego should avoid the object. + const double arc_length = calcSignedArcLength(path.points, ego_pos, point); min_distance = std::min(min_distance, arc_length); max_distance = std::max(max_distance, arc_length); } @@ -406,13 +411,15 @@ void fillLongitudinalAndLengthByClosestEnvelopeFootprint( } double calcEnvelopeOverhangDistance( - const ObjectData & object_data, const Pose & base_pose, Point & overhang_pose) + const ObjectData & object_data, const PathWithLaneId & path, Point & overhang_pose) { double largest_overhang = isOnRight(object_data) ? -100.0 : 100.0; // large number for (const auto & p : object_data.envelope_poly.outer()) { const auto point = tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0); - const auto lateral = tier4_autoware_utils::calcLateralDeviation(base_pose, point); + // TODO(someone): search around first position where the ego should avoid the object. + const auto idx = findNearestIndex(path.points, point); + const auto lateral = calcLateralDeviation(getPose(path.points.at(idx)), point); const auto & overhang_pose_on_right = [&overhang_pose, &largest_overhang, &point, &lateral]() { if (lateral > largest_overhang) { @@ -1014,7 +1021,7 @@ void filterTargetObjects( const auto lines = rh->getFurthestLinestring(target_lanelet, get_right, get_left, get_opposite, true); const auto & line = isOnRight(o) ? lines.back() : lines.front(); - const auto d = distance2d(to2D(overhang_basic_pose), to2D(line.basicLineString())); + const auto d = boost::geometry::distance(o.envelope_poly, to2D(line.basicLineString())); if (d < o.to_road_shoulder_distance) { o.to_road_shoulder_distance = d; target_line = line; diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index 2b62d5e7c3b53..52be0ac5a81ef 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -19,20 +19,74 @@ #include "behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" -#include "interpolation/linear_interpolation.hpp" #include +#include +#include +#include +#include +#include #include namespace drivable_area_expansion { +std::vector crop_and_resample( + const std::vector & points, + const std::shared_ptr planner_data, + const double resample_interval) +{ + auto lon_offset = 0.0; + auto crop_pose = *planner_data->drivable_area_expansion_prev_crop_pose; + // reuse or update the previous crop point + if (planner_data->drivable_area_expansion_prev_crop_pose) { + const auto lon_offset = motion_utils::calcSignedArcLength( + points, points.front().point.pose.position, crop_pose.position); + if (lon_offset < 0.0) { + planner_data->drivable_area_expansion_prev_crop_pose.reset(); + } else { + const auto is_behind_ego = + motion_utils::calcSignedArcLength( + points, crop_pose.position, planner_data->self_odometry->pose.pose.position) > 0.0; + const auto is_too_far = motion_utils::calcLateralOffset(points, crop_pose.position) > 0.1; + if (!is_behind_ego || is_too_far) + planner_data->drivable_area_expansion_prev_crop_pose.reset(); + } + } + if (!planner_data->drivable_area_expansion_prev_crop_pose) { + crop_pose = planner_data->drivable_area_expansion_prev_crop_pose.value_or( + motion_utils::calcInterpolatedPose(points, resample_interval - lon_offset)); + } + // crop + const auto crop_seg_idx = motion_utils::findNearestSegmentIndex(points, crop_pose.position); + const auto cropped_points = motion_utils::cropPoints( + points, crop_pose.position, crop_seg_idx + 1, + planner_data->drivable_area_expansion_parameters.max_path_arc_length, 0.0); + planner_data->drivable_area_expansion_prev_crop_pose = crop_pose; + // resample + PathWithLaneId cropped_path; + if (tier4_autoware_utils::calcDistance2d(crop_pose, cropped_points.front()) > 1e-3) { + PathPointWithLaneId crop_path_point; + crop_path_point.point.pose = crop_pose; + cropped_path.points.push_back(crop_path_point); + } + cropped_path.points.insert( + cropped_path.points.end(), cropped_points.begin(), cropped_points.end()); + const auto resampled_path = + motion_utils::resamplePath(cropped_path, resample_interval, true, true, false); + + return resampled_path.points; +} + void expandDrivableArea( - PathWithLaneId & path, const DrivableAreaExpansionParameters & params, - const PredictedObjects & dynamic_objects, const route_handler::RouteHandler & route_handler, + PathWithLaneId & path, + const std::shared_ptr planner_data, const lanelet::ConstLanelets & path_lanes) { + const auto & params = planner_data->drivable_area_expansion_parameters; + const auto & dynamic_objects = *planner_data->dynamic_object; + const auto & route_handler = *planner_data->route_handler; const auto uncrossable_lines = extractUncrossableLines(*route_handler.getLaneletMapPtr(), params.avoid_linestring_types); multi_linestring_t uncrossable_lines_in_range; @@ -40,7 +94,8 @@ void expandDrivableArea( for (const auto & line : uncrossable_lines) if (boost::geometry::distance(line, point_t{p.x, p.y}) < params.max_path_arc_length) uncrossable_lines_in_range.push_back(line); - const auto path_footprints = createPathFootprints(path, params); + const auto points = crop_and_resample(path.points, planner_data, params.resample_interval); + const auto path_footprints = createPathFootprints(points, params); const auto predicted_paths = createObjectFootprints(dynamic_objects, params); const auto expansion_polygons = params.expansion_method == "lanelet" @@ -113,8 +168,8 @@ void updateDrivableAreaBounds(PathWithLaneId & path, const polygon_t & expanded_ { const auto original_left_bound = path.left_bound; const auto original_right_bound = path.right_bound; - const auto is_left_of_segment = [](const point_t & a, const point_t & b, const point_t & p) { - return (b.x() - a.x()) * (p.y() - a.y()) - (b.y() - a.y()) * (p.x() - a.x()) > 0; + const auto is_left_of_path = [&](const point_t & p) { + return motion_utils::calcLateralOffset(path.points, convert_point(p)) > 0.0; }; // prepare delimiting lines: start and end of the original expanded drivable area const auto start_segment = @@ -178,12 +233,10 @@ void updateDrivableAreaBounds(PathWithLaneId & path, const polygon_t & expanded_ *it, *std::next(it), start_segment.first, start_segment.second); if (inter_start) { const auto dist = boost::geometry::distance(*inter_start, path_start_segment); - const auto is_left_of_path_start = is_left_of_segment( - convert_point(path.points[0].point.pose.position), - convert_point(path.points[1].point.pose.position), *inter_start); - if (is_left_of_path_start && dist < start_left.distance) + const auto is_left = is_left_of_path(*inter_start); + if (is_left && dist < start_left.distance) start_left.update(*inter_start, it, dist); - else if (!is_left_of_path_start && dist < start_right.distance) + else if (!is_left && dist < start_right.distance) start_right.update(*inter_start, it, dist); } const auto inter_end = @@ -192,11 +245,10 @@ void updateDrivableAreaBounds(PathWithLaneId & path, const polygon_t & expanded_ : segment_to_line_intersection(*it, *std::next(it), end_segment.first, end_segment.second); if (inter_end) { const auto dist = boost::geometry::distance(*inter_end, path_end_segment); - const auto is_left_of_path_end = is_left_of_segment( - convert_point(path.points.back().point.pose.position), end_segment_center, *inter_end); - if (is_left_of_path_end && dist < end_left.distance) + const auto is_left = is_left_of_path(*inter_end); + if (is_left && dist < end_left.distance) end_left.update(*inter_end, it, dist); - else if (!is_left_of_path_end && dist < end_right.distance) + else if (!is_left && dist < end_right.distance) end_right.update(*inter_end, it, dist); } } diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp index 035cc579d2a82..8fe8b12252aea 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp @@ -25,10 +25,15 @@ double calculateDistanceLimit( const multi_linestring_t & limit_lines) { auto dist_limit = std::numeric_limits::max(); - multi_point_t intersections; - boost::geometry::intersection(expansion_polygon, limit_lines, intersections); - for (const auto & p : intersections) - dist_limit = std::min(dist_limit, boost::geometry::distance(p, base_ls)); + for (const auto & line : limit_lines) { + multi_point_t intersections; + boost::geometry::intersection(expansion_polygon, limit_lines, intersections); + for (const auto & p : intersections) + dist_limit = std::min(dist_limit, boost::geometry::distance(p, base_ls)); + for (const auto & p : line) + if (boost::geometry::within(p, expansion_polygon)) + dist_limit = std::min(dist_limit, boost::geometry::distance(p, base_ls)); + } return dist_limit; } @@ -59,7 +64,7 @@ polygon_t createExpansionPolygon( boost::geometry::buffer( base_ls, polygons, distance_strategy, strategy::side_straight(), strategy::join_miter(), strategy::end_flat(), strategy::point_square()); - return polygons.front(); + return polygons.empty() ? polygon_t{} : polygons.front(); } std::array calculate_arc_length_range_and_distance( @@ -80,7 +85,7 @@ std::array calculate_arc_length_range_and_distance( } for (const auto & p : footprint.outer()) { const auto projection = point_to_linestring_projection(p, path_ls); - if (projection.arc_length <= 0.0 || projection.arc_length >= path_length) continue; + if (projection.arc_length <= 0.0 || projection.arc_length >= path_length - 1e-3) continue; if (is_left == (projection.distance > 0) && std::abs(projection.distance) > expansion_dist) { expansion_dist = std::abs(projection.distance); from_arc_length = std::min(from_arc_length, projection.arc_length); diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp index 0c31093a83c0e..4a45dce6a0bcc 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp @@ -16,6 +16,9 @@ #include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" +#include +#include + #include #include @@ -63,7 +66,7 @@ multi_polygon_t createObjectFootprints( } multi_polygon_t createPathFootprints( - const PathWithLaneId & path, const DrivableAreaExpansionParameters & params) + const std::vector & points, const DrivableAreaExpansionParameters & params) { const auto left = params.ego_left_offset + params.ego_extra_left_offset; const auto right = params.ego_right_offset - params.ego_extra_right_offset; @@ -75,9 +78,9 @@ multi_polygon_t createPathFootprints( point_t{front, left}}; multi_polygon_t footprints; // skip the last footprint as its orientation is usually wrong - footprints.reserve(path.points.size() - 1); + footprints.reserve(points.size() - 1); double arc_length = 0.0; - for (auto it = path.points.begin(); std::next(it) != path.points.end(); ++it) { + for (auto it = points.begin(); std::next(it) != points.end(); ++it) { footprints.push_back(createFootprint(it->point.pose, base_footprint)); if (params.max_path_arc_length > 0.0) { arc_length += tier4_autoware_utils::calcDistance2d(it->point.pose, std::next(it)->point.pose); diff --git a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp index a5ccfc9517771..5b054de43ef74 100644 --- a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp @@ -17,12 +17,13 @@ #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "motion_utils/trajectory/path_with_lane_id.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/math/unit_conversion.hpp" #include #include #include -#include #ifdef ROS_DISTRO_GALACTIC #include @@ -163,6 +164,7 @@ void GeometricParallelParking::clearPaths() current_path_idx_ = 0; arc_paths_.clear(); paths_.clear(); + pairs_terminal_velocity_and_accel_.clear(); } bool GeometricParallelParking::planPullOver( @@ -182,6 +184,7 @@ bool GeometricParallelParking::planPullOver( if (is_forward) { // When turning forward to the right, the front left goes out, // so reduce the steer angle at that time for seach no lane departure path. + // TODO(Sugahara): define in the config constexpr double start_pose_offset = 0.0; constexpr double min_steer_rad = 0.05; constexpr double steer_interval = 0.1; @@ -445,7 +448,6 @@ std::vector GeometricParallelParking::planOneTrial( if (std::abs(end_pose_offset) > 0) { PathPointWithLaneId straight_point{}; straight_point.point.pose = goal_pose; - // setLaneIds(straight_point); path_turn_right.points.push_back(straight_point); } @@ -481,6 +483,19 @@ std::vector GeometricParallelParking::planOneTrial( paths_.push_back(path_turn_left); paths_.push_back(path_turn_right); + // set terminal velocity and acceleration(temporary implementation) + if (is_forward) { + pairs_terminal_velocity_and_accel_.push_back( + std::make_pair(parameters_.forward_parking_velocity, 0.0)); + pairs_terminal_velocity_and_accel_.push_back( + std::make_pair(parameters_.forward_parking_velocity, 0.0)); + } else { + pairs_terminal_velocity_and_accel_.push_back( + std::make_pair(parameters_.backward_parking_velocity, 0.0)); + pairs_terminal_velocity_and_accel_.push_back( + std::make_pair(parameters_.backward_parking_velocity, 0.0)); + } + // set pull_over start and end pose // todo: make start and end pose for pull_out start_pose_ = start_pose; diff --git a/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp b/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp index 5d77027efa5d0..7efdbdf1552d5 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp @@ -35,7 +35,7 @@ BehaviorModuleOutput DefaultFixedGoalPlanner::plan( const PathWithLaneId smoothed_path = modifyPathForSmoothGoalConnection(*(output.path), planner_data); output.path = std::make_shared(smoothed_path); - output.reference_path = std::make_shared(smoothed_path); + output.reference_path = getPreviousModuleOutput().reference_path; return output; } diff --git a/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp index f4aabb13b8916..5bb4e924b6d89 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp @@ -16,6 +16,8 @@ #include "behavior_path_planner/utils/goal_planner/util.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" +#include "tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp" #include #include @@ -55,16 +57,16 @@ boost::optional FreespacePullOver::plan(const Pose & goal_pose) const Pose end_pose = use_back_ ? goal_pose : tier4_autoware_utils::calcOffsetPose(goal_pose, -straight_distance, 0.0, 0.0); - const bool found_path = planner_->makePlan(current_pose, end_pose); - if (!found_path) { + if (!planner_->makePlan(current_pose, end_pose)) { return {}; } const auto road_lanes = utils::getExtendedCurrentLanes( planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, /*forward_only_in_route*/ false); - const auto pull_over_lanes = - goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_); + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(planner_data_->route_handler), left_side_parking_, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length); if (road_lanes.empty() || pull_over_lanes.empty()) { return {}; } @@ -113,18 +115,21 @@ boost::optional FreespacePullOver::plan(const Pose & goal_pose) addPose(goal_pose); } - utils::correctDividedPathVelocity(partial_paths); + std::vector> pairs_terminal_velocity_and_accel{}; + pairs_terminal_velocity_and_accel.resize(partial_paths.size()); + utils::start_goal_planner_common::modifyVelocityByDirection( + partial_paths, pairs_terminal_velocity_and_accel, velocity_, 0); + // Check if driving forward for each path, return empty if not for (auto & path : partial_paths) { - const auto is_driving_forward = motion_utils::isDrivingForward(path.points); - if (!is_driving_forward) { - // path points is less than 2 + if (!motion_utils::isDrivingForward(path.points)) { return {}; } } PullOverPath pull_over_path{}; pull_over_path.partial_paths = partial_paths; + pull_over_path.pairs_terminal_velocity_and_accel = pairs_terminal_velocity_and_accel; pull_over_path.start_pose = current_pose; pull_over_path.end_pose = goal_pose; pull_over_path.type = getPlannerType(); diff --git a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp index 6a5ccc827db29..f350262cf4b80 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp @@ -16,6 +16,7 @@ #include "behavior_path_planner/utils/goal_planner/util.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "motion_utils/trajectory/path_with_lane_id.hpp" #include #include @@ -47,13 +48,13 @@ boost::optional GeometricPullOver::plan(const Pose & goal_pose) const auto road_lanes = utils::getExtendedCurrentLanes( planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, /*forward_only_in_route*/ false); - const auto shoulder_lanes = - goal_planner_utils::getPullOverLanes(*route_handler, left_side_parking_); - if (road_lanes.empty() || shoulder_lanes.empty()) { + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *route_handler, left_side_parking_, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length); + if (road_lanes.empty() || pull_over_lanes.empty()) { return {}; } - auto lanes = road_lanes; - lanes.insert(lanes.end(), shoulder_lanes.begin(), shoulder_lanes.end()); + const auto lanes = utils::combineLanelets(road_lanes, pull_over_lanes); const auto & p = parallel_parking_parameters_; const double max_steer_angle = @@ -62,7 +63,7 @@ boost::optional GeometricPullOver::plan(const Pose & goal_pose) planner_.setPlannerData(planner_data_); const bool found_valid_path = - planner_.planPullOver(goal_pose, road_lanes, shoulder_lanes, is_forward_); + planner_.planPullOver(goal_pose, road_lanes, pull_over_lanes, is_forward_); if (!found_valid_path) { return {}; } @@ -75,6 +76,7 @@ boost::optional GeometricPullOver::plan(const Pose & goal_pose) PullOverPath pull_over_path{}; pull_over_path.type = getPlannerType(); pull_over_path.partial_paths = planner_.getPaths(); + pull_over_path.pairs_terminal_velocity_and_accel = planner_.getPairsTerminalVelocityAndAccel(); pull_over_path.start_pose = planner_.getStartPose(); pull_over_path.end_pose = planner_.getArcEndPose(); diff --git a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp index 1e57dcc319dc5..2dd81eba7e6c7 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp @@ -18,7 +18,9 @@ #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "lanelet2_extension/utility/utilities.hpp" +#include "motion_utils/trajectory/path_with_lane_id.hpp" +#include #include #include @@ -53,9 +55,13 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) const double lateral_offset_interval = parameters_.lateral_offset_interval; const double max_lateral_offset = parameters_.max_lateral_offset; const double ignore_distance_from_lane_start = parameters_.ignore_distance_from_lane_start; + const double vehicle_width = planner_data_->parameters.vehicle_width; + const double base_link2front = planner_data_->parameters.base_link2front; + const double base_link2rear = planner_data_->parameters.base_link2rear; - const auto pull_over_lanes = - goal_planner_utils::getPullOverLanes(*route_handler, left_side_parking_); + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *route_handler, left_side_parking_, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length); auto lanes = utils::getExtendedCurrentLanes( planner_data_, backward_length, forward_length, /*forward_only_in_route*/ false); @@ -76,7 +82,10 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) std::vector original_search_poses{}; // for search area visualizing size_t goal_id = 0; for (const auto & p : center_line_path.points) { - const Pose & center_pose = p.point.pose; + // todo(kosuke55): fix orientation for inverseTransformPoint temporarily + Pose center_pose = p.point.pose; + center_pose.orientation = + tier4_autoware_utils::createQuaternionFromYaw(tf2::getYaw(center_pose.orientation)); // ignore goal_pose near lane start const double distance_from_lane_start = @@ -85,13 +94,14 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) continue; } - const auto distance_from_left_bound = utils::getSignedDistanceFromBoundary( - pull_over_lanes, vehicle_footprint_, center_pose, left_side_parking_); - if (!distance_from_left_bound) continue; + const auto distance_from_bound = utils::getSignedDistanceFromBoundary( + pull_over_lanes, vehicle_width, base_link2front, base_link2rear, center_pose, + left_side_parking_); + if (!distance_from_bound) continue; - const double sign = left_side_parking_ ? 1.0 : -1.0; + const double sign = left_side_parking_ ? -1.0 : 1.0; const double offset_from_center_line = - sign * (std::abs(distance_from_left_bound.value()) - margin_from_boundary); + -distance_from_bound.value() + sign * margin_from_boundary; const Pose original_search_pose = calcOffsetPose(center_pose, 0, offset_from_center_line, 0); const double longitudinal_distance_from_original_goal = std::abs(motion_utils::calcSignedArcLength( @@ -102,7 +112,7 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) double lateral_offset = 0.0; for (double dy = 0; dy <= max_lateral_offset; dy += lateral_offset_interval) { lateral_offset = dy; - search_pose = calcOffsetPose(original_search_pose, 0, -sign * dy, 0); + search_pose = calcOffsetPose(original_search_pose, 0, sign * dy, 0); const auto transformed_vehicle_footprint = transformVector(vehicle_footprint_, tier4_autoware_utils::pose2transform(search_pose)); @@ -150,8 +160,9 @@ void GoalSearcher::update(GoalCandidates & goal_candidates) const } // check margin with pull over lane objects - const auto pull_over_lanes = - goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_); + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(planner_data_->route_handler), left_side_parking_, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length); const auto [shoulder_lane_objects, others] = utils::path_safety_checker::separateObjectsByLanelets( *(planner_data_->dynamic_object), pull_over_lanes); diff --git a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp index 2f5c1d9b05b7f..d6a3746187b11 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp @@ -16,6 +16,8 @@ #include "behavior_path_planner/utils/goal_planner/util.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" +#include "motion_utils/trajectory/path_with_lane_id.hpp" #include #include @@ -49,16 +51,16 @@ boost::optional ShiftPullOver::plan(const Pose & goal_pose) const auto road_lanes = utils::getExtendedCurrentLanes( planner_data_, backward_search_length, forward_search_length, /*forward_only_in_route*/ false); - const auto shoulder_lanes = - goal_planner_utils::getPullOverLanes(*route_handler, left_side_parking_); - if (road_lanes.empty() || shoulder_lanes.empty()) { + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *route_handler, left_side_parking_, backward_search_length, forward_search_length); + if (road_lanes.empty() || pull_over_lanes.empty()) { return {}; } // find safe one from paths with different jerk for (double lateral_jerk = min_jerk; lateral_jerk <= max_jerk; lateral_jerk += jerk_resolution) { const auto pull_over_path = - generatePullOverPath(road_lanes, shoulder_lanes, goal_pose, lateral_jerk); + generatePullOverPath(road_lanes, pull_over_lanes, goal_pose, lateral_jerk); if (!pull_over_path) continue; return *pull_over_path; } @@ -170,6 +172,9 @@ boost::optional ShiftPullOver::generatePullOverPath( shifted_path.path.points.push_back(p); } + shifted_path.path = + utils::start_goal_planner_common::removeInverseOrderPathPoints(shifted_path.path); + // set the same z as the goal for (auto & p : shifted_path.path.points) { p.point.pose.position.z = goal_pose.position.z; @@ -204,6 +209,7 @@ boost::optional ShiftPullOver::generatePullOverPath( PullOverPath pull_over_path{}; pull_over_path.type = getPlannerType(); pull_over_path.partial_paths.push_back(shifted_path.path); + pull_over_path.pairs_terminal_velocity_and_accel.push_back(std::make_pair(pull_over_velocity, 0)); pull_over_path.start_pose = path_shifter.getShiftLines().front().start; pull_over_path.end_pose = path_shifter.getShiftLines().front().end; pull_over_path.debug_poses.push_back(shift_end_pose_road_lane); diff --git a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp index 19b6bd27ea07c..aa3380c2b99df 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp @@ -55,36 +55,37 @@ PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWith return path; } -lanelet::ConstLanelets getPullOverLanes(const RouteHandler & route_handler, const bool left_side) +lanelet::ConstLanelets getPullOverLanes( + const RouteHandler & route_handler, const bool left_side, const double backward_distance, + const double forward_distance) { - const Pose goal_pose = route_handler.getGoalPose(); + const Pose goal_pose = route_handler.getOriginalGoalPose(); + + // Buffer to get enough lanes in front of the goal, need much longer than the pull over distance. + // In the case of loop lanes, it may not be possible to extend the lane forward. + // todo(kosuek55): automatically calculates this distance. + const double backward_distance_with_buffer = backward_distance + 100; lanelet::ConstLanelet target_shoulder_lane{}; if (route_handler::RouteHandler::getPullOverTarget( route_handler.getShoulderLanelets(), goal_pose, &target_shoulder_lane)) { // pull over on shoulder lane - return route_handler.getShoulderLaneletSequence(target_shoulder_lane, goal_pose); + return route_handler.getShoulderLaneletSequence( + target_shoulder_lane, goal_pose, backward_distance_with_buffer, forward_distance); } - // pull over on road lane lanelet::ConstLanelet closest_lane{}; route_handler.getClosestLaneletWithinRoute(goal_pose, &closest_lane); - lanelet::ConstLanelet outermost_lane{}; if (left_side) { - outermost_lane = route_handler.getMostLeftLanelet(closest_lane); + outermost_lane = route_handler.getMostLeftLanelet(closest_lane, false, true); } else { - outermost_lane = route_handler.getMostRightLanelet(closest_lane); + outermost_lane = route_handler.getMostRightLanelet(closest_lane, false, true); } - lanelet::ConstLanelet outermost_shoulder_lane; - if (route_handler.getLeftShoulderLanelet(outermost_lane, &outermost_shoulder_lane)) { - return route_handler.getShoulderLaneletSequence(outermost_shoulder_lane, goal_pose); - } - - const bool dist = std::numeric_limits::max(); constexpr bool only_route_lanes = false; - return route_handler.getLaneletSequence(outermost_lane, dist, dist, only_route_lanes); + return route_handler.getLaneletSequence( + outermost_lane, backward_distance_with_buffer, forward_distance, only_route_lanes); } PredictedObjects filterObjectsByLateralDistance( @@ -181,25 +182,22 @@ MarkerArray createGoalCandidatesMarkerArray( return marker_array; } -bool isAllowedGoalModification( - const std::shared_ptr & route_handler, const bool left_side_parking) +bool isAllowedGoalModification(const std::shared_ptr & route_handler) { - return route_handler->isAllowedGoalModification() || - checkOriginalGoalIsInShoulder(route_handler, left_side_parking); + return route_handler->isAllowedGoalModification() || checkOriginalGoalIsInShoulder(route_handler); } -bool checkOriginalGoalIsInShoulder( - const std::shared_ptr & route_handler, const bool left_side_parking) +bool checkOriginalGoalIsInShoulder(const std::shared_ptr & route_handler) { const Pose & goal_pose = route_handler->getGoalPose(); + const auto shoulder_lanes = route_handler->getShoulderLanelets(); - const lanelet::ConstLanelets pull_over_lanes = - goal_planner_utils::getPullOverLanes(*(route_handler), left_side_parking); - lanelet::ConstLanelet target_lane{}; - lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &target_lane); + lanelet::ConstLanelet closest_shoulder_lane{}; + if (lanelet::utils::query::getClosestLanelet(shoulder_lanes, goal_pose, &closest_shoulder_lane)) { + return lanelet::utils::isInLanelet(goal_pose, closest_shoulder_lane, 0.1); + } - return route_handler->isShoulderLanelet(target_lane) && - lanelet::utils::isInLanelet(goal_pose, target_lane, 0.1); + return false; } } // namespace goal_planner_utils diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index f3a1260eecadc..ffd5116a5d2dd 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -25,8 +25,11 @@ #include #include #include +#include +#include #include #include +#include #include #include diff --git a/planning/behavior_path_planner/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp b/planning/behavior_path_planner/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp index d244d08abf99b..0f87c68e38289 100644 --- a/planning/behavior_path_planner/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp +++ b/planning/behavior_path_planner/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp @@ -15,7 +15,7 @@ #include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" #include -#include +#include #include diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp index a775e7c16efed..7205b273ff281 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp @@ -16,6 +16,10 @@ #include "behavior_path_planner/utils/utils.hpp" +#include +#include +#include + namespace behavior_path_planner::utils::path_safety_checker { @@ -343,13 +347,13 @@ TargetObjectsOnLane createTargetObjectsOnLane( }; // TODO(Sugahara): Consider shoulder and other lane objects - if (object_lane_configuration.check_current_lane) { + if (object_lane_configuration.check_current_lane && !current_lanes.empty()) { append_objects_on_lane(target_objects_on_lane.on_current_lane, current_lanes); } - if (object_lane_configuration.check_left_lane) { + if (object_lane_configuration.check_left_lane && !all_left_lanelets.empty()) { append_objects_on_lane(target_objects_on_lane.on_left_lane, all_left_lanelets); } - if (object_lane_configuration.check_right_lane) { + if (object_lane_configuration.check_right_lane && !all_right_lanelets.empty()) { append_objects_on_lane(target_objects_on_lane.on_right_lane, all_right_lanelets); } diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index 2939ca29c0a40..dddcc672e7107 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp @@ -16,8 +16,10 @@ #include "behavior_path_planner/marker_utils/utils.hpp" #include "interpolation/linear_interpolation.hpp" +#include "motion_utils/trajectory/path_with_lane_id.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" namespace behavior_path_planner::utils::path_safety_checker { @@ -253,7 +255,11 @@ bool checkCollision( const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, double hysteresis_factor, CollisionCheckDebug & debug) { - debug.lerped_path.reserve(target_object_path.path.size()); + { + debug.ego_predicted_path = predicted_ego_path; + debug.obj_predicted_path = target_object_path.path; + debug.current_obj_pose = target_object.initial_pose.pose; + } for (const auto & obj_pose_with_poly : target_object_path.path) { const auto & current_time = obj_pose_with_poly.time; @@ -277,7 +283,6 @@ bool checkCollision( const auto & ego_velocity = interpolated_data->velocity; { - debug.lerped_path.push_back(ego_pose); debug.expected_ego_pose = ego_pose; debug.expected_obj_pose = obj_pose; debug.extended_ego_polygon = ego_polygon; diff --git a/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp b/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp index 9ca083af35c92..44ba6bd364bc4 100644 --- a/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp +++ b/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include diff --git a/planning/behavior_path_planner/src/utils/path_utils.cpp b/planning/behavior_path_planner/src/utils/path_utils.cpp index ec14a064bf51b..112c7e49c7c86 100644 --- a/planning/behavior_path_planner/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner/src/utils/path_utils.cpp @@ -15,11 +15,13 @@ #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "motion_utils/trajectory/path_with_lane_id.hpp" #include #include #include -#include +#include +#include #include @@ -418,6 +420,7 @@ void correctDividedPathVelocity(std::vector & divided_paths) { for (auto & path : divided_paths) { const auto is_driving_forward = motion_utils::isDrivingForward(path.points); + // If the number of points in the path is less than 2, don't correct the velocity if (!is_driving_forward) { continue; } diff --git a/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp b/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp new file mode 100644 index 0000000000000..bd695055b2079 --- /dev/null +++ b/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp @@ -0,0 +1,187 @@ +// Copyright 2023 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 "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" + +#include + +namespace behavior_path_planner::utils::start_goal_planner_common +{ + +using motion_utils::calcDecelDistWithJerkAndAccConstraints; + +boost::optional calcFeasibleDecelDistance( + std::shared_ptr planner_data, const double acc_lim, const double jerk_lim, + const double target_velocity) +{ + const auto v_now = planner_data->self_odometry->twist.twist.linear.x; + const auto a_now = planner_data->self_acceleration->accel.accel.linear.x; + + if (v_now < target_velocity) { + return 0.0; + } + + auto min_stop_distance = calcDecelDistWithJerkAndAccConstraints( + v_now, target_velocity, a_now, -acc_lim, jerk_lim, -1.0 * jerk_lim); + + if (!min_stop_distance) { + return {}; + } + + min_stop_distance = std::max(*min_stop_distance, 0.0); + + return min_stop_distance; +} + +void modifyVelocityByDirection( + std::vector & paths, + std::vector> & terminal_vel_acc_pairs, const double target_velocity, + const double acceleration) +{ + assert(paths.size() == terminal_vel_acc_pairs.size()); + + auto path_itr = std::begin(paths); + auto pair_itr = std::begin(terminal_vel_acc_pairs); + + for (; path_itr != std::end(paths); ++path_itr, ++pair_itr) { + const auto is_driving_forward = motion_utils::isDrivingForward(path_itr->points); + + // If the number of points in the path is less than 2, don't insert stop velocity and + // set pairs_terminal_velocity_and_accel to 0 + if (!is_driving_forward) { + *pair_itr = std::make_pair(0.0, 0.0); + continue; + } + + if (*is_driving_forward) { + for (auto & point : path_itr->points) { + // TODO(Sugahara): velocity calculation can be improved by considering the acceleration + point.point.longitudinal_velocity_mps = std::abs(point.point.longitudinal_velocity_mps); + } + // TODO(Sugahara): Consider the calculation of the target velocity and acceleration for ego's + // predicted path when ego will stop at the end of the path + *pair_itr = std::make_pair(target_velocity, acceleration); + } else { + for (auto & point : path_itr->points) { + point.point.longitudinal_velocity_mps = -std::abs(point.point.longitudinal_velocity_mps); + } + *pair_itr = std::make_pair(-target_velocity, -acceleration); + } + path_itr->points.back().point.longitudinal_velocity_mps = 0.0; + } +} + +void updateEgoPredictedPathParams( + std::shared_ptr & ego_predicted_path_params, + const std::shared_ptr & start_planner_params) +{ + ego_predicted_path_params = + std::make_shared(start_planner_params->ego_predicted_path_params); +} +void updateEgoPredictedPathParams( + std::shared_ptr & ego_predicted_path_params, + const std::shared_ptr & goal_planner_params) +{ + ego_predicted_path_params = + std::make_shared(goal_planner_params->ego_predicted_path_params); +} + +void updateSafetyCheckParams( + std::shared_ptr & safety_check_params, + const std::shared_ptr & start_planner_params) +{ + safety_check_params = + std::make_shared(start_planner_params->safety_check_params); +} + +void updateSafetyCheckParams( + std::shared_ptr & safety_check_params, + const std::shared_ptr & goal_planner_params) +{ + safety_check_params = + std::make_shared(goal_planner_params->safety_check_params); +} + +void updateObjectsFilteringParams( + std::shared_ptr & objects_filtering_params, + const std::shared_ptr & start_planner_params) +{ + objects_filtering_params = + std::make_shared(start_planner_params->objects_filtering_params); +} + +void updateObjectsFilteringParams( + std::shared_ptr & objects_filtering_params, + const std::shared_ptr & goal_planner_params) +{ + objects_filtering_params = + std::make_shared(goal_planner_params->objects_filtering_params); +} + +void updatePathProperty( + std::shared_ptr & ego_predicted_path_params, + const std::pair & pairs_terminal_velocity_and_accel) +{ + // If acceleration is close to 0, the ego predicted path will be too short, so a minimum value is + // necessary to ensure a reasonable path length. + // TODO(Sugahara): define them as parameter + const double min_accel_for_ego_predicted_path = 1.0; + const double acceleration = + std::max(pairs_terminal_velocity_and_accel.second, min_accel_for_ego_predicted_path); + + ego_predicted_path_params->target_velocity = pairs_terminal_velocity_and_accel.first; + ego_predicted_path_params->acceleration = acceleration; +} + +void initializeCollisionCheckDebugMap(CollisionCheckDebugMap & collision_check_debug_map) +{ + collision_check_debug_map.clear(); +} + +void updateSafetyCheckTargetObjectsData( + StartGoalPlannerData & data, const PredictedObjects & filtered_objects, + const TargetObjectsOnLane & target_objects_on_lane, + const std::vector & ego_predicted_path) +{ + data.filtered_objects = filtered_objects; + data.target_objects_on_lane = target_objects_on_lane; + data.ego_predicted_path = ego_predicted_path; +} + +std::pair getPairsTerminalVelocityAndAccel( + const std::vector> & pairs_terminal_velocity_and_accel, + const size_t current_path_idx) +{ + if (pairs_terminal_velocity_and_accel.size() <= current_path_idx) { + return std::make_pair(0.0, 0.0); + } + return pairs_terminal_velocity_and_accel.at(current_path_idx); +} + +PathWithLaneId removeInverseOrderPathPoints(const PathWithLaneId & path) +{ + PathWithLaneId fixed_path; + fixed_path.header = path.header; + fixed_path.points.push_back(path.points.at(0)); + for (size_t i = 1; i < path.points.size(); i++) { + const auto p1 = path.points.at(i - 1).point.pose; + const auto p2 = path.points.at(i).point.pose; + if (tier4_autoware_utils::isDrivingForward(p1, p2)) { + fixed_path.points.push_back(path.points.at(i)); + } + } + return fixed_path; +} + +} // namespace behavior_path_planner::utils::start_goal_planner_common diff --git a/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp index ba34901d9df6a..36fb6381ac967 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp @@ -44,7 +44,7 @@ FreespacePullOut::FreespacePullOut( } } -boost::optional FreespacePullOut::plan(const Pose start_pose, const Pose end_pose) +boost::optional FreespacePullOut::plan(const Pose & start_pose, const Pose & end_pose) { const auto & route_handler = planner_data_->route_handler; const double backward_path_length = planner_data_->parameters.backward_path_length; @@ -86,7 +86,7 @@ boost::optional FreespacePullOut::plan(const Pose start_pose, const } // push back generate road lane path between end pose and goal pose to last path - const auto & goal_pose = route_handler->getGoalPose(); + const Pose goal_pose = route_handler->getGoalPose(); constexpr double offset_from_end_pose = 1.0; const auto arc_position_end = lanelet::utils::getArcCoordinates(road_lanes, end_pose); const double s_start = std::max(arc_position_end.length + offset_from_end_pose, 0.0); diff --git a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp index 45d36d51d69b4..0d75391f9d4a6 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp @@ -19,6 +19,7 @@ #include "behavior_path_planner/utils/utils.hpp" #include +#include using lanelet::utils::getArcCoordinates; using motion_utils::findNearestIndex; @@ -36,7 +37,7 @@ GeometricPullOut::GeometricPullOut(rclcpp::Node & node, const StartPlannerParame planner_.setParameters(parallel_parking_parameters_); } -boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_pose) +boost::optional GeometricPullOut::plan(const Pose & start_pose, const Pose & goal_pose) { PullOutPath output; @@ -47,16 +48,6 @@ boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_p planner_data_, backward_path_length, std::numeric_limits::max(), /*forward_only_in_route*/ true); const auto pull_out_lanes = getPullOutLanes(planner_data_, backward_path_length); - auto lanes = road_lanes; - for (const auto & pull_out_lane : pull_out_lanes) { - auto it = std::find_if( - lanes.begin(), lanes.end(), [&pull_out_lane](const lanelet::ConstLanelet & lane) { - return lane.id() == pull_out_lane.id(); - }); - if (it == lanes.end()) { - lanes.push_back(pull_out_lane); - } - } planner_.setTurningRadius( planner_data_->parameters, parallel_parking_parameters_.pull_out_max_steer_angle); @@ -69,11 +60,10 @@ boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_p // collision check with stop objects in pull out lanes const auto arc_path = planner_.getArcPath(); - const auto [pull_out_lane_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets( - *(planner_data_->dynamic_object), pull_out_lanes); - const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( - pull_out_lane_objects, parameters_.th_moving_object_velocity); + const auto & stop_objects = utils::path_safety_checker::filterObjectsByVelocity( + *(planner_data_->dynamic_object), parameters_.th_moving_object_velocity); + const auto [pull_out_lane_stop_objects, others] = + utils::path_safety_checker::separateObjectsByLanelets(stop_objects, pull_out_lanes); if (utils::checkCollisionBetweenPathFootprintsAndObjects( vehicle_footprint_, arc_path, pull_out_lane_stop_objects, @@ -123,6 +113,7 @@ boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_p output.pairs_terminal_velocity_and_accel.push_back( std::make_pair(velocity, velocity * velocity / 2 * arc_length_on_path)); } + output.start_pose = planner_.getArcPaths().at(0).points.front().point.pose; output.end_pose = planner_.getArcPaths().at(1).points.back().point.pose; diff --git a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp index 7352821e140be..6225ce598afe3 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp @@ -16,8 +16,10 @@ #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "motion_utils/trajectory/path_with_lane_id.hpp" #include @@ -40,7 +42,7 @@ ShiftPullOut::ShiftPullOut( { } -boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) +boost::optional ShiftPullOut::plan(const Pose & start_pose, const Pose & goal_pose) { const auto & route_handler = planner_data_->route_handler; const auto & common_parameters = planner_data_->parameters; @@ -96,17 +98,48 @@ boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) shift_path.points.begin() + collision_check_end_idx + 1); } - // check lane departure + // extract shoulder lanes from pull out lanes + lanelet::ConstLanelets shoulder_lanes; + std::copy_if( + pull_out_lanes.begin(), pull_out_lanes.end(), std::back_inserter(shoulder_lanes), + [&route_handler](const auto & pull_out_lane) { + return route_handler->isShoulderLanelet(pull_out_lane); + }); const auto drivable_lanes = - utils::generateDrivableLanesWithShoulderLanes(road_lanes, pull_out_lanes); + utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes); const auto & dp = planner_data_->drivable_area_expansion_parameters; - const auto expanded_lanes = utils::expandLanelets( + const auto expanded_lanes = utils::transformToLanelets(utils::expandLanelets( drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, - dp.drivable_area_types_to_skip); + dp.drivable_area_types_to_skip)); + + // crop backward path + // removes points which are out of lanes up to the start pose. + // this ensures that the backward_path stays within the drivable area when starting from a + // narrow place. + const size_t start_segment_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + shift_path.points, start_pose, common_parameters.ego_nearest_dist_threshold, + common_parameters.ego_nearest_yaw_threshold); + PathWithLaneId cropped_path{}; + for (size_t i = 0; i < shift_path.points.size(); ++i) { + const Pose pose = shift_path.points.at(i).point.pose; + const auto transformed_vehicle_footprint = + transformVector(vehicle_footprint_, tier4_autoware_utils::pose2transform(pose)); + const bool is_out_of_lane = + LaneDepartureChecker::isOutOfLane(expanded_lanes, transformed_vehicle_footprint); + if (i <= start_segment_idx) { + if (!is_out_of_lane) { + cropped_path.points.push_back(shift_path.points.at(i)); + } + } else { + cropped_path.points.push_back(shift_path.points.at(i)); + } + } + shift_path.points = cropped_path.points; + + // check lane departure if ( parameters_.check_shift_path_lane_departure && - lane_departure_checker_->checkPathWillLeaveLane( - utils::transformToLanelets(expanded_lanes), path_start_to_end)) { + lane_departure_checker_->checkPathWillLeaveLane(expanded_lanes, path_start_to_end)) { continue; } @@ -266,6 +299,9 @@ std::vector ShiftPullOut::calcPullOutPaths( continue; } + shifted_path.path = + utils::start_goal_planner_common::removeInverseOrderPathPoints(shifted_path.path); + // set velocity const size_t pull_out_end_idx = findNearestIndex(shifted_path.path.points, shift_end_pose_ptr->position); diff --git a/planning/behavior_path_planner/src/utils/start_planner/util.cpp b/planning/behavior_path_planner/src/utils/start_planner/util.cpp index 9ee0491ca017e..f5b4c9b979ee3 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/util.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/util.cpp @@ -20,6 +20,7 @@ #include "behavior_path_planner/utils/utils.hpp" #include +#include #include #include diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index eb2859ff700af..d6af88f39a8d2 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -20,6 +20,9 @@ #include #include #include +#include +#include +#include #include "autoware_auto_perception_msgs/msg/predicted_object.hpp" #include "autoware_auto_perception_msgs/msg/predicted_path.hpp" @@ -1532,9 +1535,7 @@ void generateDrivableArea( } const auto & expansion_params = planner_data->drivable_area_expansion_parameters; if (expansion_params.enabled) { - drivable_area_expansion::expandDrivableArea( - path, expansion_params, *planner_data->dynamic_object, *planner_data->route_handler, - transformed_lanes); + drivable_area_expansion::expandDrivableArea(path, planner_data, transformed_lanes); } // make bound longitudinally monotonic @@ -1869,6 +1870,7 @@ void makeBoundLongitudinallyMonotonic( if (intersect_point) { Pose pose; pose.position = *intersect_point; + pose.position.z = bound_with_pose.at(i).position.z; const auto yaw = calcAzimuthAngle(*intersect_point, bound_with_pose.at(i + 1).position); pose.orientation = createQuaternionFromRPY(0.0, 0.0, yaw); monotonic_bound.push_back(pose); @@ -1902,18 +1904,18 @@ void makeBoundLongitudinallyMonotonic( std::vector ret; - ret.push_back(bound.front()); + for (size_t i = 0; i < bound.size(); i++) { + const auto & p_new = bound.at(i); + const auto duplicated_points_itr = std::find_if( + ret.begin(), ret.end(), + [&p_new](const auto & p) { return calcDistance2d(p, p_new) < 0.1; }); - for (size_t i = 0; i < bound.size() - 2; i++) { - try { - motion_utils::validateNonSharpAngle(bound.at(i), bound.at(i + 1), bound.at(i + 2)); - ret.push_back(bound.at(i + 1)); - } catch (const std::exception & e) { - continue; + if (duplicated_points_itr != ret.end() && std::next(duplicated_points_itr, 2) == ret.end()) { + ret.erase(duplicated_points_itr, ret.end()); } - } - ret.push_back(bound.back()); + ret.push_back(p_new); + } return ret; }; @@ -2406,64 +2408,126 @@ double getSignedDistanceFromBoundary( } std::optional getSignedDistanceFromBoundary( - const lanelet::ConstLanelets & lanelets, const LinearRing2d & footprint, - const Pose & vehicle_pose, const bool left_side) -{ - double min_distance = std::numeric_limits::max(); - - const auto transformed_footprint = - transformVector(footprint, tier4_autoware_utils::pose2transform(vehicle_pose)); - bool found_neighbor_bound = false; - for (const auto & vehicle_corner_point : transformed_footprint) { - // convert point of footprint to pose + const lanelet::ConstLanelets & lanelets, const double vehicle_width, const double base_link2front, + const double base_link2rear, const Pose & vehicle_pose, const bool left_side) +{ + // Depending on which side is selected, calculate the transformed coordinates of the front and + // rear vehicle corners + Point rear_corner_point, front_corner_point; + if (left_side) { + Point front_left, rear_left; + rear_left.x = -base_link2rear; + rear_left.y = vehicle_width / 2; + front_left.x = base_link2front; + front_left.y = vehicle_width / 2; + rear_corner_point = tier4_autoware_utils::transformPoint(rear_left, vehicle_pose); + front_corner_point = tier4_autoware_utils::transformPoint(front_left, vehicle_pose); + } else { + Point front_right, rear_right; + rear_right.x = -base_link2rear; + rear_right.y = -vehicle_width / 2; + front_right.x = base_link2front; + front_right.y = -vehicle_width / 2; + rear_corner_point = tier4_autoware_utils::transformPoint(rear_right, vehicle_pose); + front_corner_point = tier4_autoware_utils::transformPoint(front_right, vehicle_pose); + } + + const auto combined_lane = lanelet::utils::combineLaneletsShape(lanelets); + const auto & bound_line_2d = left_side ? lanelet::utils::to2D(combined_lane.leftBound3d()) + : lanelet::utils::to2D(combined_lane.rightBound3d()); + + // Initialize the lateral distance to the maximum (for left side) or the minimum (for right side) + // possible value. + double lateral_distance = + left_side ? -std::numeric_limits::max() : std::numeric_limits::max(); + + // Find the closest bound segment that contains the corner point in the X-direction + // and calculate the lateral distance from that segment. + const auto calcLateralDistanceFromBound = [&]( + const Point & vehicle_corner_point, + boost::optional & lateral_distance, + boost::optional & segment_idx) { Pose vehicle_corner_pose{}; - vehicle_corner_pose.position = tier4_autoware_utils::toMsg(vehicle_corner_point.to_3d()); + vehicle_corner_pose.position = vehicle_corner_point; vehicle_corner_pose.orientation = vehicle_pose.orientation; - // calculate distance to the bound directly next to footprint points - lanelet::ConstLanelet closest_lanelet{}; - if (lanelet::utils::query::getClosestLanelet(lanelets, vehicle_corner_pose, &closest_lanelet)) { - const auto & bound_line_2d = left_side ? lanelet::utils::to2D(closest_lanelet.leftBound3d()) - : lanelet::utils::to2D(closest_lanelet.rightBound3d()); - - for (size_t i = 1; i < bound_line_2d.size(); ++i) { - const Point p_front = lanelet::utils::conversion::toGeomMsgPt(bound_line_2d[i - 1]); - const Point p_back = lanelet::utils::conversion::toGeomMsgPt(bound_line_2d[i]); - - const Point inverse_p_front = - tier4_autoware_utils::inverseTransformPoint(p_front, vehicle_corner_pose); - const Point inverse_p_back = - tier4_autoware_utils::inverseTransformPoint(p_back, vehicle_corner_pose); - - const double dy_front = inverse_p_front.y; - const double dy_back = inverse_p_back.y; - const double dx_front = inverse_p_front.x; - const double dx_back = inverse_p_back.x; - // is in segment - if (dx_front < 0 && dx_back > 0) { - const double lateral_distance_from_pose_to_segment = - (dy_front * dx_back + dy_back * -dx_front) / (dx_back - dx_front); - - if (std::abs(lateral_distance_from_pose_to_segment) < std::abs(min_distance)) { - min_distance = lateral_distance_from_pose_to_segment; - } - - found_neighbor_bound = true; - break; - } + // Euclidean distance to find the closest segment containing the corner point. + double min_distance = std::numeric_limits::max(); + + for (size_t i = 0; i < bound_line_2d.size() - 1; i++) { + const Point p1 = lanelet::utils::conversion::toGeomMsgPt(bound_line_2d[i]); + const Point p2 = lanelet::utils::conversion::toGeomMsgPt(bound_line_2d[i + 1]); + + const Point inverse_p1 = tier4_autoware_utils::inverseTransformPoint(p1, vehicle_corner_pose); + const Point inverse_p2 = tier4_autoware_utils::inverseTransformPoint(p2, vehicle_corner_pose); + const double dx_p1 = inverse_p1.x; + const double dx_p2 = inverse_p2.x; + const double dy_p1 = inverse_p1.y; + const double dy_p2 = inverse_p2.y; + + // Calculate the Euclidean distances between vehicle's corner and the current and next points. + const double distance1 = tier4_autoware_utils::calcDistance2d(p1, vehicle_corner_point); + const double distance2 = tier4_autoware_utils::calcDistance2d(p2, vehicle_corner_point); + + // If one of the bound points is behind and the other is in front of the vehicle corner point + // and any of these points is closer than the current minimum distance, + // then update minimum distance, lateral distance and the segment index. + if (dx_p1 < 0 && dx_p2 > 0 && (distance1 < min_distance || distance2 < min_distance)) { + min_distance = std::min(distance1, distance2); + // Update lateral distance using the formula derived from similar triangles in the lateral + // cross-section view. + lateral_distance = -1.0 * (dy_p1 * dx_p2 + dy_p2 * -dx_p1) / (dx_p2 - dx_p1); + segment_idx = i; } } - } + }; - if (!found_neighbor_bound) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("utils"), - "neighbor shoulder bound to footprint is not found."); + // Calculate the lateral distance for both the rear and front corners of the vehicle. + boost::optional rear_segment_idx{}; + boost::optional rear_lateral_distance{}; + calcLateralDistanceFromBound(rear_corner_point, rear_lateral_distance, rear_segment_idx); + boost::optional front_segment_idx{}; + boost::optional front_lateral_distance{}; + calcLateralDistanceFromBound(front_corner_point, front_lateral_distance, front_segment_idx); + + // If no closest bound segment was found for both corners, return an empty optional. + if (!rear_lateral_distance && !front_lateral_distance) { return {}; } + // If only one of them found the closest bound, return the found lateral distance. + if (!rear_lateral_distance) { + return *front_lateral_distance; + } else if (!front_lateral_distance) { + return *rear_lateral_distance; + } + // If both corners found their closest bound, return the maximum (for left side) or the minimum + // (for right side) lateral distance. + lateral_distance = left_side ? std::max(*rear_lateral_distance, *front_lateral_distance) + : std::min(*rear_lateral_distance, *front_lateral_distance); + + // Iterate through all segments between the segments closest to the rear and front corners. + // Update the lateral distance in case any of these inner segments are closer to the vehicle. + for (size_t i = *rear_segment_idx + 1; i < *front_segment_idx; i++) { + Pose bound_pose; + bound_pose.position = lanelet::utils::conversion::toGeomMsgPt(bound_line_2d[i]); + bound_pose.orientation = vehicle_pose.orientation; + + const Point inverse_rear_point = + tier4_autoware_utils::inverseTransformPoint(rear_corner_point, bound_pose); + const Point inverse_front_point = + tier4_autoware_utils::inverseTransformPoint(front_corner_point, bound_pose); + const double dx_rear = inverse_rear_point.x; + const double dx_front = inverse_front_point.x; + const double dy_rear = inverse_rear_point.y; + const double dy_front = inverse_front_point.y; + + const double current_lateral_distance = + (dy_rear * dx_front + dy_front * -dx_rear) / (dx_front - dx_rear); + lateral_distance = left_side ? std::max(lateral_distance, current_lateral_distance) + : std::min(lateral_distance, current_lateral_distance); + } - // min_distance is the distance from corner_pose to bound, so reverse this value - return -min_distance; + return lateral_distance; } double getArcLengthToTargetLanelet( @@ -2787,7 +2851,7 @@ BehaviorModuleOutput getReferencePath( // clip backward length // NOTE: In order to keep backward_path_length at least, resampling interval is added to the // backward. - const size_t current_seg_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( reference_path.points, no_shift_pose, p.ego_nearest_dist_threshold, p.ego_nearest_yaw_threshold); reference_path.points = motion_utils::cropPoints( diff --git a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp b/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp index 605b3b96ee9b9..7a5bb68decdfa 100644 --- a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/expansion.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp" @@ -257,6 +258,7 @@ TEST(DrivableAreaExpansionProjection, expandDrivableArea) params.compensate_extra_dist = false; params.max_expansion_distance = 0.0; // means no limit params.max_path_arc_length = 0.0; // means no limit + params.resample_interval = 1.0; params.extra_arc_length = 1.0; params.expansion_method = "polygon"; // 2m x 4m ego footprint @@ -265,9 +267,15 @@ TEST(DrivableAreaExpansionProjection, expandDrivableArea) params.ego_left_offset = 2.0; params.ego_right_offset = -2.0; } + behavior_path_planner::PlannerData planner_data; + planner_data.drivable_area_expansion_parameters = params; + planner_data.reference_path = std::make_shared(path); + planner_data.dynamic_object = + std::make_shared(dynamic_objects); + planner_data.route_handler = std::make_shared(route_handler); // we expect the drivable area to be expanded by 1m on each side drivable_area_expansion::expandDrivableArea( - path, params, dynamic_objects, route_handler, path_lanes); + path, std::make_shared(planner_data), path_lanes); // unchanged path points ASSERT_EQ(path.points.size(), 3ul); for (auto i = 0.0; i < path.points.size(); ++i) { @@ -276,13 +284,15 @@ TEST(DrivableAreaExpansionProjection, expandDrivableArea) } // expanded left bound - ASSERT_EQ(path.left_bound.size(), 3ul); + ASSERT_EQ(path.left_bound.size(), 4ul); EXPECT_NEAR(path.left_bound[0].x, 0.0, eps); EXPECT_NEAR(path.left_bound[0].y, 1.0, eps); EXPECT_NEAR(path.left_bound[1].x, 0.0, eps); EXPECT_NEAR(path.left_bound[1].y, 2.0, eps); EXPECT_NEAR(path.left_bound[2].x, 2.0, eps); EXPECT_NEAR(path.left_bound[2].y, 2.0, eps); + EXPECT_NEAR(path.left_bound[3].x, 2.0, eps); + EXPECT_NEAR(path.left_bound[3].y, 1.0, eps); // expanded right bound ASSERT_EQ(path.right_bound.size(), 3ul); EXPECT_NEAR(path.right_bound[0].x, 0.0, eps); @@ -304,7 +314,7 @@ TEST(DrivableAreaExpansion, calculateDistanceLimit) const linestring_t base_ls = {{0.0, 0.0}, {10.0, 0.0}}; const multi_linestring_t uncrossable_lines = {}; const polygon_t expansion_polygon = { - {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {10.0, -4.0}}, {}}; + {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {0.0, -4.0}}, {}}; const auto limit_distance = calculateDistanceLimit(base_ls, expansion_polygon, uncrossable_lines); EXPECT_NEAR(limit_distance, std::numeric_limits::max(), 1e-9); @@ -313,7 +323,7 @@ TEST(DrivableAreaExpansion, calculateDistanceLimit) const linestring_t base_ls = {{0.0, 0.0}, {10.0, 0.0}}; const linestring_t uncrossable_line = {{0.0, 2.0}, {10.0, 2.0}}; const polygon_t expansion_polygon = { - {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {10.0, -4.0}}, {}}; + {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {0.0, -4.0}}, {}}; const auto limit_distance = calculateDistanceLimit(base_ls, expansion_polygon, {uncrossable_line}); EXPECT_NEAR(limit_distance, 2.0, 1e-9); @@ -323,9 +333,44 @@ TEST(DrivableAreaExpansion, calculateDistanceLimit) const multi_linestring_t uncrossable_lines = { {{0.0, 2.0}, {10.0, 2.0}}, {{0.0, 1.5}, {10.0, 1.0}}}; const polygon_t expansion_polygon = { - {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {10.0, -4.0}}, {}}; + {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {0.0, -4.0}}, {}}; const auto limit_distance = calculateDistanceLimit(base_ls, expansion_polygon, uncrossable_lines); EXPECT_NEAR(limit_distance, 1.0, 1e-9); } } + +TEST(DrivableAreaExpansion, calculateDistanceLimitEdgeCases) +{ + using drivable_area_expansion::calculateDistanceLimit; + using drivable_area_expansion::linestring_t; + using drivable_area_expansion::polygon_t; + + const linestring_t base_ls = {{0.0, 0.0}, {10.0, 0.0}}; + const polygon_t expansion_polygon = { + {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {0.0, -4.0}}, {}}; + { // intersection points further than the line point inside the expansion polygon + const linestring_t uncrossable_lines = {{4.0, 5.0}, {6.0, 3.0}}; + const auto limit_distance = + calculateDistanceLimit(base_ls, expansion_polygon, {uncrossable_lines}); + EXPECT_NEAR(limit_distance, 3.0, 1e-9); + } + { // intersection points further than the line point inside the expansion polygon + const linestring_t uncrossable_lines = {{4.0, 5.0}, {5.0, 2.0}, {6.0, 4.5}}; + const auto limit_distance = + calculateDistanceLimit(base_ls, expansion_polygon, {uncrossable_lines}); + EXPECT_NEAR(limit_distance, 2.0, 1e-9); + } + { // line completely inside the expansion polygon + const linestring_t uncrossable_lines = {{4.0, 2.0}, {6.0, 3.0}}; + const auto limit_distance = + calculateDistanceLimit(base_ls, expansion_polygon, {uncrossable_lines}); + EXPECT_NEAR(limit_distance, 2.0, 1e-9); + } + { // line completely inside the expansion polygon + const linestring_t uncrossable_lines = {{4.0, 3.5}, {6.0, 3.0}}; + const auto limit_distance = + calculateDistanceLimit(base_ls, expansion_polygon, {uncrossable_lines}); + EXPECT_NEAR(limit_distance, 3.0, 1e-9); + } +} diff --git a/planning/behavior_path_planner/test/test_lane_change_utils.cpp b/planning/behavior_path_planner/test/test_lane_change_utils.cpp index 3a809509641f4..ad64a7ae3feb5 100644 --- a/planning/behavior_path_planner/test/test_lane_change_utils.cpp +++ b/planning/behavior_path_planner/test/test_lane_change_utils.cpp @@ -12,7 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. #include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#include +#include #include #include diff --git a/planning/behavior_path_planner/test/test_safety_check.cpp b/planning/behavior_path_planner/test/test_safety_check.cpp index 9dfcef487cc81..39e831bd5565b 100644 --- a/planning/behavior_path_planner/test/test_safety_check.cpp +++ b/planning/behavior_path_planner/test/test_safety_check.cpp @@ -15,7 +15,8 @@ #include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#include #include diff --git a/planning/behavior_path_planner/test/test_turn_signal.cpp b/planning/behavior_path_planner/test/test_turn_signal.cpp index 8c6f8de7ee6a0..4917443064e78 100644 --- a/planning/behavior_path_planner/test/test_turn_signal.cpp +++ b/planning/behavior_path_planner/test/test_turn_signal.cpp @@ -12,8 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. #include "behavior_path_planner/turn_signal_decider.hpp" +#include "motion_utils/trajectory/path_with_lane_id.hpp" + +#include +#include +#include #include "autoware_auto_planning_msgs/msg/path_point.hpp" +#include #include #include diff --git a/planning/behavior_path_planner/test/test_utils.cpp b/planning/behavior_path_planner/test/test_utils.cpp index b9e2727ebaa13..1452e45980793 100644 --- a/planning/behavior_path_planner/test/test_utils.cpp +++ b/planning/behavior_path_planner/test/test_utils.cpp @@ -16,6 +16,7 @@ #include "lanelet2_core/Attribute.h" #include "lanelet2_core/geometry/Point.h" #include "lanelet2_core/primitives/Lanelet.h" +#include "motion_utils/trajectory/path_with_lane_id.hpp" #include #include diff --git a/planning/behavior_velocity_blind_spot_module/src/debug.cpp b/planning/behavior_velocity_blind_spot_module/src/debug.cpp index dbd4764585d81..1652d9d56a8a0 100644 --- a/planning/behavior_velocity_blind_spot_module/src/debug.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/debug.cpp @@ -16,7 +16,8 @@ #include #include -#include +#include +#include #include diff --git a/planning/behavior_velocity_crosswalk_module/package.xml b/planning/behavior_velocity_crosswalk_module/package.xml index 75f96c9194aef..66f326ed799af 100644 --- a/planning/behavior_velocity_crosswalk_module/package.xml +++ b/planning/behavior_velocity_crosswalk_module/package.xml @@ -21,6 +21,7 @@ autoware_auto_perception_msgs autoware_auto_planning_msgs autoware_auto_tf2 + autoware_perception_msgs behavior_velocity_planner_common eigen geometry_msgs diff --git a/planning/behavior_velocity_crosswalk_module/scripts/time_to_collision_plotter.py b/planning/behavior_velocity_crosswalk_module/scripts/time_to_collision_plotter.py index 5093683e17c22..8a9de1a563249 100755 --- a/planning/behavior_velocity_crosswalk_module/scripts/time_to_collision_plotter.py +++ b/planning/behavior_velocity_crosswalk_module/scripts/time_to_collision_plotter.py @@ -109,7 +109,9 @@ def on_collision_info(self, msg): return for i in range(int(len(collision_info_str_vec) / 5)): - collision_info_data_vec.append(CollisionInfo(*collision_info_str_vec[i : i + 5])) + collision_info_data_vec.append( + CollisionInfo(*collision_info_str_vec[i * 5 : i * 5 + 5]) + ) # memorize data in the history dictionary for collision_info_data in collision_info_data_vec: diff --git a/planning/behavior_velocity_crosswalk_module/src/debug.cpp b/planning/behavior_velocity_crosswalk_module/src/debug.cpp index eae7491872cfd..d6a2a3cb185a1 100644 --- a/planning/behavior_velocity_crosswalk_module/src/debug.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/debug.cpp @@ -15,8 +15,9 @@ #include "scene_crosswalk.hpp" #include -#include -#include +#include +#include +#include #include diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 7adde028f59c0..ab84caa782d1c 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -18,8 +18,11 @@ #include #include #include +#include #include -#include +#include +#include +#include #include #include @@ -37,6 +40,7 @@ using motion_utils::calcLateralOffset; using motion_utils::calcLongitudinalOffsetPoint; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; +using motion_utils::calcSignedArcLengthPartialSum; using motion_utils::findNearestSegmentIndex; using motion_utils::insertTargetPoint; using motion_utils::resamplePath; @@ -231,9 +235,12 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto // Calculate stop point with margin const auto p_stop_line = getStopPointWithMargin(*path, path_intersects); - // TODO(murooka) add a guard of p_stop_line - const auto default_stop_pose = toStdOptional( - calcLongitudinalOffsetPose(path->points, p_stop_line->first, p_stop_line->second)); + + std::optional default_stop_pose = std::nullopt; + if (p_stop_line.has_value()) { + default_stop_pose = toStdOptional( + calcLongitudinalOffsetPose(path->points, p_stop_line->first, p_stop_line->second)); + } // Resample path sparsely for less computation cost constexpr double resample_interval = 4.0; @@ -748,15 +755,15 @@ Polygon2d CrosswalkModule::getAttentionArea( { const auto & ego_pos = planner_data_->current_odometry->pose.position; const auto ego_polygon = createVehiclePolygon(planner_data_->vehicle_info_); + const auto backward_path_length = + calcSignedArcLength(sparse_resample_path.points, size_t(0), ego_pos); + const auto length_sum = calcSignedArcLengthPartialSum( + sparse_resample_path.points, size_t(0), sparse_resample_path.points.size()); Polygon2d attention_area; for (size_t j = 0; j < sparse_resample_path.points.size() - 1; ++j) { - const auto & p_ego_front = sparse_resample_path.points.at(j).point.pose; - const auto & p_ego_back = sparse_resample_path.points.at(j + 1).point.pose; - const auto front_length = - calcSignedArcLength(sparse_resample_path.points, ego_pos, p_ego_front.position); - const auto back_length = - calcSignedArcLength(sparse_resample_path.points, ego_pos, p_ego_back.position); + const auto front_length = length_sum.at(j) - backward_path_length; + const auto back_length = length_sum.at(j + 1) - backward_path_length; if (back_length < crosswalk_attention_range.first) { continue; @@ -934,28 +941,25 @@ bool CrosswalkModule::isRedSignalForPedestrians() const crosswalk_.regulatoryElementsAs(); for (const auto & traffic_lights_reg_elem : traffic_lights_reg_elems) { - lanelet::ConstLineStringsOrPolygons3d traffic_lights = traffic_lights_reg_elem->trafficLights(); - for (const auto & traffic_light : traffic_lights) { - const auto ll_traffic_light = static_cast(traffic_light); - const auto traffic_signal_stamped = planner_data_->getTrafficSignal(ll_traffic_light.id()); - if (!traffic_signal_stamped) { - continue; - } + const auto traffic_signal_stamped = + planner_data_->getTrafficSignal(traffic_lights_reg_elem->id()); + if (!traffic_signal_stamped) { + continue; + } - if ( - planner_param_.traffic_light_state_timeout < - (clock_->now() - traffic_signal_stamped->header.stamp).seconds()) { - continue; - } + if ( + planner_param_.traffic_light_state_timeout < + (clock_->now() - traffic_signal_stamped->stamp).seconds()) { + continue; + } - const auto & lights = traffic_signal_stamped->signal.lights; - if (lights.empty()) { - continue; - } + const auto & lights = traffic_signal_stamped->signal.elements; + if (lights.empty()) { + continue; + } - if (lights.front().color == TrafficLight::RED) { - return true; - } + if (lights.front().color == TrafficSignalElement::RED) { + return true; } } @@ -1068,6 +1072,8 @@ void CrosswalkModule::setDistanceToStop( const auto & ego_pos = planner_data_->current_odometry->pose.position; const double dist_ego2stop = calcSignedArcLength(ego_path.points, ego_pos, *stop_pos); setDistance(dist_ego2stop); + } else { + setDistance(std::numeric_limits::lowest()); } } diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index e0851366d7f71..11e2d600c687b 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -22,7 +22,8 @@ #include #include #include -#include +#include +#include #include #include @@ -50,8 +51,8 @@ namespace behavior_velocity_planner using autoware_auto_perception_msgs::msg::ObjectClassification; using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::TrafficLight; using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::TrafficSignalElement; using lanelet::autoware::Crosswalk; using tier4_api_msgs::msg::CrosswalkStatus; using tier4_autoware_utils::Polygon2d; diff --git a/planning/behavior_velocity_crosswalk_module/src/util.cpp b/planning/behavior_velocity_crosswalk_module/src/util.cpp index 72c091cd8bc85..6fabc4c201687 100644 --- a/planning/behavior_velocity_crosswalk_module/src/util.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/util.cpp @@ -15,7 +15,9 @@ #include "behavior_velocity_crosswalk_module/util.hpp" #include -#include +#include +#include +#include #include diff --git a/planning/behavior_velocity_detection_area_module/src/debug.cpp b/planning/behavior_velocity_detection_area_module/src/debug.cpp index 60479d5adfd88..b3d3d5c781ff8 100644 --- a/planning/behavior_velocity_detection_area_module/src/debug.cpp +++ b/planning/behavior_velocity_detection_area_module/src/debug.cpp @@ -16,7 +16,8 @@ #include #include -#include +#include +#include #ifdef ROS_DISTRO_GALACTIC #include diff --git a/planning/behavior_velocity_intersection_module/README.md b/planning/behavior_velocity_intersection_module/README.md index 3e62ab75d23cd..5f26ef34c1186 100644 --- a/planning/behavior_velocity_intersection_module/README.md +++ b/planning/behavior_velocity_intersection_module/README.md @@ -62,7 +62,7 @@ Objects that satisfy all of the following conditions are considered as target ob #### Stuck Vehicle Detection -If there is any object in a certain distance (`stuck_vehicle_ignore_dist` and `stuck_vehicle_detect_dist`) before and after the exit of the intersection lane and the object velocity is less than a threshold (=`stuck_vehicle_vel_thr`), the object is regarded as a stuck vehicle. If stuck vehicles exist, this module inserts a stopline a certain distance (=`stop_line_margin`) before the overlapped region with other lanes. The stuck vehicle detection area is generated based on the vehicle path, so the stuck vehicle stopline is not inserted if the upstream module generated avoidance path +If there is any object on the path in inside the intersection and at the exit of the intersection (up to `stuck_vehicle_detect_dist`) lane and its velocity is less than a threshold (`stuck_vehicle.stuck_vehicle_vel_thr`), the object is regarded as a stuck vehicle. If stuck vehicles exist, this module inserts a stopline a certain distance (=`stop_line_margin`) before the overlapped region with other lanes. The stuck vehicle detection area is generated based on the vehicle path, so the stuck vehicle stopline is not inserted if the upstream module generated avoidance path ![stuck_vehicle_detection](./docs/stuck-vehicle.drawio.svg) @@ -107,7 +107,6 @@ To avoid a rapid braking, if deceleration and jerk more than a threshold (`behav | `common.stop_overshoot_margin` | double | [m] margin for the overshoot from stopline | | `common.path_interpolation_ds` | double | [m] path interpolation interval | | `stuck_vehicle.stuck_vehicle_detect_dist` | double | [m] length toward from the exit of intersection for stuck vehicle detection | -| `stuck_vehicle.stuck_vehicle_ignore_dist` | double | [m] length behind the exit of intersection for stuck vehicle detection | | `stuck_vehicle.stuck_vehicle_vel_thr` | double | [m/s] velocity threshold for stuck vehicle detection | | `collision_detection.state_transit_margin_time` | double | [m] time margin to change state | | `collision_detection.min_predicted_path_confidence` | double | [-] minimum confidence value of predicted path to use for collision detection | diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index fb822cdc5cf4d..c30c5d70f8c60 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -15,7 +15,6 @@ stuck_vehicle: use_stuck_stopline: true # stopline generated before the first conflicting area stuck_vehicle_detect_dist: 3.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length. - stuck_vehicle_ignore_dist: 10.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-) stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h # enable_front_car_decel_prediction: false # By default this feature is disabled # assumed_front_car_decel: 1.0 # [m/ss] the expected deceleration of front car when front car as well as ego are turning diff --git a/planning/behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg b/planning/behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg index d2a8286122e54..8802dc786b617 100644 --- a/planning/behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg +++ b/planning/behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg @@ -5,28 +5,28 @@ xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" - width="2831px" - height="1073px" - viewBox="-0.5 -0.5 2831 1073" - content="<mxfile host="Electron" modified="2023-06-07T05:26:53.508Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="B7vxgoBz0X2z8hjEAQoV" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" + width="2842px" + height="1201px" + viewBox="-0.5 -0.5 2842 1201" + content="<mxfile host="Electron" modified="2023-08-28T08:00:31.600Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="CbgId97oWJwBUVWni98L" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" > - - - - - - - - - - - - - - + + + + + + + + + + + + + - + - + - + - - - - - + + + + + - + - - - + + + - - - + + + - - - - + + + + + + + - - - - + + + + - - - + + + - - + + - + - - - - - + + + + + - +
@@ -259,22 +262,22 @@
- stuck vehicle detection area is generated on the path around the exit o... + stuck vehicle detection area is generated on the path around the exit o... - - - - - - - - - - - - + + + + + + + + + + + - + - + - + - - - - - + + + + + - + - +
@@ -366,14 +369,14 @@
- %3CmxGraphModel%3E%3Croot%3E%3... + %3CmxGraphModel%3E%3Croot%3E%3... - - - + + + - - - + + + - - - - + + + + - - - - + + + + - - - + + + - - + + - + - - + + - - + + @@ -510,7 +513,7 @@
@@ -521,18 +524,18 @@
- stuck vehicle detection area is generated from the path, so in this... + stuck vehicle detection area is generated from the path, so in this... - - - - - + + + + + -
+
@@ -542,76 +545,158 @@
- stop_line_margin + stop_line_margin - + - - + +
`
 - ` + ` + + + + +
+
+
+ + + stuck_vehicle_detect_dist + + +
+
+
+ + stuck_vehicle_detect_dist + + + + + +
+
+
+ + + %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22%26lt%3Bfont%20style%3D%26quot%3Bfont-size%3A%2035px%3B%26quot%3B%26gt%3B%26lt%3Bb%26gt%3Bw%2F%20traffic%20light%2C%20right%26lt%3B%2Fb%26gt%3B%26lt%3Bbr%26gt%3B%26lt%3Bbr%26gt%3B%26lt%3Bb%26gt%3B(Left-hand%20traffic)%26lt%3B%2Fb%26gt%3B%26lt%3B%2Ffont%26gt%3B%26lt%3Bspan%20style%3D%26quot%3Bfont-size%3A%2030px%3B%26quot%3B%26gt%3B%26lt%3Bb%26gt%3B%26lt%3Bbr%26gt%3B%26lt%3B%2Fb%26gt%3B%26lt%3B%2Fspan%26gt%3B%22%20style%3D%22text%3Bhtml%3D1%3Balign%3Dleft%3BverticalAlign%3Dmiddle%3Bresizable%3D0%3Bpoints%3D%5B%5D%3Bautosize%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3B%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22-17325%22%20y%3D%22-7209%22%20width%3D%22350%22%20height%3D%22140%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E + + straight +
+
+ (Right-hand traffic) + + + +
+ + +
+
+
+ + %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%... + + + + + +
+
+
+ + + %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22%26lt%3Bfont%20style%3D%26quot%3Bfont-size%3A%2035px%3B%26quot%3B%26gt%3B%26lt%3Bb%26gt%3Bw%2F%20traffic%20light%2C%20right%26lt%3B%2Fb%26gt%3B%26lt%3Bbr%26gt%3B%26lt%3Bbr%26gt%3B%26lt%3Bb%26gt%3B(Left-hand%20traffic)%26lt%3B%2Fb%26gt%3B%26lt%3B%2Ffont%26gt%3B%26lt%3Bspan%20style%3D%26quot%3Bfont-size%3A%2030px%3B%26quot%3B%26gt%3B%26lt%3Bb%26gt%3B%26lt%3Bbr%26gt%3B%26lt%3B%2Fb%26gt%3B%26lt%3B%2Fspan%26gt%3B%22%20style%3D%22text%3Bhtml%3D1%3Balign%3Dleft%3BverticalAlign%3Dmiddle%3Bresizable%3D0%3Bpoints%3D%5B%5D%3Bautosize%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3B%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22-17325%22%20y%3D%22-7209%22%20width%3D%22350%22%20height%3D%22140%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E + + left turn +
+
+ (Right-hand traffic) + + + +
+ + +
+
+
+ + %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%... + + diff --git a/planning/behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_intersection_module/package.xml index 1d6067d4be322..879b26209a05e 100644 --- a/planning/behavior_velocity_intersection_module/package.xml +++ b/planning/behavior_velocity_intersection_module/package.xml @@ -19,10 +19,12 @@ autoware_auto_perception_msgs autoware_auto_planning_msgs + autoware_perception_msgs behavior_velocity_planner_common geometry_msgs interpolation lanelet2_extension + libopencv-dev magic_enum motion_utils nav_msgs diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index ae8eee39d6556..dfaa10ec9d86f 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -17,7 +17,8 @@ #include #include -#include +#include +#include #include #include diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 8a4a3e046022d..bf0970f3640d1 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -43,7 +43,6 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) { const std::string ns(getModuleName()); auto & ip = intersection_param_; - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); ip.common.attention_area_margin = getOrDeclareParameter(node, ns + ".common.attention_area_margin"); ip.common.attention_area_length = @@ -68,9 +67,6 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".stuck_vehicle.use_stuck_stopline"); ip.stuck_vehicle.stuck_vehicle_detect_dist = getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_detect_dist"); - ip.stuck_vehicle.stuck_vehicle_ignore_dist = - getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_ignore_dist") + - vehicle_info.max_longitudinal_offset_m; ip.stuck_vehicle.stuck_vehicle_vel_thr = getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_vel_thr"); /* diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index c5fcaca40c24f..5f9d838b7c220 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include @@ -124,7 +125,7 @@ template void prepareRTCByDecisionResult( const T & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, - double * occlusion_distance, bool * occlusion_first_stop_required) + double * occlusion_distance) { static_assert("Unsupported type passed to prepareRTCByDecisionResult"); return; @@ -135,8 +136,7 @@ void prepareRTCByDecisionResult( [[maybe_unused]] const IntersectionModule::Indecisive & result, [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path, [[maybe_unused]] bool * default_safety, [[maybe_unused]] double * default_distance, - [[maybe_unused]] bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance, - [[maybe_unused]] bool * occlusion_first_stop_required) + [[maybe_unused]] bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance) { return; } @@ -145,17 +145,16 @@ template <> void prepareRTCByDecisionResult( const IntersectionModule::StuckStop & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance, - [[maybe_unused]] bool * occlusion_first_stop_required) + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "StuckStop"); - const auto closest_idx = result.stop_lines.closest_idx; - const auto stop_line_idx = result.stop_line_idx; + const auto closest_idx = result.closest_idx; + const auto stop_line_idx = result.stuck_stop_line_idx; *default_safety = false; *default_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, stop_line_idx); *occlusion_safety = true; - if (!result.is_detection_area_empty) { - const auto occlusion_stop_line_idx = result.stop_lines.occlusion_peeking_stop_line; + if (result.occlusion_stop_line_idx) { + const auto occlusion_stop_line_idx = result.occlusion_stop_line_idx.value(); *occlusion_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stop_line_idx); } @@ -166,15 +165,15 @@ template <> void prepareRTCByDecisionResult( const IntersectionModule::NonOccludedCollisionStop & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance, - [[maybe_unused]] bool * occlusion_first_stop_required) + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "NonOccludedCollisionStop"); - const auto closest_idx = result.stop_lines.closest_idx; - const auto stop_line_idx = result.stop_line_idx; + const auto closest_idx = result.closest_idx; + const auto collision_stop_line_idx = result.collision_stop_line_idx; *default_safety = false; - *default_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, stop_line_idx); - const auto occlusion_stop_line = result.stop_lines.occlusion_peeking_stop_line; + *default_distance = + motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stop_line_idx); + const auto occlusion_stop_line = result.occlusion_stop_line_idx; *occlusion_safety = true; *occlusion_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stop_line); @@ -185,11 +184,10 @@ template <> void prepareRTCByDecisionResult( const IntersectionModule::FirstWaitBeforeOcclusion & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance, - bool * occlusion_first_stop_required) + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "FirstWaitBeforeOcclusion"); - const auto closest_idx = result.stop_lines.closest_idx; + const auto closest_idx = result.closest_idx; const auto first_stop_line_idx = result.first_stop_line_idx; const auto occlusion_stop_line_idx = result.occlusion_stop_line_idx; *default_safety = false; @@ -198,7 +196,6 @@ void prepareRTCByDecisionResult( *occlusion_safety = result.is_actually_occlusion_cleared; *occlusion_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stop_line_idx); - *occlusion_first_stop_required = true; return; } @@ -206,18 +203,18 @@ template <> void prepareRTCByDecisionResult( const IntersectionModule::PeekingTowardOcclusion & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance, - [[maybe_unused]] bool * occlusion_first_stop_required) + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "PeekingTowardOcclusion"); - const auto closest_idx = result.stop_lines.closest_idx; - const auto stop_line_idx = result.stop_line_idx; - *occlusion_safety = result.is_actually_occlusion_cleared; - *occlusion_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, stop_line_idx); - const auto default_stop_line_idx = result.stop_lines.default_stop_line; + const auto closest_idx = result.closest_idx; + const auto collision_stop_line_idx = result.collision_stop_line_idx; + const auto occlusion_stop_line_idx = result.occlusion_stop_line_idx; *default_safety = true; *default_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, default_stop_line_idx); + motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stop_line_idx); + *occlusion_safety = result.is_actually_occlusion_cleared; + *occlusion_distance = + motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stop_line_idx); return; } @@ -225,15 +222,15 @@ template <> void prepareRTCByDecisionResult( const IntersectionModule::OccludedCollisionStop & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance, - [[maybe_unused]] bool * occlusion_first_stop_required) + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "OccludedCollisionStop"); - const auto closest_idx = result.stop_lines.closest_idx; - const auto stop_line_idx = result.stop_line_idx; + const auto closest_idx = result.closest_idx; + const auto collision_stop_line_idx = result.collision_stop_line_idx; const auto occlusion_stop_line_idx = result.occlusion_stop_line_idx; *default_safety = false; - *default_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, stop_line_idx); + *default_distance = + motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stop_line_idx); *occlusion_safety = result.is_actually_occlusion_cleared; *occlusion_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stop_line_idx); @@ -244,16 +241,15 @@ template <> void prepareRTCByDecisionResult( const IntersectionModule::Safe & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance, - [[maybe_unused]] bool * occlusion_first_stop_required) + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "Safe"); - const auto closest_idx = result.stop_lines.closest_idx; - const auto default_stop_line_idx = result.stop_lines.default_stop_line; - const auto occlusion_stop_line_idx = result.stop_lines.occlusion_peeking_stop_line; + const auto closest_idx = result.closest_idx; + const auto collision_stop_line_idx = result.collision_stop_line_idx; + const auto occlusion_stop_line_idx = result.occlusion_stop_line_idx; *default_safety = true; *default_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, default_stop_line_idx); + motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stop_line_idx); *occlusion_safety = true; *occlusion_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stop_line_idx); @@ -264,16 +260,15 @@ template <> void prepareRTCByDecisionResult( const IntersectionModule::TrafficLightArrowSolidOn & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance, - [[maybe_unused]] bool * occlusion_first_stop_required) + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "TrafficLightArrowSolidOn"); - const auto closest_idx = result.stop_lines.closest_idx; - const auto default_stop_line_idx = result.stop_lines.default_stop_line; - const auto occlusion_stop_line_idx = result.stop_lines.occlusion_peeking_stop_line; + const auto closest_idx = result.closest_idx; + const auto collision_stop_line_idx = result.collision_stop_line_idx; + const auto occlusion_stop_line_idx = result.occlusion_stop_line_idx; *default_safety = !result.collision_detected; *default_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, default_stop_line_idx); + motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stop_line_idx); *occlusion_safety = true; *occlusion_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stop_line_idx); @@ -290,11 +285,13 @@ void IntersectionModule::prepareRTCStatus( VisitorSwitch{[&](const auto & decision) { prepareRTCByDecisionResult( decision, path, &default_safety, &default_distance, &occlusion_safety_, - &occlusion_stop_distance_, &occlusion_first_stop_required_); + &occlusion_stop_distance_); }}, decision_result); setSafe(default_safety); setDistance(default_distance); + occlusion_first_stop_required_ = + std::holds_alternative(decision_result); } template @@ -335,9 +332,10 @@ void reactRTCApprovalByDecisionResult( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), "StuckStop, approval = (default: %d, occlusion: %d)", rtc_default_approved, rtc_occlusion_approved); + const auto closest_idx = decision_result.closest_idx; if (!rtc_default_approved) { // use default_rtc uuid for stuck vehicle detection - const auto stop_line_idx = decision_result.stop_line_idx; + const auto stop_line_idx = decision_result.stuck_stop_line_idx; planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -347,14 +345,14 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data->conflicting_targets); planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } if ( - !rtc_occlusion_approved && !decision_result.is_detection_area_empty && + !rtc_occlusion_approved && decision_result.occlusion_stop_line_idx && planner_param.occlusion.enable) { - const auto occlusion_stop_line_idx = decision_result.stop_lines.occlusion_peeking_stop_line; + const auto occlusion_stop_line_idx = decision_result.occlusion_stop_line_idx.value(); planning_utils::setVelocityFromIndex(occlusion_stop_line_idx, 0.0, path); debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(occlusion_stop_line_idx, baselink2front, *path); @@ -363,7 +361,7 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(occlusion_stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(closest_idx).point.pose, path->points.at(occlusion_stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } @@ -383,7 +381,7 @@ void reactRTCApprovalByDecisionResult( "NonOccludedCollisionStop, approval = (default: %d, occlusion: %d)", rtc_default_approved, rtc_occlusion_approved); if (!rtc_default_approved) { - const auto stop_line_idx = decision_result.stop_line_idx; + const auto stop_line_idx = decision_result.collision_stop_line_idx; planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -392,12 +390,12 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } if (!rtc_occlusion_approved && planner_param.occlusion.enable) { - const auto stop_line_idx = decision_result.stop_lines.occlusion_peeking_stop_line; + const auto stop_line_idx = decision_result.occlusion_stop_line_idx; planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -406,7 +404,7 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } @@ -435,14 +433,14 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } if (!rtc_occlusion_approved && planner_param.occlusion.enable) { if (planner_param.occlusion.enable_creeping) { const size_t occlusion_peeking_stop_line = decision_result.occlusion_stop_line_idx; - const size_t closest_idx = decision_result.stop_lines.closest_idx; + const size_t closest_idx = decision_result.closest_idx; for (size_t i = closest_idx; i < occlusion_peeking_stop_line; i++) { planning_utils::setVelocityFromIndex( i, planner_param.occlusion.occlusion_creep_velocity, path); @@ -457,7 +455,7 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } @@ -479,10 +477,9 @@ void reactRTCApprovalByDecisionResult( // NOTE: creep_velocity should be inserted first at closest_idx if !rtc_default_approved if (!rtc_occlusion_approved && planner_param.occlusion.enable) { - const size_t occlusion_peeking_stop_line = - decision_result.stop_lines.occlusion_peeking_stop_line; + const size_t occlusion_peeking_stop_line = decision_result.occlusion_stop_line_idx; if (planner_param.occlusion.enable_creeping) { - const size_t closest_idx = decision_result.stop_lines.closest_idx; + const size_t closest_idx = decision_result.closest_idx; for (size_t i = closest_idx; i < occlusion_peeking_stop_line; i++) { planning_utils::setVelocityFromIndex( i, planner_param.occlusion.occlusion_creep_velocity, path); @@ -496,12 +493,12 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(occlusion_peeking_stop_line).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(occlusion_peeking_stop_line).point.pose, VelocityFactor::INTERSECTION); } } - if (!rtc_default_approved && planner_param.occlusion.enable) { - const auto stop_line_idx = decision_result.stop_lines.default_stop_line; + if (!rtc_default_approved) { + const auto stop_line_idx = decision_result.collision_stop_line_idx; planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -510,7 +507,7 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } @@ -530,7 +527,7 @@ void reactRTCApprovalByDecisionResult( "OccludedCollisionStop, approval = (default: %d, occlusion: %d)", rtc_default_approved, rtc_occlusion_approved); if (!rtc_default_approved) { - const auto stop_line_idx = decision_result.stop_line_idx; + const auto stop_line_idx = decision_result.collision_stop_line_idx; planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -539,7 +536,7 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } @@ -553,7 +550,7 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } @@ -572,7 +569,7 @@ void reactRTCApprovalByDecisionResult( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), "Safe, approval = (default: %d, occlusion: %d)", rtc_default_approved, rtc_occlusion_approved); if (!rtc_default_approved) { - const auto stop_line_idx = decision_result.stop_lines.default_stop_line; + const auto stop_line_idx = decision_result.collision_stop_line_idx; planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -581,12 +578,12 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } if (!rtc_occlusion_approved && planner_param.occlusion.enable) { - const auto stop_line_idx = decision_result.stop_lines.occlusion_peeking_stop_line; + const auto stop_line_idx = decision_result.occlusion_stop_line_idx; planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -595,7 +592,7 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } @@ -615,7 +612,7 @@ void reactRTCApprovalByDecisionResult( "TrafficLightArrowSolidOn, approval = (default: %d, occlusion: %d)", rtc_default_approved, rtc_occlusion_approved); if (!rtc_default_approved) { - const auto stop_line_idx = decision_result.stop_lines.default_stop_line; + const auto stop_line_idx = decision_result.collision_stop_line_idx; planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -624,7 +621,21 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, + path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); + } + } + if (!rtc_occlusion_approved && planner_param.occlusion.enable) { + const auto stop_line_idx = decision_result.occlusion_stop_line_idx; + planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); + debug_data->occlusion_stop_wall_pose = + planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); + { + tier4_planning_msgs::msg::StopFactor stop_factor; + stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; + planning_utils::appendStopReason(stop_factor, stop_reason); + velocity_factor->set( + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } @@ -687,6 +698,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * // calculate the const auto decision_result = modifyPathVelocityDetail(path, stop_reason); + prev_decision_result_ = decision_result; const std::string decision_type = "intersection" + std::to_string(module_id_) + " : " + formatDecisionResult(decision_result); @@ -723,13 +735,11 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( lane_id_, associative_ids_, *path, planner_param_.common.path_interpolation_ds, logger_); if (!interpolated_path_info_opt) { RCLCPP_DEBUG(logger_, "splineInterpolate failed"); - is_peeking_ = false; return IntersectionModule::Indecisive{}; } const auto & interpolated_path_info = interpolated_path_info_opt.value(); if (!interpolated_path_info.lane_id_interval) { RCLCPP_WARN(logger_, "Path has no interval on intersection lane %ld", lane_id_); - is_peeking_ = false; return IntersectionModule::Indecisive{}; } @@ -748,47 +758,44 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( intersection_lanelets_.value().update(tl_arrow_solid_on, interpolated_path_info); const auto & conflicting_lanelets = intersection_lanelets_.value().conflicting(); - const auto & first_conflicting_area = intersection_lanelets_.value().first_conflicting_area(); - if (conflicting_lanelets.empty() || !first_conflicting_area) { - is_peeking_ = false; + const auto & first_conflicting_area_opt = intersection_lanelets_.value().first_conflicting_area(); + if (conflicting_lanelets.empty() || !first_conflicting_area_opt) { + RCLCPP_DEBUG(logger_, "conflicting area is empty"); return IntersectionModule::Indecisive{}; } + const auto first_conflicting_area = first_conflicting_area_opt.value(); - const auto & first_attention_area = intersection_lanelets_.value().first_attention_area(); + const auto & first_attention_area_opt = intersection_lanelets_.value().first_attention_area(); const auto & dummy_first_attention_area = - first_attention_area ? first_attention_area.value() : first_conflicting_area.value(); + first_attention_area_opt ? first_attention_area_opt.value() : first_conflicting_area; const auto intersection_stop_lines_opt = util::generateIntersectionStopLines( - first_conflicting_area.value(), dummy_first_attention_area, planner_data_, - interpolated_path_info, planner_param_.stuck_vehicle.use_stuck_stopline, - planner_param_.common.stop_line_margin, planner_param_.occlusion.peeking_offset, path); + first_conflicting_area, dummy_first_attention_area, planner_data_, interpolated_path_info, + planner_param_.stuck_vehicle.use_stuck_stopline, planner_param_.common.stop_line_margin, + planner_param_.occlusion.peeking_offset, path); if (!intersection_stop_lines_opt) { RCLCPP_DEBUG(logger_, "failed to generate intersection_stop_lines"); - is_peeking_ = false; return IntersectionModule::Indecisive{}; } const auto & intersection_stop_lines = intersection_stop_lines_opt.value(); const auto - [closest_idx, stuck_stop_line_idx, default_stop_line_idx, occlusion_peeking_stop_line_idx, - pass_judge_line_idx] = intersection_stop_lines; + [closest_idx, stuck_stop_line_idx_opt, default_stop_line_idx_opt, + occlusion_peeking_stop_line_idx_opt, pass_judge_line_idx] = intersection_stop_lines; + const auto & conflicting_area = intersection_lanelets_.value().conflicting_area(); const auto path_lanelets_opt = util::generatePathLanelets( - lanelets_on_path, *path, associative_ids_, closest_idx, - planner_data_->vehicle_info_.vehicle_width_m); + lanelets_on_path, interpolated_path_info, associative_ids_, first_conflicting_area, + conflicting_area, first_attention_area_opt, intersection_lanelets_.value().attention_area(), + closest_idx, planner_data_->vehicle_info_.vehicle_width_m); if (!path_lanelets_opt.has_value()) { RCLCPP_DEBUG(logger_, "failed to generate PathLanelets"); return IntersectionModule::Indecisive{}; } const auto path_lanelets = path_lanelets_opt.value(); - const auto ego_lane_with_next_lane = - path_lanelets.next.has_value() - ? std::vector< - lanelet::ConstLanelet>{path_lanelets.ego_or_entry2exit, path_lanelets.next.value()} - : std::vector{path_lanelets.ego_or_entry2exit}; - const bool stuck_detected = - checkStuckVehicle(planner_data_, ego_lane_with_next_lane, *path, intersection_stop_lines); + const bool stuck_detected = checkStuckVehicle(planner_data_, path_lanelets); - if (stuck_detected) { + if (stuck_detected && stuck_stop_line_idx_opt) { + auto stuck_stop_line_idx = stuck_stop_line_idx_opt.value(); const double dist_stopline = motion_utils::calcSignedArcLength( path->points, path->points.at(closest_idx).point.pose.position, path->points.at(stuck_stop_line_idx).point.pose.position); @@ -802,24 +809,30 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const bool timeout = (is_private_area_ && stuck_private_area_timeout_.getState() == StateMachine::State::GO); if (!timeout) { - is_peeking_ = false; + if ( + default_stop_line_idx_opt && + motion_utils::calcSignedArcLength(path->points, stuck_stop_line_idx, closest_idx) > + planner_param_.common.stop_overshoot_margin) { + stuck_stop_line_idx = default_stop_line_idx_opt.value(); + } return IntersectionModule::StuckStop{ - stuck_stop_line_idx, !first_attention_area.has_value(), intersection_stop_lines}; + closest_idx, stuck_stop_line_idx, occlusion_peeking_stop_line_idx_opt}; } } - if (!first_attention_area) { + if (!first_attention_area_opt) { RCLCPP_DEBUG(logger_, "attention area is empty"); - is_peeking_ = false; return IntersectionModule::Indecisive{}; } + const auto first_attention_area = first_attention_area_opt.value(); - if (default_stop_line_idx == 0) { - RCLCPP_DEBUG(logger_, "stop line index is 0"); - is_peeking_ = false; + if (!default_stop_line_idx_opt) { + RCLCPP_DEBUG(logger_, "default stop line is null"); return IntersectionModule::Indecisive{}; } + const auto default_stop_line_idx = default_stop_line_idx_opt.value(); + // TODO(Mamoru Sobue): this part needs more formal handling const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; debug_data_.pass_judge_wall_pose = planning_utils::getAheadPose(pass_judge_line_idx, baselink2front, *path); @@ -832,24 +845,30 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( planner_data_->current_velocity->twist.linear.y); const bool keep_detection = (vel_norm < planner_param_.collision_detection.keep_detection_vel_thr); + const bool was_safe = std::holds_alternative(prev_decision_result_); // if ego is over the pass judge line and not stopped - if (is_peeking_) { - // do nothing - RCLCPP_DEBUG(logger_, "peeking now"); - } else if (is_over_default_stop_line && !is_over_pass_judge_line && keep_detection) { + if (is_over_default_stop_line && !is_over_pass_judge_line && keep_detection) { RCLCPP_DEBUG( logger_, "is_over_default_stop_line && !is_over_pass_judge_line && keep_detection"); // do nothing } else if ( - (is_over_default_stop_line && is_over_pass_judge_line && is_go_out_) || is_permanent_go_) { + (was_safe && is_over_default_stop_line && is_over_pass_judge_line && is_go_out_) || + is_permanent_go_) { // is_go_out_: previous RTC approval // activated_: current RTC approval is_permanent_go_ = true; RCLCPP_DEBUG(logger_, "over the pass judge line. no plan needed."); - is_peeking_ = false; return IntersectionModule::Indecisive{}; } + if (!occlusion_peeking_stop_line_idx_opt) { + RCLCPP_DEBUG(logger_, "occlusion stop line is null"); + return IntersectionModule::Indecisive{}; + } + const auto collision_stop_line_idx = + is_over_default_stop_line ? closest_idx : default_stop_line_idx; + const auto occlusion_stop_line_idx = occlusion_peeking_stop_line_idx_opt.value(); + const auto & attention_lanelets = intersection_lanelets_.value().attention(); const auto & adjacent_lanelets = intersection_lanelets_.value().adjacent(); const auto & occlusion_attention_lanelets = intersection_lanelets_.value().occlusion_attention(); @@ -883,8 +902,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( collision_state_machine_.getState() == StateMachine::State::STOP; if (tl_arrow_solid_on) { - is_peeking_ = false; - return TrafficLightArrowSolidOn{has_collision, intersection_stop_lines}; + return TrafficLightArrowSolidOn{ + has_collision_with_margin, closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; } // check occlusion on detection lane @@ -893,6 +912,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( occlusion_attention_lanelets, routing_graph_ptr, planner_data_->occupancy_grid->info.resolution / std::sqrt(2.0)); } + const auto & occlusion_attention_divisions = occlusion_attention_divisions_.value(); const double occlusion_dist_thr = std::fabs( std::pow(planner_param_.occlusion.max_vehicle_velocity_for_rss, 2) / @@ -910,15 +930,17 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !tl_arrow_solid_on) ? isOcclusionCleared( *planner_data_->occupancy_grid, occlusion_attention_area, adjacent_lanelets, - first_attention_area.value(), interpolated_path_info, - occlusion_attention_divisions_.value(), parked_attention_objects, occlusion_dist_thr) + first_attention_area, interpolated_path_info, occlusion_attention_divisions, + parked_attention_objects, occlusion_dist_thr) : true; occlusion_stop_state_machine_.setStateWithMarginTime( is_occlusion_cleared ? StateMachine::State::GO : StateMachine::STOP, logger_.get_child("occlusion_stop"), *clock_); + const bool is_occlusion_cleared_with_margin = + (occlusion_stop_state_machine_.getState() == StateMachine::State::GO); // check safety - const bool ext_occlusion_requested = (is_occlusion_cleared && !occlusion_activated_); + const bool ext_occlusion_requested = (is_occlusion_cleared_with_margin && !occlusion_activated_); if ( occlusion_stop_state_machine_.getState() == StateMachine::State::STOP || ext_occlusion_requested) { @@ -934,73 +956,45 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( before_creep_state_machine_.setState(StateMachine::State::GO); } if (before_creep_state_machine_.getState() == StateMachine::State::GO) { - if (has_collision) { - is_peeking_ = true; + if (has_collision_with_margin) { return IntersectionModule::OccludedCollisionStop{ - default_stop_line_idx, occlusion_peeking_stop_line_idx, is_occlusion_cleared, - intersection_stop_lines}; + is_occlusion_cleared_with_margin, closest_idx, collision_stop_line_idx, + occlusion_stop_line_idx}; } else { - is_peeking_ = true; return IntersectionModule::PeekingTowardOcclusion{ - occlusion_peeking_stop_line_idx, is_occlusion_cleared, intersection_stop_lines}; + is_occlusion_cleared_with_margin, closest_idx, collision_stop_line_idx, + occlusion_stop_line_idx}; } } else { if (is_stopped && approached_stop_line) { // start waiting at the first stop line before_creep_state_machine_.setState(StateMachine::State::GO); } - is_peeking_ = true; return IntersectionModule::FirstWaitBeforeOcclusion{ - default_stop_line_idx, occlusion_peeking_stop_line_idx, is_occlusion_cleared, - intersection_stop_lines}; + is_occlusion_cleared_with_margin, closest_idx, default_stop_line_idx, + occlusion_stop_line_idx}; } } else if (has_collision_with_margin) { - const bool is_over_default_stopLine = - util::isOverTargetIndex(*path, closest_idx, current_pose, default_stop_line_idx); - const auto stop_line_idx = is_over_default_stopLine ? closest_idx : default_stop_line_idx; - is_peeking_ = false; - return IntersectionModule::NonOccludedCollisionStop{stop_line_idx, intersection_stop_lines}; + return IntersectionModule::NonOccludedCollisionStop{ + closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; } - is_peeking_ = false; - return IntersectionModule::Safe{intersection_stop_lines}; + return IntersectionModule::Safe{closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; } bool IntersectionModule::checkStuckVehicle( - const std::shared_ptr & planner_data, - const lanelet::ConstLanelets & ego_lane_with_next_lane, - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, - const util::IntersectionStopLines & intersection_stop_lines) + const std::shared_ptr & planner_data, const util::PathLanelets & path_lanelets) { const auto & objects_ptr = planner_data->predicted_objects; - const geometry_msgs::msg::Pose & current_pose = planner_data->current_odometry->pose; - const auto closest_idx = intersection_stop_lines.closest_idx; - const auto stuck_line_idx = intersection_stop_lines.stuck_stop_line; // considering lane change in the intersection, these lanelets are generated from the path - const auto ego_lane = ego_lane_with_next_lane.front(); - debug_data_.ego_lane = ego_lane.polygon3d(); const auto stuck_vehicle_detect_area = util::generateStuckVehicleDetectAreaPolygon( - input_path, ego_lane_with_next_lane, closest_idx, - planner_param_.stuck_vehicle.stuck_vehicle_detect_dist, - planner_param_.stuck_vehicle.stuck_vehicle_ignore_dist, - planner_data->vehicle_info_.vehicle_length_m); + path_lanelets, planner_param_.stuck_vehicle.stuck_vehicle_detect_dist); debug_data_.stuck_vehicle_detect_area = toGeomPoly(stuck_vehicle_detect_area); - const double dist_stuck_stopline = motion_utils::calcSignedArcLength( - input_path.points, input_path.points.at(stuck_line_idx).point.pose.position, - input_path.points.at(closest_idx).point.pose.position); - const bool is_over_stuck_stopline = - util::isOverTargetIndex(input_path, closest_idx, current_pose, stuck_line_idx) && - (dist_stuck_stopline > planner_param_.common.stop_overshoot_margin); - - bool is_stuck = false; - if (!is_over_stuck_stopline) { - is_stuck = util::checkStuckVehicleInIntersection( - objects_ptr, stuck_vehicle_detect_area, planner_param_.stuck_vehicle.stuck_vehicle_vel_thr, - &debug_data_); - } - return is_stuck; + return util::checkStuckVehicleInIntersection( + objects_ptr, stuck_vehicle_detect_area, planner_param_.stuck_vehicle.stuck_vehicle_vel_thr, + &debug_data_); } autoware_auto_perception_msgs::msg::PredictedObjects IntersectionModule::filterTargetObjects( @@ -1079,6 +1073,7 @@ bool IntersectionModule::checkCollision( const auto closest_arc_coords = getArcCoordinates( concat_lanelets, tier4_autoware_utils::getPose(path.points.at(closest_idx).point)); const auto & ego_lane = path_lanelets.ego_or_entry2exit; + debug_data_.ego_lane = ego_lane.polygon3d(); const auto ego_poly = ego_lane.polygon2d().basicPolygon(); // check collision between predicted_path and ego_area @@ -1530,54 +1525,54 @@ bool IntersectionModule::isOcclusionCleared( } /* -bool IntersectionModule::checkFrontVehicleDeceleration( + bool IntersectionModule::checkFrontVehicleDeceleration( lanelet::ConstLanelets & ego_lane_with_next_lane, lanelet::ConstLanelet & closest_lanelet, const Polygon2d & stuck_vehicle_detect_area, const autoware_auto_perception_msgs::msg::PredictedObject & object, const double assumed_front_car_decel) -{ + { const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; // consider vehicle in ego-lane && in front of ego const auto lon_vel = object.kinematics.initial_twist_with_covariance.twist.linear.x; const double object_decel = - planner_param_.stuck_vehicle.assumed_front_car_decel; // NOTE: this is positive + planner_param_.stuck_vehicle.assumed_front_car_decel; // NOTE: this is positive const double stopping_distance = lon_vel * lon_vel / (2 * object_decel); std::vector center_points; for (auto && p : ego_lane_with_next_lane[0].centerline()) - center_points.push_back(std::move(lanelet::utils::conversion::toGeomMsgPt(p))); + center_points.push_back(std::move(lanelet::utils::conversion::toGeomMsgPt(p))); for (auto && p : ego_lane_with_next_lane[1].centerline()) - center_points.push_back(std::move(lanelet::utils::conversion::toGeomMsgPt(p))); + center_points.push_back(std::move(lanelet::utils::conversion::toGeomMsgPt(p))); const double lat_offset = - std::fabs(motion_utils::calcLateralOffset(center_points, object_pose.position)); + std::fabs(motion_utils::calcLateralOffset(center_points, object_pose.position)); // get the nearest centerpoint to object std::vector dist_obj_center_points; for (const auto & p : center_points) - dist_obj_center_points.push_back(tier4_autoware_utils::calcDistance2d(object_pose.position, -p)); const int obj_closest_centerpoint_idx = std::distance( dist_obj_center_points.begin(), - std::min_element(dist_obj_center_points.begin(), dist_obj_center_points.end())); + dist_obj_center_points.push_back(tier4_autoware_utils::calcDistance2d(object_pose.position, + p)); const int obj_closest_centerpoint_idx = std::distance( dist_obj_center_points.begin(), + std::min_element(dist_obj_center_points.begin(), dist_obj_center_points.end())); // find two center_points whose distances from `closest_centerpoint` cross stopping_distance double acc_dist_prev = 0.0, acc_dist = 0.0; auto p1 = center_points[obj_closest_centerpoint_idx]; auto p2 = center_points[obj_closest_centerpoint_idx]; for (unsigned i = obj_closest_centerpoint_idx; i < center_points.size() - 1; ++i) { - p1 = center_points[i]; - p2 = center_points[i + 1]; - acc_dist_prev = acc_dist; - const auto arc_position_p1 = - lanelet::utils::getArcCoordinates(ego_lane_with_next_lane, toPose(p1)); - const auto arc_position_p2 = - lanelet::utils::getArcCoordinates(ego_lane_with_next_lane, toPose(p2)); - const double delta = arc_position_p2.length - arc_position_p1.length; - acc_dist += delta; - if (acc_dist > stopping_distance) { - break; - } + p1 = center_points[i]; + p2 = center_points[i + 1]; + acc_dist_prev = acc_dist; + const auto arc_position_p1 = + lanelet::utils::getArcCoordinates(ego_lane_with_next_lane, toPose(p1)); + const auto arc_position_p2 = + lanelet::utils::getArcCoordinates(ego_lane_with_next_lane, toPose(p2)); + const double delta = arc_position_p2.length - arc_position_p1.length; + acc_dist += delta; + if (acc_dist > stopping_distance) { + break; + } } // if stopping_distance >= center_points, stopping_point is center_points[end] const double ratio = (acc_dist <= stopping_distance) - ? 0.0 - : (acc_dist - stopping_distance) / (stopping_distance - acc_dist_prev); + ? 0.0 + : (acc_dist - stopping_distance) / (stopping_distance - acc_dist_prev); // linear interpolation geometry_msgs::msg::Point stopping_point; stopping_point.x = (p1.x * ratio + p2.x) / (1 + ratio); @@ -1591,18 +1586,18 @@ p)); const int obj_closest_centerpoint_idx = std::distance( dist_obj_center_poin autoware_auto_perception_msgs::msg::PredictedObject predicted_object = object; predicted_object.kinematics.initial_pose_with_covariance.pose.position = stopping_point; predicted_object.kinematics.initial_pose_with_covariance.pose.orientation = - tier4_autoware_utils::createQuaternionFromRPY(0, 0, lane_yaw); + tier4_autoware_utils::createQuaternionFromRPY(0, 0, lane_yaw); auto predicted_obj_footprint = tier4_autoware_utils::toPolygon2d(predicted_object); const bool is_in_stuck_area = !bg::disjoint(predicted_obj_footprint, stuck_vehicle_detect_area); debug_data_.predicted_obj_pose.position = stopping_point; debug_data_.predicted_obj_pose.orientation = - tier4_autoware_utils::createQuaternionFromRPY(0, 0, lane_yaw); + tier4_autoware_utils::createQuaternionFromRPY(0, 0, lane_yaw); if (is_in_stuck_area) { - return true; + return true; } return false; -} + } */ } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 6684ae5ae0b45..8063300b9b787 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -20,9 +20,9 @@ #include #include #include -#include +#include +#include #include -#include #include #include @@ -65,8 +65,6 @@ class IntersectionModule : public SceneModuleInterface { bool use_stuck_stopline; //! stopline generate before the intersection lanelet when is_stuck. double stuck_vehicle_detect_dist; //! distance from end point to finish stuck vehicle check - double stuck_vehicle_ignore_dist; //! distance from intersection start to start stuck vehicle - //! check double stuck_vehicle_vel_thr; //! Threshold of the speed to be recognized as stopped /* double @@ -113,62 +111,55 @@ class IntersectionModule : public SceneModuleInterface } occlusion; }; - /* - enum OcclusionState { - NONE, - BEFORE_FIRST_STOP_LINE, - WAIT_FIRST_STOP_LINE, - CREEP_SECOND_STOP_LINE, - COLLISION_DETECTED, - }; - */ - using Indecisive = std::monostate; struct StuckStop { - size_t stop_line_idx; - // NOTE: this is optional because stuck vehicle detection is possible - // even if the detection area is empty. - // Still this may be required for RTC's default stop line - bool is_detection_area_empty; - util::IntersectionStopLines stop_lines; + size_t closest_idx{0}; + size_t stuck_stop_line_idx{0}; + std::optional occlusion_stop_line_idx{std::nullopt}; }; struct NonOccludedCollisionStop { - size_t stop_line_idx; - util::IntersectionStopLines stop_lines; + size_t closest_idx{0}; + size_t collision_stop_line_idx{0}; + size_t occlusion_stop_line_idx{0}; }; struct FirstWaitBeforeOcclusion { - size_t first_stop_line_idx; - size_t occlusion_stop_line_idx; - bool is_actually_occlusion_cleared; - util::IntersectionStopLines stop_lines; + bool is_actually_occlusion_cleared{false}; + size_t closest_idx{0}; + size_t first_stop_line_idx{0}; + size_t occlusion_stop_line_idx{0}; }; struct PeekingTowardOcclusion { - size_t stop_line_idx; // NOTE: if intersection_occlusion is disapproved externally through RTC, // it indicates "is_forcefully_occluded" - bool is_actually_occlusion_cleared; - util::IntersectionStopLines stop_lines; + bool is_actually_occlusion_cleared{false}; + size_t closest_idx{0}; + size_t collision_stop_line_idx{0}; + size_t occlusion_stop_line_idx{0}; }; struct OccludedCollisionStop { - size_t stop_line_idx; - size_t occlusion_stop_line_idx; - bool is_actually_occlusion_cleared; - util::IntersectionStopLines stop_lines; + bool is_actually_occlusion_cleared{false}; + size_t closest_idx{0}; + size_t collision_stop_line_idx{0}; + size_t occlusion_stop_line_idx{0}; }; struct Safe { // NOTE: if RTC is disapproved status, default stop lines are still needed. - util::IntersectionStopLines stop_lines; + size_t closest_idx{0}; + size_t collision_stop_line_idx{0}; + size_t occlusion_stop_line_idx{0}; }; struct TrafficLightArrowSolidOn { - bool collision_detected; - util::IntersectionStopLines stop_lines; + bool collision_detected{false}; + size_t closest_idx{0}; + size_t collision_stop_line_idx{0}; + size_t occlusion_stop_line_idx{0}; }; using DecisionResult = std::variant< Indecisive, // internal process error, or over the pass judge line @@ -209,14 +200,15 @@ class IntersectionModule : public SceneModuleInterface const int64_t lane_id_; const std::set associative_ids_; std::string turn_direction_; + bool is_go_out_ = false; bool is_permanent_go_ = false; - bool is_peeking_ = false; + DecisionResult prev_decision_result_; + // Parameter PlannerParam planner_param_; + std::optional intersection_lanelets_; - // for an intersection lane, its associative lanes are those that share same parent lanelet and - // have same turn_direction // for occlusion detection const bool enable_occlusion_detection_; @@ -237,7 +229,6 @@ class IntersectionModule : public SceneModuleInterface double occlusion_stop_distance_; bool occlusion_activated_ = true; // for first stop in two-phase stop - const UUID occlusion_first_stop_uuid_; bool occlusion_first_stop_required_ = false; void initializeRTCStatus(); @@ -248,9 +239,7 @@ class IntersectionModule : public SceneModuleInterface bool checkStuckVehicle( const std::shared_ptr & planner_data, - const lanelet::ConstLanelets & ego_lane_with_next_lane, - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, - const util::IntersectionStopLines & intersection_stop_lines); + const util::PathLanelets & path_lanelets); autoware_auto_perception_msgs::msg::PredictedObjects filterTargetObjects( const lanelet::ConstLanelets & attention_area_lanelets, diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp index aa334f11be84d..e00b2a8b886d1 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp @@ -21,7 +21,6 @@ #include #include #include -#include #include #include diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index a987ce9af9dfe..ce2d7252ac93d 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include @@ -227,15 +228,20 @@ std::optional generateIntersectionStopLines( const auto first_inside_detection_ip = first_inside_detection_idx_ip_opt.value(); // (1) default stop line position on interpolated path - int stop_idx_ip_int = 0; + bool default_stop_line_valid = true; + int stop_idx_ip_int = -1; if (const auto map_stop_idx_ip = getStopLineIndexFromMap(interpolated_path_info, planner_data, 10.0); map_stop_idx_ip) { stop_idx_ip_int = static_cast(map_stop_idx_ip.value()) - base2front_idx_dist; - } else { + } + if (stop_idx_ip_int < 0) { stop_idx_ip_int = static_cast(first_inside_detection_ip) - stop_line_margin_idx_dist - base2front_idx_dist; } + if (stop_idx_ip_int < 0) { + default_stop_line_valid = false; + } const auto default_stop_line_ip = stop_idx_ip_int >= 0 ? static_cast(stop_idx_ip_int) : 0; // (2) ego front stop line position on interpolated path @@ -251,13 +257,25 @@ std::optional generateIntersectionStopLines( const auto local_footprint = planner_data->vehicle_info_.createFootprint(0.0, 0.0); const auto area_2d = lanelet::utils::to2D(first_detection_area).basicPolygon(); int occlusion_peeking_line_ip_int = static_cast(default_stop_line_ip); - for (size_t i = default_stop_line_ip; i <= std::get<1>(lane_interval_ip); ++i) { - const auto & base_pose = path_ip.points.at(i).point.pose; - const auto path_footprint = tier4_autoware_utils::transformVector( - local_footprint, tier4_autoware_utils::pose2transform(base_pose)); - if (bg::intersects(path_footprint, area_2d)) { - occlusion_peeking_line_ip_int = i; - break; + bool occlusion_peeking_line_valid = true; + { + // NOTE: if footprints[0] is already inside the detection area, invalid + const auto & base_pose0 = path_ip.points.at(default_stop_line_ip).point.pose; + const auto path_footprint0 = tier4_autoware_utils::transformVector( + local_footprint, tier4_autoware_utils::pose2transform(base_pose0)); + if (bg::intersects(path_footprint0, area_2d)) { + occlusion_peeking_line_valid = false; + } + } + if (occlusion_peeking_line_valid) { + for (size_t i = default_stop_line_ip + 1; i <= std::get<1>(lane_interval_ip); ++i) { + const auto & base_pose = path_ip.points.at(i).point.pose; + const auto path_footprint = tier4_autoware_utils::transformVector( + local_footprint, tier4_autoware_utils::pose2transform(base_pose)); + if (bg::intersects(path_footprint, area_2d)) { + occlusion_peeking_line_ip_int = i; + break; + } } } occlusion_peeking_line_ip_int += std::ceil(peeking_offset / ds); @@ -279,26 +297,43 @@ std::optional generateIntersectionStopLines( // (5) stuck vehicle stop line int stuck_stop_line_ip_int = 0; + bool stuck_stop_line_valid = true; if (use_stuck_stopline) { + // NOTE: when ego vehicle is approaching detection area and already passed + // first_conflicting_area, this could be null. const auto stuck_stop_line_idx_ip_opt = getFirstPointInsidePolygon(path_ip, lane_interval_ip, first_conflicting_area); if (!stuck_stop_line_idx_ip_opt) { - return std::nullopt; + stuck_stop_line_valid = false; + stuck_stop_line_ip_int = 0; + } else { + stuck_stop_line_ip_int = stuck_stop_line_idx_ip_opt.value(); } - stuck_stop_line_ip_int = stuck_stop_line_idx_ip_opt.value(); } else { stuck_stop_line_ip_int = std::get<0>(lane_interval_ip); } - const auto stuck_stop_line_ip = static_cast( - std::max(0, stuck_stop_line_ip_int - stop_line_margin_idx_dist - base2front_idx_dist)); + stuck_stop_line_ip_int -= (stop_line_margin_idx_dist + base2front_idx_dist); + if (stuck_stop_line_ip_int < 0) { + stuck_stop_line_valid = false; + } + const auto stuck_stop_line_ip = static_cast(std::max(0, stuck_stop_line_ip_int)); - IntersectionStopLines intersection_stop_lines; + struct IntersectionStopLinesTemp + { + size_t closest_idx{0}; + size_t stuck_stop_line{0}; + size_t default_stop_line{0}; + size_t occlusion_peeking_stop_line{0}; + size_t pass_judge_line{0}; + }; + + IntersectionStopLinesTemp intersection_stop_lines_temp; std::list> stop_lines = { - {&closest_idx_ip, &intersection_stop_lines.closest_idx}, - {&stuck_stop_line_ip, &intersection_stop_lines.stuck_stop_line}, - {&default_stop_line_ip, &intersection_stop_lines.default_stop_line}, - {&occlusion_peeking_line_ip, &intersection_stop_lines.occlusion_peeking_stop_line}, - {&pass_judge_line_ip, &intersection_stop_lines.pass_judge_line}, + {&closest_idx_ip, &intersection_stop_lines_temp.closest_idx}, + {&stuck_stop_line_ip, &intersection_stop_lines_temp.stuck_stop_line}, + {&default_stop_line_ip, &intersection_stop_lines_temp.default_stop_line}, + {&occlusion_peeking_line_ip, &intersection_stop_lines_temp.occlusion_peeking_stop_line}, + {&pass_judge_line_ip, &intersection_stop_lines_temp.pass_judge_line}, }; stop_lines.sort( [](const auto & it1, const auto & it2) { return *(std::get<0>(it1)) < *(std::get<0>(it2)); }); @@ -311,27 +346,64 @@ std::optional generateIntersectionStopLines( *stop_idx = insert_idx.value(); } if ( - intersection_stop_lines.occlusion_peeking_stop_line < - intersection_stop_lines.default_stop_line) { - intersection_stop_lines.occlusion_peeking_stop_line = intersection_stop_lines.default_stop_line; + intersection_stop_lines_temp.occlusion_peeking_stop_line < + intersection_stop_lines_temp.default_stop_line) { + intersection_stop_lines_temp.occlusion_peeking_stop_line = + intersection_stop_lines_temp.default_stop_line; } if ( - intersection_stop_lines.occlusion_peeking_stop_line > intersection_stop_lines.pass_judge_line) { - intersection_stop_lines.pass_judge_line = intersection_stop_lines.occlusion_peeking_stop_line; + intersection_stop_lines_temp.occlusion_peeking_stop_line > + intersection_stop_lines_temp.pass_judge_line) { + intersection_stop_lines_temp.pass_judge_line = + intersection_stop_lines_temp.occlusion_peeking_stop_line; + } + + IntersectionStopLines intersection_stop_lines; + intersection_stop_lines.closest_idx = intersection_stop_lines_temp.closest_idx; + if (stuck_stop_line_valid) { + intersection_stop_lines.stuck_stop_line = intersection_stop_lines_temp.stuck_stop_line; } + if (default_stop_line_valid) { + intersection_stop_lines.default_stop_line = intersection_stop_lines_temp.default_stop_line; + } + if (occlusion_peeking_line_valid) { + intersection_stop_lines.occlusion_peeking_stop_line = + intersection_stop_lines_temp.occlusion_peeking_stop_line; + } + intersection_stop_lines.pass_judge_line = intersection_stop_lines_temp.pass_judge_line; return intersection_stop_lines; } std::optional getFirstPointInsidePolygon( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon) + const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon, + const bool search_forward) { + // NOTE: if first point is already inside the polygon, returns nullopt const auto polygon_2d = lanelet::utils::to2D(polygon); - for (size_t i = lane_interval.first; i <= lane_interval.second; ++i) { - auto p = path.points.at(i).point.pose.position; - const auto is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); - if (is_in_lanelet) { - return std::make_optional(i); + if (search_forward) { + const auto & p0 = path.points.at(lane_interval.first).point.pose.position; + if (bg::within(to_bg2d(p0), polygon_2d)) { + return std::nullopt; + } + for (size_t i = lane_interval.first; i <= lane_interval.second; ++i) { + const auto & p = path.points.at(i).point.pose.position; + const auto is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); + if (is_in_lanelet) { + return std::make_optional(i); + } + } + } else { + const auto & p0 = path.points.at(lane_interval.second).point.pose.position; + if (bg::within(to_bg2d(p0), polygon_2d)) { + return std::nullopt; + } + for (size_t i = lane_interval.second; i >= lane_interval.first; --i) { + const auto & p = path.points.at(i).point.pose.position; + const auto is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); + if (is_in_lanelet) { + return std::make_optional(i); + } } } return std::nullopt; @@ -341,21 +413,39 @@ static std::optional> getFirstPointInsidePolygons( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::pair lane_interval, - const std::vector & polygons) + const std::vector & polygons, const bool search_forward = true) { - for (size_t i = lane_interval.first; i <= lane_interval.second; ++i) { - bool is_in_lanelet = false; - auto p = path.points.at(i).point.pose.position; - for (const auto & polygon : polygons) { - const auto polygon_2d = lanelet::utils::to2D(polygon); - is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); + if (search_forward) { + for (size_t i = lane_interval.first; i <= lane_interval.second; ++i) { + bool is_in_lanelet = false; + const auto & p = path.points.at(i).point.pose.position; + for (const auto & polygon : polygons) { + const auto polygon_2d = lanelet::utils::to2D(polygon); + is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); + if (is_in_lanelet) { + return std::make_optional>( + i, polygon); + } + } if (is_in_lanelet) { - return std::make_optional>( - i, polygon); + break; } } - if (is_in_lanelet) { - break; + } else { + for (size_t i = lane_interval.second; i >= lane_interval.first; --i) { + bool is_in_lanelet = false; + const auto & p = path.points.at(i).point.pose.position; + for (const auto & polygon : polygons) { + const auto polygon_2d = lanelet::utils::to2D(polygon); + is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); + if (is_in_lanelet) { + return std::make_optional>( + i, polygon); + } + } + if (is_in_lanelet) { + break; + } } } return std::nullopt; @@ -675,9 +765,10 @@ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane) } bool isTrafficLightArrowActivated( - lanelet::ConstLanelet lane, - const std::map & tl_infos) + lanelet::ConstLanelet lane, const std::map & tl_infos) { + using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; + const auto & turn_direction = lane.attributeOr("turn_direction", "else"); std::optional tl_id = std::nullopt; for (auto && tl_reg_elem : lane.regulatoryElementsAs()) { @@ -694,16 +785,13 @@ bool isTrafficLightArrowActivated( return false; } const auto & tl_info = tl_info_it->second; - for (auto && tl_light : tl_info.signal.lights) { - if (tl_light.color != autoware_auto_perception_msgs::msg::TrafficLight::GREEN) continue; - if (tl_light.status != autoware_auto_perception_msgs::msg::TrafficLight::SOLID_ON) continue; - if ( - turn_direction == std::string("left") && - tl_light.shape == autoware_auto_perception_msgs::msg::TrafficLight::LEFT_ARROW) + for (auto && tl_light : tl_info.signal.elements) { + if (tl_light.color != TrafficSignalElement::GREEN) continue; + if (tl_light.status != TrafficSignalElement::SOLID_ON) continue; + if (turn_direction == std::string("left") && tl_light.shape == TrafficSignalElement::LEFT_ARROW) return true; if ( - turn_direction == std::string("right") && - tl_light.shape == autoware_auto_perception_msgs::msg::TrafficLight::RIGHT_ARROW) + turn_direction == std::string("right") && tl_light.shape == TrafficSignalElement::RIGHT_ARROW) return true; } return false; @@ -919,35 +1007,29 @@ bool checkStuckVehicleInIntersection( } Polygon2d generateStuckVehicleDetectAreaPolygon( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::ConstLanelets & ego_lane_with_next_lane, const int closest_idx, - const double stuck_vehicle_detect_dist, const double stuck_vehicle_ignore_dist, - const double vehicle_length_m) + const util::PathLanelets & path_lanelets, const double stuck_vehicle_detect_dist) { using lanelet::utils::getArcCoordinates; using lanelet::utils::getLaneletLength3d; using lanelet::utils::getPolygonFromArcLength; using lanelet::utils::to2D; - const double extra_dist = stuck_vehicle_detect_dist + vehicle_length_m; - const double ignore_dist = stuck_vehicle_ignore_dist + vehicle_length_m; - - const double intersection_exit_length = getLaneletLength3d(ego_lane_with_next_lane.front()); - - const auto closest_arc_coords = getArcCoordinates( - ego_lane_with_next_lane, tier4_autoware_utils::getPose(path.points.at(closest_idx).point)); - - const double start_arc_length = intersection_exit_length - ignore_dist > closest_arc_coords.length - ? intersection_exit_length - ignore_dist - : closest_arc_coords.length; - - const double end_arc_length = getLaneletLength3d(ego_lane_with_next_lane.front()) + extra_dist; + Polygon2d polygon{}; + if (path_lanelets.conflicting_interval_and_remaining.size() == 0) { + return polygon; + } + double target_polygon_length = + getLaneletLength3d(path_lanelets.conflicting_interval_and_remaining); + lanelet::ConstLanelets targets = path_lanelets.conflicting_interval_and_remaining; + if (path_lanelets.next) { + targets.push_back(path_lanelets.next.value()); + const double next_arc_length = + std::min(stuck_vehicle_detect_dist, getLaneletLength3d(path_lanelets.next.value())); + target_polygon_length += next_arc_length; + } const auto target_polygon = - to2D(getPolygonFromArcLength(ego_lane_with_next_lane, start_arc_length, end_arc_length)) - .basicPolygon(); - - Polygon2d polygon{}; + to2D(getPolygonFromArcLength(targets, 0, target_polygon_length)).basicPolygon(); if (target_polygon.empty()) { return polygon; @@ -1130,13 +1212,19 @@ static lanelet::ConstLanelets getPrevLanelets( std::optional generatePathLanelets( const lanelet::ConstLanelets & lanelets_on_path, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::set & associative_ids, const size_t closest_idx, const double width) + const util::InterpolatedPathInfo & interpolated_path_info, const std::set & associative_ids, + const lanelet::CompoundPolygon3d & first_conflicting_area, + const std::vector & conflicting_areas, + const std::optional & first_attention_area, + const std::vector & attention_areas, const size_t closest_idx, + const double width) { - const auto assigned_lane_interval_opt = findLaneIdsInterval(path, associative_ids); + const auto & assigned_lane_interval_opt = interpolated_path_info.lane_id_interval; if (!assigned_lane_interval_opt) { return std::nullopt; } + const auto assigned_lane_interval = assigned_lane_interval_opt.value(); + const auto & path = interpolated_path_info.path; PathLanelets path_lanelets; // prev @@ -1144,7 +1232,7 @@ std::optional generatePathLanelets( path_lanelets.all = path_lanelets.prev; // entry2ego if exist - const auto [assigned_lane_start, assigned_lane_end] = assigned_lane_interval_opt.value(); + const auto [assigned_lane_start, assigned_lane_end] = assigned_lane_interval; if (closest_idx > assigned_lane_start) { path_lanelets.all.push_back( planning_utils::generatePathLanelet(path, assigned_lane_start, closest_idx, width)); @@ -1162,12 +1250,31 @@ std::optional generatePathLanelets( const auto next_lane_interval_opt = findLaneIdsInterval(path, {next_id}); if (next_lane_interval_opt) { const auto [next_start, next_end] = next_lane_interval_opt.value(); - path_lanelets.next = - planning_utils::generatePathLanelet(path, next_start + 1, next_end, width); + path_lanelets.next = planning_utils::generatePathLanelet(path, next_start, next_end, width); path_lanelets.all.push_back(path_lanelets.next.value()); } } + const auto first_inside_conflicting_idx_opt = + first_attention_area.has_value() + ? getFirstPointInsidePolygon(path, assigned_lane_interval, first_attention_area.value()) + : getFirstPointInsidePolygon(path, assigned_lane_interval, first_conflicting_area); + const auto last_inside_conflicting_idx_opt = + first_attention_area.has_value() + ? getFirstPointInsidePolygons(path, assigned_lane_interval, attention_areas, false) + : getFirstPointInsidePolygons(path, assigned_lane_interval, conflicting_areas, false); + if (first_inside_conflicting_idx_opt && last_inside_conflicting_idx_opt) { + const auto first_inside_conflicting_idx = first_inside_conflicting_idx_opt.value(); + const auto last_inside_conflicting_idx = last_inside_conflicting_idx_opt.value().first; + lanelet::ConstLanelet conflicting_interval = planning_utils::generatePathLanelet( + path, first_inside_conflicting_idx, last_inside_conflicting_idx, width); + path_lanelets.conflicting_interval_and_remaining.push_back(std::move(conflicting_interval)); + if (last_inside_conflicting_idx < assigned_lane_end) { + lanelet::ConstLanelet remaining_interval = planning_utils::generatePathLanelet( + path, last_inside_conflicting_idx, assigned_lane_end, width); + path_lanelets.conflicting_interval_and_remaining.push_back(std::move(remaining_interval)); + } + } return path_lanelets; } diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index 287a75979927b..e766da6651417 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -83,7 +83,8 @@ std::optional generateIntersectionStopLines( std::optional getFirstPointInsidePolygon( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon); + const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon, + const bool search_forward = true); /** * @brief check if ego is over the target_idx. If the index is same, compare the exact pose @@ -111,8 +112,7 @@ std::optional getIntersectionArea( bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane); bool isTrafficLightArrowActivated( - lanelet::ConstLanelet lane, - const std::map & tl_infos); + lanelet::ConstLanelet lane, const std::map & tl_infos); std::vector generateDetectionLaneDivisions( lanelet::ConstLanelets detection_lanelets, @@ -132,10 +132,7 @@ bool checkStuckVehicleInIntersection( DebugData * debug_data); Polygon2d generateStuckVehicleDetectAreaPolygon( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::ConstLanelets & ego_lane_with_next_lane, const int closest_idx, - const double stuck_vehicle_detect_dist, const double stuck_vehicle_ignore_dist, - const double vehicle_length_m); + const util::PathLanelets & path_lanelets, const double stuck_vehicle_detect_dist); bool checkAngleForTargetLanelets( const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, @@ -158,8 +155,12 @@ double calcDistanceUntilIntersectionLanelet( std::optional generatePathLanelets( const lanelet::ConstLanelets & lanelets_on_path, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::set & associative_ids, const size_t closest_idx, const double width); + const util::InterpolatedPathInfo & interpolated_path_info, const std::set & associative_ids, + const lanelet::CompoundPolygon3d & first_conflicting_area, + const std::vector & conflicting_areas, + const std::optional & first_attention_area, + const std::vector & attention_areas, const size_t closest_idx, + const double width); } // namespace util } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index 26187d75ff53c..64be330a99232 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -55,10 +55,10 @@ struct DebugData struct InterpolatedPathInfo { autoware_auto_planning_msgs::msg::PathWithLaneId path; - double ds; - int lane_id; - std::set associative_lane_ids; - std::optional> lane_id_interval; + double ds{0.0}; + int lane_id{0}; + std::set associative_lane_ids{}; + std::optional> lane_id_interval{std::nullopt}; }; struct IntersectionLanelets @@ -117,19 +117,23 @@ struct IntersectionLanelets struct DiscretizedLane { - int lane_id; + int lane_id{0}; // discrete fine lines from left to right - std::vector divisions; + std::vector divisions{}; }; struct IntersectionStopLines { // NOTE: for baselink - size_t closest_idx; - size_t stuck_stop_line; - size_t default_stop_line; - size_t occlusion_peeking_stop_line; - size_t pass_judge_line; + size_t closest_idx{0}; + // NOTE: null if path does not conflict with first_conflicting_area + std::optional stuck_stop_line{std::nullopt}; + // NOTE: null if path is over map stop_line OR its value is calculated negative area + std::optional default_stop_line{std::nullopt}; + // NOTE: null if footprints do not change from outside to inside of detection area + std::optional occlusion_peeking_stop_line{std::nullopt}; + // if the value is calculated negative, its value is 0 + size_t pass_judge_line{0}; }; struct PathLanelets @@ -142,6 +146,9 @@ struct PathLanelets std::optional next = std::nullopt; // this is nullopt is the goal is inside intersection lanelet::ConstLanelets all; + lanelet::ConstLanelets + conflicting_interval_and_remaining; // the left/right-most interval of path conflicting with + // conflicting lanelets plus the next lane part of the path }; } // namespace behavior_velocity_planner::util diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/debug.cpp b/planning/behavior_velocity_no_drivable_lane_module/src/debug.cpp index a4f693dfaa883..0c6bb747a854b 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/debug.cpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/debug.cpp @@ -15,8 +15,9 @@ #include "scene.hpp" #include -#include -#include +#include +#include +#include #include diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp b/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp index 418f05baeaf20..2c57ff9458833 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp @@ -14,17 +14,20 @@ #include "manager.hpp" +#include + #include namespace behavior_velocity_planner { +using tier4_autoware_utils::getOrDeclareParameter; NoDrivableLaneModuleManager::NoDrivableLaneModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) { const std::string ns(getModuleName()); - planner_param_.stop_margin = node.declare_parameter(ns + ".stop_margin", 1.5); - planner_param_.print_debug_info = node.declare_parameter(ns + ".print_debug_info", false); + planner_param_.stop_margin = getOrDeclareParameter(node, ns + ".stop_margin"); + planner_param_.print_debug_info = getOrDeclareParameter(node, ns + ".print_debug_info"); } void NoDrivableLaneModuleManager::launchNewModules( diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/scene.cpp b/planning/behavior_velocity_no_drivable_lane_module/src/scene.cpp index f08154cda336c..8109b99166332 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/scene.cpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/scene.cpp @@ -14,8 +14,8 @@ #include "scene.hpp" -#include "motion_utils/motion_utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include "util.hpp" #include diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/util.cpp b/planning/behavior_velocity_no_drivable_lane_module/src/util.cpp index 9b50a73a725db..889d0dbb064c5 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/util.cpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/util.cpp @@ -14,7 +14,8 @@ #include "util.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/path_with_lane_id.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include diff --git a/planning/behavior_velocity_no_stopping_area_module/src/debug.cpp b/planning/behavior_velocity_no_stopping_area_module/src/debug.cpp index 859ac4b4026f8..e8bf83868d122 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/debug.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/debug.cpp @@ -16,7 +16,8 @@ #include #include -#include +#include +#include #ifdef ROS_DISTRO_GALACTIC #include diff --git a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index 77fbb18be2b5b..3e24689f2e774 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -18,7 +18,8 @@ #include #include #include -#include +#include +#include #include diff --git a/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp b/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp index a8e4aee2d2815..527f6f02140ea 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp @@ -17,7 +17,7 @@ #include #include -#include +#include #include #include diff --git a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp index 21a8cce93a1be..029b516e93219 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp @@ -20,7 +20,9 @@ #include #include #include +#include #include +#include #include #include diff --git a/planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp b/planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp index ef2e776112766..85ae85f1895d9 100644 --- a/planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp @@ -14,7 +14,6 @@ #include "gtest/gtest.h" #include "occlusion_spot_utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include "utils.hpp" #include diff --git a/planning/behavior_velocity_out_of_lane_module/README.md b/planning/behavior_velocity_out_of_lane_module/README.md index da46055460a99..885c0dc2859ab 100644 --- a/planning/behavior_velocity_out_of_lane_module/README.md +++ b/planning/behavior_velocity_out_of_lane_module/README.md @@ -140,9 +140,9 @@ Moreover, parameter `action.distance_buffer` adds an extra distance between the | Parameter /objects | Type | Description | | ------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `minimum_velocity` | double | [m/s] consider objects with an estimated time to collision bellow this value while on the overlap | -| `use_predicted_paths` | bool | [-] if true, use the predicted paths to estimate future positions; if false, assume the object moves at constant velocity along _all_ lanelets it currently is located in | +| `minimum_velocity` | double | [m/s] ignore objects with a velocity lower than this value | | `predicted_path_min_confidence` | double | [-] minimum confidence required for a predicted path to be considered | +| `use_predicted_paths` | bool | [-] if true, use the predicted paths to estimate future positions; if false, assume the object moves at constant velocity along _all_ lanelets it currently is located in | | Parameter /overlap | Type | Description | | ------------------ | ------ | ---------------------------------------------------------------------------------------------------- | @@ -155,7 +155,7 @@ Moreover, parameter `action.distance_buffer` adds an extra distance between the | `strict` | bool | [-] if true, when a decision is taken to avoid entering a lane, the stop point will make sure no lane at all is entered by ego; if false, ego stops just before entering a lane but may then be overlapping another lane | | `distance_buffer` | double | [m] buffer distance to try to keep between the ego footprint and lane | | `slowdown.distance_threshold` | double | [m] insert a slow down when closer than this distance from an overlap | -| `slowdown.velocity` | double | [m] slow down velocity | +| `slowdown.velocity` | double | [m/s] slow down velocity | | `stop.distance_threshold` | double | [m] insert a stop when closer than this distance from an overlap | | Parameter /ego | Type | Description | diff --git a/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml b/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml index dd4c1c610261c..510dc86ef651d 100644 --- a/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml +++ b/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml @@ -34,6 +34,7 @@ distance_threshold: 15.0 # [m] insert a stop when closer than this distance from an overlap ego: + min_assumed_velocity: 2.0 # [m/s] minimum velocity used to calculate the enter and exit times of ego extra_front_offset: 0.0 # [m] extra front distance extra_rear_offset: 0.0 # [m] extra rear distance extra_right_offset: 0.0 # [m] extra right distance diff --git a/planning/behavior_velocity_out_of_lane_module/src/debug.cpp b/planning/behavior_velocity_out_of_lane_module/src/debug.cpp index 475e341d76528..0574a5226dc4a 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/debug.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/debug.cpp @@ -14,6 +14,8 @@ #include "debug.hpp" +#include + #include namespace behavior_velocity_planner::out_of_lane::debug diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp index 163c8632e40c7..2ebfa4ecbe431 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp @@ -15,6 +15,10 @@ #include "decisions.hpp" #include +#include +#include +#include +#include #include #include @@ -30,11 +34,11 @@ double distance_along_path(const EgoData & ego_data, const size_t target_idx) ego_data.path->points, ego_data.pose.position, ego_data.first_path_idx + target_idx); } -double time_along_path(const EgoData & ego_data, const size_t target_idx) +double time_along_path(const EgoData & ego_data, const size_t target_idx, const double min_velocity) { const auto dist = distance_along_path(ego_data, target_idx); const auto v = std::max( - ego_data.velocity, + std::max(ego_data.velocity, min_velocity), ego_data.path->points[ego_data.first_path_idx + target_idx].point.longitudinal_velocity_mps * 0.5); return dist / v; @@ -55,8 +59,7 @@ bool object_is_incoming( std::optional> object_time_to_range( const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, - const std::shared_ptr route_handler, const double min_confidence, - const rclcpp::Logger & logger) + const std::shared_ptr route_handler, const rclcpp::Logger & logger) { // skip the dynamic object if it is not in a lane preceding the overlapped lane // lane changes are intentionally not considered @@ -65,13 +68,12 @@ std::optional> object_time_to_range( object.kinematics.initial_pose_with_covariance.pose.position.y); if (!object_is_incoming(object_point, route_handler, range.lane)) return {}; - const auto max_deviation = object.shape.dimensions.y * 2.0; + const auto max_deviation = object.shape.dimensions.y + range.inside_distance; auto worst_enter_time = std::optional(); auto worst_exit_time = std::optional(); for (const auto & predicted_path : object.kinematics.predicted_paths) { - if (predicted_path.confidence < min_confidence) continue; const auto time_step = rclcpp::Duration(predicted_path.time_step).seconds(); const auto enter_point = geometry_msgs::msg::Point().set__x(range.entering_point.x()).set__y(range.entering_point.y()); @@ -281,8 +283,10 @@ bool should_not_enter( const rclcpp::Logger & logger) { RangeTimes range_times{}; - range_times.ego.enter_time = time_along_path(inputs.ego_data, range.entering_path_idx); - range_times.ego.exit_time = time_along_path(inputs.ego_data, range.exiting_path_idx); + range_times.ego.enter_time = + time_along_path(inputs.ego_data, range.entering_path_idx, params.ego_min_velocity); + range_times.ego.exit_time = + time_along_path(inputs.ego_data, range.exiting_path_idx, params.ego_min_velocity); RCLCPP_DEBUG( logger, "\t[%lu -> %lu] %ld | ego enters at %2.2f, exits at %2.2f\n", range.entering_path_idx, range.exiting_path_idx, range.lane.id(), range_times.ego.enter_time, range_times.ego.exit_time); @@ -298,8 +302,7 @@ bool should_not_enter( // skip objects that are already on the interval const auto enter_exit_time = params.objects_use_predicted_paths - ? object_time_to_range( - object, range, inputs.route_handler, params.objects_min_confidence, logger) + ? object_time_to_range(object, range, inputs.route_handler, logger) : object_time_to_range(object, range, inputs, logger); if (!enter_exit_time) { RCLCPP_DEBUG(logger, " SKIP (no enter/exit times found)\n"); diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp index bf6bf81e506cf..1d1fe8bea94a6 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp @@ -48,6 +48,7 @@ double distance_along_path(const EgoData & ego_data, const size_t target_idx); /// @brief estimate the time when ego will reach some target path index /// @param [in] ego_data data related to the ego vehicle /// @param [in] target_idx target ego path index +/// @param [in] min_velocity minimum ego velocity used to estimate the time /// @return time taken by ego to reach the target [s] double time_along_path(const EgoData & ego_data, const size_t target_idx); /// @brief use an object's predicted paths to estimate the times it will reach the enter and exit @@ -57,14 +58,12 @@ double time_along_path(const EgoData & ego_data, const size_t target_idx); /// @param [in] object dynamic object /// @param [in] range overlapping range /// @param [in] route_handler route handler used to estimate the path of the dynamic object -/// @param [in] min_confidence minimum confidence to consider a predicted path /// @param [in] logger ros logger /// @return an optional pair (time at enter [s], time at exit [s]). If the dynamic object drives in /// the opposite direction, time at enter > time at exit std::optional> object_time_to_range( const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, - const std::shared_ptr route_handler, const double min_confidence, - const rclcpp::Logger & logger); + const std::shared_ptr route_handler, const rclcpp::Logger & logger); /// @brief use the lanelet map to estimate the times when an object will reach the enter and exit /// points of an overlapping range /// @param [in] object dynamic object diff --git a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp new file mode 100644 index 0000000000000..48869e5e3926d --- /dev/null +++ b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp @@ -0,0 +1,68 @@ +// Copyright 2023 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 FILTER_PREDICTED_OBJECTS_HPP_ +#define FILTER_PREDICTED_OBJECTS_HPP_ + +#include "types.hpp" + +#include + +namespace behavior_velocity_planner::out_of_lane +{ +/// @brief filter predicted objects and their predicted paths +/// @param [in] objects predicted objects to filter +/// @param [in] ego_data ego data +/// @param [in] params parameters +/// @return filtered predicted objects +autoware_auto_perception_msgs::msg::PredictedObjects filter_predicted_objects( + const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data, + const PlannerParam & params) +{ + autoware_auto_perception_msgs::msg::PredictedObjects filtered_objects; + filtered_objects.header = objects.header; + for (const auto & object : objects.objects) { + const auto is_pedestrian = + std::find_if(object.classification.begin(), object.classification.end(), [](const auto & c) { + return c.label == autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN; + }) != object.classification.end(); + if (is_pedestrian) continue; + + auto filtered_object = object; + const auto is_invalid_predicted_path = [&](const auto & predicted_path) { + const auto is_low_confidence = predicted_path.confidence < params.objects_min_confidence; + const auto lat_offset_to_current_ego = + std::abs(motion_utils::calcLateralOffset(predicted_path.path, ego_data.pose.position)); + const auto is_crossing_ego = + lat_offset_to_current_ego <= + object.shape.dimensions.y / 2.0 + std::max( + params.left_offset + params.extra_left_offset, + params.right_offset + params.extra_right_offset); + return is_low_confidence || is_crossing_ego; + }; + if (params.objects_use_predicted_paths) { + auto & predicted_paths = filtered_object.kinematics.predicted_paths; + const auto new_end = + std::remove_if(predicted_paths.begin(), predicted_paths.end(), is_invalid_predicted_path); + predicted_paths.erase(new_end, predicted_paths.end()); + } + if (!params.objects_use_predicted_paths || !filtered_object.kinematics.predicted_paths.empty()) + filtered_objects.objects.push_back(filtered_object); + } + return filtered_objects; +} + +} // namespace behavior_velocity_planner::out_of_lane + +#endif // FILTER_PREDICTED_OBJECTS_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp b/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp index 929fbd944af9f..9ad7437901309 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp @@ -14,7 +14,6 @@ #include "footprint.hpp" -#include #include #include diff --git a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp index a0e2b86caa623..9924319b141f7 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp @@ -54,16 +54,21 @@ lanelet::ConstLanelets calculate_other_lanelets( const auto lanelets_within_range = lanelet::geometry::findWithin2d( route_handler.getLaneletMapPtr()->laneletLayer, ego_point, std::max(params.slow_dist_threshold, params.stop_dist_threshold)); + const auto is_overlapped_by_path_lanelets = [&](const auto & l) { + for (const auto & path_ll : path_lanelets) { + if ( + boost::geometry::overlaps( + path_ll.polygon2d().basicPolygon(), l.polygon2d().basicPolygon()) || + boost::geometry::within(path_ll.polygon2d().basicPolygon(), l.polygon2d().basicPolygon()) || + boost::geometry::within(l.polygon2d().basicPolygon(), path_ll.polygon2d().basicPolygon())) { + return true; + } + } + return false; + }; for (const auto & ll : lanelets_within_range) { const auto is_path_lanelet = contains_lanelet(path_lanelets, ll.second.id()); const auto is_ignored_lanelet = contains_lanelet(ignored_lanelets, ll.second.id()); - const auto is_overlapped_by_path_lanelets = [&](const auto & l) { - for (const auto & path_ll : path_lanelets) - if (boost::geometry::overlaps( - path_ll.polygon2d().basicPolygon(), l.polygon2d().basicPolygon())) - return true; - return false; - }; if (!is_path_lanelet && !is_ignored_lanelet && !is_overlapped_by_path_lanelets(ll.second)) other_lanelets.push_back(ll.second); } diff --git a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp index 075738f07d62e..5765363024aed 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp @@ -64,6 +64,7 @@ OutOfLaneModuleManager::OutOfLaneModuleManager(rclcpp::Node & node) pp.stop_dist_threshold = getOrDeclareParameter(node, ns + ".action.stop.distance_threshold"); + pp.ego_min_velocity = getOrDeclareParameter(node, ns + ".ego.min_assumed_velocity"); pp.extra_front_offset = getOrDeclareParameter(node, ns + ".ego.extra_front_offset"); pp.extra_rear_offset = getOrDeclareParameter(node, ns + ".ego.extra_rear_offset"); pp.extra_left_offset = getOrDeclareParameter(node, ns + ".ego.extra_left_offset"); diff --git a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp index 5922a5b9b4488..280a832f6d684 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp @@ -16,6 +16,7 @@ #include "debug.hpp" #include "decisions.hpp" +#include "filter_predicted_objects.hpp" #include "footprint.hpp" #include "lanelets_selection.hpp" #include "overlapping_range.hpp" @@ -25,6 +26,8 @@ #include #include #include +#include +#include #include #include @@ -103,7 +106,7 @@ bool OutOfLaneModule::modifyPathVelocity( DecisionInputs inputs; inputs.ranges = ranges; inputs.ego_data = ego_data; - inputs.objects = *planner_data_->predicted_objects; + inputs.objects = filter_predicted_objects(*planner_data_->predicted_objects, ego_data, params_); inputs.route_handler = planner_data_->route_handler_; inputs.lanelets = other_lanelets; auto decisions = calculate_decisions(inputs, params_, logger_); diff --git a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp index 79c66148926fc..1a64c11a3c921 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp @@ -18,6 +18,7 @@ #include "types.hpp" #include +#include #include #include diff --git a/planning/behavior_velocity_out_of_lane_module/src/types.hpp b/planning/behavior_velocity_out_of_lane_module/src/types.hpp index 85266d779f34e..b81fa09884819 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/types.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/types.hpp @@ -42,10 +42,11 @@ struct PlannerParam double intervals_ego_buffer; // [s](mode="intervals") buffer to extend the ego time range double intervals_obj_buffer; // [s](mode="intervals") buffer to extend the objects time range double ttc_threshold; // [s](mode="ttc") threshold on time to collision between ego and an object + double ego_min_velocity; // [m/s] minimum velocity of ego used to calculate its ttc or time range - bool objects_use_predicted_paths; // # whether to use the objects' predicted paths - double objects_min_vel; // # [m/s] objects lower than this velocity will be ignored - double objects_min_confidence; // # minimum confidence to consider a predicted path + bool objects_use_predicted_paths; // whether to use the objects' predicted paths + double objects_min_vel; // [m/s] objects lower than this velocity will be ignored + double objects_min_confidence; // minimum confidence to consider a predicted path double overlap_extra_length; // [m] extra length to add around an overlap range double overlap_min_dist; // [m] min distance inside another lane to consider an overlap diff --git a/planning/behavior_velocity_planner/README.md b/planning/behavior_velocity_planner/README.md index 25546c95074c3..519e68764b450 100644 --- a/planning/behavior_velocity_planner/README.md +++ b/planning/behavior_velocity_planner/README.md @@ -29,15 +29,15 @@ So for example, in order to stop at a stop line with the vehicles' front on the ## Input topics -| Name | Type | Description | -| ----------------------------------------- | ------------------------------------------------------ | ------------------------------------------------------------------------------------------------------------------------------- | -| `~input/path_with_lane_id` | autoware_auto_planning_msgs::msg::PathWithLaneId | path with lane_id | -| `~input/vector_map` | autoware_auto_mapping_msgs::msg::HADMapBin | vector map | -| `~input/vehicle_odometry` | nav_msgs::msg::Odometry | vehicle velocity | -| `~input/dynamic_objects` | autoware_auto_perception_msgs::msg::PredictedObjects | dynamic objects | -| `~input/no_ground_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud | -| `~/input/compare_map_filtered_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud filtered by compare map. Note that this is used only when the detection method of run out module is Points. | -| `~input/traffic_signals` | autoware_auto_perception_msgs::msg::TrafficSignalArray | traffic light states | +| Name | Type | Description | +| ----------------------------------------- | ---------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------- | +| `~input/path_with_lane_id` | autoware_auto_planning_msgs::msg::PathWithLaneId | path with lane_id | +| `~input/vector_map` | autoware_auto_mapping_msgs::msg::HADMapBin | vector map | +| `~input/vehicle_odometry` | nav_msgs::msg::Odometry | vehicle velocity | +| `~input/dynamic_objects` | autoware_auto_perception_msgs::msg::PredictedObjects | dynamic objects | +| `~input/no_ground_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud | +| `~/input/compare_map_filtered_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud filtered by compare map. Note that this is used only when the detection method of run out module is Points. | +| `~input/traffic_signals` | autoware_perception_msgs::msg::TrafficSignalArray | traffic light states | ## Output topics diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 12503f743b0f2..c4f4edd26416c 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -16,7 +16,10 @@ #include #include +#include +#include #include +#include #include #include @@ -99,7 +102,7 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio std::bind(&BehaviorVelocityPlannerNode::onLaneletMap, this, _1), createSubscriptionOptions(this)); sub_traffic_signals_ = - this->create_subscription( + this->create_subscription( "~/input/traffic_signals", 1, std::bind(&BehaviorVelocityPlannerNode::onTrafficSignals, this, _1), createSubscriptionOptions(this)); @@ -287,15 +290,15 @@ void BehaviorVelocityPlannerNode::onLaneletMap( } void BehaviorVelocityPlannerNode::onTrafficSignals( - const autoware_auto_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg) + const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg) { std::lock_guard lock(mutex_); for (const auto & signal : msg->signals) { - autoware_auto_perception_msgs::msg::TrafficSignalStamped traffic_signal; - traffic_signal.header = msg->header; + TrafficSignalStamped traffic_signal; + traffic_signal.stamp = msg->stamp; traffic_signal.signal = signal; - planner_data_.traffic_light_id_map[signal.map_primitive_id] = traffic_signal; + planner_data_.traffic_light_id_map[signal.traffic_signal_id] = traffic_signal; } } diff --git a/planning/behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/src/node.hpp index 91172fdd77da8..bf7c53f974ef6 100644 --- a/planning/behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/src/node.hpp @@ -30,6 +30,7 @@ #include #include #include +#include #include #include @@ -61,7 +62,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_vehicle_odometry_; rclcpp::Subscription::SharedPtr sub_acceleration_; rclcpp::Subscription::SharedPtr sub_lanelet_map_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr sub_traffic_signals_; rclcpp::Subscription::SharedPtr sub_virtual_traffic_light_states_; @@ -77,7 +78,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node void onAcceleration(const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg); void onLaneletMap(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); void onTrafficSignals( - const autoware_auto_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg); + const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg); void onVirtualTrafficLightStates( const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg); void onOccupancyGrid(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg); diff --git a/planning/behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/behavior_velocity_planner/test/src/test_node_interface.cpp index eb405f06c8c4b..6756a82ca20d4 100644 --- a/planning/behavior_velocity_planner/test/src/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/test/src/test_node_interface.cpp @@ -79,7 +79,8 @@ std::shared_ptr generateNode() get_behavior_velocity_module_config("stop_line"), get_behavior_velocity_module_config("traffic_light"), get_behavior_velocity_module_config("virtual_traffic_light"), - get_behavior_velocity_module_config("out_of_lane")}); + get_behavior_velocity_module_config("out_of_lane"), + get_behavior_velocity_module_config("no_drivable_lane")}); // TODO(Takagi, Isamu): set launch_modules // TODO(Kyoichi Sugahara) set to true launch_virtual_traffic_light diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp index 44d3184a5f6b7..2668d83b0f510 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp @@ -17,14 +17,14 @@ #include "route_handler/route_handler.hpp" +#include #include #include #include #include #include -#include -#include +#include #include #include #include @@ -83,7 +83,7 @@ struct PlannerData double ego_nearest_yaw_threshold; // other internal data - std::map traffic_light_id_map; + std::map traffic_light_id_map; boost::optional external_velocity_limit; tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states; @@ -132,14 +132,12 @@ struct PlannerData return true; } - std::shared_ptr getTrafficSignal( - const int id) const + std::shared_ptr getTrafficSignal(const int id) const { if (traffic_light_id_map.count(id) == 0) { return {}; } - return std::make_shared( - traffic_light_id_map.at(id)); + return std::make_shared(traffic_light_id_map.at(id)); } }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp index c33ce84ff09de..4735bb34c7b00 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp @@ -21,7 +21,9 @@ #include #include #include -#include +#include +#include +#include #include #include diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp index 14b009ceb0748..6513a3c9a43a9 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp @@ -17,8 +17,8 @@ #include #include -#include -#include +#include +#include #include #include @@ -27,6 +27,7 @@ #include #include #include +#include #include #include #include @@ -78,6 +79,12 @@ struct PointWithSearchRangeIndex SearchRangeIndex index; }; +struct TrafficSignalStamped +{ + builtin_interfaces::msg::Time stamp; + autoware_perception_msgs::msg::TrafficSignal signal; +}; + using geometry_msgs::msg::Pose; using BasicPolygons2d = std::vector; using Polygons2d = std::vector; diff --git a/planning/behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner_common/package.xml index 6a7a6f698bfbc..aedbab65068fb 100644 --- a/planning/behavior_velocity_planner_common/package.xml +++ b/planning/behavior_velocity_planner_common/package.xml @@ -19,9 +19,9 @@ autoware_adapi_v1_msgs autoware_auto_mapping_msgs - autoware_auto_perception_msgs autoware_auto_planning_msgs autoware_auto_tf2 + autoware_perception_msgs diagnostic_msgs eigen geometry_msgs diff --git a/planning/behavior_velocity_planner_common/src/utilization/debug.cpp b/planning/behavior_velocity_planner_common/src/utilization/debug.cpp index dc1eaeacb2f71..845b93576bec8 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/debug.cpp +++ b/planning/behavior_velocity_planner_common/src/utilization/debug.cpp @@ -13,7 +13,7 @@ // limitations under the License. #include -#include +#include namespace behavior_velocity_planner { diff --git a/planning/behavior_velocity_planner_common/src/utilization/util.cpp b/planning/behavior_velocity_planner_common/src/utilization/util.cpp index d98e9002b2057..3f8180956cf10 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/util.cpp +++ b/planning/behavior_velocity_planner_common/src/utilization/util.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include #include diff --git a/planning/behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp b/planning/behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp index c4fdb83e00609..60a0ff878b478 100644 --- a/planning/behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp +++ b/planning/behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp @@ -16,6 +16,7 @@ #include #include +#include #include diff --git a/planning/behavior_velocity_run_out_module/src/debug.cpp b/planning/behavior_velocity_run_out_module/src/debug.cpp index 7fa17b0cea52a..90f69e0faae5e 100644 --- a/planning/behavior_velocity_run_out_module/src/debug.cpp +++ b/planning/behavior_velocity_run_out_module/src/debug.cpp @@ -16,7 +16,8 @@ #include "scene.hpp" -#include +#include +#include using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::calcOffsetPose; diff --git a/planning/behavior_velocity_run_out_module/src/debug.hpp b/planning/behavior_velocity_run_out_module/src/debug.hpp index 273abb10c780a..0dcd3099b616a 100644 --- a/planning/behavior_velocity_run_out_module/src/debug.hpp +++ b/planning/behavior_velocity_run_out_module/src/debug.hpp @@ -18,6 +18,9 @@ #include +#include +#include + #include #include #include diff --git a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp index e2f9880435d5e..728e0fbad92a5 100644 --- a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp +++ b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp @@ -14,6 +14,11 @@ #include "dynamic_obstacle.hpp" +#include +#include +#include +#include + #include #include diff --git a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp index f9b2b12298d2d..7eece8fac29af 100644 --- a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp +++ b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp @@ -21,7 +21,6 @@ #include #include #include -#include #include diff --git a/planning/behavior_velocity_run_out_module/src/path_utils.hpp b/planning/behavior_velocity_run_out_module/src/path_utils.hpp index b01ee70f8bdfa..11ed2b7282ab3 100644 --- a/planning/behavior_velocity_run_out_module/src/path_utils.hpp +++ b/planning/behavior_velocity_run_out_module/src/path_utils.hpp @@ -15,8 +15,6 @@ #ifndef PATH_UTILS_HPP_ #define PATH_UTILS_HPP_ -#include - #include #include #include diff --git a/planning/behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_run_out_module/src/scene.cpp index 3b2ed6a78a96c..b062bf9e3175e 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_run_out_module/src/scene.cpp @@ -18,6 +18,7 @@ #include #include +#include #include #include diff --git a/planning/behavior_velocity_run_out_module/src/utils.cpp b/planning/behavior_velocity_run_out_module/src/utils.cpp index 3def2ade46731..71ab07922b27d 100644 --- a/planning/behavior_velocity_run_out_module/src/utils.cpp +++ b/planning/behavior_velocity_run_out_module/src/utils.cpp @@ -14,6 +14,9 @@ #include "utils.hpp" +#include +#include + #include #include diff --git a/planning/behavior_velocity_run_out_module/src/utils.hpp b/planning/behavior_velocity_run_out_module/src/utils.hpp index 1f725291a38b7..74bc2489d7c32 100644 --- a/planning/behavior_velocity_run_out_module/src/utils.hpp +++ b/planning/behavior_velocity_run_out_module/src/utils.hpp @@ -17,9 +17,10 @@ #include #include -#include #include +#include + #include #include diff --git a/planning/behavior_velocity_speed_bump_module/src/debug.cpp b/planning/behavior_velocity_speed_bump_module/src/debug.cpp index 62e8cd31ec062..3f9a60d1d27e4 100644 --- a/planning/behavior_velocity_speed_bump_module/src/debug.cpp +++ b/planning/behavior_velocity_speed_bump_module/src/debug.cpp @@ -15,8 +15,9 @@ #include "scene.hpp" #include -#include -#include +#include +#include +#include #include diff --git a/planning/behavior_velocity_speed_bump_module/src/scene.cpp b/planning/behavior_velocity_speed_bump_module/src/scene.cpp index fda92f037cf7a..dc944cc0c292b 100644 --- a/planning/behavior_velocity_speed_bump_module/src/scene.cpp +++ b/planning/behavior_velocity_speed_bump_module/src/scene.cpp @@ -14,8 +14,8 @@ #include "scene.hpp" -#include "motion_utils/motion_utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include "util.hpp" #include diff --git a/planning/behavior_velocity_speed_bump_module/src/util.cpp b/planning/behavior_velocity_speed_bump_module/src/util.cpp index 89845a4b5e271..c815dab29af75 100644 --- a/planning/behavior_velocity_speed_bump_module/src/util.cpp +++ b/planning/behavior_velocity_speed_bump_module/src/util.cpp @@ -14,10 +14,11 @@ #include "util.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/path_with_lane_id.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include -#include +#include #include #include diff --git a/planning/behavior_velocity_stop_line_module/src/debug.cpp b/planning/behavior_velocity_stop_line_module/src/debug.cpp index 09949751f4e93..8d7de00635d4f 100644 --- a/planning/behavior_velocity_stop_line_module/src/debug.cpp +++ b/planning/behavior_velocity_stop_line_module/src/debug.cpp @@ -15,7 +15,8 @@ #include "scene.hpp" #include -#include +#include +#include #ifdef ROS_DISTRO_GALACTIC #include diff --git a/planning/behavior_velocity_traffic_light_module/package.xml b/planning/behavior_velocity_traffic_light_module/package.xml index a8733c5dcf5be..c658f0890b986 100644 --- a/planning/behavior_velocity_traffic_light_module/package.xml +++ b/planning/behavior_velocity_traffic_light_module/package.xml @@ -18,8 +18,8 @@ eigen3_cmake_module autoware_adapi_v1_msgs - autoware_auto_perception_msgs autoware_auto_planning_msgs + autoware_perception_msgs behavior_velocity_planner_common eigen geometry_msgs diff --git a/planning/behavior_velocity_traffic_light_module/src/debug.cpp b/planning/behavior_velocity_traffic_light_module/src/debug.cpp index 9e475441a3f3c..6afd5381ce315 100644 --- a/planning/behavior_velocity_traffic_light_module/src/debug.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/debug.cpp @@ -15,7 +15,8 @@ #include "scene.hpp" #include -#include +#include +#include #ifdef ROS_DISTRO_GALACTIC #include diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp index 1b0646f4bee00..f035544b81592 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.cpp @@ -41,7 +41,7 @@ TrafficLightModuleManager::TrafficLightModuleManager(rclcpp::Node & node) planner_param_.enable_pass_judge = getOrDeclareParameter(node, ns + ".enable_pass_judge"); planner_param_.yellow_lamp_period = getOrDeclareParameter(node, ns + ".yellow_lamp_period"); - pub_tl_state_ = node.create_publisher( + pub_tl_state_ = node.create_publisher( "~/output/traffic_signal", 1); } @@ -51,9 +51,7 @@ void TrafficLightModuleManager::modifyPathVelocity( visualization_msgs::msg::MarkerArray debug_marker_array; visualization_msgs::msg::MarkerArray virtual_wall_marker_array; - autoware_auto_perception_msgs::msg::LookingTrafficSignal tl_state; - tl_state.header.stamp = path->header.stamp; - tl_state.is_module_running = false; + autoware_perception_msgs::msg::TrafficSignal tl_state; autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factor_array; velocity_factor_array.header.frame_id = "map"; diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.hpp b/planning/behavior_velocity_traffic_light_module/src/manager.hpp index 450cba09e1e08..c36c6af1128ce 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.hpp @@ -55,8 +55,7 @@ class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC const lanelet::TrafficLightConstPtr registered_element) const; // Debug - rclcpp::Publisher::SharedPtr - pub_tl_state_; + rclcpp::Publisher::SharedPtr pub_tl_state_; boost::optional first_ref_stop_path_point_index_; }; diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index 679a2f0cb9320..e0633926e35df 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -159,32 +159,6 @@ bool calcStopPointAndInsertIndex( } return false; } - -geometry_msgs::msg::Point getTrafficLightPosition( - const lanelet::ConstLineStringOrPolygon3d & traffic_light) -{ - if (!traffic_light.lineString()) { - throw std::invalid_argument{"Traffic light is not LineString"}; - } - geometry_msgs::msg::Point tl_center; - auto traffic_light_ls = traffic_light.lineString().value(); - for (const auto & tl_point : traffic_light_ls) { - tl_center.x += tl_point.x() / traffic_light_ls.size(); - tl_center.y += tl_point.y() / traffic_light_ls.size(); - tl_center.z += tl_point.z() / traffic_light_ls.size(); - } - return tl_center; -} -autoware_auto_perception_msgs::msg::LookingTrafficSignal initializeTrafficSignal( - const rclcpp::Time stamp) -{ - autoware_auto_perception_msgs::msg::LookingTrafficSignal state; - state.header.stamp = stamp; - state.is_module_running = true; - state.perception.has_state = false; - state.result.has_state = false; - return state; -} } // namespace TrafficLightModule::TrafficLightModule( @@ -197,7 +171,6 @@ TrafficLightModule::TrafficLightModule( traffic_light_reg_elem_(traffic_light_reg_elem), lane_(lane), state_(State::APPROACH), - input_(Input::PERCEPTION), is_prev_state_stop_(false) { velocity_factor_.init(VelocityFactor::TRAFFIC_SIGNAL); @@ -206,7 +179,6 @@ TrafficLightModule::TrafficLightModule( bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) { - looking_tl_state_ = initializeTrafficSignal(path->header.stamp); debug_data_ = DebugData(); debug_data_.base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; first_stop_path_point_index_ = static_cast(path->points.size()) - 1; @@ -217,9 +189,8 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * const auto & self_pose = planner_data_->current_odometry; - // Get lanelet2 traffic lights and stop lines. + // Get lanelet2 stop lines. lanelet::ConstLineString3d lanelet_stop_lines = *(traffic_light_reg_elem_.stopLine()); - lanelet::ConstLineStringsOrPolygons3d traffic_lights = traffic_light_reg_elem_.trafficLights(); // Calculate stop pose and insert index Eigen::Vector2d stop_line_point{}; @@ -255,7 +226,7 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * first_ref_stop_path_point_index_ = stop_line_point_idx; // Check if stop is coming. - setSafe(!isStopSignal(traffic_lights)); + setSafe(!isStopSignal()); if (isActivated()) { is_prev_state_stop_ = false; return true; @@ -283,33 +254,27 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * return false; } -bool TrafficLightModule::isStopSignal(const lanelet::ConstLineStringsOrPolygons3d & traffic_lights) +bool TrafficLightModule::isStopSignal() { - if (!updateTrafficSignal(traffic_lights)) { + if (!updateTrafficSignal()) { return false; } - return looking_tl_state_.result.judge == - autoware_auto_perception_msgs::msg::TrafficSignalWithJudge::STOP; + return isTrafficSignalStop(looking_tl_state_); } -bool TrafficLightModule::updateTrafficSignal( - const lanelet::ConstLineStringsOrPolygons3d & traffic_lights) +bool TrafficLightModule::updateTrafficSignal() { - autoware_auto_perception_msgs::msg::TrafficSignalStamped tl_state_perception; - bool found_perception = getHighestConfidenceTrafficSignal(traffic_lights, tl_state_perception); + TrafficSignal signal; + bool found_signal = findValidTrafficSignal(signal); - if (!found_perception) { + if (!found_signal) { // Don't stop when UNKNOWN or TIMEOUT as discussed at #508 - input_ = Input::NONE; return false; } - if (found_perception) { - looking_tl_state_.perception = generateTlStateWithJudgeFromTlState(tl_state_perception.signal); - looking_tl_state_.result = looking_tl_state_.perception; - input_ = Input::PERCEPTION; - } + // Found signal associated with the lanelet + looking_tl_state_ = signal; return true; } @@ -337,7 +302,7 @@ bool TrafficLightModule::isPassthrough(const double & signed_arc_length) const const auto & enable_pass_judge = planner_param_.enable_pass_judge; - if (enable_pass_judge && !stoppable && !is_prev_state_stop_ && input_ == Input::PERCEPTION) { + if (enable_pass_judge && !stoppable && !is_prev_state_stop_) { // Cannot stop under acceleration and jerk limits. // However, ego vehicle can't enter the intersection while the light is yellow. // It is called dilemma zone. Make a stop decision to be safe. @@ -359,10 +324,9 @@ bool TrafficLightModule::isPassthrough(const double & signed_arc_length) const } bool TrafficLightModule::isTrafficSignalStop( - const autoware_auto_perception_msgs::msg::TrafficSignal & tl_state) const + const autoware_perception_msgs::msg::TrafficSignal & tl_state) const { - if (hasTrafficLightCircleColor( - tl_state, autoware_auto_perception_msgs::msg::TrafficLight::GREEN)) { + if (hasTrafficLightCircleColor(tl_state, TrafficSignalElement::GREEN)) { return false; } @@ -373,86 +337,46 @@ bool TrafficLightModule::isTrafficSignalStop( } if ( turn_direction == "right" && - hasTrafficLightShape(tl_state, autoware_auto_perception_msgs::msg::TrafficLight::RIGHT_ARROW)) { + hasTrafficLightShape(tl_state, TrafficSignalElement::RIGHT_ARROW)) { return false; } if ( - turn_direction == "left" && - hasTrafficLightShape(tl_state, autoware_auto_perception_msgs::msg::TrafficLight::LEFT_ARROW)) { + turn_direction == "left" && hasTrafficLightShape(tl_state, TrafficSignalElement::LEFT_ARROW)) { return false; } if ( turn_direction == "straight" && - hasTrafficLightShape(tl_state, autoware_auto_perception_msgs::msg::TrafficLight::UP_ARROW)) { + hasTrafficLightShape(tl_state, TrafficSignalElement::UP_ARROW)) { return false; } return true; } -bool TrafficLightModule::getHighestConfidenceTrafficSignal( - const lanelet::ConstLineStringsOrPolygons3d & traffic_lights, - autoware_auto_perception_msgs::msg::TrafficSignalStamped & highest_confidence_tl_state) +bool TrafficLightModule::findValidTrafficSignal(TrafficSignal & valid_traffic_signal) { - // search traffic light state - bool found = false; - double highest_confidence = 0.0; - std::string reason; - for (const auto & traffic_light : traffic_lights) { - // traffic light must be linestrings - if (!traffic_light.isLineString()) { - reason = "NotLineString"; - continue; - } - - const int id = static_cast(traffic_light).id(); - RCLCPP_DEBUG(logger_, "traffic light id: %d (on route)", id); - const auto tl_state_stamped = planner_data_->getTrafficSignal(id); - if (!tl_state_stamped) { - reason = "TrafficSignalNotFound"; - continue; - } - - const auto header = tl_state_stamped->header; - const auto tl_state = tl_state_stamped->signal; - if (!((clock_->now() - header.stamp).seconds() < planner_param_.tl_state_timeout)) { - reason = "TimeOut"; - continue; - } - - if ( - tl_state.lights.empty() || - tl_state.lights.front().color == autoware_auto_perception_msgs::msg::TrafficLight::UNKNOWN) { - reason = "TrafficLightUnknown"; - continue; - } - - if (highest_confidence < tl_state.lights.front().confidence) { - highest_confidence = tl_state.lights.front().confidence; - highest_confidence_tl_state = *tl_state_stamped; - try { - auto tl_position = getTrafficLightPosition(traffic_light); - debug_data_.traffic_light_points.push_back(tl_position); - debug_data_.highest_confidence_traffic_light_point = std::make_optional(tl_position); - } catch (const std::invalid_argument & ex) { - RCLCPP_WARN_STREAM(logger_, ex.what()); - continue; - } - found = true; - } + // get traffic signal associated with the regulatory element id + const auto traffic_signal_stamped = planner_data_->getTrafficSignal(traffic_light_reg_elem_.id()); + if (!traffic_signal_stamped) { + RCLCPP_WARN_THROTTLE( + logger_, *clock_, 5000 /* ms */, + "the traffic signal data associated with regulatory element id is not received"); + return false; } - if (!found) { - std::string not_found_traffic_ids{""}; - for (size_t i = 0; i < traffic_lights.size(); ++i) { - const int id = static_cast(traffic_lights.at(i)).id(); - not_found_traffic_ids += (i != 0 ? "," : "") + std::to_string(id); - } + // check if the traffic signal data is outdated + const auto is_traffic_signal_timeout = + (clock_->now() - traffic_signal_stamped->stamp).seconds() > planner_param_.tl_state_timeout; + if (is_traffic_signal_timeout) { RCLCPP_WARN_THROTTLE( - logger_, *clock_, 5000 /* ms */, "cannot find traffic light lamp state (%s) due to %s.", - not_found_traffic_ids.c_str(), reason.c_str()); + logger_, *clock_, 5000 /* ms */, "the received traffic signal data is outdated"); + RCLCPP_WARN_STREAM_THROTTLE( + logger_, *clock_, 5000 /* ms */, + "time diff: " << (clock_->now() - traffic_signal_stamped->stamp).seconds()); return false; } + + valid_traffic_signal = traffic_signal_stamped->signal; return true; } @@ -496,40 +420,24 @@ autoware_auto_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopP } bool TrafficLightModule::hasTrafficLightCircleColor( - const autoware_auto_perception_msgs::msg::TrafficSignal & tl_state, - const uint8_t & lamp_color) const + const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_color) const { - using autoware_auto_perception_msgs::msg::TrafficLight; - const auto it_lamp = - std::find_if(tl_state.lights.begin(), tl_state.lights.end(), [&lamp_color](const auto & x) { - return x.shape == TrafficLight::CIRCLE && x.color == lamp_color; + std::find_if(tl_state.elements.begin(), tl_state.elements.end(), [&lamp_color](const auto & x) { + return x.shape == TrafficSignalElement::CIRCLE && x.color == lamp_color; }); - return it_lamp != tl_state.lights.end(); + return it_lamp != tl_state.elements.end(); } bool TrafficLightModule::hasTrafficLightShape( - const autoware_auto_perception_msgs::msg::TrafficSignal & tl_state, - const uint8_t & lamp_shape) const + const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_shape) const { const auto it_lamp = std::find_if( - tl_state.lights.begin(), tl_state.lights.end(), + tl_state.elements.begin(), tl_state.elements.end(), [&lamp_shape](const auto & x) { return x.shape == lamp_shape; }); - return it_lamp != tl_state.lights.end(); + return it_lamp != tl_state.elements.end(); } -autoware_auto_perception_msgs::msg::TrafficSignalWithJudge -TrafficLightModule::generateTlStateWithJudgeFromTlState( - const autoware_auto_perception_msgs::msg::TrafficSignal tl_state) const -{ - autoware_auto_perception_msgs::msg::TrafficSignalWithJudge tl_state_with_judge; - tl_state_with_judge.signal = tl_state; - tl_state_with_judge.has_state = true; - tl_state_with_judge.judge = isTrafficSignalStop(tl_state) - ? autoware_auto_perception_msgs::msg::TrafficSignalWithJudge::STOP - : autoware_auto_perception_msgs::msg::TrafficSignalWithJudge::GO; - return tl_state_with_judge; -} } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_traffic_light_module/src/scene.hpp index 99ece6cbca09d..a24db2c440e91 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.hpp @@ -29,8 +29,6 @@ #include #include -#include - #include #include @@ -39,15 +37,15 @@ namespace behavior_velocity_planner class TrafficLightModule : public SceneModuleInterface { public: + using TrafficSignal = autoware_perception_msgs::msg::TrafficSignal; + using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; enum class State { APPROACH, GO_OUT }; - enum class Input { PERCEPTION, NONE }; // EXTERNAL: FOA, V2X, etc. struct DebugData { double base_link2front; std::vector, - autoware_auto_perception_msgs::msg::TrafficSignal>> + std::shared_ptr, autoware_perception_msgs::msg::TrafficSignal>> tl_state; std::vector stop_poses; geometry_msgs::msg::Pose first_stop_pose; @@ -77,10 +75,7 @@ class TrafficLightModule : public SceneModuleInterface visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; motion_utils::VirtualWalls createVirtualWalls() override; - inline autoware_auto_perception_msgs::msg::LookingTrafficSignal getTrafficSignal() const - { - return looking_tl_state_; - } + inline TrafficSignal getTrafficSignal() const { return looking_tl_state_; } inline State getTrafficLightModuleState() const { return state_; } @@ -90,10 +85,9 @@ class TrafficLightModule : public SceneModuleInterface } private: - bool isStopSignal(const lanelet::ConstLineStringsOrPolygons3d & traffic_lights); + bool isStopSignal(); - bool isTrafficSignalStop( - const autoware_auto_perception_msgs::msg::TrafficSignal & tl_state) const; + bool isTrafficSignalStop(const TrafficSignal & tl_state) const; autoware_auto_planning_msgs::msg::PathWithLaneId insertStopPose( const autoware_auto_planning_msgs::msg::PathWithLaneId & input, @@ -102,22 +96,13 @@ class TrafficLightModule : public SceneModuleInterface bool isPassthrough(const double & signed_arc_length) const; - bool hasTrafficLightCircleColor( - const autoware_auto_perception_msgs::msg::TrafficSignal & tl_state, - const uint8_t & lamp_color) const; + bool hasTrafficLightCircleColor(const TrafficSignal & tl_state, const uint8_t & lamp_color) const; - bool hasTrafficLightShape( - const autoware_auto_perception_msgs::msg::TrafficSignal & tl_state, - const uint8_t & lamp_shape) const; + bool hasTrafficLightShape(const TrafficSignal & tl_state, const uint8_t & lamp_shape) const; - bool getHighestConfidenceTrafficSignal( - const lanelet::ConstLineStringsOrPolygons3d & traffic_lights, - autoware_auto_perception_msgs::msg::TrafficSignalStamped & highest_confidence_tl_state); + bool findValidTrafficSignal(TrafficSignal & valid_traffic_signal); - bool updateTrafficSignal(const lanelet::ConstLineStringsOrPolygons3d & traffic_lights); - - autoware_auto_perception_msgs::msg::TrafficSignalWithJudge generateTlStateWithJudgeFromTlState( - const autoware_auto_perception_msgs::msg::TrafficSignal tl_state) const; + bool updateTrafficSignal(); // Lane id const int64_t lane_id_; @@ -129,9 +114,6 @@ class TrafficLightModule : public SceneModuleInterface // State State state_; - // Input - Input input_; - // Parameter PlannerParam planner_param_; @@ -144,7 +126,7 @@ class TrafficLightModule : public SceneModuleInterface boost::optional first_ref_stop_path_point_index_; // Traffic Light State - autoware_auto_perception_msgs::msg::LookingTrafficSignal looking_tl_state_; + TrafficSignal looking_tl_state_; }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_virtual_traffic_light_module/src/debug.cpp b/planning/behavior_velocity_virtual_traffic_light_module/src/debug.cpp index 1324525ebc6e1..e5cb4dcbc42e7 100644 --- a/planning/behavior_velocity_virtual_traffic_light_module/src/debug.cpp +++ b/planning/behavior_velocity_virtual_traffic_light_module/src/debug.cpp @@ -14,8 +14,9 @@ #include "scene.hpp" -#include -#include +#include +#include +#include using motion_utils::createStopVirtualWallMarker; using tier4_autoware_utils::appendMarkerArray; diff --git a/planning/behavior_velocity_walkway_module/src/manager.cpp b/planning/behavior_velocity_walkway_module/src/manager.cpp index d10ff52850752..f9c222f662e82 100644 --- a/planning/behavior_velocity_walkway_module/src/manager.cpp +++ b/planning/behavior_velocity_walkway_module/src/manager.cpp @@ -44,11 +44,8 @@ WalkwayModuleManager::WalkwayModuleManager(rclcpp::Node & node) void WalkwayModuleManager::launchNewModules(const PathWithLaneId & path) { const auto rh = planner_data_->route_handler_; - if (!opt_use_regulatory_element_) { - opt_use_regulatory_element_ = checkRegulatoryElementExistence(rh->getLaneletMapPtr()); - } - const auto launch = [this, &path](const auto & lanelet) { + const auto launch = [this, &path](const auto & lanelet, const auto use_regulatory_element) { const auto attribute = lanelet.attributeOr(lanelet::AttributeNamesString::Subtype, std::string("")); if (attribute != lanelet::AttributeValueString::Walkway) { @@ -64,24 +61,21 @@ void WalkwayModuleManager::launchNewModules(const PathWithLaneId & path) const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); registerModule(std::make_shared( - lanelet.id(), lanelet_map_ptr, p, *opt_use_regulatory_element_, logger, clock_)); + lanelet.id(), lanelet_map_ptr, p, use_regulatory_element, logger, clock_)); }; - if (*opt_use_regulatory_element_) { - const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( - path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); + const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( + path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); - for (const auto & crosswalk : crosswalk_leg_elem_map) { - launch(crosswalk.first->crosswalkLanelet()); - } - } else { - const auto crosswalk_lanelets = getCrosswalksOnPath( - planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), - rh->getOverallGraphPtr()); + for (const auto & crosswalk : crosswalk_leg_elem_map) { + launch(crosswalk.first->crosswalkLanelet(), true); + } - for (const auto & crosswalk : crosswalk_lanelets) { - launch(crosswalk); - } + const auto crosswalk_lanelets = getCrosswalksOnPath( + planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr()); + + for (const auto & crosswalk : crosswalk_lanelets) { + launch(crosswalk, false); } } @@ -92,17 +86,14 @@ WalkwayModuleManager::getModuleExpiredFunction(const PathWithLaneId & path) std::set walkway_id_set; - if (*opt_use_regulatory_element_) { - const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( - path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); + walkway_id_set = getCrosswalkIdSetOnPath( + planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr()); - for (const auto & crosswalk : crosswalk_leg_elem_map) { - walkway_id_set.insert(crosswalk.first->id()); - } - } else { - walkway_id_set = getCrosswalkIdSetOnPath( - planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), - rh->getOverallGraphPtr()); + const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( + path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); + + for (const auto & crosswalk : crosswalk_leg_elem_map) { + walkway_id_set.insert(crosswalk.first->id()); } return [walkway_id_set](const std::shared_ptr & scene_module) { diff --git a/planning/freespace_planner/package.xml b/planning/freespace_planner/package.xml index a748b7f80c9ad..f92c0a449970d 100644 --- a/planning/freespace_planner/package.xml +++ b/planning/freespace_planner/package.xml @@ -19,6 +19,7 @@ autoware_auto_planning_msgs freespace_planning_algorithms geometry_msgs + motion_utils nav_msgs planning_test_utils rclcpp diff --git a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp index e0d7f86e45245..025cc32d4ed24 100644 --- a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp +++ b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp @@ -32,7 +32,8 @@ #include "freespace_planning_algorithms/abstract_algorithm.hpp" -#include +#include +#include #include #include diff --git a/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp b/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp index 159c0df82fcc3..2c7340072eb53 100644 --- a/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp +++ b/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp @@ -14,7 +14,8 @@ #include "freespace_planning_algorithms/abstract_algorithm.hpp" -#include +#include +#include #include diff --git a/planning/freespace_planning_algorithms/src/astar_search.cpp b/planning/freespace_planning_algorithms/src/astar_search.cpp index b2b2321adacd0..1ab5e9d5df487 100644 --- a/planning/freespace_planning_algorithms/src/astar_search.cpp +++ b/planning/freespace_planning_algorithms/src/astar_search.cpp @@ -14,7 +14,8 @@ #include "freespace_planning_algorithms/astar_search.hpp" -#include +#include +#include #include diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp index d8289824e517c..979800aea0754 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -20,7 +20,11 @@ #include #include #include -#include +#include +#include +#include +#include +#include #include #include diff --git a/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp b/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp index 0bfb0553e75ef..3ea6237f38501 100644 --- a/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp +++ b/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp @@ -15,8 +15,6 @@ #ifndef LANELET2_PLUGINS__UTILITY_FUNCTIONS_HPP_ #define LANELET2_PLUGINS__UTILITY_FUNCTIONS_HPP_ -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" - #include #include #include diff --git a/planning/mission_planner/src/mission_planner/arrival_checker.cpp b/planning/mission_planner/src/mission_planner/arrival_checker.cpp index ac7a37acd4d5b..1e9f02ff0338d 100644 --- a/planning/mission_planner/src/mission_planner/arrival_checker.cpp +++ b/planning/mission_planner/src/mission_planner/arrival_checker.cpp @@ -14,7 +14,9 @@ #include "arrival_checker.hpp" -#include +#include +#include +#include #include diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp index 1b8a40ef8ef02..d0fc995da0ef4 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp @@ -17,7 +17,8 @@ #include "obstacle_avoidance_planner/type_alias.hpp" #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" +#include "tier4_autoware_utils/system/stop_watch.hpp" #include #include diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp index ea2e18b099f1d..b87cfc7a1e743 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp @@ -22,7 +22,7 @@ #include "obstacle_avoidance_planner/state_equation_generator.hpp" #include "obstacle_avoidance_planner/type_alias.hpp" #include "osqp_interface/osqp_interface.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp index f716f497b90da..14925a8facf88 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp @@ -15,13 +15,12 @@ #ifndef OBSTACLE_AVOIDANCE_PLANNER__NODE_HPP_ #define OBSTACLE_AVOIDANCE_PLANNER__NODE_HPP_ -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "obstacle_avoidance_planner/common_structs.hpp" #include "obstacle_avoidance_planner/mpt_optimizer.hpp" #include "obstacle_avoidance_planner/replan_checker.hpp" #include "obstacle_avoidance_planner/type_alias.hpp" #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include diff --git a/planning/obstacle_avoidance_planner/src/debug_marker.cpp b/planning/obstacle_avoidance_planner/src/debug_marker.cpp index 79f3dfd2fe2e4..8bf46c697104a 100644 --- a/planning/obstacle_avoidance_planner/src/debug_marker.cpp +++ b/planning/obstacle_avoidance_planner/src/debug_marker.cpp @@ -14,7 +14,6 @@ #include "obstacle_avoidance_planner/debug_marker.hpp" -#include "motion_utils/motion_utils.hpp" #include "obstacle_avoidance_planner/mpt_optimizer.hpp" #include "visualization_msgs/msg/marker_array.hpp" diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index 0a55cc8d91f8e..359485fadcfff 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -15,11 +15,12 @@ #include "obstacle_avoidance_planner/mpt_optimizer.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "obstacle_avoidance_planner/utils/geometry_utils.hpp" #include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" #include "tf2/utils.h" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/math/normalization.hpp" #include #include diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index 6c4730e9d86f9..b5dd0e91a541e 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -15,6 +15,7 @@ #include "obstacle_avoidance_planner/node.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" +#include "motion_utils/marker/marker_helper.hpp" #include "obstacle_avoidance_planner/debug_marker.hpp" #include "obstacle_avoidance_planner/utils/geometry_utils.hpp" #include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" diff --git a/planning/obstacle_avoidance_planner/src/replan_checker.cpp b/planning/obstacle_avoidance_planner/src/replan_checker.cpp index 8d281ced9a5ac..6d7c6e702471a 100644 --- a/planning/obstacle_avoidance_planner/src/replan_checker.cpp +++ b/planning/obstacle_avoidance_planner/src/replan_checker.cpp @@ -14,9 +14,10 @@ #include "obstacle_avoidance_planner/replan_checker.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" #include diff --git a/planning/obstacle_avoidance_planner/src/utils/geometry_utils.cpp b/planning/obstacle_avoidance_planner/src/utils/geometry_utils.cpp index 67b500571fa06..3f35de9147e6a 100644 --- a/planning/obstacle_avoidance_planner/src/utils/geometry_utils.cpp +++ b/planning/obstacle_avoidance_planner/src/utils/geometry_utils.cpp @@ -14,15 +14,21 @@ #include "obstacle_avoidance_planner/utils/geometry_utils.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "obstacle_avoidance_planner/mpt_optimizer.hpp" #include "tf2/utils.h" +#include + #include "autoware_auto_planning_msgs/msg/path_point.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/point32.hpp" #include "geometry_msgs/msg/pose.hpp" +#include +#include +#include + #include #include #include diff --git a/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp b/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp index 8390cf379586b..e86782c9cb0cf 100644 --- a/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp +++ b/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp @@ -14,7 +14,9 @@ #include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/resample/resample.hpp" +#include "motion_utils/trajectory/tmp_conversion.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "obstacle_avoidance_planner/mpt_optimizer.hpp" #include "obstacle_avoidance_planner/utils/geometry_utils.hpp" #include "tf2/utils.h" diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml index 08ebea4284bf1..f3f1932c44c43 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -20,7 +20,11 @@ nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index min_behavior_stop_margin: 3.0 # [m] - suppress_sudden_obstacle_stop: false + stop_on_curve: + enable_approaching: true # false + additional_safe_distance_margin: 0.0 # [m] + min_safe_distance_margin: 3.0 # [m] + suppress_sudden_obstacle_stop: true stop_obstacle_type: unknown: true @@ -54,7 +58,7 @@ pedestrian: false slow_down_obstacle_type: - unknown: true + unknown: false car: true truck: true bus: true diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp index 12ebadf770996..e958074971f2a 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp @@ -15,9 +15,13 @@ #ifndef OBSTACLE_CRUISE_PLANNER__COMMON_STRUCTS_HPP_ #define OBSTACLE_CRUISE_PLANNER__COMMON_STRUCTS_HPP_ -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/interpolation.hpp" +#include "motion_utils/trajectory/tmp_conversion.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "obstacle_cruise_planner/type_alias.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" +#include "tier4_autoware_utils/ros/uuid_helper.hpp" #include @@ -104,12 +108,15 @@ struct StopObstacle : public TargetObstacleInterface { StopObstacle( const std::string & arg_uuid, const rclcpp::Time & arg_stamp, - const geometry_msgs::msg::Pose & arg_pose, const double arg_lon_velocity, - const double arg_lat_velocity, const geometry_msgs::msg::Point arg_collision_point) + const geometry_msgs::msg::Pose & arg_pose, const Shape & arg_shape, + const double arg_lon_velocity, const double arg_lat_velocity, + const geometry_msgs::msg::Point arg_collision_point) : TargetObstacleInterface(arg_uuid, arg_stamp, arg_pose, arg_lon_velocity, arg_lat_velocity), + shape(arg_shape), collision_point(arg_collision_point) { } + Shape shape; geometry_msgs::msg::Point collision_point; }; diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp index ac6684d163aea..00afc11985d72 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -100,6 +100,9 @@ class ObstacleCruisePlannerNode : public rclcpp::Node bool enable_debug_info_; bool enable_calculation_time_info_; double min_behavior_stop_margin_; + bool enable_approaching_on_curve_; + double additional_safe_distance_margin_on_curve_; + double min_safe_distance_margin_on_curve_; bool suppress_sudden_obstacle_stop_; std::vector stop_obstacle_types_; diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp index 55a97ccc2785d..44626206ec038 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp @@ -19,7 +19,6 @@ #include "obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp" #include "obstacle_cruise_planner/planner_interface.hpp" #include "obstacle_cruise_planner/type_alias.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp index c3fa364da269e..9a5a6521a6494 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp @@ -15,12 +15,13 @@ #ifndef OBSTACLE_CRUISE_PLANNER__PLANNER_INTERFACE_HPP_ #define OBSTACLE_CRUISE_PLANNER__PLANNER_INTERFACE_HPP_ -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "obstacle_cruise_planner/common_structs.hpp" #include "obstacle_cruise_planner/stop_planning_debug_info.hpp" #include "obstacle_cruise_planner/type_alias.hpp" #include "obstacle_cruise_planner/utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" +#include "tier4_autoware_utils/system/stop_watch.hpp" #include #include @@ -53,11 +54,16 @@ class PlannerInterface void setParam( const bool enable_debug_info, const bool enable_calculation_time_info, - const double min_behavior_stop_margin, const bool suppress_sudden_obstacle_stop) + const double min_behavior_stop_margin, const double enable_approaching_on_curve, + const double additional_safe_distance_margin_on_curve, + const double min_safe_distance_margin_on_curve, const bool suppress_sudden_obstacle_stop) { enable_debug_info_ = enable_debug_info; enable_calculation_time_info_ = enable_calculation_time_info; min_behavior_stop_margin_ = min_behavior_stop_margin; + enable_approaching_on_curve_ = enable_approaching_on_curve; + additional_safe_distance_margin_on_curve_ = additional_safe_distance_margin_on_curve; + min_safe_distance_margin_on_curve_ = min_safe_distance_margin_on_curve; suppress_sudden_obstacle_stop_ = suppress_sudden_obstacle_stop; } @@ -102,6 +108,9 @@ class PlannerInterface bool enable_calculation_time_info_{false}; LongitudinalInfo longitudinal_info_; double min_behavior_stop_margin_; + bool enable_approaching_on_curve_; + double additional_safe_distance_margin_on_curve_; + double min_safe_distance_margin_on_curve_; bool suppress_sudden_obstacle_stop_; // stop watch @@ -194,6 +203,8 @@ class PlannerInterface std::optional start_point{std::nullopt}; std::optional end_point{std::nullopt}; }; + double calculateMarginFromObstacleOnCurve( + const PlannerData & planner_data, const StopObstacle & stop_obstacle) const; double calculateSlowDownVelocity( const SlowDownObstacle & obstacle, const std::optional & prev_output) const; std::optional> calculateDistanceToSlowDownWithConstraints( diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp index fdbf5b413d978..5178597bc4588 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp @@ -17,7 +17,7 @@ #include "obstacle_cruise_planner/common_structs.hpp" #include "obstacle_cruise_planner/type_alias.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/boost_geometry.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp index ed6d0f52691e3..c635b400ed104 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp @@ -16,9 +16,8 @@ #define OBSTACLE_CRUISE_PLANNER__UTILS_HPP_ #include "common_structs.hpp" -#include "motion_utils/motion_utils.hpp" #include "obstacle_cruise_planner/type_alias.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 266e184a06a08..14324709ba9e1 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -20,6 +20,7 @@ #include "obstacle_cruise_planner/polygon_utils.hpp" #include "obstacle_cruise_planner/utils.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" +#include "tier4_autoware_utils/ros/marker_helper.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" #include @@ -397,11 +398,18 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & } min_behavior_stop_margin_ = declare_parameter("common.min_behavior_stop_margin"); + additional_safe_distance_margin_on_curve_ = + declare_parameter("common.stop_on_curve.additional_safe_distance_margin"); + enable_approaching_on_curve_ = + declare_parameter("common.stop_on_curve.enable_approaching"); + min_safe_distance_margin_on_curve_ = + declare_parameter("common.stop_on_curve.min_safe_distance_margin"); suppress_sudden_obstacle_stop_ = declare_parameter("common.suppress_sudden_obstacle_stop"); planner_ptr_->setParam( enable_debug_info_, enable_calculation_time_info_, min_behavior_stop_margin_, - suppress_sudden_obstacle_stop_); + enable_approaching_on_curve_, additional_safe_distance_margin_on_curve_, + min_safe_distance_margin_on_curve_, suppress_sudden_obstacle_stop_); } { // stop/cruise/slow down obstacle type @@ -438,9 +446,20 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam( parameters, "common.enable_debug_info", enable_debug_info_); tier4_autoware_utils::updateParam( parameters, "common.enable_calculation_time_info", enable_calculation_time_info_); + + tier4_autoware_utils::updateParam( + parameters, "common.stop_on_curve.enable_approaching", enable_approaching_on_curve_); + tier4_autoware_utils::updateParam( + parameters, "common.stop_on_curve.additional_safe_distance_margin", + additional_safe_distance_margin_on_curve_); + tier4_autoware_utils::updateParam( + parameters, "common.stop_on_curve.min_safe_distance_margin", + min_safe_distance_margin_on_curve_); + planner_ptr_->setParam( enable_debug_info_, enable_calculation_time_info_, min_behavior_stop_margin_, - suppress_sudden_obstacle_stop_); + enable_approaching_on_curve_, additional_safe_distance_margin_on_curve_, + min_safe_distance_margin_on_curve_, suppress_sudden_obstacle_stop_); tier4_autoware_utils::updateParam( parameters, "common.enable_slow_down_planning", enable_slow_down_planning_); @@ -911,7 +930,7 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( } const auto [tangent_vel, normal_vel] = projectObstacleVelocityToTrajectory(traj_points, obstacle); - return StopObstacle{obstacle.uuid, obstacle.stamp, obstacle.pose, + return StopObstacle{obstacle.uuid, obstacle.stamp, obstacle.pose, obstacle.shape, tangent_vel, normal_vel, *collision_point}; } diff --git a/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp b/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp index f420993c1adac..08f893a0a9a50 100644 --- a/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp +++ b/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp @@ -17,11 +17,13 @@ #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" #include "interpolation/zero_order_hold.hpp" +#include "motion_utils/marker/marker_helper.hpp" #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/interpolation.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "obstacle_cruise_planner/utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/ros/marker_helper.hpp" #ifdef ROS_DISTRO_GALACTIC #include diff --git a/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index ffd2f97b1eb89..005a85d9b872d 100644 --- a/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp +++ b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp @@ -15,8 +15,11 @@ #include "obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp" #include "interpolation/spline_interpolation.hpp" +#include "motion_utils/marker/marker_helper.hpp" #include "obstacle_cruise_planner/utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/ros/marker_helper.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" #include "tier4_planning_msgs/msg/velocity_limit.hpp" diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index 03cae6e6f9d88..5ea637655a01a 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -15,7 +15,12 @@ #include "obstacle_cruise_planner/planner_interface.hpp" #include "motion_utils/distance/distance.hpp" +#include "motion_utils/marker/marker_helper.hpp" +#include "motion_utils/resample/resample.hpp" +#include "motion_utils/trajectory/tmp_conversion.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "signal_processing/lowpass_filter_1d.hpp" +#include "tier4_autoware_utils/ros/marker_helper.hpp" namespace { @@ -204,6 +209,19 @@ double calcDecelerationVelocityFromDistanceToTarget( } return current_velocity; } + +std::vector resampleTrajectoryPoints( + const std::vector & traj_points, const double interval) +{ + const auto traj = motion_utils::convertToTrajectory(traj_points); + const auto resampled_traj = motion_utils::resampleTrajectory(traj, interval); + return motion_utils::convertToTrajectoryPointArray(resampled_traj); +} + +tier4_autoware_utils::Point2d convertPoint(const geometry_msgs::msg::Point & p) +{ + return tier4_autoware_utils::Point2d{p.x, p.y}; +} } // namespace std::vector PlannerInterface::generateStopTrajectory( @@ -259,16 +277,19 @@ std::vector PlannerInterface::generateStopTrajectory( planner_data.traj_points, planner_data.ego_pose.position, ego_segment_idx, 0); const double dist_to_ego = -negative_dist_to_ego; + const double margin_from_obstacle = + calculateMarginFromObstacleOnCurve(planner_data, *closest_stop_obstacle); + // If behavior stop point is ahead of the closest_obstacle_stop point within a certain margin // we set closest_obstacle_stop_distance to closest_behavior_stop_distance - const double margin_from_obstacle = [&]() { + const double margin_from_obstacle_considering_behavior_module = [&]() { const size_t nearest_segment_idx = findEgoSegmentIndex(planner_data.traj_points, planner_data.ego_pose); const auto closest_behavior_stop_idx = motion_utils::searchZeroVelocityIndex(planner_data.traj_points, nearest_segment_idx + 1); if (!closest_behavior_stop_idx) { - return longitudinal_info_.safe_distance_margin; + return margin_from_obstacle; } const double closest_behavior_stop_dist_from_ego = motion_utils::calcSignedArcLength( @@ -282,29 +303,28 @@ std::vector PlannerInterface::generateStopTrajectory( abs_ego_offset; const double stop_dist_diff = closest_behavior_stop_dist_from_ego - closest_obstacle_stop_dist_from_ego; - if (stop_dist_diff < longitudinal_info_.safe_distance_margin) { + if (stop_dist_diff < margin_from_obstacle) { // Use terminal margin (terminal_safe_distance_margin) for obstacle stop return longitudinal_info_.terminal_safe_distance_margin; } } else { - const double closest_obstacle_stop_dist_from_ego = closest_obstacle_dist - dist_to_ego - - longitudinal_info_.safe_distance_margin - - abs_ego_offset; + const double closest_obstacle_stop_dist_from_ego = + closest_obstacle_dist - dist_to_ego - margin_from_obstacle - abs_ego_offset; const double stop_dist_diff = closest_behavior_stop_dist_from_ego - closest_obstacle_stop_dist_from_ego; - if (0.0 < stop_dist_diff && stop_dist_diff < longitudinal_info_.safe_distance_margin) { + if (0.0 < stop_dist_diff && stop_dist_diff < margin_from_obstacle) { // Use shorter margin (min_behavior_stop_margin) for obstacle stop return min_behavior_stop_margin_; } } - return longitudinal_info_.safe_distance_margin; + return margin_from_obstacle; }(); const auto [stop_margin_from_obstacle, will_collide_with_obstacle] = [&]() { // Check feasibility to stop if (suppress_sudden_obstacle_stop_) { const double closest_obstacle_stop_dist = - closest_obstacle_dist - margin_from_obstacle - abs_ego_offset; + closest_obstacle_dist - margin_from_obstacle_considering_behavior_module - abs_ego_offset; // Calculate feasible stop margin (Check the feasibility) const double feasible_stop_dist = calcMinimumDistanceToStop( @@ -314,11 +334,12 @@ std::vector PlannerInterface::generateStopTrajectory( if (closest_obstacle_stop_dist < feasible_stop_dist) { const auto feasible_margin_from_obstacle = - margin_from_obstacle - (feasible_stop_dist - closest_obstacle_stop_dist); + margin_from_obstacle_considering_behavior_module - + (feasible_stop_dist - closest_obstacle_stop_dist); return std::make_pair(feasible_margin_from_obstacle, true); } } - return std::make_pair(margin_from_obstacle, false); + return std::make_pair(margin_from_obstacle_considering_behavior_module, false); }(); // Generate Output Trajectory @@ -395,6 +416,91 @@ std::vector PlannerInterface::generateStopTrajectory( return output_traj_points; } +double PlannerInterface::calculateMarginFromObstacleOnCurve( + const PlannerData & planner_data, const StopObstacle & stop_obstacle) const +{ + if (!enable_approaching_on_curve_) { + return longitudinal_info_.safe_distance_margin; + } + + const double abs_ego_offset = planner_data.is_driving_forward + ? std::abs(vehicle_info_.max_longitudinal_offset_m) + : std::abs(vehicle_info_.min_longitudinal_offset_m); + + // calculate short trajectory points towards obstacle + const size_t obj_segment_idx = + motion_utils::findNearestSegmentIndex(planner_data.traj_points, stop_obstacle.collision_point); + std::vector short_traj_points{planner_data.traj_points.at(obj_segment_idx + 1)}; + double sum_short_traj_length{0.0}; + for (int i = obj_segment_idx; 0 <= i; --i) { + short_traj_points.push_back(planner_data.traj_points.at(i)); + + if ( + 1 < short_traj_points.size() && + longitudinal_info_.safe_distance_margin + abs_ego_offset < sum_short_traj_length) { + break; + } + sum_short_traj_length += tier4_autoware_utils::calcDistance2d( + planner_data.traj_points.at(i), planner_data.traj_points.at(i + 1)); + } + std::reverse(short_traj_points.begin(), short_traj_points.end()); + if (short_traj_points.size() < 2) { + return longitudinal_info_.safe_distance_margin; + } + + // calculate collision index between straight line from ego pose and object + const auto calculate_distance_from_straight_ego_path = + [&](const auto & ego_pose, const auto & object_polygon) { + const auto forward_ego_pose = tier4_autoware_utils::calcOffsetPose( + ego_pose, longitudinal_info_.safe_distance_margin + 3.0, 0.0, 0.0); + const auto ego_straight_segment = tier4_autoware_utils::Segment2d{ + convertPoint(ego_pose.position), convertPoint(forward_ego_pose.position)}; + return boost::geometry::distance(ego_straight_segment, object_polygon); + }; + const auto resampled_short_traj_points = resampleTrajectoryPoints(short_traj_points, 0.5); + const auto object_polygon = + tier4_autoware_utils::toPolygon2d(stop_obstacle.pose, stop_obstacle.shape); + const auto collision_idx = [&]() -> std::optional { + for (size_t i = 0; i < resampled_short_traj_points.size(); ++i) { + const double dist_to_obj = calculate_distance_from_straight_ego_path( + resampled_short_traj_points.at(i).pose, object_polygon); + if (dist_to_obj < vehicle_info_.vehicle_width_m / 2.0) { + return i; + } + } + return std::nullopt; + }(); + if (!collision_idx) { + return min_safe_distance_margin_on_curve_; + } + if (*collision_idx == 0) { + return longitudinal_info_.safe_distance_margin; + } + + // calculate margin from obstacle + const double partial_segment_length = [&]() { + const double collision_segment_length = tier4_autoware_utils::calcDistance2d( + resampled_short_traj_points.at(*collision_idx - 1), + resampled_short_traj_points.at(*collision_idx)); + const double prev_dist = calculate_distance_from_straight_ego_path( + resampled_short_traj_points.at(*collision_idx - 1).pose, object_polygon); + const double next_dist = calculate_distance_from_straight_ego_path( + resampled_short_traj_points.at(*collision_idx).pose, object_polygon); + return (next_dist - vehicle_info_.vehicle_width_m / 2.0) / (next_dist - prev_dist) * + collision_segment_length; + }(); + + const double short_margin_from_obstacle = + partial_segment_length + + motion_utils::calcSignedArcLength( + resampled_short_traj_points, *collision_idx, stop_obstacle.collision_point) - + abs_ego_offset + additional_safe_distance_margin_on_curve_; + + return std::min( + longitudinal_info_.safe_distance_margin, + std::max(min_safe_distance_margin_on_curve_, short_margin_from_obstacle)); +} + double PlannerInterface::calcDistanceToCollisionPoint( const PlannerData & planner_data, const geometry_msgs::msg::Point & collision_point) { diff --git a/planning/obstacle_cruise_planner/src/utils.cpp b/planning/obstacle_cruise_planner/src/utils.cpp index 091adfab586b4..42bac55e63dd8 100644 --- a/planning/obstacle_cruise_planner/src/utils.cpp +++ b/planning/obstacle_cruise_planner/src/utils.cpp @@ -15,6 +15,7 @@ #include "obstacle_cruise_planner/utils.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" +#include "tier4_autoware_utils/ros/marker_helper.hpp" namespace obstacle_cruise_utils { diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp index 40a3f36bff903..9d95c5a4796d4 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp @@ -16,7 +16,6 @@ #define OBSTACLE_STOP_PLANNER__ADAPTIVE_CRUISE_CONTROL_HPP_ #include -#include #include #include diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp index c836b380e5cf2..4cb71c710a225 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -23,8 +23,7 @@ #include #include #include -#include -#include +#include #include #include diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp index 8023e86583885..7e7e659325abf 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp @@ -18,7 +18,7 @@ #include "obstacle_stop_planner/planner_data.hpp" -#include +#include #include #include @@ -46,7 +46,6 @@ using diagnostic_msgs::msg::KeyValue; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseArray; -using geometry_msgs::msg::TransformStamped; using std_msgs::msg::Header; using autoware_auto_planning_msgs::msg::TrajectoryPoint; diff --git a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp index fb1e93185f738..8064aa4764a6d 100644 --- a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp +++ b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp @@ -28,6 +28,8 @@ #include #endif +#include + #include #include #include diff --git a/planning/obstacle_stop_planner/src/debug_marker.cpp b/planning/obstacle_stop_planner/src/debug_marker.cpp index 15b0a721ed170..9277d373d498d 100644 --- a/planning/obstacle_stop_planner/src/debug_marker.cpp +++ b/planning/obstacle_stop_planner/src/debug_marker.cpp @@ -14,8 +14,9 @@ #include "obstacle_stop_planner/debug_marker.hpp" -#include -#include +#include +#include +#include #ifdef ROS_DISTRO_GALACTIC #include diff --git a/planning/obstacle_stop_planner/src/planner_utils.cpp b/planning/obstacle_stop_planner/src/planner_utils.cpp index 45cced4599602..21e45cac79fdf 100644 --- a/planning/obstacle_stop_planner/src/planner_utils.cpp +++ b/planning/obstacle_stop_planner/src/planner_utils.cpp @@ -17,10 +17,13 @@ #include #include #include +#include #include #include +#include +#include #include diff --git a/planning/path_smoother/include/path_smoother/common_structs.hpp b/planning/path_smoother/include/path_smoother/common_structs.hpp index 94f4d62c4a6fd..a14c42ed056af 100644 --- a/planning/path_smoother/include/path_smoother/common_structs.hpp +++ b/planning/path_smoother/include/path_smoother/common_structs.hpp @@ -17,7 +17,8 @@ #include "path_smoother/type_alias.hpp" #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" +#include "tier4_autoware_utils/system/stop_watch.hpp" #include #include diff --git a/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp b/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp index 1085ca3815728..8bd73292a56de 100644 --- a/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp +++ b/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp @@ -15,13 +15,12 @@ #ifndef PATH_SMOOTHER__ELASTIC_BAND_SMOOTHER_HPP_ #define PATH_SMOOTHER__ELASTIC_BAND_SMOOTHER_HPP_ -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "path_smoother/common_structs.hpp" #include "path_smoother/elastic_band.hpp" #include "path_smoother/replan_checker.hpp" #include "path_smoother/type_alias.hpp" #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include #include diff --git a/planning/path_smoother/src/elastic_band.cpp b/planning/path_smoother/src/elastic_band.cpp index d77ad542e2e7e..fb3aff7b18419 100644 --- a/planning/path_smoother/src/elastic_band.cpp +++ b/planning/path_smoother/src/elastic_band.cpp @@ -14,7 +14,7 @@ #include "path_smoother/elastic_band.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "path_smoother/type_alias.hpp" #include "path_smoother/utils/geometry_utils.hpp" #include "path_smoother/utils/trajectory_utils.hpp" diff --git a/planning/path_smoother/src/utils/trajectory_utils.cpp b/planning/path_smoother/src/utils/trajectory_utils.cpp index 56432d16d1a7f..d9d02a15628dd 100644 --- a/planning/path_smoother/src/utils/trajectory_utils.cpp +++ b/planning/path_smoother/src/utils/trajectory_utils.cpp @@ -14,7 +14,8 @@ #include "path_smoother/utils/trajectory_utils.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/resample/resample.hpp" +#include "motion_utils/trajectory/tmp_conversion.hpp" #include "path_smoother/utils/geometry_utils.hpp" #include "autoware_auto_planning_msgs/msg/path_point.hpp" diff --git a/planning/planning_debug_tools/Readme.md b/planning/planning_debug_tools/README.md similarity index 100% rename from planning/planning_debug_tools/Readme.md rename to planning/planning_debug_tools/README.md diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py index b45357c7bd8b2..79a97f0033b18 100755 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py @@ -20,6 +20,7 @@ import sys from PyQt5.QtWidgets import QApplication +from geometry_msgs.msg import PoseWithCovarianceStamped from perception_replayer_common import PerceptionReplayerCommon import rclpy from time_manager_widget import TimeManagerWidget @@ -34,6 +35,7 @@ def __init__(self, args): self.bag_timestamp = self.rosbag_objects_data[0][0] self.is_pause = False self.rate = 1.0 + self.prev_traffic_signals_msg = None # initialize widget self.widget = TimeManagerWidget( @@ -43,6 +45,7 @@ def __init__(self, args): self.widget.button.clicked.connect(self.onPushed) for button in self.widget.rate_button: button.clicked.connect(functools.partial(self.onSetRate, button)) + self.widget.pub_recorded_ego_pose_button.clicked.connect(self.publish_recorded_ego_pose) # start timer callback self.delta_time = 0.1 @@ -92,13 +95,22 @@ def on_timer(self): self.objects_pub.publish(objects_msg) # traffic signals + # temporary support old auto msgs if traffic_signals_msg: - traffic_signals_msg.header.stamp = timestamp - self.traffic_signals_pub.publish(traffic_signals_msg) + if self.is_auto_traffic_signals: + traffic_signals_msg.header.stamp = timestamp + self.auto_traffic_signals_pub.publish(traffic_signals_msg) + else: + traffic_signals_msg.stamp = timestamp + self.traffic_signals_pub.publish(traffic_signals_msg) self.prev_traffic_signals_msg = traffic_signals_msg elif self.prev_traffic_signals_msg: - self.prev_traffic_signals_msg.header.stamp = timestamp - self.traffic_signals_pub.publish(self.prev_traffic_signals_msg) + if self.is_auto_traffic_signals: + self.prev_traffic_signals_msg.header.stamp = timestamp + self.auto_traffic_signals_pub.publish(self.prev_traffic_signals_msg) + else: + self.prev_traffic_signals_msg.stamp = timestamp + self.traffic_signals_pub.publish(self.prev_traffic_signals_msg) def onPushed(self, event): if self.widget.button.isChecked(): @@ -109,6 +121,57 @@ def onPushed(self, event): def onSetRate(self, button): self.rate = float(button.text()) + def publish_recorded_ego_pose(self): + ego_odom = self.find_ego_odom_by_timestamp(self.bag_timestamp) + if not ego_odom: + return + + recorded_ego_pose = PoseWithCovarianceStamped() + recorded_ego_pose.header.stamp = self.get_clock().now().to_msg() + recorded_ego_pose.header.frame_id = "map" + recorded_ego_pose.pose.pose = ego_odom.pose.pose + recorded_ego_pose.pose.covariance = [ + 0.25, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.25, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.06853892326654787, + ] + + self.recorded_ego_pub.publish(recorded_ego_pose) + print("Published recorded ego pose as /initialpose") + if __name__ == "__main__": parser = argparse.ArgumentParser() diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py index a86a9ae9b2bb0..6b758bfba24ee 100644 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py @@ -22,7 +22,9 @@ from autoware_auto_perception_msgs.msg import DetectedObjects from autoware_auto_perception_msgs.msg import PredictedObjects from autoware_auto_perception_msgs.msg import TrackedObjects -from autoware_auto_perception_msgs.msg import TrafficSignalArray +from autoware_auto_perception_msgs.msg import TrafficSignalArray as AutoTrafficSignalArray +from autoware_perception_msgs.msg import TrafficSignalArray +from geometry_msgs.msg import PoseWithCovarianceStamped from nav_msgs.msg import Odometry import psutil from rclpy.node import Node @@ -42,6 +44,7 @@ def __init__(self, args, name): self.rosbag_objects_data = [] self.rosbag_ego_odom_data = [] self.rosbag_traffic_signals_data = [] + self.is_auto_traffic_signals = False # subscriber self.sub_odom = self.create_subscription( @@ -65,9 +68,7 @@ def __init__(self, args, name): self.pointcloud_pub = self.create_publisher( PointCloud2, "/perception/obstacle_segmentation/pointcloud", 1 ) - self.traffic_signals_pub = self.create_publisher( - TrafficSignalArray, "/perception/traffic_light_recognition/traffic_signals", 1 - ) + self.recorded_ego_pub = self.create_publisher(PoseWithCovarianceStamped, "/initialpose", 1) # load rosbag print("Stared loading rosbag") @@ -78,6 +79,16 @@ def __init__(self, args, name): self.load_rosbag(args.bag) print("Ended loading rosbag") + # temporary support old auto msgs + if self.is_auto_traffic_signals: + self.auto_traffic_signals_pub = self.create_publisher( + AutoTrafficSignalArray, "/perception/traffic_light_recognition/traffic_signals", 1 + ) + else: + self.traffic_signals_pub = self.create_publisher( + TrafficSignalArray, "/perception/traffic_light_recognition/traffic_signals", 1 + ) + # wait for ready to publish/subscribe time.sleep(1.0) @@ -113,6 +124,9 @@ def load_rosbag(self, rosbag2_path: str): self.rosbag_ego_odom_data.append((stamp, msg)) if topic == traffic_signals_topic: self.rosbag_traffic_signals_data.append((stamp, msg)) + self.is_auto_traffic_signals = ( + "autoware_auto_perception_msgs" in type(msg).__module__ + ) def kill_online_perception_node(self): # kill node if required diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py index 4228d506d5bec..1b75c86de2709 100755 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py @@ -66,13 +66,22 @@ def on_timer(self): self.objects_pub.publish(objects_msg) # traffic signals + # temporary support old auto msgs if traffic_signals_msg: - traffic_signals_msg.header.stamp = timestamp - self.traffic_signals_pub.publish(traffic_signals_msg) + if self.is_auto_traffic_signals: + traffic_signals_msg.header.stamp = timestamp + self.auto_traffic_signals_pub.publish(traffic_signals_msg) + else: + traffic_signals_msg.stamp = timestamp + self.traffic_signals_pub.publish(traffic_signals_msg) self.prev_traffic_signals_msg = traffic_signals_msg elif self.prev_traffic_signals_msg: - self.prev_traffic_signals_msg.header.stamp = timestamp - self.traffic_signals_pub.publish(self.prev_traffic_signals_msg) + if self.is_auto_traffic_signals: + self.prev_traffic_signals_msg.header.stamp = timestamp + self.auto_traffic_signals_pub.publish(self.prev_traffic_signals_msg) + else: + self.prev_traffic_signals_msg.stamp = timestamp + self.traffic_signals_pub.publish(self.prev_traffic_signals_msg) def find_nearest_ego_odom_by_observation(self, ego_pose): if self.ego_pose_idx: diff --git a/planning/planning_debug_tools/scripts/perception_replayer/time_manager_widget.py b/planning/planning_debug_tools/scripts/perception_replayer/time_manager_widget.py index 382e725b114cd..a1d87a8b06266 100644 --- a/planning/planning_debug_tools/scripts/perception_replayer/time_manager_widget.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/time_manager_widget.py @@ -89,13 +89,15 @@ def setupUI(self): self.button.setCheckable(True) self.button.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) self.grid_layout.addWidget(self.button, 1, 0, 1, -1) + self.pub_recorded_ego_pose_button = QPushButton("publish recorded ego pose") + self.grid_layout.addWidget(self.pub_recorded_ego_pose_button, 2, 0, 1, -1) # slider self.slider = QJumpSlider(QtCore.Qt.Horizontal, self.max_value) self.slider.setMinimum(0) self.slider.setMaximum(self.max_value) self.slider.setValue(0) - self.grid_layout.addWidget(self.slider, 2, 0, 1, -1) + self.grid_layout.addWidget(self.slider, 3, 0, 1, -1) self.setCentralWidget(self.central_widget) diff --git a/planning/planning_debug_tools/scripts/trajectory_visualizer.py b/planning/planning_debug_tools/scripts/trajectory_visualizer.py index 3ce69aa265090..73e41b052cd90 100755 --- a/planning/planning_debug_tools/scripts/trajectory_visualizer.py +++ b/planning/planning_debug_tools/scripts/trajectory_visualizer.py @@ -17,8 +17,11 @@ import argparse from autoware_auto_planning_msgs.msg import Path +from autoware_auto_planning_msgs.msg import PathPoint +from autoware_auto_planning_msgs.msg import PathPointWithLaneId from autoware_auto_planning_msgs.msg import PathWithLaneId from autoware_auto_planning_msgs.msg import Trajectory +from autoware_auto_planning_msgs.msg import TrajectoryPoint from autoware_auto_vehicle_msgs.msg import VelocityReport from geometry_msgs.msg import Pose from matplotlib import animation @@ -40,6 +43,13 @@ help="Options VEL(default): show velocity only, VEL_ACC_JERK: show vel & acc & jerk", ) +parser.add_argument( + "-v", + "--max-velocity", + type=int, + help="maximum plotting velocity in Matplotlib", +) + args = parser.parse_args() PLOT_MIN_ARCLENGTH = -5 @@ -61,6 +71,11 @@ PLOT_TYPE = "VEL" print("plot type = " + PLOT_TYPE) +if args.max_velocity is None: + MAX_VELOCITY = 20 +else: + MAX_VELOCITY = args.max_velocity + class TrajectoryVisualizer(Node): def __init__(self): @@ -189,7 +204,7 @@ def CallbackVelocityLimit(self, cmd): self.velocity_limit = cmd.max_velocity def CallbackMotionVelOptTraj(self, cmd1, cmd2, cmd3, cmd4, cmd5): - print("CallbackMotionVelOptTraj called") + self.get_logger().info("CallbackMotionVelOptTraj called") self.CallBackTrajExVelLim(cmd1) self.CallBackTrajLatAccFiltered(cmd2) self.CallBackTrajRaw(cmd3) @@ -221,7 +236,7 @@ def CallBackTrajFinal(self, cmd): self.update_traj_final = True def CallBackLaneDrivingTraj(self, cmd5, cmd6, cmd7, cmd8, cmd9): - print("CallBackLaneDrivingTraj called") + self.get_logger().info("CallBackLaneDrivingTraj called") self.CallBackTrajFinal(cmd5) self.CallBackLBehaviorPathPlannerPath(cmd6) self.CallBackBehaviorVelocityPlannerPath(cmd7) @@ -274,6 +289,7 @@ def setPlotTrajectoryVelocity(self): self.ax1.set_title("trajectory's velocity") self.ax1.legend() self.ax1.set_xlim([PLOT_MIN_ARCLENGTH, PLOT_MAX_ARCLENGTH]) + self.ax1.set_ylim([0, MAX_VELOCITY]) self.ax1.set_ylabel("vel [m/s]") return ( @@ -293,9 +309,8 @@ def setPlotTrajectoryVelocity(self): ) def plotTrajectoryVelocity(self, data): - # self.updatePose(PATH_ORIGIN_FRAME, SELF_POSE_FRAME) if self.self_pose_received is False: - print("plot start but self pose is not received") + self.get_logger().info("plot start but self pose is not received") return ( self.im1, self.im2, @@ -311,82 +326,9 @@ def plotTrajectoryVelocity(self, data): self.im11, self.im12, ) - print("plot start") - - # copy - behavior_path_planner_path = self.behavior_path_planner_path - behavior_velocity_planner_path = self.behavior_velocity_planner_path - obstacle_avoid_traj = self.obstacle_avoid_traj - obstacle_stop_traj = self.obstacle_stop_traj - trajectory_raw = self.trajectory_raw - trajectory_external_velocity_limited = self.trajectory_external_velocity_limited - trajectory_lateral_acc_filtered = self.trajectory_lateral_acc_filtered - trajectory_steer_rate_filtered = self.trajectory_steer_rate_filtered - trajectory_time_resampled = self.trajectory_time_resampled - trajectory_final = self.trajectory_final - - if self.update_behavior_path_planner_path: - x = self.CalcArcLengthPathWLid(behavior_path_planner_path) - y = self.ToVelListPathWLid(behavior_path_planner_path) - self.im1.set_data(x, y) - self.update_behavior_path_planner_path = False - if len(y) != 0: - self.max_vel = max(10.0, np.max(y)) - - if self.update_behavior_velocity_planner_path: - x = self.CalcArcLengthPath(behavior_velocity_planner_path) - y = self.ToVelListPath(behavior_velocity_planner_path) - self.im2.set_data(x, y) - self.update_behavior_velocity_planner_path = False - - if self.update_traj_ob_avoid: - x = self.CalcArcLength(obstacle_avoid_traj) - y = self.ToVelList(obstacle_avoid_traj) - self.im3.set_data(x, y) - self.update_traj_ob_avoid = False - - if self.update_traj_ob_stop: - x = self.CalcArcLength(obstacle_stop_traj) - y = self.ToVelList(obstacle_stop_traj) - self.im4.set_data(x, y) - self.update_traj_ob_stop = False - - if self.update_traj_raw: - x = self.CalcArcLength(trajectory_raw) - y = self.ToVelList(trajectory_raw) - self.im5.set_data(x, y) - self.update_traj_raw = False - - if self.update_ex_vel_lim: - x = self.CalcArcLength(trajectory_external_velocity_limited) - y = self.ToVelList(trajectory_external_velocity_limited) - self.im6.set_data(x, y) - self.update_ex_vel_lim = False - - if self.update_lat_acc_fil: - x = self.CalcArcLength(trajectory_lateral_acc_filtered) - y = self.ToVelList(trajectory_lateral_acc_filtered) - self.im7.set_data(x, y) - self.update_lat_acc_fil = False - - if self.update_steer_rate_fil: - x = self.CalcArcLength(trajectory_steer_rate_filtered) - y = self.ToVelList(trajectory_steer_rate_filtered) - self.im71.set_data(x, y) - self.update_steer_rate_fil = False - - if self.update_traj_resample: - x = self.CalcArcLength(trajectory_time_resampled) - y = self.ToVelList(trajectory_time_resampled) - self.im8.set_data(x, y) - self.update_traj_resample = False + self.get_logger().info("plot start") if self.update_traj_final: - x = self.CalcArcLength(trajectory_final) - y = self.ToVelList(trajectory_final) - self.im9.set_data(x, y) - self.update_traj_final = False - self.im10.set_data(0, self.localization_vx) self.im11.set_data(0, self.vehicle_vx) @@ -395,11 +337,33 @@ def plotTrajectoryVelocity(self, data): y = [self.velocity_limit, self.velocity_limit] self.im12.set_data(x, y) - if len(y) != 0: - self.min_vel = np.min(y) + if len(y) != 0: + self.min_vel = np.min(y) - # change y-range - self.ax1.set_ylim([self.min_vel - 1.0, self.max_vel + 1.0]) + trajectory_data = [ + (self.behavior_path_planner_path, self.update_behavior_path_planner_path, self.im1), + ( + self.behavior_velocity_planner_path, + self.update_behavior_velocity_planner_path, + self.im2, + ), + (self.obstacle_avoid_traj, self.update_traj_ob_avoid, self.im3), + (self.obstacle_stop_traj, self.update_traj_ob_stop, self.im4), + (self.trajectory_raw, self.update_traj_raw, self.im5), + (self.trajectory_external_velocity_limited, self.update_ex_vel_lim, self.im6), + (self.trajectory_lateral_acc_filtered, self.update_lat_acc_fil, self.im7), + (self.trajectory_steer_rate_filtered, self.update_steer_rate_fil, self.im71), + (self.trajectory_time_resampled, self.update_traj_resample, self.im8), + (self.trajectory_final, self.update_traj_final, self.im9), + ] + + # update all trajectory plots + for traj, update_flag, image in trajectory_data: + if update_flag: + x = self.CalcArcLength(traj) + y = self.ToVelList(traj) + image.set_data(x, y) + update_flag = False return ( self.im1, @@ -417,107 +381,43 @@ def plotTrajectoryVelocity(self, data): self.im12, ) - def CalcArcLength(self, traj): - if len(traj.points) == 0: - return - - s_arr = [] - ds = 0.0 - s_sum = 0.0 - - closest_id = self.calcClosestTrajectory(traj) - for i in range(1, closest_id): - p0 = traj.points[i - 1] - p1 = traj.points[i] - dx = p1.pose.position.x - p0.pose.position.x - dy = p1.pose.position.y - p0.pose.position.y - ds = np.sqrt(dx**2 + dy**2) - s_sum -= ds - - s_arr.append(s_sum) - for i in range(1, len(traj.points)): - p0 = traj.points[i - 1] - p1 = traj.points[i] - dx = p1.pose.position.x - p0.pose.position.x - dy = p1.pose.position.y - p0.pose.position.y - ds = np.sqrt(dx**2 + dy**2) - s_sum += ds - s_arr.append(s_sum) - return s_arr - - def CalcArcLengthPathWLid(self, traj): - if len(traj.points) == 0: - return - - s_arr = [] - ds = 0.0 - s_sum = 0.0 - - closest_id = self.calcClosestPathWLid(traj) - for i in range(1, closest_id): - p0 = traj.points[i - 1].point - p1 = traj.points[i].point - dx = p1.pose.position.x - p0.pose.position.x - dy = p1.pose.position.y - p0.pose.position.y - ds = np.sqrt(dx**2 + dy**2) - s_sum -= ds + def getPoint(self, path_point): + if isinstance(path_point, (TrajectoryPoint, PathPoint)): + return path_point + elif isinstance(path_point, PathPointWithLaneId): + return path_point.point + else: + raise TypeError("invalid path_point argument type") - s_arr.append(s_sum) - for i in range(1, len(traj.points)): - p0 = traj.points[i - 1].point - p1 = traj.points[i].point - dx = p1.pose.position.x - p0.pose.position.x - dy = p1.pose.position.y - p0.pose.position.y - ds = np.sqrt(dx**2 + dy**2) - s_sum += ds - s_arr.append(s_sum) - return s_arr + def CalcDistance(self, point0, point1): + p0 = self.getPoint(point0).pose.position + p1 = self.getPoint(point1).pose.position + dx = p1.x - p0.x + dy = p1.y - p0.y + return np.sqrt(dx**2 + dy**2) - def CalcArcLengthPath(self, traj): + def CalcArcLength(self, traj): if len(traj.points) == 0: return s_arr = [] - ds = 0.0 s_sum = 0.0 - closest_id = self.calcClosestPath(traj) + closest_id = self.calcClosestIndex(traj) for i in range(1, closest_id): - p0 = traj.points[i - 1] - p1 = traj.points[i] - dx = p1.pose.position.x - p0.pose.position.x - dy = p1.pose.position.y - p0.pose.position.y - ds = np.sqrt(dx**2 + dy**2) - s_sum -= ds - + s_sum -= self.CalcDistance( + self.getPoint(traj.points[i - 1]), self.getPoint(traj.points[i]) + ) s_arr.append(s_sum) for i in range(1, len(traj.points)): - p0 = traj.points[i - 1] - p1 = traj.points[i] - dx = p1.pose.position.x - p0.pose.position.x - dy = p1.pose.position.y - p0.pose.position.y - ds = np.sqrt(dx**2 + dy**2) - s_sum += ds + s_sum += self.CalcDistance( + self.getPoint(traj.points[i - 1]), self.getPoint(traj.points[i]) + ) s_arr.append(s_sum) return s_arr def ToVelList(self, traj): - v_list = [] - for p in traj.points: - v_list.append(p.longitudinal_velocity_mps) - return v_list - - def ToVelListPathWLid(self, traj): - v_list = [] - for p in traj.points: - v_list.append(p.point.longitudinal_velocity_mps) - return v_list - - def ToVelListPath(self, traj): - v_list = [] - for p in traj.points: - v_list.append(p.longitudinal_velocity_mps) - return v_list + return [self.getPoint(p).longitudinal_velocity_mps for p in traj.points] def CalcAcceleration(self, traj): a_arr = [] @@ -577,6 +477,7 @@ def setPlotTrajectory(self): self.ax1.set_title("trajectory's velocity") self.ax1.legend() self.ax1.set_xlim([PLOT_MIN_ARCLENGTH, PLOT_MAX_ARCLENGTH]) + self.ax1.set_ylim([0, MAX_VELOCITY]) self.ax1.set_ylabel("vel [m/s]") self.ax2 = plt.subplot(3, 1, 2) @@ -595,8 +496,7 @@ def setPlotTrajectory(self): return self.im0, self.im1, self.im2, self.im3, self.im4 def plotTrajectory(self, data): - print("plot called") - # self.updatePose(PATH_ORIGIN_FRAME, SELF_POSE_FRAME) + self.get_logger().info("plot called") # copy trajectory_raw = self.trajectory_raw @@ -652,40 +552,16 @@ def plotTrajectory(self, data): return self.im0, self.im1, self.im2, self.im3, self.im4 - def calcClosestPath(self, path): - closest = -1 - min_dist_squared = 1.0e10 - for i in range(0, len(path.points)): - dist_sq = self.calcSquaredDist2d(self.self_pose, path.points[i].pose) - if dist_sq < min_dist_squared: - min_dist_squared = dist_sq - closest = i - return closest - - def calcClosestPathWLid(self, path): - closest = -1 - min_dist_squared = 1.0e10 - for i in range(0, len(path.points)): - dist_sq = self.calcSquaredDist2d(self.self_pose, path.points[i].point.pose) - if dist_sq < min_dist_squared: - min_dist_squared = dist_sq - closest = i - return closest - - def calcClosestTrajectory(self, path): - closest = -1 - min_dist_squared = 1.0e10 - for i in range(0, len(path.points)): - dist_sq = self.calcSquaredDist2d(self.self_pose, path.points[i].pose) - if dist_sq < min_dist_squared: - min_dist_squared = dist_sq - closest = i - return closest - - def calcSquaredDist2d(self, p1, p2): - dx = p1.position.x - p2.position.x - dy = p1.position.y - p2.position.y - return dx * dx + dy * dy + def calcClosestIndex(self, path): + points = np.array( + [ + [self.getPoint(p).pose.position.x, self.getPoint(p).pose.position.y] + for p in path.points + ] + ) + self_pose = np.array([self.self_pose.position.x, self.self_pose.position.y]) + dists_squared = np.sum((points - self_pose) ** 2, axis=1) + return np.argmin(dists_squared) def closeFigure(self): plt.close(self.fig) diff --git a/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp b/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp index ddc304c99ea6a..c5a30be126661 100644 --- a/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp +++ b/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp @@ -38,11 +38,11 @@ #include #include #include -#include #include #include #include #include +#include #include #include #include @@ -77,10 +77,10 @@ namespace planning_test_utils using autoware_adapi_v1_msgs::msg::OperationModeState; using autoware_auto_mapping_msgs::msg::HADMapBin; using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::TrafficSignalArray; using autoware_auto_planning_msgs::msg::Path; using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_perception_msgs::msg::TrafficSignalArray; using autoware_planning_msgs::msg::LaneletRoute; using geometry_msgs::msg::AccelWithCovarianceStamped; using geometry_msgs::msg::Point; diff --git a/planning/planning_test_utils/package.xml b/planning/planning_test_utils/package.xml index 72662f7ee7b76..23bfb2f44cb96 100644 --- a/planning/planning_test_utils/package.xml +++ b/planning/planning_test_utils/package.xml @@ -15,9 +15,9 @@ autoware_auto_control_msgs autoware_auto_mapping_msgs - autoware_auto_perception_msgs autoware_auto_planning_msgs autoware_auto_vehicle_msgs + autoware_perception_msgs autoware_planning_msgs component_interface_specs component_interface_utils diff --git a/planning/planning_validator/src/debug_marker.cpp b/planning/planning_validator/src/debug_marker.cpp index 89614f5c74f5c..1a0793957402a 100644 --- a/planning/planning_validator/src/debug_marker.cpp +++ b/planning/planning_validator/src/debug_marker.cpp @@ -14,8 +14,8 @@ #include "planning_validator/debug_marker.hpp" -#include -#include +#include +#include #include #include diff --git a/planning/planning_validator/src/planning_validator.cpp b/planning/planning_validator/src/planning_validator.cpp index 7bb37fdcdd467..db4510c2e5d0d 100644 --- a/planning/planning_validator/src/planning_validator.cpp +++ b/planning/planning_validator/src/planning_validator.cpp @@ -17,7 +17,7 @@ #include "planning_validator/utils.hpp" #include -#include +#include #include #include diff --git a/planning/planning_validator/src/utils.cpp b/planning/planning_validator/src/utils.cpp index e4c882cfaedb9..5a90e950bb616 100644 --- a/planning/planning_validator/src/utils.cpp +++ b/planning/planning_validator/src/utils.cpp @@ -15,7 +15,7 @@ #include "planning_validator/utils.hpp" #include -#include +#include #include #include diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 1818c460e407b..6bd9756098558 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -16,9 +16,7 @@ #define ROUTE_HANDLER__ROUTE_HANDLER_HPP_ #include -#include #include -#include #include #include @@ -98,6 +96,7 @@ class RouteHandler Pose getGoalPose() const; Pose getStartPose() const; Pose getOriginalStartPose() const; + Pose getOriginalGoalPose() const; lanelet::Id getGoalLaneId() const; bool getGoalLanelet(lanelet::ConstLanelet * goal_lanelet) const; std::vector getLanesBeforePose( @@ -121,7 +120,8 @@ class RouteHandler * @return vector of lanelet having same direction if true */ boost::optional getRightLanelet( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const; + const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false, + const bool get_shoulder_lane = false) const; /** * @brief Check if same-direction lane is available at the left side of the lanelet @@ -131,7 +131,8 @@ class RouteHandler * @return vector of lanelet having same direction if true */ boost::optional getLeftLanelet( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const; + const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false, + const bool get_shoulder_lane = false) const; lanelet::ConstLanelets getNextLanelets(const lanelet::ConstLanelet & lanelet) const; lanelet::ConstLanelets getPreviousLanelets(const lanelet::ConstLanelet & lanelet) const; @@ -194,7 +195,8 @@ class RouteHandler * @return vector of lanelet having same direction if true */ lanelet::ConstLanelet getMostRightLanelet( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const; + const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false, + const bool get_shoulder_lane = false) const; /** * @brief Check if same-direction lane is available at the left side of the lanelet @@ -204,7 +206,8 @@ class RouteHandler * @return vector of lanelet having same direction if true */ lanelet::ConstLanelet getMostLeftLanelet( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const; + const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false, + const bool get_shoulder_lane = false) const; /** * @brief Searches the furthest linestring to the right side of the lanelet @@ -378,6 +381,7 @@ class RouteHandler // save original(not modified) route start pose for start planer execution Pose original_start_pose_; + Pose original_goal_pose_; // non-const methods void setLaneletsFromRouteMsg(); diff --git a/planning/route_handler/package.xml b/planning/route_handler/package.xml index ef2051d1b244a..6304e21932676 100644 --- a/planning/route_handler/package.xml +++ b/planning/route_handler/package.xml @@ -24,7 +24,6 @@ autoware_planning_msgs geometry_msgs lanelet2_extension - motion_utils rclcpp rclcpp_components tf2_ros diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index b8cedaa947455..3d9dd8fe7a64a 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -176,6 +177,7 @@ void RouteHandler::setRoute(const LaneletRoute & route_msg) // if get not modified route but new route, reset original start pose if (!route_ptr_ || route_ptr_->uuid != route_msg.uuid) { original_start_pose_ = route_msg.start_pose; + original_goal_pose_ = route_msg.goal_pose; } route_ptr_ = std::make_shared(route_msg); is_handler_ready_ = false; @@ -426,13 +428,17 @@ Pose RouteHandler::getStartPose() const { if (!route_ptr_) { RCLCPP_WARN(logger_, "[Route Handler] getStartPose: Route has not been set yet"); - Pose(); + return Pose(); } return route_ptr_->start_pose; } Pose RouteHandler::getOriginalStartPose() const { + if (!route_ptr_) { + RCLCPP_WARN(logger_, "[Route Handler] getOriginalStartPose: Route has not been set yet"); + return Pose(); + } return original_start_pose_; } @@ -440,11 +446,20 @@ Pose RouteHandler::getGoalPose() const { if (!route_ptr_) { RCLCPP_WARN(logger_, "[Route Handler] getGoalPose: Route has not been set yet"); - Pose(); + return Pose(); } return route_ptr_->goal_pose; } +Pose RouteHandler::getOriginalGoalPose() const +{ + if (!route_ptr_) { + RCLCPP_WARN(logger_, "[Route Handler] getOriginalGoalPose: Route has not been set yet"); + return Pose(); + } + return original_goal_pose_; +} + lanelet::Id RouteHandler::getGoalLaneId() const { if (!route_ptr_ || route_ptr_->segments.empty()) { @@ -514,7 +529,14 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequenceAfter( while (rclcpp::ok() && length < min_length) { lanelet::ConstLanelet next_lanelet; if (!getNextLaneletWithinRoute(current_lanelet, &next_lanelet)) { - break; + if (only_route_lanes) { + break; + } + const auto next_lanes = getNextLanelets(current_lanelet); + if (next_lanes.empty()) { + break; + } + next_lanelet = next_lanes.front(); } // loop check if (lanelet.id() == next_lanelet.id()) { @@ -542,7 +564,14 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequenceUpTo( while (rclcpp::ok() && length < min_length) { lanelet::ConstLanelets candidate_lanelets; if (!getPreviousLaneletsWithinRoute(current_lanelet, &candidate_lanelets)) { - break; + if (only_route_lanes) { + break; + } + const auto prev_lanes = getPreviousLanelets(current_lanelet); + if (prev_lanes.empty()) { + break; + } + candidate_lanelets = prev_lanes; } // loop check if (std::any_of( @@ -940,7 +969,8 @@ bool RouteHandler::isBijectiveConnection( } boost::optional RouteHandler::getRightLanelet( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const + const lanelet::ConstLanelet & lanelet, const bool enable_same_root, + const bool get_shoulder_lane) const { // right road lanelet of shoulder lanelet if (isShoulderLanelet(lanelet)) { @@ -952,6 +982,14 @@ boost::optional RouteHandler::getRightLanelet( return boost::none; } + // right shoulder lanelet + if (get_shoulder_lane) { + lanelet::ConstLanelet right_shoulder_lanelet; + if (getRightShoulderLanelet(lanelet, &right_shoulder_lanelet)) { + return right_shoulder_lanelet; + } + } + // routable lane const auto & right_lane = routing_graph_ptr_->right(lanelet); if (right_lane) { @@ -1012,7 +1050,8 @@ bool RouteHandler::getLeftLaneletWithinRoute( } boost::optional RouteHandler::getLeftLanelet( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const + const lanelet::ConstLanelet & lanelet, const bool enable_same_root, + const bool get_shoulder_lane) const { // left road lanelet of shoulder lanelet if (isShoulderLanelet(lanelet)) { @@ -1024,6 +1063,14 @@ boost::optional RouteHandler::getLeftLanelet( return boost::none; } + // left shoulder lanelet + if (get_shoulder_lane) { + lanelet::ConstLanelet left_shoulder_lanelet; + if (getLeftShoulderLanelet(lanelet, &left_shoulder_lanelet)) { + return left_shoulder_lanelet; + } + } + // routable lane const auto & left_lane = routing_graph_ptr_->left(lanelet); if (left_lane) { @@ -1199,26 +1246,28 @@ lanelet::Lanelets RouteHandler::getLeftOppositeLanelets(const lanelet::ConstLane } lanelet::ConstLanelet RouteHandler::getMostRightLanelet( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const + const lanelet::ConstLanelet & lanelet, const bool enable_same_root, + const bool get_shoulder_lane) const { // recursively compute the width of the lanes - const auto & same = getRightLanelet(lanelet, enable_same_root); + const auto & same = getRightLanelet(lanelet, enable_same_root, get_shoulder_lane); if (same) { - return getMostRightLanelet(same.get(), enable_same_root); + return getMostRightLanelet(same.get(), enable_same_root, get_shoulder_lane); } return lanelet; } lanelet::ConstLanelet RouteHandler::getMostLeftLanelet( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const + const lanelet::ConstLanelet & lanelet, const bool enable_same_root, + const bool get_shoulder_lane) const { // recursively compute the width of the lanes - const auto & same = getLeftLanelet(lanelet, enable_same_root); + const auto & same = getLeftLanelet(lanelet, enable_same_root, get_shoulder_lane); if (same) { - return getMostLeftLanelet(same.get(), enable_same_root); + return getMostLeftLanelet(same.get(), enable_same_root, get_shoulder_lane); } return lanelet; diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/common_structs.hpp b/planning/sampling_based_planner/path_sampler/include/path_sampler/common_structs.hpp index 30d9a77f31fad..7cf88cb75e13a 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/common_structs.hpp +++ b/planning/sampling_based_planner/path_sampler/include/path_sampler/common_structs.hpp @@ -18,7 +18,10 @@ #include "path_sampler/type_alias.hpp" #include "rclcpp/rclcpp.hpp" #include "sampler_common/structures.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" +#include "tier4_autoware_utils/system/stop_watch.hpp" + +#include #include #include diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/node.hpp b/planning/sampling_based_planner/path_sampler/include/path_sampler/node.hpp index b1c6ec61db845..a9002c8cf8a9f 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/node.hpp +++ b/planning/sampling_based_planner/path_sampler/include/path_sampler/node.hpp @@ -15,13 +15,11 @@ #ifndef PATH_SAMPLER__NODE_HPP_ #define PATH_SAMPLER__NODE_HPP_ -#include "motion_utils/motion_utils.hpp" #include "path_sampler/common_structs.hpp" #include "path_sampler/parameters.hpp" #include "path_sampler/type_alias.hpp" #include "rclcpp/rclcpp.hpp" #include "sampler_common/transform/spline_transform.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include diff --git a/planning/sampling_based_planner/path_sampler/src/node.cpp b/planning/sampling_based_planner/path_sampler/src/node.cpp index 588f29b06c40d..aa037755bbedd 100644 --- a/planning/sampling_based_planner/path_sampler/src/node.cpp +++ b/planning/sampling_based_planner/path_sampler/src/node.cpp @@ -15,6 +15,7 @@ #include "path_sampler/node.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" +#include "motion_utils/marker/marker_helper.hpp" #include "path_sampler/path_generation.hpp" #include "path_sampler/prepare_inputs.hpp" #include "path_sampler/utils/geometry_utils.hpp" @@ -23,6 +24,8 @@ #include "sampler_common/constraints/hard_constraint.hpp" #include "sampler_common/constraints/soft_constraint.hpp" +#include + #include #include diff --git a/planning/sampling_based_planner/path_sampler/src/prepare_inputs.cpp b/planning/sampling_based_planner/path_sampler/src/prepare_inputs.cpp index c17f3479932f3..fd52764950ca9 100644 --- a/planning/sampling_based_planner/path_sampler/src/prepare_inputs.cpp +++ b/planning/sampling_based_planner/path_sampler/src/prepare_inputs.cpp @@ -18,6 +18,7 @@ #include "path_sampler/utils/geometry_utils.hpp" #include "sampler_common/structures.hpp" #include "sampler_common/transform/spline_transform.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include diff --git a/planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp b/planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp index ec41f3032e150..db55e5f8557e4 100644 --- a/planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp +++ b/planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp @@ -14,7 +14,8 @@ #include "path_sampler/utils/trajectory_utils.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/resample/resample.hpp" +#include "motion_utils/trajectory/tmp_conversion.hpp" #include "path_sampler/utils/geometry_utils.hpp" #include "tf2/utils.h" diff --git a/planning/static_centerline_optimizer/include/static_centerline_optimizer/type_alias.hpp b/planning/static_centerline_optimizer/include/static_centerline_optimizer/type_alias.hpp index 5798c19d0d563..1a700d36deaaf 100644 --- a/planning/static_centerline_optimizer/include/static_centerline_optimizer/type_alias.hpp +++ b/planning/static_centerline_optimizer/include/static_centerline_optimizer/type_alias.hpp @@ -15,6 +15,7 @@ #define STATIC_CENTERLINE_OPTIMIZER__TYPE_ALIAS_HPP_ #include "route_handler/route_handler.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include "autoware_auto_mapping_msgs/msg/had_map_bin.hpp" #include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" diff --git a/planning/static_centerline_optimizer/package.xml b/planning/static_centerline_optimizer/package.xml index c30a9f2807df3..89976be5bdd47 100644 --- a/planning/static_centerline_optimizer/package.xml +++ b/planning/static_centerline_optimizer/package.xml @@ -32,6 +32,7 @@ rclcpp_components route_handler tier4_autoware_utils + tier4_map_msgs vehicle_info_util python3-flask-cors diff --git a/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp b/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp index 9c30c57533335..8c3b84854a6d0 100644 --- a/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp +++ b/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp @@ -18,14 +18,20 @@ #include "lanelet2_extension/utility/query.hpp" #include "lanelet2_extension/utility/utilities.hpp" #include "map_loader/lanelet2_map_loader_node.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/tmp_conversion.hpp" #include "static_centerline_optimizer/msg/points_with_lane_id.hpp" #include "static_centerline_optimizer/successive_trajectory_optimizer_node.hpp" #include "static_centerline_optimizer/type_alias.hpp" #include "static_centerline_optimizer/utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include #include +#include + +#include + +#include #include #include @@ -242,7 +248,9 @@ void StaticCenterlineOptimizerNode::load_map(const std::string & lanelet2_input_ map_bin_ptr_ = [&]() -> HADMapBin::ConstSharedPtr { // load map lanelet::LaneletMapPtr map_ptr; - map_ptr = Lanelet2MapLoaderNode::load_map(lanelet2_input_file_path, "MGRS"); + tier4_map_msgs::msg::MapProjectorInfo map_projector_info; + map_projector_info.projector_type = tier4_map_msgs::msg::MapProjectorInfo::MGRS; + map_ptr = Lanelet2MapLoaderNode::load_map(lanelet2_input_file_path, map_projector_info); if (!map_ptr) { return nullptr; } diff --git a/planning/static_centerline_optimizer/src/successive_trajectory_optimizer_node.cpp b/planning/static_centerline_optimizer/src/successive_trajectory_optimizer_node.cpp index 325c257e228f9..ab147ab1f4f74 100644 --- a/planning/static_centerline_optimizer/src/successive_trajectory_optimizer_node.cpp +++ b/planning/static_centerline_optimizer/src/successive_trajectory_optimizer_node.cpp @@ -14,9 +14,11 @@ #include "static_centerline_optimizer/successive_trajectory_optimizer_node.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/resample/resample.hpp" +#include "motion_utils/trajectory/tmp_conversion.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include diff --git a/planning/static_centerline_optimizer/src/utils.cpp b/planning/static_centerline_optimizer/src/utils.cpp index 691ae7960968c..7871eb9e95163 100644 --- a/planning/static_centerline_optimizer/src/utils.cpp +++ b/planning/static_centerline_optimizer/src/utils.cpp @@ -16,8 +16,8 @@ #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" - +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/ros/marker_helper.hpp" namespace static_centerline_optimizer { namespace diff --git a/planning/surround_obstacle_checker/config/surround_obstacle_checker.param.yaml b/planning/surround_obstacle_checker/config/surround_obstacle_checker.param.yaml index 10b5c2814e2a8..5ec10572ffe83 100644 --- a/planning/surround_obstacle_checker/config/surround_obstacle_checker.param.yaml +++ b/planning/surround_obstacle_checker/config/surround_obstacle_checker.param.yaml @@ -1,12 +1,61 @@ /**: ros__parameters: - # obstacle check - use_pointcloud: true # use pointcloud as obstacle check - use_dynamic_object: true # use dynamic object as obstacle check - surround_check_distance: 0.5 # if objects exist in this distance, transit to "exist-surrounding-obstacle" status [m] - surround_check_recover_distance: 0.8 # if no object exists in this distance, transit to "non-surrounding-obstacle" status [m] + # surround_check_*_distance: if objects exist in this distance, transit to "exist-surrounding-obstacle" status [m] + # surround_check_hysteresis_distance: if no object exists in this hysteresis distance added to the above distance, transit to "non-surrounding-obstacle" status [m] + pointcloud: + enable_check: false + surround_check_front_distance: 0.5 + surround_check_side_distance: 0.5 + surround_check_back_distance: 0.5 + unknown: + enable_check: true + surround_check_front_distance: 0.5 + surround_check_side_distance: 0.5 + surround_check_back_distance: 0.5 + car: + enable_check: true + surround_check_front_distance: 0.5 + surround_check_side_distance: 0.0 + surround_check_back_distance: 0.5 + truck: + enable_check: true + surround_check_front_distance: 0.5 + surround_check_side_distance: 0.0 + surround_check_back_distance: 0.5 + bus: + enable_check: true + surround_check_front_distance: 0.5 + surround_check_side_distance: 0.0 + surround_check_back_distance: 0.5 + trailer: + enable_check: true + surround_check_front_distance: 0.5 + surround_check_side_distance: 0.0 + surround_check_back_distance: 0.5 + motorcycle: + enable_check: true + surround_check_front_distance: 0.5 + surround_check_side_distance: 0.0 + surround_check_back_distance: 0.5 + bicycle: + enable_check: true + surround_check_front_distance: 0.5 + surround_check_side_distance: 0.5 + surround_check_back_distance: 0.5 + pedestrian: + enable_check: true + surround_check_front_distance: 0.5 + surround_check_side_distance: 0.5 + surround_check_back_distance: 0.5 + + surround_check_hysteresis_distance: 0.3 + state_clear_time: 2.0 + # ego stop state + stop_state_ego_speed: 0.1 #[m/s] + # debug publish_debug_footprints: true # publish vehicle footprint & footprints with surround_check_distance and surround_check_recover_distance offsets + debug_footprint_label: "car" diff --git a/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp b/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp index bd0fe48bbda28..590f026942801 100644 --- a/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp +++ b/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp @@ -16,6 +16,7 @@ #define SURROUND_OBSTACLE_CHECKER__DEBUG_MARKER_HPP_ #include +#include #include #include @@ -38,6 +39,7 @@ using geometry_msgs::msg::PolygonStamped; using tier4_planning_msgs::msg::StopFactor; using tier4_planning_msgs::msg::StopReason; using tier4_planning_msgs::msg::StopReasonArray; +using vehicle_info_util::VehicleInfo; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; @@ -52,15 +54,19 @@ class SurroundObstacleCheckerDebugNode { public: explicit SurroundObstacleCheckerDebugNode( - const Polygon2d & ego_polygon, const double base_link2front, - const double & surround_check_distance, const double & surround_check_recover_distance, - const geometry_msgs::msg::Pose & self_pose, const rclcpp::Clock::SharedPtr clock, - rclcpp::Node & node); + const vehicle_info_util::VehicleInfo & vehicle_info, const double base_link2front, + const std::string & object_label, const double & surround_check_front_distance, + const double & surround_check_side_distance, const double & surround_check_back_distance, + const double & surround_check_hysteresis_distance, const geometry_msgs::msg::Pose & self_pose, + const rclcpp::Clock::SharedPtr clock, rclcpp::Node & node); bool pushPose(const geometry_msgs::msg::Pose & pose, const PoseType & type); bool pushObstaclePoint(const geometry_msgs::msg::Point & obstacle_point, const PointType & type); void publish(); void publishFootprints(); + void updateFootprintMargin( + const std::string & object_label, const double front_distance, const double side_distance, + const double back_distance); private: rclcpp::Publisher::SharedPtr debug_virtual_wall_pub_; @@ -72,10 +78,13 @@ class SurroundObstacleCheckerDebugNode rclcpp::Publisher::SharedPtr vehicle_footprint_offset_pub_; rclcpp::Publisher::SharedPtr vehicle_footprint_recover_offset_pub_; - Polygon2d ego_polygon_; + vehicle_info_util::VehicleInfo vehicle_info_; double base_link2front_; - double surround_check_distance_; - double surround_check_recover_distance_; + std::string object_label_; + double surround_check_front_distance_; + double surround_check_side_distance_; + double surround_check_back_distance_; + double surround_check_hysteresis_distance_; geometry_msgs::msg::Pose self_pose_; MarkerArray makeVirtualWallMarker(); @@ -83,7 +92,6 @@ class SurroundObstacleCheckerDebugNode StopReasonArray makeStopReasonArray(); VelocityFactorArray makeVelocityFactorArray(); - Polygon2d createSelfPolygonWithOffset(const Polygon2d & base_polygon, const double & offset); PolygonStamped boostPolygonToPolygonStamped(const Polygon2d & boost_polygon, const double & z); std::shared_ptr stop_obstacle_point_ptr_; diff --git a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp b/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp index de3e4bf120b13..f31bb7ddee331 100644 --- a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp +++ b/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp @@ -17,10 +17,8 @@ #include "surround_obstacle_checker/debug_marker.hpp" -#include +#include #include -#include -#include #include #include @@ -32,12 +30,15 @@ #include #include +#include + #include #include #include #include #include +#include #include #include @@ -62,18 +63,27 @@ class SurroundObstacleCheckerNode : public rclcpp::Node struct NodeParam { - bool use_pointcloud; - bool use_dynamic_object; bool is_surround_obstacle; - // For preventing chattering, - // surround_check_recover_distance_ must be bigger than surround_check_distance_ - double surround_check_recover_distance; - double surround_check_distance; + std::unordered_map enable_check_map; + std::unordered_map surround_check_front_distance_map; + std::unordered_map surround_check_side_distance_map; + std::unordered_map surround_check_back_distance_map; + bool pointcloud_enable_check; + double pointcloud_surround_check_front_distance; + double pointcloud_surround_check_side_distance; + double pointcloud_surround_check_back_distance; + double surround_check_hysteresis_distance; double state_clear_time; bool publish_debug_footprints; + std::string debug_footprint_label; }; private: + rcl_interfaces::msg::SetParametersResult onParam( + const std::vector & parameters); + + std::array getCheckDistances(const std::string & str_label) const; + void onTimer(); void onPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); @@ -107,6 +117,9 @@ class SurroundObstacleCheckerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_clear_velocity_limit_; rclcpp::Publisher::SharedPtr pub_velocity_limit_; + // parameter callback result + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + // stop checker std::unique_ptr vehicle_stop_checker_; diff --git a/planning/surround_obstacle_checker/src/debug_marker.cpp b/planning/surround_obstacle_checker/src/debug_marker.cpp index c30f778584fd7..d3d73a89e0a94 100644 --- a/planning/surround_obstacle_checker/src/debug_marker.cpp +++ b/planning/surround_obstacle_checker/src/debug_marker.cpp @@ -14,8 +14,9 @@ #include "surround_obstacle_checker/debug_marker.hpp" -#include -#include +#include +#include +#include #ifdef ROS_DISTRO_GALACTIC #include #else @@ -26,6 +27,29 @@ namespace surround_obstacle_checker { +namespace +{ +Polygon2d createSelfPolygon( + const VehicleInfo & vehicle_info, const double front_margin = 0.0, const double side_margin = 0.0, + const double rear_margin = 0.0) +{ + const double & front_m = vehicle_info.max_longitudinal_offset_m + front_margin; + const double & width_left_m = vehicle_info.max_lateral_offset_m + side_margin; + const double & width_right_m = vehicle_info.min_lateral_offset_m - side_margin; + const double & rear_m = vehicle_info.min_longitudinal_offset_m - rear_margin; + + Polygon2d ego_polygon; + + ego_polygon.outer().push_back(Point2d(front_m, width_left_m)); + ego_polygon.outer().push_back(Point2d(front_m, width_right_m)); + ego_polygon.outer().push_back(Point2d(rear_m, width_right_m)); + ego_polygon.outer().push_back(Point2d(rear_m, width_left_m)); + + bg::correct(ego_polygon); + + return ego_polygon; +} +} // namespace using motion_utils::createStopVirtualWallMarker; using tier4_autoware_utils::appendMarkerArray; @@ -36,14 +60,18 @@ using tier4_autoware_utils::createMarkerScale; using tier4_autoware_utils::createPoint; SurroundObstacleCheckerDebugNode::SurroundObstacleCheckerDebugNode( - const Polygon2d & ego_polygon, const double base_link2front, - const double & surround_check_distance, const double & surround_check_recover_distance, - const geometry_msgs::msg::Pose & self_pose, const rclcpp::Clock::SharedPtr clock, - rclcpp::Node & node) -: ego_polygon_(ego_polygon), + const vehicle_info_util::VehicleInfo & vehicle_info, const double base_link2front, + const std::string & object_label, const double & surround_check_front_distance, + const double & surround_check_side_distance, const double & surround_check_back_distance, + const double & surround_check_hysteresis_distance, const geometry_msgs::msg::Pose & self_pose, + const rclcpp::Clock::SharedPtr clock, rclcpp::Node & node) +: vehicle_info_(vehicle_info), base_link2front_(base_link2front), - surround_check_distance_(surround_check_distance), - surround_check_recover_distance_(surround_check_recover_distance), + object_label_(object_label), + surround_check_front_distance_(surround_check_front_distance), + surround_check_side_distance_(surround_check_side_distance), + surround_check_back_distance_(surround_check_back_distance), + surround_check_hysteresis_distance_(surround_check_hysteresis_distance), self_pose_(self_pose), clock_(clock) { @@ -86,20 +114,25 @@ bool SurroundObstacleCheckerDebugNode::pushObstaclePoint( void SurroundObstacleCheckerDebugNode::publishFootprints() { + const auto ego_polygon = createSelfPolygon(vehicle_info_); + /* publish vehicle footprint polygon */ - const auto footprint = boostPolygonToPolygonStamped(ego_polygon_, self_pose_.position.z); + const auto footprint = boostPolygonToPolygonStamped(ego_polygon, self_pose_.position.z); vehicle_footprint_pub_->publish(footprint); /* publish vehicle footprint polygon with offset */ - const auto polygon_with_offset = - createSelfPolygonWithOffset(ego_polygon_, surround_check_distance_); + const auto polygon_with_offset = createSelfPolygon( + vehicle_info_, surround_check_front_distance_, surround_check_side_distance_, + surround_check_back_distance_); const auto footprint_with_offset = boostPolygonToPolygonStamped(polygon_with_offset, self_pose_.position.z); vehicle_footprint_offset_pub_->publish(footprint_with_offset); /* publish vehicle footprint polygon with recover offset */ - const auto polygon_with_recover_offset = - createSelfPolygonWithOffset(ego_polygon_, surround_check_recover_distance_); + const auto polygon_with_recover_offset = createSelfPolygon( + vehicle_info_, surround_check_front_distance_ + surround_check_hysteresis_distance_, + surround_check_side_distance_ + surround_check_hysteresis_distance_, + surround_check_back_distance_ + surround_check_hysteresis_distance_); const auto footprint_with_recover_offset = boostPolygonToPolygonStamped(polygon_with_recover_offset, self_pose_.position.z); vehicle_footprint_recover_offset_pub_->publish(footprint_with_recover_offset); @@ -206,26 +239,6 @@ VelocityFactorArray SurroundObstacleCheckerDebugNode::makeVelocityFactorArray() return velocity_factor_array; } -Polygon2d SurroundObstacleCheckerDebugNode::createSelfPolygonWithOffset( - const Polygon2d & base_polygon, const double & offset) -{ - typedef double coordinate_type; - const double buffer_distance = offset; - const int points_per_circle = 36; - boost::geometry::strategy::buffer::distance_symmetric distance_strategy( - buffer_distance); - boost::geometry::strategy::buffer::join_round join_strategy(points_per_circle); - boost::geometry::strategy::buffer::end_round end_strategy(points_per_circle); - boost::geometry::strategy::buffer::point_circle circle_strategy(points_per_circle); - boost::geometry::strategy::buffer::side_straight side_strategy; - boost::geometry::model::multi_polygon result; - // Create the buffer of a multi polygon - boost::geometry::buffer( - base_polygon, result, distance_strategy, side_strategy, join_strategy, end_strategy, - circle_strategy); - return result.front(); -} - PolygonStamped SurroundObstacleCheckerDebugNode::boostPolygonToPolygonStamped( const Polygon2d & boost_polygon, const double & z) { @@ -244,4 +257,14 @@ PolygonStamped SurroundObstacleCheckerDebugNode::boostPolygonToPolygonStamped( return polygon_stamped; } +void SurroundObstacleCheckerDebugNode::updateFootprintMargin( + const std::string & object_label, const double front_distance, const double side_distance, + const double back_distance) +{ + object_label_ = object_label; + surround_check_front_distance_ = front_distance; + surround_check_side_distance_ = side_distance; + surround_check_back_distance_ = back_distance; +} + } // namespace surround_obstacle_checker diff --git a/planning/surround_obstacle_checker/src/node.cpp b/planning/surround_obstacle_checker/src/node.cpp index 17a47be8c0596..ab542bdfa73ed 100644 --- a/planning/surround_obstacle_checker/src/node.cpp +++ b/planning/surround_obstacle_checker/src/node.cpp @@ -15,6 +15,9 @@ #include "surround_obstacle_checker/node.hpp" #include +#include +#include +#include #include #include @@ -47,6 +50,7 @@ namespace surround_obstacle_checker namespace bg = boost::geometry; using Point2d = bg::model::d2::point_xy; using Polygon2d = bg::model::polygon; +using autoware_auto_perception_msgs::msg::ObjectClassification; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::pose2transform; @@ -120,12 +124,14 @@ Polygon2d createObjPolygon( return createObjPolygon(pose, polygon); } -Polygon2d createSelfPolygon(const VehicleInfo & vehicle_info) +Polygon2d createSelfPolygon( + const VehicleInfo & vehicle_info, const double front_margin, const double side_margin, + const double rear_margin) { - const double & front_m = vehicle_info.max_longitudinal_offset_m; - const double & width_left_m = vehicle_info.max_lateral_offset_m; - const double & width_right_m = vehicle_info.min_lateral_offset_m; - const double & rear_m = vehicle_info.min_longitudinal_offset_m; + const double & front_m = vehicle_info.max_longitudinal_offset_m + front_margin; + const double & width_left_m = vehicle_info.max_lateral_offset_m + side_margin; + const double & width_right_m = vehicle_info.min_lateral_offset_m - side_margin; + const double & rear_m = vehicle_info.min_longitudinal_offset_m - rear_margin; Polygon2d ego_polygon; @@ -146,13 +152,42 @@ SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptio // Parameters { auto & p = node_param_; - p.use_pointcloud = this->declare_parameter("use_pointcloud"); - p.use_dynamic_object = this->declare_parameter("use_dynamic_object"); - p.surround_check_distance = this->declare_parameter("surround_check_distance"); - p.surround_check_recover_distance = - this->declare_parameter("surround_check_recover_distance"); + + // for object label + std::unordered_map label_map{ + {"unknown", ObjectClassification::UNKNOWN}, {"car", ObjectClassification::CAR}, + {"truck", ObjectClassification::TRUCK}, {"bus", ObjectClassification::BUS}, + {"trailer", ObjectClassification::TRAILER}, {"motorcycle", ObjectClassification::MOTORCYCLE}, + {"bicycle", ObjectClassification::BICYCLE}, {"pedestrian", ObjectClassification::PEDESTRIAN}}; + for (const auto & label_pair : label_map) { + p.enable_check_map.emplace( + label_pair.second, this->declare_parameter(label_pair.first + ".enable_check")); + p.surround_check_front_distance_map.emplace( + label_pair.second, + this->declare_parameter(label_pair.first + ".surround_check_front_distance")); + p.surround_check_side_distance_map.emplace( + label_pair.second, + this->declare_parameter(label_pair.first + ".surround_check_side_distance")); + p.surround_check_back_distance_map.emplace( + label_pair.second, + this->declare_parameter(label_pair.first + ".surround_check_back_distance")); + } + + // for pointcloud + p.pointcloud_enable_check = this->declare_parameter("pointcloud.enable_check"); + p.pointcloud_surround_check_front_distance = + this->declare_parameter("pointcloud.surround_check_front_distance"); + p.pointcloud_surround_check_side_distance = + this->declare_parameter("pointcloud.surround_check_side_distance"); + p.pointcloud_surround_check_back_distance = + this->declare_parameter("pointcloud.surround_check_back_distance"); + + p.surround_check_hysteresis_distance = + this->declare_parameter("surround_check_hysteresis_distance"); + p.state_clear_time = this->declare_parameter("state_clear_time"); p.publish_debug_footprints = this->declare_parameter("publish_debug_footprints"); + p.debug_footprint_label = this->declare_parameter("debug_footprint_label"); } vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); @@ -176,6 +211,10 @@ SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptio "~/input/odometry", 1, std::bind(&SurroundObstacleCheckerNode::onOdometry, this, std::placeholders::_1)); + // Parameter callback + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&SurroundObstacleCheckerNode::onParam, this, std::placeholders::_1)); + using std::chrono_literals::operator""ms; timer_ = rclcpp::create_timer( this, get_clock(), 100ms, std::bind(&SurroundObstacleCheckerNode::onTimer, this)); @@ -184,15 +223,55 @@ SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptio vehicle_stop_checker_ = std::make_unique(this); // Debug - auto const self_polygon = createSelfPolygon(vehicle_info_); odometry_ptr_ = std::make_shared(); + const auto check_distances = getCheckDistances(node_param_.debug_footprint_label); debug_ptr_ = std::make_shared( - self_polygon, vehicle_info_.max_longitudinal_offset_m, node_param_.surround_check_distance, - node_param_.surround_check_recover_distance, odometry_ptr_->pose.pose, this->get_clock(), + vehicle_info_, vehicle_info_.max_longitudinal_offset_m, node_param_.debug_footprint_label, + check_distances.at(0), check_distances.at(1), check_distances.at(2), + node_param_.surround_check_hysteresis_distance, odometry_ptr_->pose.pose, this->get_clock(), *this); } +std::array SurroundObstacleCheckerNode::getCheckDistances( + const std::string & str_label) const +{ + if (str_label == "pointcloud") { + return { + node_param_.pointcloud_surround_check_front_distance, + node_param_.pointcloud_surround_check_side_distance, + node_param_.pointcloud_surround_check_back_distance}; + } + + std::unordered_map label_map{ + {"unknown", ObjectClassification::UNKNOWN}, {"car", ObjectClassification::CAR}, + {"truck", ObjectClassification::TRUCK}, {"bus", ObjectClassification::BUS}, + {"trailer", ObjectClassification::TRAILER}, {"motorcycle", ObjectClassification::MOTORCYCLE}, + {"bicycle", ObjectClassification::BICYCLE}, {"pedestrian", ObjectClassification::PEDESTRIAN}}; + + const int int_label = label_map.at(str_label); + return { + node_param_.surround_check_front_distance_map.at(int_label), + node_param_.surround_check_side_distance_map.at(int_label), + node_param_.surround_check_back_distance_map.at(int_label)}; +} + +rcl_interfaces::msg::SetParametersResult SurroundObstacleCheckerNode::onParam( + const std::vector & parameters) +{ + tier4_autoware_utils::updateParam( + parameters, "debug_footprint_label", node_param_.debug_footprint_label); + const auto check_distances = getCheckDistances(node_param_.debug_footprint_label); + debug_ptr_->updateFootprintMargin( + node_param_.debug_footprint_label, check_distances.at(0), check_distances.at(1), + check_distances.at(2)); + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + return result; +} + void SurroundObstacleCheckerNode::onTimer() { if (!odometry_ptr_) { @@ -205,13 +284,22 @@ void SurroundObstacleCheckerNode::onTimer() debug_ptr_->publishFootprints(); } - if (node_param_.use_pointcloud && !pointcloud_ptr_) { + if (node_param_.pointcloud_enable_check && !pointcloud_ptr_) { RCLCPP_INFO_THROTTLE( this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for pointcloud info..."); return; } - if (node_param_.use_dynamic_object && !object_ptr_) { + const bool use_dynamic_object = + node_param_.enable_check_map.at(ObjectClassification::UNKNOWN) || + node_param_.enable_check_map.at(ObjectClassification::CAR) || + node_param_.enable_check_map.at(ObjectClassification::TRUCK) || + node_param_.enable_check_map.at(ObjectClassification::BUS) || + node_param_.enable_check_map.at(ObjectClassification::TRAILER) || + node_param_.enable_check_map.at(ObjectClassification::MOTORCYCLE) || + node_param_.enable_check_map.at(ObjectClassification::BICYCLE) || + node_param_.enable_check_map.at(ObjectClassification::PEDESTRIAN); + if (use_dynamic_object && !object_ptr_) { RCLCPP_INFO_THROTTLE( this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for dynamic object info..."); return; @@ -220,11 +308,11 @@ void SurroundObstacleCheckerNode::onTimer() const auto nearest_obstacle = getNearestObstacle(); const auto is_vehicle_stopped = vehicle_stop_checker_->isVehicleStopped(); + constexpr double epsilon = 1e-3; switch (state_) { case State::PASS: { const auto is_obstacle_found = - !nearest_obstacle ? false - : nearest_obstacle.get().first < node_param_.surround_check_distance; + !nearest_obstacle ? false : nearest_obstacle.get().first < epsilon; if (!isStopRequired(is_obstacle_found, is_vehicle_stopped)) { break; @@ -250,7 +338,7 @@ void SurroundObstacleCheckerNode::onTimer() const auto is_obstacle_found = !nearest_obstacle ? false - : nearest_obstacle.get().first < node_param_.surround_check_recover_distance; + : nearest_obstacle.get().first < node_param_.surround_check_hysteresis_distance; if (isStopRequired(is_obstacle_found, is_vehicle_stopped)) { break; @@ -304,17 +392,8 @@ void SurroundObstacleCheckerNode::onOdometry(const nav_msgs::msg::Odometry::Cons boost::optional SurroundObstacleCheckerNode::getNearestObstacle() const { - boost::optional nearest_pointcloud{boost::none}; - boost::optional nearest_object{boost::none}; - - if (node_param_.use_pointcloud) { - nearest_pointcloud = getNearestObstacleByPointCloud(); - } - - if (node_param_.use_dynamic_object) { - nearest_object = getNearestObstacleByDynamicObject(); - } - + const auto nearest_pointcloud = getNearestObstacleByPointCloud(); + const auto nearest_object = getNearestObstacleByDynamicObject(); if (!nearest_pointcloud && !nearest_object) { return {}; } @@ -333,15 +412,13 @@ boost::optional SurroundObstacleCheckerNode::getNearestObstacle() cons boost::optional SurroundObstacleCheckerNode::getNearestObstacleByPointCloud() const { - if (pointcloud_ptr_->data.empty()) { + if (!node_param_.pointcloud_enable_check || pointcloud_ptr_->data.empty()) { return boost::none; } + const auto transform_stamped = getTransform("base_link", pointcloud_ptr_->header.frame_id, pointcloud_ptr_->header.stamp, 0.5); - geometry_msgs::msg::Point nearest_point; - auto minimum_distance = std::numeric_limits::max(); - if (!transform_stamped) { return {}; } @@ -352,8 +429,14 @@ boost::optional SurroundObstacleCheckerNode::getNearestObstacleByPoint tier4_autoware_utils::transformPointCloud( transformed_pointcloud, transformed_pointcloud, isometry); - const auto ego_polygon = createSelfPolygon(vehicle_info_); + const double front_margin = node_param_.pointcloud_surround_check_front_distance; + const double side_margin = node_param_.pointcloud_surround_check_side_distance; + const double back_margin = node_param_.pointcloud_surround_check_back_distance; + const auto ego_polygon = createSelfPolygon(vehicle_info_, front_margin, side_margin, back_margin); + geometry_msgs::msg::Point nearest_point; + double minimum_distance = std::numeric_limits::max(); + bool was_minimum_distance_updated = false; for (const auto & p : transformed_pointcloud) { Point2d boost_point(p.x, p.y); @@ -362,10 +445,14 @@ boost::optional SurroundObstacleCheckerNode::getNearestObstacleByPoint if (distance_to_object < minimum_distance) { nearest_point = createPoint(p.x, p.y, p.z); minimum_distance = distance_to_object; + was_minimum_distance_updated = true; } } - return std::make_pair(minimum_distance, nearest_point); + if (was_minimum_distance_updated) { + return std::make_pair(minimum_distance, nearest_point); + } + return boost::none; } boost::optional SurroundObstacleCheckerNode::getNearestObstacleByDynamicObject() const @@ -373,9 +460,6 @@ boost::optional SurroundObstacleCheckerNode::getNearestObstacleByDynam const auto transform_stamped = getTransform(object_ptr_->header.frame_id, "base_link", object_ptr_->header.stamp, 0.5); - geometry_msgs::msg::Point nearest_point; - auto minimum_distance = std::numeric_limits::max(); - if (!transform_stamped) { return {}; } @@ -383,10 +467,23 @@ boost::optional SurroundObstacleCheckerNode::getNearestObstacleByDynam tf2::Transform tf_src2target; tf2::fromMsg(transform_stamped.get().transform, tf_src2target); - const auto ego_polygon = createSelfPolygon(vehicle_info_); - + // TODO(murooka) check computation cost + geometry_msgs::msg::Point nearest_point; + double minimum_distance = std::numeric_limits::max(); + bool was_minimum_distance_updated = false; for (const auto & object : object_ptr_->objects) { const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; + const int label = object.classification.front().label; + + if (!node_param_.enable_check_map.at(label)) { + continue; + } + + const double front_margin = node_param_.surround_check_front_distance_map.at(label); + const double side_margin = node_param_.surround_check_side_distance_map.at(label); + const double back_margin = node_param_.surround_check_back_distance_map.at(label); + const auto ego_polygon = + createSelfPolygon(vehicle_info_, front_margin, side_margin, back_margin); tf2::Transform tf_src2object; tf2::fromMsg(object_pose, tf_src2object); @@ -404,10 +501,14 @@ boost::optional SurroundObstacleCheckerNode::getNearestObstacleByDynam if (distance_to_object < minimum_distance) { nearest_point = object_pose.position; minimum_distance = distance_to_object; + was_minimum_distance_updated = true; } } - return std::make_pair(minimum_distance, nearest_point); + if (was_minimum_distance_updated) { + return std::make_pair(minimum_distance, nearest_point); + } + return boost::none; } boost::optional SurroundObstacleCheckerNode::getTransform( diff --git a/sensing/gnss_poser/CMakeLists.txt b/sensing/gnss_poser/CMakeLists.txt index 993cf2d9da9e1..167f2c51337ce 100644 --- a/sensing/gnss_poser/CMakeLists.txt +++ b/sensing/gnss_poser/CMakeLists.txt @@ -16,9 +16,7 @@ find_library(GeographicLib_LIBRARIES ) set(GNSS_POSER_HEADERS - include/gnss_poser/convert.hpp include/gnss_poser/gnss_poser_core.hpp - include/gnss_poser/gnss_stat.hpp ) ament_auto_add_library(gnss_poser_node SHARED @@ -37,5 +35,4 @@ rclcpp_components_register_node(gnss_poser_node ament_auto_package(INSTALL_TO_SHARE launch - config ) diff --git a/sensing/gnss_poser/README.md b/sensing/gnss_poser/README.md index 3487e9515c48a..b271f75eeeb6c 100644 --- a/sensing/gnss_poser/README.md +++ b/sensing/gnss_poser/README.md @@ -12,6 +12,7 @@ The `gnss_poser` is a node that subscribes gnss sensing messages and calculates | Name | Type | Description | | ------------------------------ | ------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------ | +| `/map/map_projector_info` | `tier4_map_msgs::msg::MapProjectorInfo` | map projection info | | `~/input/fix` | `sensor_msgs::msg::NavSatFix` | gnss status message | | `~/input/autoware_orientation` | `autoware_sensing_msgs::msg::GnssInsOrientationStamped` | orientation [click here for more details](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_sensing_msgs) | @@ -33,7 +34,6 @@ The `gnss_poser` is a node that subscribes gnss sensing messages and calculates | `gnss_frame` | string | "gnss" | frame id | | `gnss_base_frame` | string | "gnss_base_link" | frame id | | `map_frame` | string | "map" | frame id | -| `coordinate_system` | int | "4" | coordinate system enumeration; 1: MGRS, 4: UTM Local Coordinate System | | `gnss_pose_pub_method` | int | 0 | 0: Instant Value 1: Average Value 2: Median Value. If 0 is chosen buffer_epoch parameter loses affect. | ## Assumptions / Known limits diff --git a/sensing/gnss_poser/config/set_local_origin.param.yaml b/sensing/gnss_poser/config/set_local_origin.param.yaml deleted file mode 100644 index e931e5adceb68..0000000000000 --- a/sensing/gnss_poser/config/set_local_origin.param.yaml +++ /dev/null @@ -1,11 +0,0 @@ -/**: - ros__parameters: - - # Latitude of local origin - latitude: 40.81187906 - - # Longitude of local origin - longitude: 29.35810110 - - # Altitude of local origin - altitude: 47.62 diff --git a/sensing/gnss_poser/include/gnss_poser/convert.hpp b/sensing/gnss_poser/include/gnss_poser/convert.hpp deleted file mode 100644 index 74c9734833232..0000000000000 --- a/sensing/gnss_poser/include/gnss_poser/convert.hpp +++ /dev/null @@ -1,160 +0,0 @@ -// Copyright 2020 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 GNSS_POSER__CONVERT_HPP_ -#define GNSS_POSER__CONVERT_HPP_ - -#include "gnss_poser/gnss_stat.hpp" - -#include -#include -#include -#include - -#include - -#include - -namespace gnss_poser -{ -enum class MGRSPrecision { - _10_KILO_METER = 1, - _1_KILO_METER = 2, - _100_METER = 3, - _10_METER = 4, - _1_METER = 5, - _100_MIllI_METER = 6, - _10_MIllI_METER = 7, - _1_MIllI_METER = 8, - _100MICRO_METER = 9, -}; -// EllipsoidHeight:height above ellipsoid -// OrthometricHeight:height above geoid -double EllipsoidHeight2OrthometricHeight( - const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const rclcpp::Logger & logger) -{ - double OrthometricHeight{0.0}; - try { - GeographicLib::Geoid egm2008("egm2008-1"); - OrthometricHeight = egm2008.ConvertHeight( - nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, nav_sat_fix_msg.altitude, - GeographicLib::Geoid::ELLIPSOIDTOGEOID); - } catch (const GeographicLib::GeographicErr & err) { - RCLCPP_ERROR_STREAM( - logger, "Failed to convert Height from Ellipsoid to Orthometric" << err.what()); - } - return OrthometricHeight; -} -GNSSStat NavSatFix2UTM( - const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const rclcpp::Logger & logger, - int height_system) -{ - GNSSStat utm; - - try { - GeographicLib::UTMUPS::Forward( - nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, utm.zone, utm.east_north_up, utm.x, - utm.y); - if (height_system == 0) { - utm.z = EllipsoidHeight2OrthometricHeight(nav_sat_fix_msg, logger); - } else { - utm.z = nav_sat_fix_msg.altitude; - } - utm.latitude = nav_sat_fix_msg.latitude; - utm.longitude = nav_sat_fix_msg.longitude; - utm.altitude = nav_sat_fix_msg.altitude; - } catch (const GeographicLib::GeographicErr & err) { - RCLCPP_ERROR_STREAM(logger, "Failed to convert from LLH to UTM" << err.what()); - } - return utm; -} -GNSSStat NavSatFix2LocalCartesianUTM( - const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, - sensor_msgs::msg::NavSatFix nav_sat_fix_origin, const rclcpp::Logger & logger, int height_system) -{ - GNSSStat utm_local; - try { - // origin of the local coordinate system in global frame - GNSSStat utm_origin; - GeographicLib::UTMUPS::Forward( - nav_sat_fix_origin.latitude, nav_sat_fix_origin.longitude, utm_origin.zone, - utm_origin.east_north_up, utm_origin.x, utm_origin.y); - if (height_system == 0) { - utm_origin.z = EllipsoidHeight2OrthometricHeight(nav_sat_fix_origin, logger); - } else { - utm_origin.z = nav_sat_fix_origin.altitude; - } - - // individual coordinates of global coordinate system - double global_x = 0.0; - double global_y = 0.0; - GeographicLib::UTMUPS::Forward( - nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, utm_origin.zone, - utm_origin.east_north_up, global_x, global_y); - utm_local.latitude = nav_sat_fix_msg.latitude; - utm_local.longitude = nav_sat_fix_msg.longitude; - utm_local.altitude = nav_sat_fix_msg.altitude; - // individual coordinates of local coordinate system - utm_local.x = global_x - utm_origin.x; - utm_local.y = global_y - utm_origin.y; - if (height_system == 0) { - utm_local.z = EllipsoidHeight2OrthometricHeight(nav_sat_fix_msg, logger) - utm_origin.z; - } else { - utm_local.z = nav_sat_fix_msg.altitude - utm_origin.z; - } - } catch (const GeographicLib::GeographicErr & err) { - RCLCPP_ERROR_STREAM( - logger, "Failed to convert from LLH to UTM in local coordinates" << err.what()); - } - return utm_local; -} -GNSSStat UTM2MGRS( - const GNSSStat & utm, const MGRSPrecision & precision, const rclcpp::Logger & logger) -{ - constexpr int GZD_ID_size = 5; // size of header like "53SPU" - - GNSSStat mgrs = utm; - try { - std::string mgrs_code; - GeographicLib::MGRS::Forward( - utm.zone, utm.east_north_up, utm.x, utm.y, utm.latitude, static_cast(precision), - mgrs_code); - mgrs.mgrs_zone = std::string(mgrs_code.substr(0, GZD_ID_size)); - mgrs.x = std::stod(mgrs_code.substr(GZD_ID_size, static_cast(precision))) * - std::pow( - 10, static_cast(MGRSPrecision::_1_METER) - - static_cast(precision)); // set unit as [m] - mgrs.y = std::stod(mgrs_code.substr( - GZD_ID_size + static_cast(precision), static_cast(precision))) * - std::pow( - 10, static_cast(MGRSPrecision::_1_METER) - - static_cast(precision)); // set unit as [m] - mgrs.z = utm.z; // TODO(ryo.watanabe) - } catch (const GeographicLib::GeographicErr & err) { - RCLCPP_ERROR_STREAM(logger, "Failed to convert from UTM to MGRS" << err.what()); - } - return mgrs; -} - -GNSSStat NavSatFix2MGRS( - const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const MGRSPrecision & precision, - const rclcpp::Logger & logger, int height_system) -{ - const auto utm = NavSatFix2UTM(nav_sat_fix_msg, logger, height_system); - const auto mgrs = UTM2MGRS(utm, precision, logger); - return mgrs; -} - -} // namespace gnss_poser - -#endif // GNSS_POSER__CONVERT_HPP_ diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp index 4baaec2dfe80e..2118d33bc4b30 100644 --- a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp +++ b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp @@ -14,9 +14,8 @@ #ifndef GNSS_POSER__GNSS_POSER_CORE_HPP_ #define GNSS_POSER__GNSS_POSER_CORE_HPP_ -#include "gnss_poser/convert.hpp" -#include "gnss_poser/gnss_stat.hpp" - +#include +#include #include #include @@ -48,16 +47,15 @@ class GNSSPoser : public rclcpp::Node explicit GNSSPoser(const rclcpp::NodeOptions & node_options); private: + using MapProjectorInfo = map_interface::MapProjectorInfo; + + void callbackMapProjectorInfo(const MapProjectorInfo::Message::ConstSharedPtr msg); void callbackNavSatFix(const sensor_msgs::msg::NavSatFix::ConstSharedPtr nav_sat_fix_msg_ptr); void callbackGnssInsOrientationStamped( const autoware_sensing_msgs::msg::GnssInsOrientationStamped::ConstSharedPtr msg); bool isFixed(const sensor_msgs::msg::NavSatStatus & nav_sat_status_msg); bool canGetCovariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg); - GNSSStat convert( - const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, CoordinateSystem coordinate_system, - int height_system); - geometry_msgs::msg::Point getPosition(const GNSSStat & gnss_stat); geometry_msgs::msg::Point getMedianPosition( const boost::circular_buffer & position_buffer); geometry_msgs::msg::Point getAveragePosition( @@ -81,6 +79,7 @@ class GNSSPoser : public rclcpp::Node tf2_ros::TransformListener tf2_listener_; tf2_ros::TransformBroadcaster tf2_broadcaster_; + component_interface_utils::Subscription::SharedPtr sub_map_projector_info_; rclcpp::Subscription::SharedPtr nav_sat_fix_sub_; rclcpp::Subscription::SharedPtr autoware_orientation_sub_; @@ -89,20 +88,18 @@ class GNSSPoser : public rclcpp::Node rclcpp::Publisher::SharedPtr pose_cov_pub_; rclcpp::Publisher::SharedPtr fixed_pub_; - CoordinateSystem coordinate_system_; + MapProjectorInfo::Message projector_info_; std::string base_frame_; std::string gnss_frame_; std::string gnss_base_frame_; std::string map_frame_; - - sensor_msgs::msg::NavSatFix nav_sat_fix_origin_; + bool received_map_projector_info_ = false; bool use_gnss_ins_orientation_; boost::circular_buffer position_buffer_; autoware_sensing_msgs::msg::GnssInsOrientationStamped::SharedPtr msg_gnss_ins_orientation_stamped_; - int height_system_; int gnss_pose_pub_method; }; } // namespace gnss_poser diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp deleted file mode 100644 index d33bd022c3714..0000000000000 --- a/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp +++ /dev/null @@ -1,50 +0,0 @@ -// Copyright 2020 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 GNSS_POSER__GNSS_STAT_HPP_ -#define GNSS_POSER__GNSS_STAT_HPP_ - -#include - -namespace gnss_poser -{ -enum class CoordinateSystem { MGRS = 1, LOCAL_CARTESIAN_UTM = 4 }; - -struct GNSSStat -{ - GNSSStat() - : east_north_up(true), - zone(0), - mgrs_zone(""), - x(0), - y(0), - z(0), - latitude(0), - longitude(0), - altitude(0) - { - } - - bool east_north_up; - int zone; - std::string mgrs_zone; - double x; - double y; - double z; - double latitude; - double longitude; - double altitude; -}; -} // namespace gnss_poser - -#endif // GNSS_POSER__GNSS_STAT_HPP_ diff --git a/sensing/gnss_poser/launch/gnss_poser.launch.xml b/sensing/gnss_poser/launch/gnss_poser.launch.xml index 8a0ffb8cab8c3..60e952c6845f4 100755 --- a/sensing/gnss_poser/launch/gnss_poser.launch.xml +++ b/sensing/gnss_poser/launch/gnss_poser.launch.xml @@ -2,7 +2,6 @@ - @@ -13,10 +12,8 @@ - - @@ -31,11 +28,8 @@ - - - diff --git a/sensing/gnss_poser/package.xml b/sensing/gnss_poser/package.xml index 1488f329edecb..7d80bc903a550 100644 --- a/sensing/gnss_poser/package.xml +++ b/sensing/gnss_poser/package.xml @@ -14,7 +14,11 @@ libboost-dev autoware_sensing_msgs + component_interface_specs + component_interface_utils + geographic_msgs geographiclib + geography_utils geometry_msgs rclcpp rclcpp_components diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp index 4f0723944913b..1ae8bce2ed7f2 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -14,6 +14,9 @@ #include "gnss_poser/gnss_poser_core.hpp" +#include +#include + #include #include @@ -34,16 +37,13 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options) use_gnss_ins_orientation_(declare_parameter("use_gnss_ins_orientation", true)), msg_gnss_ins_orientation_stamped_( std::make_shared()), - height_system_(declare_parameter("height_system", 1)), gnss_pose_pub_method(declare_parameter("gnss_pose_pub_method", 0)) { - int coordinate_system = - declare_parameter("coordinate_system", static_cast(CoordinateSystem::MGRS)); - coordinate_system_ = static_cast(coordinate_system); - - nav_sat_fix_origin_.latitude = declare_parameter("latitude", 0.0); - nav_sat_fix_origin_.longitude = declare_parameter("longitude", 0.0); - nav_sat_fix_origin_.altitude = declare_parameter("altitude", 0.0); + // Subscribe to map_projector_info topic + const auto adaptor = component_interface_utils::NodeAdaptor(this); + adaptor.init_sub( + sub_map_projector_info_, + [this](const MapProjectorInfo::Message::ConstSharedPtr msg) { callbackMapProjectorInfo(msg); }); int buff_epoch = declare_parameter("buff_epoch", 1); position_buffer_.set_capacity(buff_epoch); @@ -61,9 +61,24 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options) fixed_pub_ = create_publisher("gnss_fixed", rclcpp::QoS{1}); } +void GNSSPoser::callbackMapProjectorInfo(const MapProjectorInfo::Message::ConstSharedPtr msg) +{ + projector_info_ = *msg; + received_map_projector_info_ = true; +} + void GNSSPoser::callbackNavSatFix( const sensor_msgs::msg::NavSatFix::ConstSharedPtr nav_sat_fix_msg_ptr) { + // Return immediately if map_projector_info has not been received yet. + if (!received_map_projector_info_) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), + "map_projector_info has not been received yet. Check if the map_projection_loader is " + "successfully launched."); + return; + } + // check fixed topic const bool is_fixed = isFixed(nav_sat_fix_msg_ptr->status); @@ -80,9 +95,15 @@ void GNSSPoser::callbackNavSatFix( return; } - // get position in coordinate_system - const auto gnss_stat = convert(*nav_sat_fix_msg_ptr, coordinate_system_, height_system_); - const auto position = getPosition(gnss_stat); + // get position + geographic_msgs::msg::GeoPoint gps_point; + gps_point.latitude = nav_sat_fix_msg_ptr->latitude; + gps_point.longitude = nav_sat_fix_msg_ptr->longitude; + gps_point.altitude = nav_sat_fix_msg_ptr->altitude; + geometry_msgs::msg::Point position = geography_utils::project_forward(gps_point, projector_info_); + position.z = geography_utils::convert_height( + position.z, gps_point.latitude, gps_point.longitude, MapProjectorInfo::Message::WGS84, + projector_info_.vertical_datum); geometry_msgs::msg::Pose gnss_antenna_pose{}; @@ -185,34 +206,6 @@ bool GNSSPoser::canGetCovariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN; } -GNSSStat GNSSPoser::convert( - const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, CoordinateSystem coordinate_system, - int height_system) -{ - GNSSStat gnss_stat; - if (coordinate_system == CoordinateSystem::LOCAL_CARTESIAN_UTM) { - gnss_stat = NavSatFix2LocalCartesianUTM( - nav_sat_fix_msg, nav_sat_fix_origin_, this->get_logger(), height_system); - } else if (coordinate_system == CoordinateSystem::MGRS) { - gnss_stat = NavSatFix2MGRS( - nav_sat_fix_msg, MGRSPrecision::_100MICRO_METER, this->get_logger(), height_system); - } else { - RCLCPP_ERROR_STREAM_THROTTLE( - this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), - "Unknown Coordinate System"); - } - return gnss_stat; -} - -geometry_msgs::msg::Point GNSSPoser::getPosition(const GNSSStat & gnss_stat) -{ - geometry_msgs::msg::Point point; - point.x = gnss_stat.x; - point.y = gnss_stat.y; - point.z = gnss_stat.z; - return point; -} - geometry_msgs::msg::Point GNSSPoser::getMedianPosition( const boost::circular_buffer & position_buffer) { diff --git a/sensing/imu_corrector/CMakeLists.txt b/sensing/imu_corrector/CMakeLists.txt index 8fda61f6f32ad..ded596a8a9898 100644 --- a/sensing/imu_corrector/CMakeLists.txt +++ b/sensing/imu_corrector/CMakeLists.txt @@ -8,31 +8,23 @@ ament_auto_add_library(gyro_bias_estimation_module SHARED src/gyro_bias_estimation_module.cpp ) -ament_auto_add_library(imu_corrector_node SHARED +ament_auto_add_executable(imu_corrector src/imu_corrector_core.cpp + src/imu_corrector_node.cpp ) -ament_auto_add_library(gyro_bias_estimator_node SHARED +ament_auto_add_executable(gyro_bias_estimator src/gyro_bias_estimator.cpp + src/gyro_bias_estimator_node.cpp ) -target_link_libraries(gyro_bias_estimator_node gyro_bias_estimation_module) - -rclcpp_components_register_node(imu_corrector_node - PLUGIN "imu_corrector::ImuCorrector" - EXECUTABLE imu_corrector -) - -rclcpp_components_register_node(gyro_bias_estimator_node - PLUGIN "imu_corrector::GyroBiasEstimator" - EXECUTABLE gyro_bias_estimator -) +target_link_libraries(gyro_bias_estimator gyro_bias_estimation_module) function(add_testcase filepath) get_filename_component(filename ${filepath} NAME) string(REGEX REPLACE ".cpp" "" test_name ${filename}) ament_add_gmock(${test_name} ${filepath}) - target_link_libraries("${test_name}" gyro_bias_estimation_module imu_corrector_node gyro_bias_estimator_node) + target_link_libraries("${test_name}" gyro_bias_estimation_module) ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) endfunction() diff --git a/sensing/imu_corrector/package.xml b/sensing/imu_corrector/package.xml index 67cb5c1596040..7466d158da2df 100644 --- a/sensing/imu_corrector/package.xml +++ b/sensing/imu_corrector/package.xml @@ -17,6 +17,7 @@ rclcpp_components sensor_msgs tf2 + tf2_geometry_msgs tf2_ros tier4_autoware_utils diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.cpp b/sensing/imu_corrector/src/gyro_bias_estimator.cpp index 42795805f803e..581eccb0097d4 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.cpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.cpp @@ -16,8 +16,8 @@ namespace imu_corrector { -GyroBiasEstimator::GyroBiasEstimator(const rclcpp::NodeOptions & node_options) -: Node("gyro_bias_validator", node_options), +GyroBiasEstimator::GyroBiasEstimator() +: Node("gyro_bias_validator"), gyro_bias_threshold_(declare_parameter("gyro_bias_threshold")), angular_velocity_offset_x_(declare_parameter("angular_velocity_offset_x")), angular_velocity_offset_y_(declare_parameter("angular_velocity_offset_y")), @@ -104,6 +104,3 @@ void GyroBiasEstimator::update_diagnostics(diagnostic_updater::DiagnosticStatusW } } // namespace imu_corrector - -#include -RCLCPP_COMPONENTS_REGISTER_NODE(imu_corrector::GyroBiasEstimator) diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.hpp b/sensing/imu_corrector/src/gyro_bias_estimator.hpp index 37ca1d81dc8fa..e74a0390af3aa 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.hpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.hpp @@ -38,7 +38,7 @@ class GyroBiasEstimator : public rclcpp::Node using Vector3 = geometry_msgs::msg::Vector3; public: - explicit GyroBiasEstimator(const rclcpp::NodeOptions & node_options); + GyroBiasEstimator(); private: void update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat); diff --git a/sensing/imu_corrector/src/gyro_bias_estimator_node.cpp b/sensing/imu_corrector/src/gyro_bias_estimator_node.cpp new file mode 100644 index 0000000000000..a491957bbb57f --- /dev/null +++ b/sensing/imu_corrector/src/gyro_bias_estimator_node.cpp @@ -0,0 +1,26 @@ +// Copyright 2023 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 "gyro_bias_estimator.hpp" + +#include + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/sensing/imu_corrector/src/imu_corrector_core.cpp b/sensing/imu_corrector/src/imu_corrector_core.cpp index c31f4968f557b..a26c64925729c 100644 --- a/sensing/imu_corrector/src/imu_corrector_core.cpp +++ b/sensing/imu_corrector/src/imu_corrector_core.cpp @@ -14,6 +14,13 @@ #include "imu_corrector_core.hpp" +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif +#include + #include std::array transformCovariance(const std::array & cov) @@ -46,9 +53,8 @@ geometry_msgs::msg::Vector3 transformVector3( namespace imu_corrector { -ImuCorrector::ImuCorrector(const rclcpp::NodeOptions & node_options) -: Node("imu_corrector", node_options), - output_frame_(declare_parameter("base_link", "base_link")) +ImuCorrector::ImuCorrector() +: Node("imu_corrector"), output_frame_(declare_parameter("base_link", "base_link")) { transform_listener_ = std::make_shared(this); @@ -117,6 +123,3 @@ void ImuCorrector::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_m } } // namespace imu_corrector - -#include -RCLCPP_COMPONENTS_REGISTER_NODE(imu_corrector::ImuCorrector) diff --git a/sensing/imu_corrector/src/imu_corrector_core.hpp b/sensing/imu_corrector/src/imu_corrector_core.hpp index c155288f3cb17..7709c611ab714 100644 --- a/sensing/imu_corrector/src/imu_corrector_core.hpp +++ b/sensing/imu_corrector/src/imu_corrector_core.hpp @@ -14,8 +14,8 @@ #ifndef IMU_CORRECTOR_CORE_HPP_ #define IMU_CORRECTOR_CORE_HPP_ +#include "tier4_autoware_utils/ros/msg_covariance.hpp" #include "tier4_autoware_utils/ros/transform_listener.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include @@ -34,7 +34,7 @@ class ImuCorrector : public rclcpp::Node using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; public: - explicit ImuCorrector(const rclcpp::NodeOptions & node_options); + ImuCorrector(); private: void callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr); diff --git a/sensing/imu_corrector/src/imu_corrector_node.cpp b/sensing/imu_corrector/src/imu_corrector_node.cpp new file mode 100644 index 0000000000000..c1df5bee28439 --- /dev/null +++ b/sensing/imu_corrector/src/imu_corrector_node.cpp @@ -0,0 +1,26 @@ +// Copyright 2023 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 "imu_corrector_core.hpp" + +#include + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp b/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp index 7c302eec20431..5829277335433 100644 --- a/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp +++ b/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp @@ -36,7 +36,12 @@ void FasterVoxelGridDownsampleFilter::set_field_offsets(const PointCloud2ConstPt x_offset_ = input->fields[pcl::getFieldIndex(*input, "x")].offset; y_offset_ = input->fields[pcl::getFieldIndex(*input, "y")].offset; z_offset_ = input->fields[pcl::getFieldIndex(*input, "z")].offset; - intensity_offset_ = input->fields[pcl::getFieldIndex(*input, "intensity")].offset; + int intensity_index = pcl::getFieldIndex(*input, "intensity"); + if (intensity_index != -1) { + intensity_offset_ = input->fields[intensity_index].offset; + } else { + intensity_offset_ = z_offset_ + sizeof(float); + } offset_initialized_ = true; } diff --git a/sensing/radar_static_pointcloud_filter/CMakeLists.txt b/sensing/radar_static_pointcloud_filter/CMakeLists.txt index 1f08f7842cfdf..ecd33d3166b19 100644 --- a/sensing/radar_static_pointcloud_filter/CMakeLists.txt +++ b/sensing/radar_static_pointcloud_filter/CMakeLists.txt @@ -27,4 +27,5 @@ endif() ament_auto_package( INSTALL_TO_SHARE launch + config ) diff --git a/sensing/radar_static_pointcloud_filter/config/radar_static_pointcloud_filter.param.yaml b/sensing/radar_static_pointcloud_filter/config/radar_static_pointcloud_filter.param.yaml new file mode 100644 index 0000000000000..a429f7a065ffe --- /dev/null +++ b/sensing/radar_static_pointcloud_filter/config/radar_static_pointcloud_filter.param.yaml @@ -0,0 +1,17 @@ +# Copyright 2023 Foxconn, 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. + +/**: + ros__parameters: + doppler_velocity_sd: 4.0 diff --git a/sensing/radar_static_pointcloud_filter/include/radar_static_pointcloud_filter/radar_static_pointcloud_filter_node.hpp b/sensing/radar_static_pointcloud_filter/include/radar_static_pointcloud_filter/radar_static_pointcloud_filter_node.hpp index e97e8fa5ca500..f1ab9344d58f3 100644 --- a/sensing/radar_static_pointcloud_filter/include/radar_static_pointcloud_filter/radar_static_pointcloud_filter_node.hpp +++ b/sensing/radar_static_pointcloud_filter/include/radar_static_pointcloud_filter/radar_static_pointcloud_filter_node.hpp @@ -15,7 +15,7 @@ #ifndef RADAR_STATIC_POINTCLOUD_FILTER__RADAR_STATIC_POINTCLOUD_FILTER_NODE_HPP_ #define RADAR_STATIC_POINTCLOUD_FILTER__RADAR_STATIC_POINTCLOUD_FILTER_NODE_HPP_ -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/ros/transform_listener.hpp" #include diff --git a/sensing/radar_static_pointcloud_filter/launch/radar_static_pointcloud_filter.launch.xml b/sensing/radar_static_pointcloud_filter/launch/radar_static_pointcloud_filter.launch.xml index 08fdeee74f15a..b9491227837c5 100644 --- a/sensing/radar_static_pointcloud_filter/launch/radar_static_pointcloud_filter.launch.xml +++ b/sensing/radar_static_pointcloud_filter/launch/radar_static_pointcloud_filter.launch.xml @@ -3,13 +3,14 @@ - + + - + diff --git a/sensing/radar_static_pointcloud_filter/schema/radar_static_pointcloud_filter.schema.json b/sensing/radar_static_pointcloud_filter/schema/radar_static_pointcloud_filter.schema.json new file mode 100644 index 0000000000000..65340d889f62e --- /dev/null +++ b/sensing/radar_static_pointcloud_filter/schema/radar_static_pointcloud_filter.schema.json @@ -0,0 +1,31 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Radar Static Pointcloud Filter Node", + "type": "object", + "definitions": { + "radar_static_pointcloud_filter": { + "type": "object", + "properties": { + "doppler_velocity_sd": { + "type": "number", + "default": "4.0", + "minimum": 0.0, + "description": "Standard deviation for radar doppler velocity. [m/s]" + } + }, + "required": ["doppler_velocity_sd"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/radar_static_pointcloud_filter" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp b/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp index 812a96e7cdcd5..772dcef753c11 100644 --- a/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp +++ b/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2022 TIER IV, Inc. +// Copyright 2022-2023 Foxconn, 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. @@ -16,6 +16,7 @@ #include #include +#include #include #include @@ -92,7 +93,7 @@ RadarStaticPointcloudFilterNode::RadarStaticPointcloudFilterNode( std::bind(&RadarStaticPointcloudFilterNode::onSetParam, this, _1)); // Node Parameter - node_param_.doppler_velocity_sd = declare_parameter("doppler_velocity_sd", 2.0); + node_param_.doppler_velocity_sd = declare_parameter("doppler_velocity_sd"); // Subscriber transform_listener_ = std::make_shared(this); diff --git a/simulator/dummy_perception_publisher/src/node.cpp b/simulator/dummy_perception_publisher/src/node.cpp index 6036e760fed47..f4cb6063a9bf6 100644 --- a/simulator/dummy_perception_publisher/src/node.cpp +++ b/simulator/dummy_perception_publisher/src/node.cpp @@ -14,7 +14,7 @@ #include "dummy_perception_publisher/node.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include #include diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index 742f87411b7d8..8eca49d9ffadf 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -22,6 +22,7 @@ #include "autoware_auto_control_msgs/msg/ackermann_control_command.hpp" #include "autoware_auto_geometry_msgs/msg/complex32.hpp" +#include "autoware_auto_mapping_msgs/msg/had_map_bin.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_vehicle_msgs/msg/control_mode_command.hpp" #include "autoware_auto_vehicle_msgs/msg/control_mode_report.hpp" @@ -47,6 +48,7 @@ #include "sensor_msgs/msg/imu.hpp" #include "tier4_external_api_msgs/srv/initialize_pose.hpp" +#include #include #include @@ -62,6 +64,7 @@ namespace simple_planning_simulator using autoware_auto_control_msgs::msg::AckermannControlCommand; using autoware_auto_geometry_msgs::msg::Complex32; +using autoware_auto_mapping_msgs::msg::HADMapBin; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_vehicle_msgs::msg::ControlModeReport; using autoware_auto_vehicle_msgs::msg::Engage; @@ -143,6 +146,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_vehicle_cmd_; rclcpp::Subscription::SharedPtr sub_ackermann_cmd_; rclcpp::Subscription::SharedPtr sub_manual_ackermann_cmd_; + rclcpp::Subscription::SharedPtr sub_map_; rclcpp::Subscription::SharedPtr sub_init_pose_; rclcpp::Subscription::SharedPtr sub_init_twist_; rclcpp::Subscription::SharedPtr sub_trajectory_; @@ -160,6 +164,8 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node rcl_interfaces::msg::SetParametersResult on_parameter( const std::vector & parameters); + lanelet::ConstLanelets road_lanelets_; + /* tf */ tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; @@ -179,6 +185,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node Trajectory::ConstSharedPtr current_trajectory_ptr_; bool simulate_motion_; //!< stop vehicle motion simulation if false ControlModeReport current_control_mode_; + bool enable_road_slope_simulation_; /* frame_id */ std::string simulated_frame_id_; //!< @brief simulated vehicle frame id @@ -214,7 +221,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node /** * @brief set input steering, velocity, and acceleration of the vehicle model */ - void set_input(const AckermannControlCommand & cmd); + void set_input(const AckermannControlCommand & cmd, const double acc_by_slope); /** * @brief set current_vehicle_state_ with received message @@ -226,6 +233,11 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node */ void on_hazard_lights_cmd(const HazardLightsCommand::ConstSharedPtr msg); + /** + * @brief subscribe lanelet map + */ + void on_map(const HADMapBin::ConstSharedPtr msg); + /** * @brief set initial pose for simulation with received message */ @@ -276,6 +288,12 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node */ TransformStamped get_transform_msg(const std::string parent_frame, const std::string child_frame); + /** + * @brief calculate ego pitch angle from trajectory + * @return ego pitch angle + */ + double calculate_ego_pitch() const; + /** * @brief timer callback for simulation with loop_rate */ diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py index 7837c61ec6593..2f9c2cfe333f4 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py @@ -53,6 +53,7 @@ def launch_setup(context, *args, **kwargs): }, ], remappings=[ + ("input/vector_map", "/map/vector_map"), ("input/initialpose", "/initialpose3d"), ("input/ackermann_control_command", "/control/command/control_cmd"), ("input/manual_ackermann_control_command", "/vehicle/command/manual_control_cmd"), diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml index c83433d35bb1b..5652be138b18f 100644 --- a/simulator/simple_planning_simulator/package.xml +++ b/simulator/simple_planning_simulator/package.xml @@ -12,10 +12,14 @@ autoware_cmake autoware_auto_control_msgs + autoware_auto_mapping_msgs autoware_auto_planning_msgs autoware_auto_tf2 autoware_auto_vehicle_msgs geometry_msgs + lanelet2_core + lanelet2_extension + motion_utils nav_msgs rclcpp rclcpp_components diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index cfc58cdaeed63..fe28d063eedd3 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -15,11 +15,19 @@ #include "simple_planning_simulator/simple_planning_simulator_core.hpp" #include "autoware_auto_tf2/tf2_autoware_auto_msgs.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "rclcpp_components/register_node_macro.hpp" #include "simple_planning_simulator/vehicle_model/sim_model.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/ros/msg_covariance.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" +#include +#include + +#include +#include #include #include @@ -45,13 +53,14 @@ autoware_auto_vehicle_msgs::msg::VelocityReport to_velocity_report( return velocity; } -nav_msgs::msg::Odometry to_odometry(const std::shared_ptr vehicle_model_ptr) +nav_msgs::msg::Odometry to_odometry( + const std::shared_ptr vehicle_model_ptr, const double ego_pitch_angle) { nav_msgs::msg::Odometry odometry; odometry.pose.pose.position.x = vehicle_model_ptr->getX(); odometry.pose.pose.position.y = vehicle_model_ptr->getY(); - odometry.pose.pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(vehicle_model_ptr->getYaw()); + odometry.pose.pose.orientation = tier4_autoware_utils::createQuaternionFromRPY( + 0.0, ego_pitch_angle, vehicle_model_ptr->getYaw()); odometry.twist.twist.linear.x = vehicle_model_ptr->getVx(); odometry.twist.twist.angular.z = vehicle_model_ptr->getWz(); @@ -66,6 +75,19 @@ autoware_auto_vehicle_msgs::msg::SteeringReport to_steering_report( return steer; } +std::vector convert_centerline_to_points( + const lanelet::Lanelet & lanelet) +{ + std::vector centerline_points; + for (const auto & point : lanelet.centerline()) { + geometry_msgs::msg::Point center_point; + center_point.x = point.basicPoint().x(); + center_point.y = point.basicPoint().y(); + center_point.z = point.basicPoint().z(); + centerline_points.push_back(center_point); + } + return centerline_points; +} } // namespace namespace simulation @@ -80,11 +102,15 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt origin_frame_id_ = declare_parameter("origin_frame_id", "odom"); add_measurement_noise_ = declare_parameter("add_measurement_noise", false); simulate_motion_ = declare_parameter("initial_engage_state"); + enable_road_slope_simulation_ = declare_parameter("enable_road_slope_simulation", false); using rclcpp::QoS; using std::placeholders::_1; using std::placeholders::_2; + sub_map_ = create_subscription( + "input/vector_map", rclcpp::QoS(10).transient_local(), + std::bind(&SimplePlanningSimulator::on_map, this, _1)); sub_init_pose_ = create_subscription( "input/initialpose", QoS{1}, std::bind(&SimplePlanningSimulator::on_initialpose, this, _1)); sub_init_twist_ = create_subscription( @@ -251,6 +277,44 @@ rcl_interfaces::msg::SetParametersResult SimplePlanningSimulator::on_parameter( return result; } +double SimplePlanningSimulator::calculate_ego_pitch() const +{ + const double ego_x = vehicle_model_ptr_->getX(); + const double ego_y = vehicle_model_ptr_->getY(); + const double ego_yaw = vehicle_model_ptr_->getYaw(); + + geometry_msgs::msg::Pose ego_pose; + ego_pose.position.x = ego_x; + ego_pose.position.y = ego_y; + ego_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(ego_yaw); + + // calculate prev/next point of lanelet centerline nearest to ego pose. + lanelet::Lanelet ego_lanelet; + if (!lanelet::utils::query::getClosestLaneletWithConstrains( + road_lanelets_, ego_pose, &ego_lanelet, 2.0, std::numeric_limits::max())) { + return 0.0; + } + const auto centerline_points = convert_centerline_to_points(ego_lanelet); + const size_t ego_seg_idx = + motion_utils::findNearestSegmentIndex(centerline_points, ego_pose.position); + + const auto & prev_point = centerline_points.at(ego_seg_idx); + const auto & next_point = centerline_points.at(ego_seg_idx + 1); + + // calculate ego yaw angle on lanelet coordinates + const double lanelet_yaw = std::atan2(next_point.y - prev_point.y, next_point.x - prev_point.x); + const double ego_yaw_against_lanelet = ego_yaw - lanelet_yaw; + + // calculate ego pitch angle considering ego yaw. + const double diff_z = next_point.z - prev_point.z; + const double diff_xy = std::hypot(next_point.x - prev_point.x, next_point.y - prev_point.y) / + std::cos(ego_yaw_against_lanelet); + const bool reverse_sign = std::cos(ego_yaw_against_lanelet) < 0.0; + const double ego_pitch_angle = + reverse_sign ? std::atan2(-diff_z, -diff_xy) : std::atan2(diff_z, diff_xy); + return ego_pitch_angle; +} + void SimplePlanningSimulator::on_timer() { if (!is_initialized_) { @@ -258,16 +322,21 @@ void SimplePlanningSimulator::on_timer() return; } + // calculate longitudinal acceleration by slope + const double ego_pitch_angle = calculate_ego_pitch(); + const double acc_by_slope = + enable_road_slope_simulation_ ? -9.81 * std::sin(ego_pitch_angle) : 0.0; + // update vehicle dynamics { const double dt = delta_time_.get_dt(get_clock()->now()); if (current_control_mode_.mode == ControlModeReport::AUTONOMOUS) { vehicle_model_ptr_->setGear(current_gear_cmd_.command); - set_input(current_ackermann_cmd_); + set_input(current_ackermann_cmd_, acc_by_slope); } else { vehicle_model_ptr_->setGear(current_manual_gear_cmd_.command); - set_input(current_manual_ackermann_cmd_); + set_input(current_manual_ackermann_cmd_, acc_by_slope); } if (simulate_motion_) { @@ -276,7 +345,7 @@ void SimplePlanningSimulator::on_timer() } // set current state - current_odometry_ = to_odometry(vehicle_model_ptr_); + current_odometry_ = to_odometry(vehicle_model_ptr_, ego_pitch_angle); current_odometry_.pose.pose.position.z = get_z_pose_from_trajectory( current_odometry_.pose.pose.position.x, current_odometry_.pose.pose.position.y); @@ -308,6 +377,19 @@ void SimplePlanningSimulator::on_timer() publish_tf(current_odometry_); } +void SimplePlanningSimulator::on_map(const HADMapBin::ConstSharedPtr msg) +{ + auto lanelet_map_ptr = std::make_shared(); + + lanelet::routing::RoutingGraphPtr routing_graph_ptr; + lanelet::traffic_rules::TrafficRulesPtr traffic_rules_ptr; + lanelet::utils::conversion::fromBinMsg( + *msg, lanelet_map_ptr, &traffic_rules_ptr, &routing_graph_ptr); + + lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr); + road_lanelets_ = lanelet::utils::query::roadLanelets(all_lanelets); +} + void SimplePlanningSimulator::on_initialpose(const PoseWithCovarianceStamped::ConstSharedPtr msg) { // save initial pose @@ -344,7 +426,8 @@ void SimplePlanningSimulator::on_set_pose( response->status = tier4_api_utils::response_success(); } -void SimplePlanningSimulator::set_input(const AckermannControlCommand & cmd) +void SimplePlanningSimulator::set_input( + const AckermannControlCommand & cmd, const double acc_by_slope) { const auto steer = cmd.lateral.steering_tire_angle; const auto vel = cmd.longitudinal.speed; @@ -356,11 +439,11 @@ void SimplePlanningSimulator::set_input(const AckermannControlCommand & cmd) // TODO(Watanabe): The definition of the sign of acceleration in REVERSE mode is different // between .auto and proposal.iv, and will be discussed later. - float acc = accel; + float acc = accel + acc_by_slope; if (gear == GearCommand::NONE) { acc = 0.0; } else if (gear == GearCommand::REVERSE || gear == GearCommand::REVERSE_2) { - acc = -accel; + acc = -accel - acc_by_slope; } if ( diff --git a/system/default_ad_api/package.xml b/system/default_ad_api/package.xml index dfceeabe41122..6a9a22f39cdc4 100644 --- a/system/default_ad_api/package.xml +++ b/system/default_ad_api/package.xml @@ -23,7 +23,8 @@ autoware_planning_msgs component_interface_specs component_interface_utils - lanelet2_extension + geographic_msgs + geography_utils motion_utils nav_msgs rclcpp diff --git a/system/default_ad_api/src/planning.cpp b/system/default_ad_api/src/planning.cpp index 46c8ea4125c9a..5df76e9b5f3cc 100644 --- a/system/default_ad_api/src/planning.cpp +++ b/system/default_ad_api/src/planning.cpp @@ -14,6 +14,8 @@ #include "planning.hpp" +#include + #include #include #include diff --git a/system/default_ad_api/src/planning.hpp b/system/default_ad_api/src/planning.hpp index 03b45fd3b5d41..b533390dcc6fb 100644 --- a/system/default_ad_api/src/planning.hpp +++ b/system/default_ad_api/src/planning.hpp @@ -18,7 +18,7 @@ #include #include #include -#include +#include #include #include diff --git a/system/default_ad_api/src/vehicle.cpp b/system/default_ad_api/src/vehicle.cpp index fb434adbb0e40..e25933e6ca2ad 100644 --- a/system/default_ad_api/src/vehicle.cpp +++ b/system/default_ad_api/src/vehicle.cpp @@ -14,6 +14,11 @@ #include "vehicle.hpp" +#include +#include + +#include + #include namespace default_ad_api @@ -89,15 +94,6 @@ void VehicleNode::kinematic_state( kinematic_state_msgs_ = msg_ptr; } -Eigen::Vector3d VehicleNode::toBasicPoint3dPt(const geometry_msgs::msg::Point src) -{ - Eigen::Vector3d dst; - dst.x() = src.x; - dst.y() = src.y; - dst.z() = src.z; - return dst; -} - void VehicleNode::acceleration_status( const localization_interface::Acceleration::Message::ConstSharedPtr msg_ptr) { @@ -146,26 +142,16 @@ void VehicleNode::publish_kinematics() vehicle_kinematics.twist.header = kinematic_state_msgs_->header; vehicle_kinematics.twist.header.frame_id = kinematic_state_msgs_->child_frame_id; vehicle_kinematics.twist.twist = kinematic_state_msgs_->twist; - if (map_projector_info_->projector_type == MapProjectorInfo::MGRS) { - lanelet::GPSPoint projected_gps_point = lanelet::projection::MGRSProjector::reverse( - toBasicPoint3dPt(kinematic_state_msgs_->pose.pose.position), map_projector_info_->mgrs_grid); - vehicle_kinematics.geographic_pose.header = kinematic_state_msgs_->header; - vehicle_kinematics.geographic_pose.header.frame_id = "global"; - vehicle_kinematics.geographic_pose.position.latitude = projected_gps_point.lat; - vehicle_kinematics.geographic_pose.position.longitude = projected_gps_point.lon; - vehicle_kinematics.geographic_pose.position.altitude = projected_gps_point.ele; - } else if (map_projector_info_->projector_type == MapProjectorInfo::LOCAL_CARTESIAN_UTM) { - lanelet::GPSPoint position{ - map_projector_info_->map_origin.latitude, map_projector_info_->map_origin.longitude}; - lanelet::Origin origin{position}; - lanelet::projection::UtmProjector projector{origin}; - lanelet::GPSPoint projected_gps_point = - projector.reverse(toBasicPoint3dPt(kinematic_state_msgs_->pose.pose.position)); + if (map_projector_info_->projector_type != MapProjectorInfo::LOCAL) { + const geographic_msgs::msg::GeoPoint projected_gps_point = geography_utils::project_reverse( + kinematic_state_msgs_->pose.pose.position, *map_projector_info_); vehicle_kinematics.geographic_pose.header = kinematic_state_msgs_->header; vehicle_kinematics.geographic_pose.header.frame_id = "global"; - vehicle_kinematics.geographic_pose.position.latitude = projected_gps_point.lat; - vehicle_kinematics.geographic_pose.position.longitude = projected_gps_point.lon; - vehicle_kinematics.geographic_pose.position.altitude = projected_gps_point.ele; + vehicle_kinematics.geographic_pose.position.latitude = projected_gps_point.latitude; + vehicle_kinematics.geographic_pose.position.longitude = projected_gps_point.longitude; + vehicle_kinematics.geographic_pose.position.altitude = geography_utils::convert_height( + projected_gps_point.altitude, projected_gps_point.latitude, projected_gps_point.longitude, + map_projector_info_->vertical_datum, MapProjectorInfo::WGS84); } else { vehicle_kinematics.geographic_pose.position.latitude = std::numeric_limits::quiet_NaN(); vehicle_kinematics.geographic_pose.position.longitude = diff --git a/system/default_ad_api/src/vehicle.hpp b/system/default_ad_api/src/vehicle.hpp index 90f8b686206c5..56012152a132b 100644 --- a/system/default_ad_api/src/vehicle.hpp +++ b/system/default_ad_api/src/vehicle.hpp @@ -15,8 +15,6 @@ #ifndef VEHICLE_HPP_ #define VEHICLE_HPP_ -#include "lanelet2_extension/projection/mgrs_projector.hpp" - #include #include #include @@ -27,9 +25,6 @@ #include #include -#include -#include - #include // This file should be included after messages. @@ -83,7 +78,6 @@ class VehicleNode : public rclcpp::Node void publish_kinematics(); void publish_status(); void on_timer(); - Eigen::Vector3d toBasicPoint3dPt(const geometry_msgs::msg::Point src); }; } // namespace default_ad_api diff --git a/vehicle/vehicle_info_util/CMakeLists.txt b/vehicle/vehicle_info_util/CMakeLists.txt index f490b0680a19a..ca72b0ed2db65 100644 --- a/vehicle/vehicle_info_util/CMakeLists.txt +++ b/vehicle/vehicle_info_util/CMakeLists.txt @@ -5,6 +5,7 @@ find_package(autoware_cmake REQUIRED) autoware_package() ament_auto_add_library(vehicle_info_util SHARED + src/vehicle_info.cpp src/vehicle_info_util.cpp ) diff --git a/vehicle/vehicle_info_util/include/vehicle_info_util/vehicle_info.hpp b/vehicle/vehicle_info_util/include/vehicle_info_util/vehicle_info.hpp index 967e71933a209..cdbbe0427b145 100644 --- a/vehicle/vehicle_info_util/include/vehicle_info_util/vehicle_info.hpp +++ b/vehicle/vehicle_info_util/include/vehicle_info_util/vehicle_info.hpp @@ -15,7 +15,7 @@ #ifndef VEHICLE_INFO_UTIL__VEHICLE_INFO_HPP_ #define VEHICLE_INFO_UTIL__VEHICLE_INFO_HPP_ -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/boost_geometry.hpp" namespace vehicle_info_util { @@ -47,76 +47,18 @@ struct VehicleInfo double min_height_offset_m; double max_height_offset_m; - tier4_autoware_utils::LinearRing2d createFootprint(const double margin = 0.0) const - { - return createFootprint(margin, margin); - } - + tier4_autoware_utils::LinearRing2d createFootprint(const double margin = 0.0) const; tier4_autoware_utils::LinearRing2d createFootprint( - const double lat_margin, const double lon_margin) const - { - using tier4_autoware_utils::LinearRing2d; - using tier4_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; - } + const double lat_margin, const double lon_margin) const; }; /// Create vehicle info from base parameters -inline VehicleInfo createVehicleInfo( +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) -{ - // 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; + const double max_steer_angle_rad); - 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_, - }; -} } // namespace vehicle_info_util #endif // VEHICLE_INFO_UTIL__VEHICLE_INFO_HPP_ diff --git a/vehicle/vehicle_info_util/src/vehicle_info.cpp b/vehicle/vehicle_info_util/src/vehicle_info.cpp new file mode 100644 index 0000000000000..1a47252f82d04 --- /dev/null +++ b/vehicle/vehicle_info_util/src/vehicle_info.cpp @@ -0,0 +1,88 @@ +// 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 "vehicle_info_util/vehicle_info.hpp" + +namespace vehicle_info_util +{ +tier4_autoware_utils::LinearRing2d VehicleInfo::createFootprint(const double margin) const +{ + return createFootprint(margin, margin); +} + +tier4_autoware_utils::LinearRing2d VehicleInfo::createFootprint( + const double lat_margin, const double lon_margin) const +{ + using tier4_autoware_utils::LinearRing2d; + using tier4_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, + 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) +{ + // 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_, + }; +} + +} // namespace vehicle_info_util