diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 82f7197d87aed..f9983f0fccc3f 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -41,6 +41,7 @@ common/tier4_datetime_rviz_plugin/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp common/tier4_debug_rviz_plugin/** takayuki.murooka@tier4.jp common/tier4_debug_tools/** ryohsuke.mitsudome@tier4.jp common/tier4_localization_rviz_plugin/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp takamasa.horibe@tier4.jp +common/tier4_logging_level_configure_rviz_plugin/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takamasa.horibe@tier4.jp common/tier4_perception_rviz_plugin/** yukihiro.saito@tier4.jp common/tier4_planning_rviz_plugin/** takayuki.murooka@tier4.jp yukihiro.saito@tier4.jp yutaka.shimizu@tier4.jp common/tier4_screen_capture_rviz_plugin/** taiki.tanaka@tier4.jp @@ -84,9 +85,10 @@ launch/tier4_sensing_launch/** yukihiro.saito@tier4.jp launch/tier4_simulator_launch/** keisuke.shima@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp launch/tier4_system_launch/** akihiro.sakurai@tier4.jp fumihito.ito@tier4.jp launch/tier4_vehicle_launch/** yukihiro.saito@tier4.jp -localization/ar_tag_based_localizer/** masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp localization/ekf_localizer/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp takamasa.horibe@tier4.jp takeshi.ishita@tier4.jp yamato.ando@tier4.jp localization/gyro_odometer/** masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp +localization/landmark_based_localizer/ar_tag_based_localizer/** masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp +localization/landmark_based_localizer/landmark_tf_caster/** masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp localization/localization_error_monitor/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp localization/ndt_scan_matcher/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp localization/pose2twist/** masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp @@ -150,7 +152,7 @@ planning/behavior_path_planner/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4 planning/behavior_velocity_blind_spot_module/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_crosswalk_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_detection_area_module/** kyoichi.sugahara@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_intersection_module/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_intersection_module/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_no_drivable_lane_module/** ahmed.ebrahim@leodrive.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_no_stopping_area_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_occlusion_spot_module/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp @@ -160,6 +162,7 @@ planning/behavior_velocity_planner_common/** isamu.takagi@tier4.jp shumpei.wakab planning/behavior_velocity_run_out_module/** makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_stop_line_module/** shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp +planning/behavior_velocity_template_module/** daniel.sanchez@tier4.jp planning/behavior_velocity_traffic_light_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_virtual_traffic_light_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_walkway_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp @@ -215,6 +218,8 @@ system/dummy_infrastructure/** ryohsuke.mitsudome@tier4.jp system/emergency_handler/** makoto.kurihara@tier4.jp system/mrm_comfortable_stop_operator/** makoto.kurihara@tier4.jp system/mrm_emergency_stop_operator/** makoto.kurihara@tier4.jp +system/system_diagnostic_graph/** isamu.takagi@tier4.jp +system/system_diagnostic_monitor/** isamu.takagi@tier4.jp system/system_error_monitor/** fumihito.ito@tier4.jp system/system_monitor/** akihiro.sakurai@tier4.jp fumihito.ito@tier4.jp system/topic_state_monitor/** ryohsuke.mitsudome@tier4.jp @@ -224,4 +229,4 @@ vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/** taiki.tanaka@ti vehicle/external_cmd_converter/** takamasa.horibe@tier4.jp vehicle/raw_vehicle_cmd_converter/** makoto.kurihara@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp vehicle/steer_offset_estimator/** taiki.tanaka@tier4.jp -vehicle/vehicle_info_util/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp yamato.ando@tier4.jp +vehicle/vehicle_info_util/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp diff --git a/.github/sync-files.yaml b/.github/sync-files.yaml index 4a1f20cada359..f9cb3065f12a7 100644 --- a/.github/sync-files.yaml +++ b/.github/sync-files.yaml @@ -42,20 +42,6 @@ - \"\" - -cuda include:" {source} - - source: .github/workflows/build-and-test-differential.yaml - pre-commands: | - sd "container: ros:(\w+)" "container: ghcr.io/autowarefoundation/autoware-universe:\$1-latest" {source} - - sd -s 'container: ${{ matrix.container }}' 'container: ${{ matrix.container }}${{ matrix.container-suffix }}' {source} - sd -- \ - " include:" \ - " container-suffix: - - \"\" - - -cuda - include:" {source} - - sd "^ container: ghcr.io/autowarefoundation/autoware-universe:(\w+)-latest\$" \ - " container: ghcr.io/autowarefoundation/autoware-universe:\$1-latest-cuda" {source} - source: .github/workflows/build-and-test-differential-self-hosted.yaml pre-commands: | sd "container: ros:(\w+)" "container: ghcr.io/autowarefoundation/autoware-universe:\$1-latest" {source} diff --git a/common/component_interface_specs/CMakeLists.txt b/common/component_interface_specs/CMakeLists.txt index 31c723600183f..b519a0ddce86a 100644 --- a/common/component_interface_specs/CMakeLists.txt +++ b/common/component_interface_specs/CMakeLists.txt @@ -4,4 +4,38 @@ project(component_interface_specs) find_package(autoware_cmake REQUIRED) autoware_package() +include_directories( + include + SYSTEM + ${rclcpp_INCLUDE_DIRS} + ${rosidl_runtime_cpp_INCLUDE_DIRS} + ${rcl_INCLUDE_DIRS} + ${autoware_adapi_v1_msgs_INCLUDE_DIRS} + ${autoware_auto_planning_msgs_INCLUDE_DIRS} + ${autoware_planning_msgs_INCLUDE_DIRS} + ${autoware_auto_vehicle_msgs_INCLUDE_DIRS} + ${tier4_control_msgs_INCLUDE_DIRS} + ${nav_msgs_INCLUDE_DIRS} + ${tier4_system_msgs_INCLUDE_DIRS} + ${tier4_vehicle_msgs_INCLUDE_DIRS} + ${autoware_auto_perception_msgs_INCLUDE_DIRS} + ${tier4_map_msgs_INCLUDE_DIRS} +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_component_interface_specs + test/gtest_main.cpp + test/test_planning.cpp + test/test_control.cpp + test/test_localization.cpp + test/test_system.cpp + test/test_map.cpp + test/test_perception.cpp + test/test_vehicle.cpp + ) + target_include_directories(test_component_interface_specs + PRIVATE include + ) +endif() + ament_auto_package() diff --git a/common/component_interface_specs/package.xml b/common/component_interface_specs/package.xml index 7ebc3f0d1bea9..93f77c651869d 100644 --- a/common/component_interface_specs/package.xml +++ b/common/component_interface_specs/package.xml @@ -11,13 +11,28 @@ Apache License 2.0 ament_cmake_auto + ament_cmake_core + ament_cmake_export_dependencies + ament_cmake_test autoware_cmake + ament_cmake_core + ament_cmake_test + autoware_adapi_v1_msgs + autoware_auto_perception_msgs + autoware_auto_planning_msgs autoware_auto_vehicle_msgs + autoware_planning_msgs + nav_msgs + rcl + rclcpp + rosidl_runtime_cpp + tier4_control_msgs tier4_map_msgs + tier4_system_msgs tier4_vehicle_msgs - + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/common/component_interface_specs/test/gtest_main.cpp b/common/component_interface_specs/test/gtest_main.cpp new file mode 100644 index 0000000000000..81d9d5345270d --- /dev/null +++ b/common/component_interface_specs/test/gtest_main.cpp @@ -0,0 +1,21 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "gtest/gtest.h" + +int main(int argc, char * argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/common/component_interface_specs/test/test_control.cpp b/common/component_interface_specs/test/test_control.cpp new file mode 100644 index 0000000000000..366641eacbd23 --- /dev/null +++ b/common/component_interface_specs/test/test_control.cpp @@ -0,0 +1,46 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "component_interface_specs/control.hpp" +#include "gtest/gtest.h" + +TEST(control, interface) +{ + { + using control_interface::IsPaused; + IsPaused is_paused; + size_t depth = 1; + EXPECT_EQ(is_paused.depth, depth); + EXPECT_EQ(is_paused.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(is_paused.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } + + { + using control_interface::IsStartRequested; + IsStartRequested is_start_requested; + size_t depth = 1; + EXPECT_EQ(is_start_requested.depth, depth); + EXPECT_EQ(is_start_requested.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(is_start_requested.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } + + { + using control_interface::IsStopped; + IsStopped is_stopped; + size_t depth = 1; + EXPECT_EQ(is_stopped.depth, depth); + EXPECT_EQ(is_stopped.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(is_stopped.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } +} diff --git a/common/component_interface_specs/test/test_localization.cpp b/common/component_interface_specs/test/test_localization.cpp new file mode 100644 index 0000000000000..31d8e139bd69a --- /dev/null +++ b/common/component_interface_specs/test/test_localization.cpp @@ -0,0 +1,46 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "component_interface_specs/localization.hpp" +#include "gtest/gtest.h" + +TEST(localization, interface) +{ + { + using localization_interface::InitializationState; + InitializationState initialization_state; + size_t depth = 1; + EXPECT_EQ(initialization_state.depth, depth); + EXPECT_EQ(initialization_state.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(initialization_state.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } + + { + using localization_interface::KinematicState; + KinematicState kinematic_state; + size_t depth = 1; + EXPECT_EQ(kinematic_state.depth, depth); + EXPECT_EQ(kinematic_state.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(kinematic_state.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } + + { + using localization_interface::Acceleration; + Acceleration acceleration; + size_t depth = 1; + EXPECT_EQ(acceleration.depth, depth); + EXPECT_EQ(acceleration.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(acceleration.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } +} diff --git a/common/component_interface_specs/test/test_map.cpp b/common/component_interface_specs/test/test_map.cpp new file mode 100644 index 0000000000000..a65f2cb7120d1 --- /dev/null +++ b/common/component_interface_specs/test/test_map.cpp @@ -0,0 +1,28 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "component_interface_specs/map.hpp" +#include "gtest/gtest.h" + +TEST(map, interface) +{ + { + using map_interface::MapProjectorInfo; + MapProjectorInfo map_projector; + size_t depth = 1; + EXPECT_EQ(map_projector.depth, depth); + EXPECT_EQ(map_projector.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(map_projector.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } +} diff --git a/common/component_interface_specs/test/test_perception.cpp b/common/component_interface_specs/test/test_perception.cpp new file mode 100644 index 0000000000000..ec80c06bb119a --- /dev/null +++ b/common/component_interface_specs/test/test_perception.cpp @@ -0,0 +1,28 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "component_interface_specs/perception.hpp" +#include "gtest/gtest.h" + +TEST(perception, interface) +{ + { + using perception_interface::ObjectRecognition; + ObjectRecognition object_recognition; + size_t depth = 1; + EXPECT_EQ(object_recognition.depth, depth); + EXPECT_EQ(object_recognition.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(object_recognition.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } +} diff --git a/common/component_interface_specs/test/test_planning.cpp b/common/component_interface_specs/test/test_planning.cpp new file mode 100644 index 0000000000000..8c504cb119854 --- /dev/null +++ b/common/component_interface_specs/test/test_planning.cpp @@ -0,0 +1,64 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "component_interface_specs/planning.hpp" +#include "gtest/gtest.h" + +TEST(planning, interface) +{ + { + using planning_interface::RouteState; + RouteState state; + size_t depth = 1; + EXPECT_EQ(state.depth, depth); + EXPECT_EQ(state.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(state.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } + + { + using planning_interface::Route; + Route route; + size_t depth = 1; + EXPECT_EQ(route.depth, depth); + EXPECT_EQ(route.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(route.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } + + { + using planning_interface::NormalRoute; + NormalRoute route; + size_t depth = 1; + EXPECT_EQ(route.depth, depth); + EXPECT_EQ(route.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(route.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } + + { + using planning_interface::MrmRoute; + MrmRoute route; + size_t depth = 1; + EXPECT_EQ(route.depth, depth); + EXPECT_EQ(route.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(route.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } + + { + using planning_interface::Trajectory; + Trajectory trajectory; + size_t depth = 1; + EXPECT_EQ(trajectory.depth, depth); + EXPECT_EQ(trajectory.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(trajectory.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } +} diff --git a/common/component_interface_specs/test/test_system.cpp b/common/component_interface_specs/test/test_system.cpp new file mode 100644 index 0000000000000..416d2effad766 --- /dev/null +++ b/common/component_interface_specs/test/test_system.cpp @@ -0,0 +1,37 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "component_interface_specs/system.hpp" +#include "gtest/gtest.h" + +TEST(system, interface) +{ + { + using system_interface::MrmState; + MrmState state; + size_t depth = 1; + EXPECT_EQ(state.depth, depth); + EXPECT_EQ(state.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(state.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } + + { + using system_interface::OperationModeState; + OperationModeState state; + size_t depth = 1; + EXPECT_EQ(state.depth, depth); + EXPECT_EQ(state.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(state.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } +} diff --git a/common/component_interface_specs/test/test_vehicle.cpp b/common/component_interface_specs/test/test_vehicle.cpp new file mode 100644 index 0000000000000..1f2041b6cb79e --- /dev/null +++ b/common/component_interface_specs/test/test_vehicle.cpp @@ -0,0 +1,64 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "component_interface_specs/vehicle.hpp" +#include "gtest/gtest.h" + +TEST(vehicle, interface) +{ + { + using vehicle_interface::SteeringStatus; + SteeringStatus status; + size_t depth = 1; + EXPECT_EQ(status.depth, depth); + EXPECT_EQ(status.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(status.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } + + { + using vehicle_interface::GearStatus; + GearStatus status; + size_t depth = 1; + EXPECT_EQ(status.depth, depth); + EXPECT_EQ(status.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(status.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } + + { + using vehicle_interface::TurnIndicatorStatus; + TurnIndicatorStatus status; + size_t depth = 1; + EXPECT_EQ(status.depth, depth); + EXPECT_EQ(status.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(status.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } + + { + using vehicle_interface::HazardLightStatus; + HazardLightStatus status; + size_t depth = 1; + EXPECT_EQ(status.depth, depth); + EXPECT_EQ(status.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(status.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } + + { + using vehicle_interface::EnergyStatus; + EnergyStatus status; + size_t depth = 1; + EXPECT_EQ(status.depth, depth); + EXPECT_EQ(status.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(status.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } +} diff --git a/common/component_interface_utils/CMakeLists.txt b/common/component_interface_utils/CMakeLists.txt index d4f6343e381c7..435d5535e0a37 100644 --- a/common/component_interface_utils/CMakeLists.txt +++ b/common/component_interface_utils/CMakeLists.txt @@ -3,4 +3,24 @@ project(component_interface_utils) find_package(autoware_cmake REQUIRED) autoware_package() + +include_directories( + include + SYSTEM + ${rclcpp_INCLUDE_DIRS} + ${rosidl_runtime_cpp_INCLUDE_DIRS} + ${rcl_INCLUDE_DIRS} + ${autoware_adapi_v1_msgs_INCLUDE_DIRS} +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_component_interface_utils + test/test_component_interface_utils.cpp + ) + + target_include_directories(test_component_interface_utils + PRIVATE include + ) +endif() + ament_auto_package() diff --git a/common/component_interface_utils/package.xml b/common/component_interface_utils/package.xml old mode 100644 new mode 100755 index a1803a35cc1c9..dc82fda3f5c64 --- a/common/component_interface_utils/package.xml +++ b/common/component_interface_utils/package.xml @@ -19,6 +19,7 @@ rclcpp rclcpp_components + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/common/component_interface_utils/test/test_component_interface_utils.cpp b/common/component_interface_utils/test/test_component_interface_utils.cpp new file mode 100644 index 0000000000000..3c41f4a85b4f9 --- /dev/null +++ b/common/component_interface_utils/test/test_component_interface_utils.cpp @@ -0,0 +1,76 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "component_interface_utils/rclcpp/exceptions.hpp" +#include "component_interface_utils/specs.hpp" +#include "component_interface_utils/status.hpp" +#include "gtest/gtest.h" + +TEST(interface, utils) +{ + { + using component_interface_utils::ServiceException; + using ResponseStatus = autoware_adapi_v1_msgs::msg::ResponseStatus; + using ResponseStatusCode = ResponseStatus::_code_type; + + ResponseStatusCode code = 10; + const std::string message = "test_exception"; + ServiceException service(code, message); + ResponseStatus code_back; + code_back = service.status(); + EXPECT_EQ(code_back.code, code); + EXPECT_EQ(code_back.message, message); + } + + { + using component_interface_utils::ServiceException; + using ResponseStatus = autoware_adapi_v1_msgs::msg::ResponseStatus; + using ResponseStatusCode = ResponseStatus::_code_type; + + ResponseStatusCode code = 10; + const std::string message = "test_exception"; + ServiceException service(code, message); + ResponseStatus code_set; + service.set(code_set); + EXPECT_EQ(code_set.code, code); + EXPECT_EQ(code_set.message, message); + } + + { + using component_interface_utils::ServiceException; + using ResponseStatus = autoware_adapi_v1_msgs::msg::ResponseStatus; + using ResponseStatusCode = ResponseStatus::_code_type; + using component_interface_utils::status::copy; + + class status_test + { + public: + status_test(ResponseStatusCode code, const std::string & message, bool success = false) + { + status.code = code; + status.message = message; + status.success = success; + } + ResponseStatus status; + }; + + const status_test status_in(10, "test_exception", true); + auto status_copy = std::make_shared(100, "test_exception_copy", false); + copy(&status_in, status_copy); + + EXPECT_EQ(status_in.status.code, status_copy->status.code); + EXPECT_EQ(status_in.status.message, status_copy->status.message); + EXPECT_EQ(status_in.status.success, status_copy->status.success); + } +} diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index 94cf48f29c57a..dcdefe61e4000 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -18,6 +18,7 @@ #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/geometry/pose_deviation.hpp" #include "tier4_autoware_utils/math/constants.hpp" +#include "tier4_autoware_utils/system/backtrace.hpp" #include @@ -43,6 +44,7 @@ template void validateNonEmpty(const T & points) { if (points.empty()) { + tier4_autoware_utils::print_backtrace(); throw std::invalid_argument("Points is empty."); } } @@ -80,6 +82,7 @@ void validateNonSharpAngle( constexpr double epsilon = 1e-3; if (std::cos(angle_threshold) < product / dist_1to2 / dist_3to2 + epsilon) { + tier4_autoware_utils::print_backtrace(); throw std::invalid_argument("Sharp angle."); } } @@ -396,6 +399,7 @@ double calcLongitudinalOffsetToSegment( { if (seg_idx >= points.size() - 1) { const std::out_of_range e("Segment index is invalid."); + tier4_autoware_utils::print_backtrace(); if (throw_exception) { throw e; } @@ -418,6 +422,7 @@ double calcLongitudinalOffsetToSegment( if (seg_idx >= overlap_removed_points.size() - 1) { const std::runtime_error e("Same points are given."); + tier4_autoware_utils::print_backtrace(); if (throw_exception) { throw e; } @@ -574,6 +579,7 @@ double calcLateralOffset( if (overlap_removed_points.size() == 1) { const std::runtime_error e("Same points are given."); + tier4_autoware_utils::print_backtrace(); if (throw_exception) { throw e; } @@ -635,6 +641,7 @@ double calcLateralOffset( if (overlap_removed_points.size() == 1) { const std::runtime_error e("Same points are given."); + tier4_autoware_utils::print_backtrace(); if (throw_exception) { throw e; } @@ -1037,6 +1044,7 @@ boost::optional calcLongitudinalOffsetPoint( if (points.size() - 1 < src_idx) { const auto e = std::out_of_range("Invalid source index"); + tier4_autoware_utils::print_backtrace(); if (throw_exception) { throw e; } @@ -1161,6 +1169,7 @@ boost::optional calcLongitudinalOffsetPose( if (points.size() - 1 < src_idx) { const auto e = std::out_of_range("Invalid source index"); + tier4_autoware_utils::print_backtrace(); if (throw_exception) { throw e; } @@ -2375,6 +2384,7 @@ double calcYawDeviation( if (overlap_removed_points.size() <= 1) { const std::runtime_error e("points size is less than 2"); + tier4_autoware_utils::print_backtrace(); if (throw_exception) { throw e; } diff --git a/common/tier4_autoware_utils/CMakeLists.txt b/common/tier4_autoware_utils/CMakeLists.txt index 938a78692d9a1..9cb54e52362a5 100644 --- a/common/tier4_autoware_utils/CMakeLists.txt +++ b/common/tier4_autoware_utils/CMakeLists.txt @@ -14,6 +14,8 @@ ament_auto_add_library(tier4_autoware_utils SHARED src/math/trigonometry.cpp src/ros/msg_operation.cpp src/ros/marker_helper.cpp + src/ros/logger_level_configure.cpp + src/system/backtrace.cpp ) if(BUILD_TESTING) diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/logger_level_configure.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/logger_level_configure.hpp new file mode 100644 index 0000000000000..5aee3a251dad2 --- /dev/null +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/logger_level_configure.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. + +// =============== Note =============== +// This is a util class implementation of the logger_config_component provided by ROS 2 +// https://github.com/ros2/demos/blob/humble/logging_demo/src/logger_config_component.cpp +// +// When ROS 2 officially supports the set_logger_level option in release version, this class can be +// removed. +// https://github.com/ros2/ros2/issues/1355 + +// =============== How to use =============== +// ___In your_node.hpp___ +// #include "tier4_autoware_utils/ros/logger_level_configure.hpp" +// class YourNode : public rclcpp::Node { +// ... +// +// // Define logger_configure as a node class member variable +// std::unique_ptr logger_configure_; +// } +// +// ___In your_node.cpp___ +// YourNode::YourNode() { +// ... +// +// // Set up logger_configure +// logger_configure_ = std::make_unique(this); +// } + +#ifndef TIER4_AUTOWARE_UTILS__ROS__LOGGER_LEVEL_CONFIGURE_HPP_ +#define TIER4_AUTOWARE_UTILS__ROS__LOGGER_LEVEL_CONFIGURE_HPP_ + +#include "logging_demo/srv/config_logger.hpp" + +#include + +namespace tier4_autoware_utils +{ +class LoggerLevelConfigure +{ +private: + using ConfigLogger = logging_demo::srv::ConfigLogger; + +public: + explicit LoggerLevelConfigure(rclcpp::Node * node); + +private: + rclcpp::Logger ros_logger_; + rclcpp::Service::SharedPtr srv_config_logger_; + + void onLoggerConfigService( + const ConfigLogger::Request::SharedPtr request, + const ConfigLogger::Response::SharedPtr response); +}; + +} // namespace tier4_autoware_utils +#endif // TIER4_AUTOWARE_UTILS__ROS__LOGGER_LEVEL_CONFIGURE_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/system/backtrace.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/system/backtrace.hpp index b0b526e8c8b9c..c784d71ad5060 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/system/backtrace.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/system/backtrace.hpp @@ -15,38 +15,10 @@ #ifndef TIER4_AUTOWARE_UTILS__SYSTEM__BACKTRACE_HPP_ #define TIER4_AUTOWARE_UTILS__SYSTEM__BACKTRACE_HPP_ -#include - -#include -#include -#include -#include - namespace tier4_autoware_utils { -inline void print_backtrace() -{ - constexpr size_t max_frames = 100; - void * addrlist[max_frames + 1]; - - int addrlen = backtrace(addrlist, sizeof(addrlist) / sizeof(void *)); - - if (addrlen == 0) { - return; - } - - char ** symbol_list = backtrace_symbols(addrlist, addrlen); - - std::stringstream ss; - ss << " ********** back trace **********" << std::endl; - for (int i = 1; i < addrlen; i++) { - ss << " @ " << symbol_list[i] << std::endl; - } - std::cerr << ss.str() << std::endl; - - free(symbol_list); -} +void print_backtrace(); } // namespace tier4_autoware_utils diff --git a/common/tier4_autoware_utils/package.xml b/common/tier4_autoware_utils/package.xml index 6a6b3812cd69b..701907929ecc0 100644 --- a/common/tier4_autoware_utils/package.xml +++ b/common/tier4_autoware_utils/package.xml @@ -20,6 +20,7 @@ diagnostic_msgs geometry_msgs libboost-dev + logging_demo pcl_conversions pcl_ros rclcpp diff --git a/common/tier4_autoware_utils/src/ros/logger_level_configure.cpp b/common/tier4_autoware_utils/src/ros/logger_level_configure.cpp new file mode 100644 index 0000000000000..d764299290d05 --- /dev/null +++ b/common/tier4_autoware_utils/src/ros/logger_level_configure.cpp @@ -0,0 +1,61 @@ +// 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 "tier4_autoware_utils/ros/logger_level_configure.hpp" + +#include + +namespace tier4_autoware_utils +{ +LoggerLevelConfigure::LoggerLevelConfigure(rclcpp::Node * node) : ros_logger_(node->get_logger()) +{ + using std::placeholders::_1; + using std::placeholders::_2; + + srv_config_logger_ = node->create_service( + "~/config_logger", std::bind(&LoggerLevelConfigure::onLoggerConfigService, this, _1, _2)); +} + +void LoggerLevelConfigure::onLoggerConfigService( + const ConfigLogger::Request::SharedPtr request, const ConfigLogger::Response::SharedPtr response) +{ + int logging_severity; + const auto ret_level = rcutils_logging_severity_level_from_string( + request->level.c_str(), rcl_get_default_allocator(), &logging_severity); + + if (ret_level != RCUTILS_RET_OK) { + response->success = false; + RCLCPP_WARN_STREAM( + ros_logger_, "Failed to change logger level for " + << request->logger_name + << " due to an invalid logging severity: " << request->level); + return; + } + + const auto ret_set = + rcutils_logging_set_logger_level(request->logger_name.c_str(), logging_severity); + + if (ret_set != RCUTILS_RET_OK) { + response->success = false; + RCLCPP_WARN_STREAM(ros_logger_, "Failed to set logger level for " << request->logger_name); + return; + } + + response->success = true; + RCLCPP_INFO_STREAM( + ros_logger_, "Logger level [" << request->level << "] is set for " << request->logger_name); + return; +} + +} // namespace tier4_autoware_utils diff --git a/common/tier4_autoware_utils/src/system/backtrace.cpp b/common/tier4_autoware_utils/src/system/backtrace.cpp new file mode 100644 index 0000000000000..343f200296cad --- /dev/null +++ b/common/tier4_autoware_utils/src/system/backtrace.cpp @@ -0,0 +1,52 @@ +// 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 "tier4_autoware_utils/system/backtrace.hpp" + +#include "rclcpp/rclcpp.hpp" + +#include + +#include +#include +#include +#include + +namespace tier4_autoware_utils +{ + +void print_backtrace() +{ + constexpr size_t max_frames = 100; + void * addrlist[max_frames + 1]; + + int addrlen = backtrace(addrlist, sizeof(addrlist) / sizeof(void *)); + + if (addrlen == 0) { + return; + } + + char ** symbol_list = backtrace_symbols(addrlist, addrlen); + + std::stringstream ss; + ss << "\n @ ********** back trace **********" << std::endl; + for (int i = 1; i < addrlen; i++) { + ss << " @ " << symbol_list[i] << std::endl; + } + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("tier4_autoware_utils"), ss.str()); + + free(symbol_list); +} + +} // namespace tier4_autoware_utils diff --git a/common/tier4_logging_level_configure_rviz_plugin/CMakeLists.txt b/common/tier4_logging_level_configure_rviz_plugin/CMakeLists.txt index f86b6d4d2efdc..cc7da7e322d19 100644 --- a/common/tier4_logging_level_configure_rviz_plugin/CMakeLists.txt +++ b/common/tier4_logging_level_configure_rviz_plugin/CMakeLists.txt @@ -24,4 +24,5 @@ pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description. ament_auto_package( INSTALL_TO_SHARE plugins + config ) diff --git a/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml b/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml new file mode 100644 index 0000000000000..da5cc757682c5 --- /dev/null +++ b/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml @@ -0,0 +1,59 @@ +# logger_config.yaml + +# ============================================================ +# planning +# ============================================================ + +behavior_path_planner: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: tier4_autoware_utils + +behavior_path_planner_avoidance: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.avoidance + +behavior_velocity_planner: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner + logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner + logger_name: tier4_autoware_utils + +behavior_velocity_planner_intersection: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner + logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner.intersection + +motion_obstacle_avoidance: + - node_name: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner + logger_name: planning.scenario_planning.lane_driving.motion_planning.obstacle_avoidance_planner + - node_name: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner + logger_name: tier4_autoware_utils + +motion_velocity_smoother: + - node_name: /planning/scenario_planning/motion_velocity_smoother + logger_name: planning.scenario_planning.motion_velocity_smoother + - node_name: /planning/scenario_planning/motion_velocity_smoother + logger_name: tier4_autoware_utils + +# ============================================================ +# control +# ============================================================ + +lateral_controller: + - node_name: /control/trajectory_follower/controller_node_exe + logger_name: control.trajectory_follower.controller_node_exe.lateral_controller + - node_name: /control/trajectory_follower/controller_node_exe + logger_name: tier4_autoware_utils + +longitudinal_controller: + - node_name: /control/trajectory_follower/controller_node_exe + logger_name: control.trajectory_follower.controller_node_exe.longitudinal_controller + - node_name: /control/trajectory_follower/controller_node_exe + logger_name: tier4_autoware_utils + +vehicle_cmd_gate: + - node_name: /control/vehicle_cmd_gate + logger_name: control.vehicle_cmd_gate + - node_name: /control/vehicle_cmd_gate + logger_name: tier4_autoware_utils diff --git a/common/tier4_logging_level_configure_rviz_plugin/include/tier4_logging_level_configure_rviz_plugin/logging_level_configure.hpp b/common/tier4_logging_level_configure_rviz_plugin/include/tier4_logging_level_configure_rviz_plugin/logging_level_configure.hpp index f513a7e04a248..4d9b81ffb86bf 100644 --- a/common/tier4_logging_level_configure_rviz_plugin/include/tier4_logging_level_configure_rviz_plugin/logging_level_configure.hpp +++ b/common/tier4_logging_level_configure_rviz_plugin/include/tier4_logging_level_configure_rviz_plugin/logging_level_configure.hpp @@ -48,24 +48,24 @@ class LoggingLevelConfigureRvizPlugin : public rviz_common::Panel QMap buttonGroups_; rclcpp::Node::SharedPtr raw_node_; - // logger_node_map_[target_name] = {container_name, logger_name} - std::map>> logger_node_map_; + // node_logger_map_[button_name] = {node_name, logger_name} + std::map>> node_logger_map_; - // client_map_[container_name] = service_client + // client_map_[node_name] = service_client std::unordered_map::SharedPtr> client_map_; - // button_map_[target_name][logging_level] = Q_button_pointer + // button_map_[button_name][logging_level] = Q_button_pointer std::unordered_map> button_map_; - QStringList getContainerList(); + QStringList getNodeList(); int getMaxModuleNameWidth(QLabel * containerLabel); void setLoggerNodeMap(); - void attachLoggingComponent(); private Q_SLOTS: void onButtonClick(QPushButton * button, const QString & name, const QString & level); - void updateButtonColors(const QString & target_module_name, QPushButton * active_button); + void updateButtonColors( + const QString & target_module_name, QPushButton * active_button, const QString & level); void changeLogLevel(const QString & container, const QString & level); }; diff --git a/common/tier4_logging_level_configure_rviz_plugin/package.xml b/common/tier4_logging_level_configure_rviz_plugin/package.xml index 4ba5bb1579e13..7849e6049a077 100644 --- a/common/tier4_logging_level_configure_rviz_plugin/package.xml +++ b/common/tier4_logging_level_configure_rviz_plugin/package.xml @@ -21,6 +21,7 @@ rviz_common rviz_default_plugins rviz_rendering + yaml-cpp ament_lint_auto autoware_lint_common diff --git a/common/tier4_logging_level_configure_rviz_plugin/src/logging_level_configure.cpp b/common/tier4_logging_level_configure_rviz_plugin/src/logging_level_configure.cpp index e645ff682d50e..b15c0f0f735fa 100644 --- a/common/tier4_logging_level_configure_rviz_plugin/src/logging_level_configure.cpp +++ b/common/tier4_logging_level_configure_rviz_plugin/src/logging_level_configure.cpp @@ -12,7 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "yaml-cpp/yaml.h" + #include +#include +#include #include #include @@ -26,59 +30,28 @@ LoggingLevelConfigureRvizPlugin::LoggingLevelConfigureRvizPlugin(QWidget * paren { } -// Calculate the maximum width among all target_module_name. -int LoggingLevelConfigureRvizPlugin::getMaxModuleNameWidth(QLabel * containerLabel) -{ - int max_width = 0; - QFontMetrics metrics(containerLabel->font()); - for (const auto & item : logger_node_map_) { - const auto & target_module_name = item.first; - int width = metrics.horizontalAdvance(target_module_name); - if (width > max_width) { - max_width = width; - } - } - return max_width; -} - -// create container list in logger_node_map_ without -QStringList LoggingLevelConfigureRvizPlugin::getContainerList() -{ - QStringList containers; - for (const auto & item : logger_node_map_) { - const auto & container_logger_vec = item.second; - for (const auto & container_logger_pair : container_logger_vec) { - if (!containers.contains(container_logger_pair.first)) { - containers.append(container_logger_pair.first); - } - } - } - return containers; -} - void LoggingLevelConfigureRvizPlugin::onInitialize() { - setLoggerNodeMap(); + raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); - attachLoggingComponent(); + setLoggerNodeMap(); QVBoxLayout * layout = new QVBoxLayout; QStringList levels = {"DEBUG", "INFO", "WARN", "ERROR", "FATAL"}; - QStringList loaded_container; constexpr int height = 20; - for (const auto & item : logger_node_map_) { - const auto & target_module_name = item.first; + for (const auto & item : node_logger_map_) { + const auto & target_node_name = item.first; QHBoxLayout * hLayout = new QHBoxLayout; - // Create a QLabel to display the container name. - QLabel * containerLabel = new QLabel(target_module_name); - containerLabel->setFixedHeight(height); // Set fixed height for the button - containerLabel->setFixedWidth(getMaxModuleNameWidth(containerLabel)); + // Create a QLabel to display the node name. + QLabel * label = new QLabel(target_node_name); + label->setFixedHeight(height); // Set fixed height for the button + label->setFixedWidth(getMaxModuleNameWidth(label)); - hLayout->addWidget(containerLabel); // Add the QLabel to the hLayout. + hLayout->addWidget(label); // Add the QLabel to the hLayout. QButtonGroup * group = new QButtonGroup(this); for (const QString & level : levels) { @@ -86,42 +59,66 @@ void LoggingLevelConfigureRvizPlugin::onInitialize() btn->setFixedHeight(height); // Set fixed height for the button hLayout->addWidget(btn); // Add each QPushButton to the hLayout. group->addButton(btn); - button_map_[target_module_name][level] = btn; - connect(btn, &QPushButton::clicked, this, [this, btn, target_module_name, level]() { - this->onButtonClick(btn, target_module_name, level); + button_map_[target_node_name][level] = btn; + connect(btn, &QPushButton::clicked, this, [this, btn, target_node_name, level]() { + this->onButtonClick(btn, target_node_name, level); }); } // Set the "INFO" button as checked by default and change its color. - updateButtonColors(target_module_name, button_map_[target_module_name]["INFO"]); + updateButtonColors(target_node_name, button_map_[target_node_name]["INFO"], "INFO"); - buttonGroups_[target_module_name] = group; + buttonGroups_[target_node_name] = group; layout->addLayout(hLayout); } - setLayout(layout); + // Create a QWidget to hold the layout. + QWidget * containerWidget = new QWidget; + containerWidget->setLayout(layout); + + // Create a QScrollArea to make the layout scrollable. + QScrollArea * scrollArea = new QScrollArea; + scrollArea->setWidget(containerWidget); + scrollArea->setWidgetResizable(true); + + // Set the QScrollArea as the layout of the main widget. + QVBoxLayout * mainLayout = new QVBoxLayout; + mainLayout->addWidget(scrollArea); + setLayout(mainLayout); // set up service clients - raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); - const auto & containers = getContainerList(); - for (const QString & container : containers) { + const auto & nodes = getNodeList(); + for (const QString & node : nodes) { const auto client = raw_node_->create_client( - container.toStdString() + "/config_logger"); - client_map_[container] = client; + node.toStdString() + "/config_logger"); + client_map_[node] = client; } } -void LoggingLevelConfigureRvizPlugin::attachLoggingComponent() +// Calculate the maximum width among all target_module_name. +int LoggingLevelConfigureRvizPlugin::getMaxModuleNameWidth(QLabel * label) { - const auto & containers = getContainerList(); - for (const auto & container_name : containers) { - // Load the component for each container - QString command = "ros2 component load --node-namespace " + container_name + - " --node-name logging_configure " + container_name + - " logging_demo logging_demo::LoggerConfig"; - int result = system(qPrintable(command)); - std::cerr << "load logger in " << container_name.toStdString() << ": result = " << result - << std::endl; + int max_width = 0; + QFontMetrics metrics(label->font()); + for (const auto & item : node_logger_map_) { + const auto & target_module_name = item.first; + max_width = std::max(metrics.horizontalAdvance(target_module_name), max_width); } + return max_width; +} + +// create node list in node_logger_map_ without +QStringList LoggingLevelConfigureRvizPlugin::getNodeList() +{ + QStringList nodes; + for (const auto & item : node_logger_map_) { + const auto & node_logger_vec = item.second; + for (const auto & node_logger_pair : node_logger_vec) { + if (!nodes.contains(node_logger_pair.first)) { + nodes.append(node_logger_pair.first); + } + } + } + return nodes; } // Modify the signature of the onButtonClick function: @@ -135,40 +132,48 @@ void LoggingLevelConfigureRvizPlugin::onButtonClick( << std::string(future.get()->success ? "success!" : "failed...") << std::endl; }; - for (const auto & container_logger_map : logger_node_map_[target_module_name]) { - const auto container_node = container_logger_map.first; - const auto logger_name = container_logger_map.second; + for (const auto & node_logger_map : node_logger_map_[target_module_name]) { + const auto node_name = node_logger_map.first; + const auto logger_name = node_logger_map.second; const auto req = std::make_shared(); req->logger_name = logger_name.toStdString(); req->level = level.toStdString(); std::cerr << "logger level of " << req->logger_name << " is set to " << req->level << std::endl; - client_map_[container_node]->async_send_request(req, callback); + client_map_[node_name]->async_send_request(req, callback); } updateButtonColors( - target_module_name, button); // Modify updateButtonColors to accept QPushButton only. + target_module_name, button, level); // Modify updateButtonColors to accept QPushButton only. } } void LoggingLevelConfigureRvizPlugin::updateButtonColors( - const QString & target_module_name, QPushButton * active_button) + const QString & target_module_name, QPushButton * active_button, const QString & level) { - const QString LIGHT_GREEN = "rgb(181, 255, 20)"; - const QString LIGHT_GRAY = "rgb(211, 211, 211)"; + std::unordered_map colorMap = { + {"DEBUG", "rgb(181, 255, 20)"}, /* green */ + {"INFO", "rgb(200, 255, 255)"}, /* light blue */ + {"WARN", "rgb(255, 255, 0)"}, /* yellow */ + {"ERROR", "rgb(255, 0, 0)"}, /* red */ + {"FATAL", "rgb(139, 0, 0)"}, /* dark red */ + {"OFF", "rgb(211, 211, 211)"} /* gray */ + }; + const QString LIGHT_GRAY_TEXT = "rgb(180, 180, 180)"; + const QString color = colorMap.count(level) ? colorMap[level] : colorMap["OFF"]; + for (const auto & button : button_map_[target_module_name]) { if (button.second == active_button) { - button.second->setStyleSheet("background-color: " + LIGHT_GREEN + "; color: black"); + button.second->setStyleSheet("background-color: " + color + "; color: black"); } else { button.second->setStyleSheet( - "background-color: " + LIGHT_GRAY + "; color: " + LIGHT_GRAY_TEXT); + "background-color: " + colorMap["OFF"] + "; color: " + LIGHT_GRAY_TEXT); } } } - void LoggingLevelConfigureRvizPlugin::save(rviz_common::Config config) const { Panel::save(config); @@ -181,68 +186,25 @@ void LoggingLevelConfigureRvizPlugin::load(const rviz_common::Config & config) void LoggingLevelConfigureRvizPlugin::setLoggerNodeMap() { - // =============================================================================================== - // ====================================== Planning =============================================== - // =============================================================================================== - - QString behavior_planning_container = - "/planning/scenario_planning/lane_driving/behavior_planning/behavior_planning_container"; - QString motion_planning_container = - "/planning/scenario_planning/lane_driving/motion_planning/motion_planning_container"; - - // behavior_path_planner (all) - logger_node_map_["behavior_path_planner"] = { - {behavior_planning_container, - "planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner"}, - {behavior_planning_container, "tier4_autoware_utils"}}; - - // behavior_path_planner: avoidance - logger_node_map_["behavior_path_planner: avoidance"] = { - {behavior_planning_container, - "planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner." - "avoidance"}}; - - // behavior_velocity_planner (all) - logger_node_map_["behavior_velocity_planner"] = { - {behavior_planning_container, - "planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner"}, - {behavior_planning_container, "tier4_autoware_utils"}}; - - // behavior_velocity_planner: intersection - logger_node_map_["behavior_velocity_planner: intersection"] = { - {behavior_planning_container, - "planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner." - "intersection"}}; - - // obstacle_avoidance_planner - logger_node_map_["motion: obstacle_avoidance"] = { - {motion_planning_container, - "planning.scenario_planning.lane_driving.motion_planning.obstacle_avoidance_planner"}, - {motion_planning_container, "tier4_autoware_utils"}}; - - // motion_velocity_smoother - QString container = "/planning/scenario_planning/motion_velocity_smoother_container"; - logger_node_map_["motion: velocity_smoother"] = { - {container, "planning.scenario_planning.motion_velocity_smoother"}, - {container, "tier4_autoware_utils"}}; - - // =============================================================================================== - // ======================================= Control =============================================== - // =============================================================================================== - - QString control_container = "/control/control_container"; - - // lateral_controller - logger_node_map_["lateral_controller"] = { - {control_container, "control.trajectory_follower.controller_node_exe.lateral_controller"}, - {control_container, "tier4_autoware_utils"}, - }; - - // longitudinal_controller - logger_node_map_["longitudinal_controller"] = { - {control_container, "control.trajectory_follower.controller_node_exe.longitudinal_controller"}, - {control_container, "tier4_autoware_utils"}, - }; + const std::string package_share_directory = + ament_index_cpp::get_package_share_directory("tier4_logging_level_configure_rviz_plugin"); + const std::string default_config_path = package_share_directory + "/config/logger_config.yaml"; + + const auto filename = + raw_node_->declare_parameter("config_filename", default_config_path); + RCLCPP_INFO(raw_node_->get_logger(), "load config file: %s", filename.c_str()); + + YAML::Node config = YAML::LoadFile(filename); + + for (YAML::const_iterator it = config.begin(); it != config.end(); ++it) { + const auto key = QString::fromStdString(it->first.as()); + const YAML::Node values = it->second; + for (size_t i = 0; i < values.size(); i++) { + const auto node_name = QString::fromStdString(values[i]["node_name"].as()); + const auto logger_name = QString::fromStdString(values[i]["logger_name"].as()); + node_logger_map_[key].push_back({node_name, logger_name}); + } + } } } // namespace rviz_plugin diff --git a/control/pid_longitudinal_controller/README.md b/control/pid_longitudinal_controller/README.md index 418b3f7d27220..9d25322c793e0 100644 --- a/control/pid_longitudinal_controller/README.md +++ b/control/pid_longitudinal_controller/README.md @@ -75,6 +75,10 @@ For instance, if the vehicle is attempting to start with an acceleration of `1.0 A suitable example of a vehicle system for the slope compensation function is one in which the output acceleration from the longitudinal_controller is converted into target accel/brake pedal input without any feedbacks. In this case, the output acceleration is just used as a feedforward term to calculate the target pedal, and hence the issue mentioned above does not arise. +Note: The angle of the slope is defined as positive for an uphill slope, while the pitch angle of the ego pose is defined as negative when facing upward. They have an opposite definition. + +![slope_definition](./media/slope_definition.drawio.svg) + #### PID control For deviations that cannot be handled by FeedForward control, such as model errors, PID control is used to construct a feedback system. diff --git a/control/pid_longitudinal_controller/media/slope_definition.drawio.svg b/control/pid_longitudinal_controller/media/slope_definition.drawio.svg new file mode 100644 index 0000000000000..7f0aa677629a3 --- /dev/null +++ b/control/pid_longitudinal_controller/media/slope_definition.drawio.svg @@ -0,0 +1,165 @@ + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Slope is defined as positive for an uphill +
+
+
+ + Slope is defined as positive for an uphi... + + + + + + + + + + + + + + + +
+
+
+ (Pitch of ego is defined as negative when facing upward) +
+
+
+ + (Pitch of ego is defined as negative when facing upwar... + + + + + + +
+
+
+ x +
+
+
+ + x + + + + + + +
+
+
+ y +
+
+
+ + z + + + + + + +
+
+
+ z +
+
+
+ + y + + + + + + +
+
+
+ d_z +
+
+
+ + d_z + + + + + + +
+
+
+ d_xy +
+
+
+ + d_xy + + + + + + + Text is not SVG - cannot display + + + diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index 644effce50824..b80874a8a2e57 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -457,7 +457,9 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData current_pose, m_trajectory, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); // pitch - const double raw_pitch = longitudinal_utils::getPitchByPose(current_pose.orientation); + // NOTE: getPitchByTraj() calculates the pitch angle as defined in + // ../media/slope_definition.drawio.svg while getPitchByPose() is not, so `raw_pitch` is reversed + const double raw_pitch = (-1.0) * longitudinal_utils::getPitchByPose(current_pose.orientation); const double traj_pitch = longitudinal_utils::getPitchByTraj(m_trajectory, control_data.nearest_idx, m_wheel_base); control_data.slope_angle = m_use_traj_for_pitch ? traj_pitch : m_lpf_pitch->filter(raw_pitch); diff --git a/control/predicted_path_checker/CMakeLists.txt b/control/predicted_path_checker/CMakeLists.txt new file mode 100644 index 0000000000000..cfbe95df99e74 --- /dev/null +++ b/control/predicted_path_checker/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 3.14) +project(predicted_path_checker) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Eigen3 REQUIRED) + +include_directories( + include + SYSTEM + ${Eigen3_INCLUDE_DIRS} +) + +ament_auto_add_library(predicted_path_checker SHARED + src/predicted_path_checker_node/predicted_path_checker_node.cpp + src/predicted_path_checker_node/collision_checker.cpp + src/predicted_path_checker_node/utils.cpp + src/predicted_path_checker_node/debug_marker.cpp + +) + +rclcpp_components_register_node(predicted_path_checker + PLUGIN "autoware::motion::control::predicted_path_checker::PredictedPathCheckerNode" + EXECUTABLE predicted_path_checker_node +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/control/predicted_path_checker/README.md b/control/predicted_path_checker/README.md new file mode 100644 index 0000000000000..24e4b91ef441b --- /dev/null +++ b/control/predicted_path_checker/README.md @@ -0,0 +1,103 @@ +# Predicted Path Checker + +## Purpose + +The Predicted Path Checker package is designed for autonomous vehicles to check the predicted path generated by control +modules. It handles potential collisions that the planning module might not be able to handle and that in the brake +distance. In case of collision in brake distance, the package will send a diagnostic message labeled "ERROR" to alert +the system to send emergency and in the case of collisions in outside reference trajectory, it sends pause request to +pause interface to make the vehicle stop. + +![general-structure.png](images%2Fgeneral-structure.png) + +## Algorithm + +The package algorithm evaluates the predicted trajectory against the reference trajectory and the predicted objects in +the environment. It checks for potential collisions and, if necessary, generates an appropriate response to avoid them ( +emergency or pause request). + +### Inner Algorithm + +![FlowChart.png](images%2FFlowChart.png) + +**cutTrajectory() ->** It cuts the predicted trajectory with input length. Length is calculated by multiplying the +velocity +of ego vehicle with "trajectory_check_time" parameter and "min_trajectory_length". + +**filterObstacles() ->** It filters the predicted objects in the environment. It filters the objects which are not in +front of the vehicle and far away from predicted trajectory. + +**checkTrajectoryForCollision() ->** It checks the predicted trajectory for collision with the predicted objects. It +calculates both polygon of trajectory points and predicted objects and checks intersection of both polygons. If there is +an intersection, it calculates the nearest collision point. It returns the nearest collision point of polygon and the +predicted object. It also checks predicted objects history which are intersect with the footprint before to avoid +unexpected behaviors. Predicted objects history stores the objects if it was detected below the "chattering_threshold" +seconds ago. + +If the "enable_z_axis_obstacle_filtering" parameter is set to true, it filters the predicted objects in the Z-axis by +using "z_axis_filtering_buffer". If the object does not intersect with the Z-axis, it is filtered out. + +![Z_axis_filtering.png](images%2FZ_axis_filtering.png) + +**calculateProjectedVelAndAcc() ->** It calculates the projected velocity and acceleration of the predicted object on +predicted trajectory's collision point's axes. + +**isInBrakeDistance() ->** It checks if the stop point is in brake distance. It gets relative velocity and +acceleration of ego vehicle with respect to the predicted object. It calculates the brake distance, if the point in +brake distance, it returns true. + +**isItDiscretePoint() ->** It checks if the stop point on predicted trajectory is discrete point or not. If it is not +discrete point, planning should handle the stop. + +**isThereStopPointOnRefTrajectory() ->** It checks if there is a stop point on reference trajectory. If there is a stop +point before the stop index, it returns true. Otherwise, it returns false, and node is going to call pause interface to +make the vehicle stop. + +## Inputs + +| Name | Type | Description | +| ------------------------------------- | ----------------------------------------------------- | --------------------------------------------------- | +| `~/input/reference_trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Reference trajectory | +| `~/input/predicted_trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Predicted trajectory | +| `~/input/objects` | `autoware_auto_perception_msgs::msg::PredictedObject` | Dynamic objects in the environment | +| `~/input/odometry` | `nav_msgs::msg::Odometry` | Odometry message of vehicle to get current velocity | +| `~/input/current_accel` | `geometry_msgs::msg::AccelWithCovarianceStamped` | Current acceleration | +| `/control/vehicle_cmd_gate/is_paused` | `tier4_control_msgs::msg::IsPaused` | Current pause state of the vehicle | + +## Outputs + +| Name | Type | Description | +| ------------------------------------- | ---------------------------------------- | -------------------------------------- | +| `~/debug/marker` | `visualization_msgs::msg::MarkerArray` | Marker for visualization | +| `~/debug/virtual_wall` | `visualization_msgs::msg::MarkerArray` | Virtual wall marker for visualization | +| `/control/vehicle_cmd_gate/set_pause` | `tier4_control_msgs::srv::SetPause` | Pause service to make the vehicle stop | +| `/diagnostics` | `diagnostic_msgs::msg::DiagnosticStatus` | Diagnostic status of vehicle | + +## Parameters + +### Node Parameters + +| Name | Type | Description | Default value | +| :---------------------------------- | :------- | :-------------------------------------------------------------------- | :------------ | +| `update_rate` | `double` | The update rate [Hz] | 10.0 | +| `delay_time` | `double` | he time delay considered for the emergency response [s] | 0.17 | +| `max_deceleration` | `double` | Max deceleration for ego vehicle to stop [m/s^2] | 1.5 | +| `resample_interval` | `double` | Interval for resampling trajectory [m] | 0.5 | +| `stop_margin` | `double` | The stopping margin [m] | 0.5 | +| `ego_nearest_dist_threshold` | `double` | The nearest distance threshold for ego vehicle [m] | 3.0 | +| `ego_nearest_yaw_threshold` | `double` | The nearest yaw threshold for ego vehicle [rad] | 1.046 | +| `min_trajectory_check_length` | `double` | The minimum trajectory check length in meters [m] | 1.5 | +| `trajectory_check_time` | `double` | The trajectory check time in seconds. [s] | 3.0 | +| `distinct_point_distance_threshold` | `double` | The distinct point distance threshold [m] | 0.3 | +| `distinct_point_yaw_threshold` | `double` | The distinct point yaw threshold [deg] | 5.0 | +| `filtering_distance_threshold` | `double` | It ignores the objects if distance is higher than this [m] | 1.5 | +| `use_object_prediction` | `bool` | If true, node predicts current pose of the objects wrt delta time [-] | true | + +### Collision Checker Parameters + +| Name | Type | Description | Default value | +| :--------------------------------- | :------- | :---------------------------------------------------------------- | :------------ | +| `width_margin` | `double` | The width margin for collision checking [Hz] | 0.2 | +| `chattering_threshold` | `double` | The chattering threshold for collision detection [s] | 0.2 | +| `z_axis_filtering_buffer` | `double` | The Z-axis filtering buffer [m] | 0.3 | +| `enable_z_axis_obstacle_filtering` | `bool` | A boolean flag indicating if Z-axis obstacle filtering is enabled | false | diff --git a/control/predicted_path_checker/config/predicted_path_checker.param.yaml b/control/predicted_path_checker/config/predicted_path_checker.param.yaml new file mode 100644 index 0000000000000..780d86b5e9804 --- /dev/null +++ b/control/predicted_path_checker/config/predicted_path_checker.param.yaml @@ -0,0 +1,22 @@ +/**: + ros__parameters: + # Node + update_rate: 10.0 + delay_time: 0.17 + max_deceleration: 1.5 + resample_interval: 0.5 + stop_margin: 0.5 # [m] + ego_nearest_dist_threshold: 3.0 # [m] + ego_nearest_yaw_threshold: 1.046 # [rad] = 60 [deg] + min_trajectory_check_length: 1.5 # [m] + trajectory_check_time: 3.0 + distinct_point_distance_threshold: 0.3 + distinct_point_yaw_threshold: 5.0 # [deg] + filtering_distance_threshold: 1.5 # [m] + use_object_prediction: true + + collision_checker_params: + width_margin: 0.2 + chattering_threshold: 0.2 + z_axis_filtering_buffer: 0.3 + enable_z_axis_obstacle_filtering: false diff --git a/control/predicted_path_checker/images/FlowChart.png b/control/predicted_path_checker/images/FlowChart.png new file mode 100644 index 0000000000000..15b32417d2a73 Binary files /dev/null and b/control/predicted_path_checker/images/FlowChart.png differ diff --git a/control/predicted_path_checker/images/Z_axis_filtering.png b/control/predicted_path_checker/images/Z_axis_filtering.png new file mode 100644 index 0000000000000..0c064aecae42e Binary files /dev/null and b/control/predicted_path_checker/images/Z_axis_filtering.png differ diff --git a/control/predicted_path_checker/images/general-structure.png b/control/predicted_path_checker/images/general-structure.png new file mode 100644 index 0000000000000..86f3016db5c2c Binary files /dev/null and b/control/predicted_path_checker/images/general-structure.png differ diff --git a/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp b/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp new file mode 100644 index 0000000000000..d21b11e5b68de --- /dev/null +++ b/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp @@ -0,0 +1,128 @@ +// Copyright 2023 LeoDrive A.Ş. 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 PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_ +#define PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include + +namespace autoware::motion::control::predicted_path_checker +{ +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::TransformStamped; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; +using PointArray = std::vector; + +namespace bg = boost::geometry; + +struct CollisionCheckerParam +{ + double width_margin; + double z_axis_filtering_buffer; + bool enable_z_axis_obstacle_filtering; + double chattering_threshold; +}; + +struct PredictedObjectWithDetectionTime +{ + explicit PredictedObjectWithDetectionTime( + const rclcpp::Time & t, geometry_msgs::msg::Point & p, PredictedObject obj) + : detection_time(t), point(p), object(std::move(obj)) + { + } + + rclcpp::Time detection_time; + geometry_msgs::msg::Point point; + PredictedObject object; +}; + +class CollisionChecker +{ +public: + explicit CollisionChecker( + rclcpp::Node * node, std::shared_ptr debug_ptr); + + boost::optional> + checkTrajectoryForCollision( + TrajectoryPoints & predicted_trajectory_array, + PredictedObjects::ConstSharedPtr dynamic_objects); + + void setParam(const CollisionCheckerParam & param); + +private: + // Functions + + boost::optional> checkObstacleHistory( + const Pose & base_pose, const Polygon2d & one_step_move_vehicle_polygon2d, const double z_min, + const double z_max); + + boost::optional> checkDynamicObjects( + const Pose & base_pose, PredictedObjects::ConstSharedPtr dynamic_objects, + const Polygon2d & one_step_move_vehicle_polygon2d, const double z_min, const double z_max); + + void updatePredictedObjectHistory(const rclcpp::Time & now) + { + for (auto itr = predicted_object_history_.begin(); itr != predicted_object_history_.end();) { + const auto expired = (now - itr->detection_time).seconds() > param_.chattering_threshold; + + if (expired) { + itr = predicted_object_history_.erase(itr); + continue; + } + + itr++; + } + } + + // Parameter + CollisionCheckerParam param_; + + // Variables + std::shared_ptr debug_ptr_; + rclcpp::Node * node_; + vehicle_info_util::VehicleInfo vehicle_info_; + std::vector predicted_object_history_{}; +}; +} // namespace autoware::motion::control::predicted_path_checker + +#endif // PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_ diff --git a/control/predicted_path_checker/include/predicted_path_checker/debug_marker.hpp b/control/predicted_path_checker/include/predicted_path_checker/debug_marker.hpp new file mode 100644 index 0000000000000..02ade624d630c --- /dev/null +++ b/control/predicted_path_checker/include/predicted_path_checker/debug_marker.hpp @@ -0,0 +1,92 @@ +// Copyright 2023 LeoDrive A.Ş. 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 PREDICTED_PATH_CHECKER__DEBUG_MARKER_HPP_ +#define PREDICTED_PATH_CHECKER__DEBUG_MARKER_HPP_ + +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#define EIGEN_MPL2_ONLY + +#include +#include +#include + +namespace autoware::motion::control::predicted_path_checker +{ + +enum class PolygonType : int8_t { Vehicle = 0, Collision }; + +enum class PointType : int8_t { Stop = 0 }; + +enum class PoseType : int8_t { Stop = 0, Collision }; + +class PredictedPathCheckerDebugNode +{ +public: + explicit PredictedPathCheckerDebugNode(rclcpp::Node * node, const double base_link2front); + + ~PredictedPathCheckerDebugNode() {} + + bool pushPolygon( + const tier4_autoware_utils::Polygon2d & polygon, const double z, const PolygonType & type); + + bool pushPolygon(const std::vector & polygon, const PolygonType & type); + + bool pushPolyhedron( + const tier4_autoware_utils::Polygon2d & polyhedron, const double z_min, const double z_max, + const PolygonType & type); + + bool pushPolyhedron(const std::vector & polyhedron, const PolygonType & type); + + bool pushPose(const geometry_msgs::msg::Pose & pose, const PoseType & type); + + bool pushObstaclePoint(const geometry_msgs::msg::Point & obstacle_point, const PointType & type); + + visualization_msgs::msg::MarkerArray makeVirtualWallMarker(); + + visualization_msgs::msg::MarkerArray makeVisualizationMarker(); + + void publish(); + +private: + rclcpp::Publisher::SharedPtr virtual_wall_pub_; + rclcpp::Publisher::SharedPtr debug_viz_pub_; + rclcpp::Node * node_; + double base_link2front_; + + std::shared_ptr stop_pose_ptr_; + std::shared_ptr collision_pose_ptr_; + std::shared_ptr stop_obstacle_point_ptr_; + std::vector> vehicle_polygons_; + std::vector> collision_polygons_; + std::vector> vehicle_polyhedrons_; + std::vector> collision_polyhedrons_; +}; + +} // namespace autoware::motion::control::predicted_path_checker + +#endif // PREDICTED_PATH_CHECKER__DEBUG_MARKER_HPP_ diff --git a/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp b/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp new file mode 100644 index 0000000000000..e3c3b5cccfb8f --- /dev/null +++ b/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp @@ -0,0 +1,178 @@ +// Copyright 2023 LeoDrive A.Ş. 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 PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_HPP_ +#define PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include + +namespace autoware::motion::control::predicted_path_checker +{ +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::TransformStamped; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; + +struct NodeParam +{ + double update_rate; + double delay_time; + double max_deceleration; + double resample_interval; + double ego_nearest_dist_threshold; + double ego_nearest_yaw_threshold; + double stop_margin; + double min_trajectory_check_length; + double trajectory_check_time; + double distinct_point_distance_threshold; + double distinct_point_yaw_threshold; + double filtering_distance_threshold; + bool use_object_prediction; +}; + +enum class State { + DRIVE = 0, + EMERGENCY = 1, + STOP = 2, +}; + +class PredictedPathCheckerNode : public rclcpp::Node +{ +public: + explicit PredictedPathCheckerNode(const rclcpp::NodeOptions & node_options); + +private: + rclcpp::CallbackGroup::SharedPtr group_cli_; + + // Subscriber + std::shared_ptr self_pose_listener_; + rclcpp::Subscription::SharedPtr sub_dynamic_objects_; + rclcpp::Subscription::SharedPtr + sub_reference_trajectory_; + rclcpp::Subscription::SharedPtr + sub_predicted_trajectory_; + rclcpp::Subscription::SharedPtr sub_odom_; + rclcpp::Subscription::SharedPtr sub_accel_; + component_interface_utils::Subscription::SharedPtr sub_stop_state_; + + // Client + component_interface_utils::Client::SharedPtr cli_set_stop_; + + // Data Buffer + geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_; + geometry_msgs::msg::Twist::ConstSharedPtr current_twist_; + geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr current_accel_; + PredictedObjects::ConstSharedPtr object_ptr_{nullptr}; + autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr reference_trajectory_; + autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr predicted_trajectory_; + control_interface::IsStopped::Message::ConstSharedPtr is_stopped_ptr_{nullptr}; + + // Core + std::unique_ptr collision_checker_; + std::shared_ptr debug_ptr_; + + // Variables + State current_state_{State::DRIVE}; + vehicle_info_util::VehicleInfo vehicle_info_; + bool is_calling_set_stop_{false}; + bool is_stopped_by_node_{false}; + + // Callback + void onDynamicObjects(PredictedObjects::ConstSharedPtr msg); + void onReferenceTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg); + void onPredictedTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg); + void onOdom(const nav_msgs::msg::Odometry::SharedPtr msg); + void onAccel(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg); + void onIsStopped(const control_interface::IsStopped::Message::ConstSharedPtr msg); + + // Timer + rclcpp::TimerBase::SharedPtr timer_; + void initTimer(double period_s); + + // Functions + bool isDataReady(); + + bool isDataTimeout(); + + bool isThereStopPointOnReferenceTrajectory( + const geometry_msgs::msg::Pose & pose, const TrajectoryPoints & reference_trajectory_array); + + void onTimer(); + + void checkVehicleState(diagnostic_updater::DiagnosticStatusWrapper & stat); + + TrajectoryPoints trimTrajectoryFromSelfPose( + const TrajectoryPoints & input, const Pose & self_pose) const; + + void sendRequest(bool make_stop_vehicle); + + bool isItDiscretePoint( + const TrajectoryPoints & reference_trajectory, const TrajectoryPoint & collision_point) const; + + static Trajectory cutTrajectory(const Trajectory & trajectory, const double length); + + size_t insertStopPoint( + TrajectoryPoints & trajectory, const geometry_msgs::msg::Point collision_point); + + void extendTrajectoryPointsArray(TrajectoryPoints & trajectory); + + static std::pair calculateProjectedVelAndAcc( + const PredictedObject & object, const TrajectoryPoint & trajectory_point); + + void filterObstacles( + const Pose & ego_pose, const TrajectoryPoints & traj, const double dist_threshold, + const bool use_prediction, PredictedObjects & filtered_objects); + + // Parameters + CollisionCheckerParam collision_checker_param_; + NodeParam node_param_; + + // Diagnostic Updater + diagnostic_updater::Updater updater_; +}; +} // namespace autoware::motion::control::predicted_path_checker + +#endif // PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_HPP_ diff --git a/control/predicted_path_checker/include/predicted_path_checker/utils.hpp b/control/predicted_path_checker/include/predicted_path_checker/utils.hpp new file mode 100644 index 0000000000000..75e90e624a17e --- /dev/null +++ b/control/predicted_path_checker/include/predicted_path_checker/utils.hpp @@ -0,0 +1,104 @@ +// Copyright 2023 LeoDrive A.Ş. 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 PREDICTED_PATH_CHECKER__UTILS_HPP_ +#define PREDICTED_PATH_CHECKER__UTILS_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace utils +{ + +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::TransformStamped; +using std_msgs::msg::Header; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; +using vehicle_info_util::VehicleInfo; +using PointArray = std::vector; + +using TrajectoryPoints = std::vector; + +void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & geom_point); + +Polygon2d createOneStepPolygon( + const geometry_msgs::msg::Pose & base_step_pose, const geometry_msgs::msg::Pose & next_step_pose, + const vehicle_info_util::VehicleInfo & vehicle_info, const double expand_width); + +TrajectoryPoint calcInterpolatedPoint( + const TrajectoryPoints & trajectory, const geometry_msgs::msg::Point & target_point, + const size_t segment_idx, const bool use_zero_order_hold_for_twist); + +std::pair findStopPoint( + TrajectoryPoints & predicted_trajectory_array, const size_t collision_idx, + const double stop_margin, vehicle_info_util::VehicleInfo & vehicle_info); + +bool isInBrakeDistance( + const TrajectoryPoints & trajectory, const size_t stop_idx, const double relative_velocity, + const double relative_acceleration, const double max_deceleration, const double delay_time_sec); + +TrajectoryPoint getExtendTrajectoryPoint( + const double extend_distance, const TrajectoryPoint & goal_point); + +bool intersectsInZAxis(const PredictedObject & object, const double z_min, const double z_max); + +double getNearestPointAndDistanceForPredictedObject( + const PointArray & points, const Pose & base_pose, + geometry_msgs::msg::Point * nearest_collision_point); + +Polygon2d convertBoundingBoxObjectToGeometryPolygon( + const Pose & current_pose, const double & base_to_front, const double & base_to_rear, + const double & base_to_width); + +Polygon2d convertCylindricalObjectToGeometryPolygon( + const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape); + +Polygon2d convertPolygonObjectToGeometryPolygon( + const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape); + +Polygon2d convertObjToPolygon(const PredictedObject & obj); + +double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape); + +void getCurrentObjectPose( + PredictedObject & predicted_object, const rclcpp::Time & obj_base_time, + const rclcpp::Time & current_time); + +bool isFrontObstacle(const Pose & ego_pose, const geometry_msgs::msg::Point & obstacle_pos); +} // namespace utils + +#endif // PREDICTED_PATH_CHECKER__UTILS_HPP_ diff --git a/control/predicted_path_checker/launch/predicted_path_checker.launch.xml b/control/predicted_path_checker/launch/predicted_path_checker.launch.xml new file mode 100755 index 0000000000000..a35c11b80c1f7 --- /dev/null +++ b/control/predicted_path_checker/launch/predicted_path_checker.launch.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/control/predicted_path_checker/package.xml b/control/predicted_path_checker/package.xml new file mode 100644 index 0000000000000..35f971907696a --- /dev/null +++ b/control/predicted_path_checker/package.xml @@ -0,0 +1,47 @@ + + + + predicted_path_checker + 1.0.0 + The predicted_path_checker package + + Berkay Karaman + Apache 2.0 + Berkay Karaman + + ament_cmake + autoware_cmake + + autoware_auto_perception_msgs + autoware_auto_planning_msgs + boost + component_interface_specs + component_interface_utils + diagnostic_updater + eigen + geometry_msgs + motion_utils + nav_msgs + rclcpp + rclcpp_components + sensor_msgs + std_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + tier4_autoware_utils + tier4_control_msgs + tier4_external_api_msgs + tier4_planning_msgs + vehicle_info_util + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + eigen + + + ament_cmake + + diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp new file mode 100644 index 0000000000000..5e8d96805b832 --- /dev/null +++ b/control/predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp @@ -0,0 +1,231 @@ +// Copyright 2022 LeoDrive A.Ş. 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 "predicted_path_checker/collision_checker.hpp" + +#include +#include + +#include +#include +#include + +namespace autoware::motion::control::predicted_path_checker +{ +CollisionChecker::CollisionChecker( + rclcpp::Node * node, std::shared_ptr debug_ptr) +: debug_ptr_(std::move(debug_ptr)), + node_(node), + vehicle_info_(vehicle_info_util::VehicleInfoUtil(*node).getVehicleInfo()) +{ +} + +void CollisionChecker::setParam(const CollisionCheckerParam & param) +{ + param_ = param; +} + +boost::optional> +CollisionChecker::checkTrajectoryForCollision( + TrajectoryPoints & predicted_trajectory_array, PredictedObjects::ConstSharedPtr dynamic_objects) +{ + // It checks the collision, if there is a collision, it updates the predicted_trajectory_array and + // returns the index of the stop point. + // If there is no collision, it returns boost::none. + const auto now = node_->now(); + + updatePredictedObjectHistory(now); + if (dynamic_objects->objects.empty() && predicted_object_history_.empty()) { + return boost::none; + } + + for (size_t i = 0; i < predicted_trajectory_array.size() - 1; i++) { + // create one step circle center for vehicle + const auto & p_front = predicted_trajectory_array.at(i).pose; + const auto & p_back = predicted_trajectory_array.at(i + 1).pose; + const auto z_min = p_front.position.z; + const auto z_max = + p_front.position.z + vehicle_info_.max_height_offset_m + param_.z_axis_filtering_buffer; + + // create one-step polygon for vehicle + Polygon2d one_step_move_vehicle_polygon2d = + utils::createOneStepPolygon(p_front, p_back, vehicle_info_, param_.width_margin); + if (param_.enable_z_axis_obstacle_filtering) { + debug_ptr_->pushPolyhedron( + one_step_move_vehicle_polygon2d, z_min, z_max, PolygonType::Vehicle); + } else { + debug_ptr_->pushPolygon( + one_step_move_vehicle_polygon2d, p_front.position.z, PolygonType::Vehicle); + } + + // check obstacle history + auto found_collision_at_history = + checkObstacleHistory(p_front, one_step_move_vehicle_polygon2d, z_min, z_max); + + auto found_collision_at_dynamic_objects = + checkDynamicObjects(p_front, dynamic_objects, one_step_move_vehicle_polygon2d, z_min, z_max); + + if (found_collision_at_dynamic_objects || found_collision_at_history) { + double distance_to_current = std::numeric_limits::max(); + double distance_to_history = std::numeric_limits::max(); + if (found_collision_at_dynamic_objects) { + distance_to_current = tier4_autoware_utils::calcDistance2d( + p_front, found_collision_at_dynamic_objects.get().first); + } + if (found_collision_at_history) { + distance_to_history = + tier4_autoware_utils::calcDistance2d(p_front, found_collision_at_history.get().first); + } else { + predicted_object_history_.clear(); + } + + if (param_.enable_z_axis_obstacle_filtering) { + debug_ptr_->pushPolyhedron( + one_step_move_vehicle_polygon2d, z_min, z_max, PolygonType::Collision); + } else { + debug_ptr_->pushPolygon( + one_step_move_vehicle_polygon2d, p_front.position.z, PolygonType::Collision); + } + + if (distance_to_current > distance_to_history) { + debug_ptr_->pushObstaclePoint(found_collision_at_history->first, PointType::Stop); + return found_collision_at_history; + } + + predicted_object_history_.emplace_back( + now, found_collision_at_dynamic_objects.get().first, + found_collision_at_dynamic_objects.get().second); + debug_ptr_->pushObstaclePoint(found_collision_at_dynamic_objects->first, PointType::Stop); + return found_collision_at_dynamic_objects; + } + } + return boost::none; +} + +boost::optional> +CollisionChecker::checkObstacleHistory( + const Pose & base_pose, const Polygon2d & one_step_move_vehicle_polygon2d, const double z_min, + const double z_max) +{ + if (predicted_object_history_.empty()) { + return boost::none; + } + + std::vector> collision_points_in_history; + for (const auto & obj_history : predicted_object_history_) { + if (param_.enable_z_axis_obstacle_filtering) { + if (!utils::intersectsInZAxis(obj_history.object, z_min, z_max)) { + continue; + } + } + const auto & point = obj_history.point; + const Point2d point2d(point.x, point.y); + if (bg::within(point2d, one_step_move_vehicle_polygon2d)) { + collision_points_in_history.emplace_back(point, obj_history.object); + } + } + if (!collision_points_in_history.empty()) { + double min_norm = 0.0; + bool is_init = false; + std::pair nearest_collision_object_with_point; + for (const auto & p : collision_points_in_history) { + double norm = tier4_autoware_utils::calcDistance2d(p.first, base_pose); + if (norm < min_norm || !is_init) { + min_norm = norm; + nearest_collision_object_with_point = p; + is_init = true; + } + } + return boost::make_optional(nearest_collision_object_with_point); + } + return boost::none; +} + +boost::optional> +CollisionChecker::checkDynamicObjects( + const Pose & base_pose, PredictedObjects::ConstSharedPtr dynamic_objects, + const Polygon2d & one_step_move_vehicle_polygon2d, const double z_min, const double z_max) +{ + if (dynamic_objects->objects.empty()) { + return boost::none; + } + double min_norm_collision_norm = 0.0; + bool is_init = false; + size_t nearest_collision_object_index = 0; + geometry_msgs::msg::Point nearest_collision_point; + + for (size_t i = 0; i < dynamic_objects->objects.size(); ++i) { + const auto & obj = dynamic_objects->objects.at(i); + if (param_.enable_z_axis_obstacle_filtering) { + if (!utils::intersectsInZAxis(obj, z_min, z_max)) { + continue; + } + } + const auto object_polygon = utils::convertObjToPolygon(obj); + if (object_polygon.outer().empty()) { + // unsupported type + continue; + } + + const auto found_collision_points = + bg::intersects(one_step_move_vehicle_polygon2d, object_polygon); + + if (found_collision_points) { + std::vector collision_points; + PointArray collision_point_array; + bg::intersection(one_step_move_vehicle_polygon2d, object_polygon, collision_points); + for (const auto & point : collision_points) { + geometry_msgs::msg::Point p; + p.x = point.x(); + p.y = point.y(); + collision_point_array.push_back(p); + } + + // Also check the corner points + + for (const auto & point : object_polygon.outer()) { + if (bg::within(point, one_step_move_vehicle_polygon2d)) { + geometry_msgs::msg::Point p; + p.x = point.x(); + p.y = point.y(); + collision_point_array.push_back(p); + } + } + geometry_msgs::msg::Point nearest_collision_point_tmp; + + double norm = utils::getNearestPointAndDistanceForPredictedObject( + collision_point_array, base_pose, &nearest_collision_point_tmp); + if (norm < min_norm_collision_norm || !is_init) { + min_norm_collision_norm = norm; + nearest_collision_point = nearest_collision_point_tmp; + is_init = true; + nearest_collision_object_index = i; + } + } + } + if (is_init) { + const auto & obj = dynamic_objects->objects.at(nearest_collision_object_index); + const auto obstacle_polygon = utils::convertObjToPolygon(obj); + if (param_.enable_z_axis_obstacle_filtering) { + debug_ptr_->pushPolyhedron(obstacle_polygon, z_min, z_max, PolygonType::Collision); + } else { + debug_ptr_->pushPolygon( + obstacle_polygon, obj.kinematics.initial_pose_with_covariance.pose.position.z, + PolygonType::Collision); + } + return boost::make_optional(std::make_pair(nearest_collision_point, obj)); + } + return boost::none; +} +} // namespace autoware::motion::control::predicted_path_checker diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/debug_marker.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/debug_marker.cpp new file mode 100644 index 0000000000000..3fb7b69d531c0 --- /dev/null +++ b/control/predicted_path_checker/src/predicted_path_checker_node/debug_marker.cpp @@ -0,0 +1,329 @@ +// Copyright 2023 LeoDrive A.Ş. 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 "predicted_path_checker/debug_marker.hpp" + +#include +#include +#include +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include + +#include +#include + +using motion_utils::createDeletedStopVirtualWallMarker; +using motion_utils::createStopVirtualWallMarker; +using tier4_autoware_utils::appendMarkerArray; +using tier4_autoware_utils::calcOffsetPose; +using tier4_autoware_utils::createDefaultMarker; +using tier4_autoware_utils::createMarkerColor; +using tier4_autoware_utils::createMarkerOrientation; +using tier4_autoware_utils::createMarkerScale; +using tier4_autoware_utils::createPoint; + +namespace autoware::motion::control::predicted_path_checker +{ +PredictedPathCheckerDebugNode::PredictedPathCheckerDebugNode( + rclcpp::Node * node, const double base_link2front) +: node_(node), base_link2front_(base_link2front) +{ + virtual_wall_pub_ = + node->create_publisher("~/debug/virtual_wall", 1); + debug_viz_pub_ = + node_->create_publisher("~/debug/marker", 1); +} + +bool PredictedPathCheckerDebugNode::pushPolygon( + const tier4_autoware_utils::Polygon2d & polygon, const double z, const PolygonType & type) +{ + std::vector eigen_polygon; + for (const auto & point : polygon.outer()) { + Eigen::Vector3d eigen_point; + eigen_point << point.x(), point.y(), z; + eigen_polygon.push_back(eigen_point); + } + return pushPolygon(eigen_polygon, type); +} + +bool PredictedPathCheckerDebugNode::pushPolygon( + const std::vector & polygon, const PolygonType & type) +{ + switch (type) { + case PolygonType::Vehicle: + if (!polygon.empty()) { + vehicle_polygons_.push_back(polygon); + } + return true; + case PolygonType::Collision: + if (!polygon.empty()) { + collision_polygons_.push_back(polygon); + } + return true; + default: + return false; + } +} + +bool PredictedPathCheckerDebugNode::pushPolyhedron( + const tier4_autoware_utils::Polygon2d & polyhedron, const double z_min, const double z_max, + const PolygonType & type) +{ + std::vector eigen_polyhedron; + for (const auto & point : polyhedron.outer()) { + eigen_polyhedron.emplace_back(point.x(), point.y(), z_min); + eigen_polyhedron.emplace_back(point.x(), point.y(), z_max); + } + + return pushPolyhedron(eigen_polyhedron, type); +} + +bool PredictedPathCheckerDebugNode::pushPolyhedron( + const std::vector & polyhedron, const PolygonType & type) +{ + switch (type) { + case PolygonType::Vehicle: + if (!polyhedron.empty()) { + vehicle_polyhedrons_.push_back(polyhedron); + } + return true; + case PolygonType::Collision: + if (!polyhedron.empty()) { + collision_polyhedrons_.push_back(polyhedron); + } + return true; + default: + return false; + } +} + +bool PredictedPathCheckerDebugNode::pushPose( + const geometry_msgs::msg::Pose & pose, const PoseType & type) +{ + switch (type) { + case PoseType::Stop: + stop_pose_ptr_ = std::make_shared(pose); + return true; + case PoseType::Collision: + collision_pose_ptr_ = std::make_shared(pose); + return true; + default: + return false; + } +} + +bool PredictedPathCheckerDebugNode::pushObstaclePoint( + const geometry_msgs::msg::Point & obstacle_point, const PointType & type) +{ + switch (type) { + case PointType::Stop: + stop_obstacle_point_ptr_ = std::make_shared(obstacle_point); + return true; + default: + return false; + } +} + +void PredictedPathCheckerDebugNode::publish() +{ + /* publish debug marker for rviz */ + const auto virtual_wall_msg = makeVirtualWallMarker(); + virtual_wall_pub_->publish(virtual_wall_msg); + + /* publish debug marker for rviz */ + const auto visualization_msg = makeVisualizationMarker(); + debug_viz_pub_->publish(visualization_msg); + + /* reset variables */ + vehicle_polygons_.clear(); + collision_polygons_.clear(); + vehicle_polyhedrons_.clear(); + collision_polyhedrons_.clear(); + collision_pose_ptr_ = nullptr; + stop_pose_ptr_ = nullptr; + stop_obstacle_point_ptr_ = nullptr; +} + +visualization_msgs::msg::MarkerArray PredictedPathCheckerDebugNode::makeVirtualWallMarker() +{ + visualization_msgs::msg::MarkerArray msg; + rclcpp::Time current_time = node_->now(); + + if (stop_pose_ptr_ != nullptr) { + const auto p = calcOffsetPose(*stop_pose_ptr_, base_link2front_, 0.0, 0.0); + const auto markers = + createStopVirtualWallMarker(p, "obstacle on predicted path", current_time, 0); + appendMarkerArray(markers, &msg); + } else { + const auto markers = createDeletedStopVirtualWallMarker(current_time, 0); + appendMarkerArray(markers, &msg); + } + + if (collision_pose_ptr_ != nullptr) { + const auto markers = + createStopVirtualWallMarker(*collision_pose_ptr_, "collision_point", current_time, 1); + appendMarkerArray(markers, &msg); + } else { + const auto markers = createDeletedStopVirtualWallMarker(current_time, 1); + appendMarkerArray(markers, &msg); + } + + return msg; +} + +visualization_msgs::msg::MarkerArray PredictedPathCheckerDebugNode::makeVisualizationMarker() +{ + visualization_msgs::msg::MarkerArray msg; + rclcpp::Time current_time = node_->now(); + + // cube + if (!vehicle_polyhedrons_.empty()) { + auto marker = createDefaultMarker( + "map", current_time, "detection_cubes", 0, visualization_msgs::msg::Marker::LINE_LIST, + createMarkerScale(0.01, 0.0, 0.0), createMarkerColor(0.0, 1.0, 0.0, 0.999)); + + for (size_t i = 0; i < vehicle_polyhedrons_.size(); ++i) { + for (size_t j = 0; j < vehicle_polyhedrons_.at(i).size(); ++j) { + const auto & p = vehicle_polyhedrons_.at(i).at(j); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + } + + for (size_t i = 0; i < vehicle_polyhedrons_.size(); ++i) { + for (size_t j = 0; j + 2 < vehicle_polyhedrons_.at(i).size(); ++j) { + const auto & p = vehicle_polyhedrons_.at(i).at(j); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + const auto & p1 = vehicle_polyhedrons_.at(i).at(j + 2); + marker.points.push_back(createPoint(p1.x(), p1.y(), p1.z())); + } + const auto & p = vehicle_polyhedrons_.at(i).at(1); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + const auto & p1 = vehicle_polyhedrons_.at(i).at(vehicle_polyhedrons_.at(i).size() - 1); + marker.points.push_back(createPoint(p1.x(), p1.y(), p1.z())); + const auto & p2 = vehicle_polyhedrons_.at(i).at(0); + marker.points.push_back(createPoint(p2.x(), p2.y(), p2.z())); + const auto & p3 = vehicle_polyhedrons_.at(i).at(vehicle_polyhedrons_.at(i).size() - 2); + marker.points.push_back(createPoint(p3.x(), p3.y(), p3.z())); + } + + msg.markers.push_back(marker); + } + + if (!collision_polyhedrons_.empty()) { + auto marker = createDefaultMarker( + "map", current_time, "collision_cubes", 0, visualization_msgs::msg::Marker::LINE_LIST, + createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(1.0, 0.0, 0.0, 0.999)); + + for (size_t i = 0; i < collision_polyhedrons_.size(); ++i) { + for (size_t j = 0; j < collision_polyhedrons_.at(i).size(); ++j) { + const auto & p = collision_polyhedrons_.at(i).at(j); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + } + + for (size_t i = 0; i < collision_polyhedrons_.size(); ++i) { + for (size_t j = 0; j + 2 < collision_polyhedrons_.at(i).size(); ++j) { + const auto & p = collision_polyhedrons_.at(i).at(j); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + const auto & p1 = collision_polyhedrons_.at(i).at(j + 2); + marker.points.push_back(createPoint(p1.x(), p1.y(), p1.z())); + } + const auto & p = collision_polyhedrons_.at(i).at(1); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + const auto & p1 = collision_polyhedrons_.at(i).at(collision_polyhedrons_.at(i).size() - 1); + marker.points.push_back(createPoint(p1.x(), p1.y(), p1.z())); + const auto & p2 = collision_polyhedrons_.at(i).at(0); + marker.points.push_back(createPoint(p2.x(), p2.y(), p2.z())); + const auto & p3 = collision_polyhedrons_.at(i).at(collision_polyhedrons_.at(i).size() - 2); + marker.points.push_back(createPoint(p3.x(), p3.y(), p3.z())); + } + + msg.markers.push_back(marker); + } + + // polygon + if (!vehicle_polygons_.empty()) { + auto marker = createDefaultMarker( + "map", current_time, "detection_polygons", 0, visualization_msgs::msg::Marker::LINE_LIST, + createMarkerScale(0.01, 0.0, 0.0), createMarkerColor(0.0, 1.0, 0.0, 0.999)); + + for (size_t i = 0; i < vehicle_polygons_.size(); ++i) { + for (size_t j = 0; j < vehicle_polygons_.at(i).size(); ++j) { + { + const auto & p = vehicle_polygons_.at(i).at(j); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + if (j + 1 == vehicle_polygons_.at(i).size()) { + const auto & p = vehicle_polygons_.at(i).at(0); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } else { + const auto & p = vehicle_polygons_.at(i).at(j + 1); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + } + } + msg.markers.push_back(marker); + } + + if (!collision_polygons_.empty()) { + auto marker = createDefaultMarker( + "map", current_time, "collision_polygons", 0, visualization_msgs::msg::Marker::LINE_LIST, + createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(1.0, 0.0, 0.0, 0.999)); + + for (size_t i = 0; i < collision_polygons_.size(); ++i) { + for (size_t j = 0; j < collision_polygons_.at(i).size(); ++j) { + { + const auto & p = collision_polygons_.at(i).at(j); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + if (j + 1 == collision_polygons_.at(i).size()) { + const auto & p = collision_polygons_.at(i).at(0); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } else { + const auto & p = collision_polygons_.at(i).at(j + 1); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + } + } + msg.markers.push_back(marker); + } + + if (stop_obstacle_point_ptr_ != nullptr) { + auto marker = createDefaultMarker( + "map", current_time, "stop_obstacle_point", 0, visualization_msgs::msg::Marker::SPHERE, + createMarkerScale(0.25, 0.25, 0.25), createMarkerColor(1.0, 0.0, 0.0, 0.999)); + marker.pose.position = *stop_obstacle_point_ptr_; + msg.markers.push_back(marker); + } + + if (stop_obstacle_point_ptr_ != nullptr) { + auto marker = createDefaultMarker( + "map", current_time, "stop_obstacle_text", 0, + visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), + createMarkerColor(1.0, 1.0, 1.0, 0.999)); + marker.pose.position = *stop_obstacle_point_ptr_; + marker.pose.position.z += 2.0; + marker.text = "!"; + msg.markers.push_back(marker); + } + + return msg; +} + +} // namespace autoware::motion::control::predicted_path_checker diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp new file mode 100644 index 0000000000000..0cf2548d69746 --- /dev/null +++ b/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp @@ -0,0 +1,585 @@ +// Copyright 2023 LeoDrive A.Ş. 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 "predicted_path_checker/predicted_path_checker_node.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace autoware::motion::control::predicted_path_checker +{ + +PredictedPathCheckerNode::PredictedPathCheckerNode(const rclcpp::NodeOptions & node_options) +: Node("predicted_path_checker_node", node_options), updater_(this) +{ + using std::placeholders::_1; + + const auto adaptor = component_interface_utils::NodeAdaptor(this); + group_cli_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + adaptor.init_cli(cli_set_stop_, group_cli_); + adaptor.init_sub(sub_stop_state_, this, &PredictedPathCheckerNode::onIsStopped); + vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + + // Node Parameter + node_param_.update_rate = declare_parameter("update_rate", 10.0); + node_param_.ego_nearest_dist_threshold = declare_parameter("ego_nearest_dist_threshold", 3.0); + node_param_.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold", 1.046); + node_param_.max_deceleration = declare_parameter("max_deceleration", 1.5); + node_param_.delay_time = declare_parameter("delay_time", 0.17); + node_param_.stop_margin = declare_parameter("stop_margin", 0.5); + node_param_.min_trajectory_check_length = declare_parameter("min_trajectory_check_length", 1.5); + node_param_.trajectory_check_time = declare_parameter("trajectory_check_time", 3.0); + node_param_.resample_interval = declare_parameter("resample_interval", 0.5); + node_param_.distinct_point_distance_threshold = + declare_parameter("distinct_point_distance_threshold", 0.3); + node_param_.distinct_point_yaw_threshold = declare_parameter("distinct_point_yaw_threshold", 5.0); + node_param_.filtering_distance_threshold = declare_parameter("filtering_distance_threshold", 1.5); + node_param_.use_object_prediction = declare_parameter("use_object_prediction", true); + + // Collision Checker Parameter + collision_checker_param_.width_margin = + declare_parameter("collision_checker_params.width_margin", 0.2); + collision_checker_param_.enable_z_axis_obstacle_filtering = + declare_parameter("collision_checker_params.enable_z_axis_obstacle_filtering", false); + collision_checker_param_.z_axis_filtering_buffer = + declare_parameter("collision_checker_params.z_axis_filtering_buffer", 0.3); + collision_checker_param_.chattering_threshold = + declare_parameter("collision_checker_params.chattering_threshold", 0.2); + + // Subscriber + self_pose_listener_ = std::make_shared(this); + + sub_dynamic_objects_ = create_subscription( + "~/input/objects", rclcpp::SensorDataQoS(), + std::bind(&PredictedPathCheckerNode::onDynamicObjects, this, _1)); + sub_reference_trajectory_ = create_subscription( + "~/input/reference_trajectory", 1, + std::bind(&PredictedPathCheckerNode::onReferenceTrajectory, this, _1)); + sub_predicted_trajectory_ = create_subscription( + "~/input/predicted_trajectory", 1, + std::bind(&PredictedPathCheckerNode::onPredictedTrajectory, this, _1)); + sub_odom_ = create_subscription( + "~/input/odometry", 1, std::bind(&PredictedPathCheckerNode::onOdom, this, _1)); + sub_accel_ = create_subscription( + "~/input/current_accel", rclcpp::QoS{1}, + std::bind(&PredictedPathCheckerNode::onAccel, this, _1)); + + debug_ptr_ = + std::make_shared(this, vehicle_info_.max_longitudinal_offset_m); + + // Core + + collision_checker_ = std::make_unique(this, debug_ptr_); + collision_checker_->setParam(collision_checker_param_); + + // Diagnostic Updater + updater_.setHardwareID("predicted_path_checker"); + updater_.add("predicted_path_checker", this, &PredictedPathCheckerNode::checkVehicleState); + + // Wait for first self pose + self_pose_listener_->waitForFirstPose(); + + // Timer + initTimer(1.0 / node_param_.update_rate); +} + +void PredictedPathCheckerNode::onDynamicObjects(const PredictedObjects::ConstSharedPtr msg) +{ + object_ptr_ = msg; +} + +void PredictedPathCheckerNode::onReferenceTrajectory( + const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) +{ + reference_trajectory_ = msg; +} + +void PredictedPathCheckerNode::onPredictedTrajectory( + const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) +{ + predicted_trajectory_ = msg; +} + +void PredictedPathCheckerNode::onOdom(const nav_msgs::msg::Odometry::SharedPtr msg) +{ + current_twist_ = std::make_shared(msg->twist.twist); +} + +void PredictedPathCheckerNode::onAccel( + const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg) +{ + current_accel_ = msg; +} + +void PredictedPathCheckerNode::onIsStopped( + const control_interface::IsStopped::Message::ConstSharedPtr msg) +{ + is_stopped_ptr_ = msg; + + is_stopped_by_node_ = + is_stopped_ptr_->data && + std::find( + is_stopped_ptr_->requested_sources.begin(), is_stopped_ptr_->requested_sources.end(), + "predicted_path_checker") != is_stopped_ptr_->requested_sources.end(); +} + +void PredictedPathCheckerNode::initTimer(double period_s) +{ + const auto period_ns = + std::chrono::duration_cast(std::chrono::duration(period_s)); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&PredictedPathCheckerNode::onTimer, this)); +} + +bool PredictedPathCheckerNode::isDataReady() +{ + if (!current_pose_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for current_pose..."); + return false; + } + + if (!object_ptr_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for dynamic objects msg..."); + return false; + } + + if (!reference_trajectory_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, + "waiting for reference_trajectory msg..."); + return false; + } + + if (!predicted_trajectory_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, + "waiting for predicted_trajectory msg..."); + return false; + } + + if (!current_twist_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for current_twist msg..."); + return false; + } + + if (!current_accel_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for current_accel msg..."); + return false; + } + + if (!is_stopped_ptr_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for is_stopped msg..."); + return false; + } + + if (!cli_set_stop_->service_is_ready()) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for stop service..."); + return false; + } + + return true; +} + +bool PredictedPathCheckerNode::isDataTimeout() +{ + const auto now = this->now(); + + constexpr double th_pose_timeout = 1.0; + const auto pose_time_diff = rclcpp::Time(current_pose_->header.stamp).seconds() - now.seconds(); + if (pose_time_diff > th_pose_timeout) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, "pose is timeout..."); + return true; + } + + return false; +} + +void PredictedPathCheckerNode::onTimer() +{ + current_pose_ = self_pose_listener_->getCurrentPose(); + + if (!isDataReady()) { + return; + } + + if (isDataTimeout()) { + return; + } + + // Cut trajectory + const auto cut_trajectory = cutTrajectory( + *predicted_trajectory_, std::max( + node_param_.min_trajectory_check_length, + current_twist_->linear.x * node_param_.trajectory_check_time)); + + // Convert to trajectory array + + TrajectoryPoints predicted_trajectory_array = motion_utils::convertToTrajectoryPointArray( + motion_utils::resampleTrajectory(cut_trajectory, node_param_.resample_interval)); + + // Filter the objects + + PredictedObjects filtered_objects; + filterObstacles( + current_pose_.get()->pose, predicted_trajectory_array, node_param_.filtering_distance_threshold, + node_param_.use_object_prediction, filtered_objects); + + PredictedObjects::ConstSharedPtr filtered_obj_ptr = + std::make_shared(filtered_objects); + + // Check collision + + const auto collision_checker_output = + collision_checker_->checkTrajectoryForCollision(predicted_trajectory_array, filtered_obj_ptr); + + if (!collision_checker_output) { + // There is no need to stop + if (is_stopped_by_node_) { + sendRequest(false); + } + current_state_ = State::DRIVE; + updater_.force_update(); + debug_ptr_->publish(); + + return; + } + + // Extend trajectory + + extendTrajectoryPointsArray(predicted_trajectory_array); + + // Insert collision and stop points + + const auto stop_idx = + insertStopPoint(predicted_trajectory_array, collision_checker_output.get().first); + + // Check ego vehicle is stopped or not + constexpr double th_stopped_velocity = 0.001; + const bool is_ego_vehicle_stopped = current_twist_->linear.x < th_stopped_velocity; + + // If ego vehicle is not stopped, check obstacle is in the brake distance + if (!is_ego_vehicle_stopped) { + // Calculate projected velocity and acceleration of the object on the trajectory point + + const auto projected_obj_vel_acc = calculateProjectedVelAndAcc( + collision_checker_output->second, predicted_trajectory_array.at(stop_idx)); + + // Calculate relative velocity and acceleration wrt the object + + const double relative_velocity = current_twist_->linear.x - projected_obj_vel_acc.first; + const double relative_acceleration = + current_accel_->accel.accel.linear.x - projected_obj_vel_acc.second; + + // Check if the vehicle is in the brake distance + + const bool is_in_brake_distance = utils::isInBrakeDistance( + predicted_trajectory_array, stop_idx, relative_velocity, relative_acceleration, + node_param_.max_deceleration, node_param_.delay_time); + + if (is_in_brake_distance) { + // Send emergency and stop request + current_state_ = State::EMERGENCY; + updater_.force_update(); + if (!is_stopped_by_node_) { + sendRequest(true); + } + debug_ptr_->publish(); + RCLCPP_ERROR_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, + "There is an obstacle in the brake distance. Sending emergency and stop request..."); + return; + } + } + + // If it is not in the brake distance, check if the collision point is discrete from the reference + // trajectory or not + + const auto reference_trajectory_array = + motion_utils::convertToTrajectoryPointArray(*reference_trajectory_); + + const auto is_discrete_point = + isItDiscretePoint(reference_trajectory_array, predicted_trajectory_array.at(stop_idx)); + + if (is_discrete_point) { + // Check reference trajectory has stop point or not + + const auto is_there_stop_in_ref_trajectory = isThereStopPointOnReferenceTrajectory( + predicted_trajectory_array.at(stop_idx).pose, reference_trajectory_array); + + if (!is_there_stop_in_ref_trajectory) { + // Send stop + if (!is_stopped_by_node_) { + sendRequest(true); + } + current_state_ = State::STOP; + updater_.force_update(); + + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, + "There is an obstacle on predicted path. Sending stop request..."); + + debug_ptr_->publish(); + return; + } + } + + // If it is not discrete point, planning should handle it. Send drive. + current_state_ = State::DRIVE; + updater_.force_update(); + + if (is_stopped_by_node_) { + sendRequest(false); + } + + debug_ptr_->publish(); +} + +TrajectoryPoints PredictedPathCheckerNode::trimTrajectoryFromSelfPose( + const TrajectoryPoints & input, const Pose & self_pose) const +{ + TrajectoryPoints output{}; + + const size_t min_distance_index = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + input, self_pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold) + + 1; + + for (size_t i = min_distance_index; i < input.size(); ++i) { + output.push_back(input.at(i)); + } + + return output; +} + +bool PredictedPathCheckerNode::isThereStopPointOnReferenceTrajectory( + const geometry_msgs::msg::Pose & pose, const TrajectoryPoints & reference_trajectory_array) +{ + const auto trimmed_reference_trajectory_array = + trimTrajectoryFromSelfPose(reference_trajectory_array, current_pose_.get()->pose); + + const auto nearest_stop_point_on_ref_trajectory = + motion_utils::findNearestIndex(trimmed_reference_trajectory_array, pose); + + const auto stop_point_on_trajectory = motion_utils::searchZeroVelocityIndex( + trimmed_reference_trajectory_array, 0, *nearest_stop_point_on_ref_trajectory); + + return !!stop_point_on_trajectory; +} + +void PredictedPathCheckerNode::checkVehicleState(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string msg = "OK"; + + if (current_state_ == State::EMERGENCY) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + msg = "vehicle will collide with obstacles"; + } + if (current_state_ == State::STOP) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + msg = "vehicle will stop due to obstacle"; + } + + stat.summary(level, msg); +} + +void PredictedPathCheckerNode::sendRequest(bool make_stop_vehicle) +{ + if (!is_calling_set_stop_ && cli_set_stop_->service_is_ready()) { + const auto req = std::make_shared(); + req->stop = make_stop_vehicle; + req->request_source = "predicted_path_checker"; + is_calling_set_stop_ = true; + cli_set_stop_->async_send_request(req, [this](auto) { is_calling_set_stop_ = false; }); + } +} + +bool PredictedPathCheckerNode::isItDiscretePoint( + const TrajectoryPoints & reference_trajectory, const TrajectoryPoint & collision_point) const +{ + const auto nearest_segment = + motion_utils::findNearestSegmentIndex(reference_trajectory, collision_point.pose); + + const auto nearest_point = utils::calcInterpolatedPoint( + reference_trajectory, collision_point.pose.position, *nearest_segment, false); + + const auto distance = tier4_autoware_utils::calcDistance2d( + nearest_point.pose.position, collision_point.pose.position); + + const auto yaw_diff = + std::abs(tier4_autoware_utils::calcYawDeviation(nearest_point.pose, collision_point.pose)); + return distance >= node_param_.distinct_point_distance_threshold || + yaw_diff >= tier4_autoware_utils::deg2rad(node_param_.distinct_point_yaw_threshold); +} + +Trajectory PredictedPathCheckerNode::cutTrajectory( + const Trajectory & trajectory, const double length) +{ + Trajectory cut; + cut.header = trajectory.header; + if (trajectory.points.empty()) { + return cut; + } + double total_length = 0.0; + cut.points.push_back(trajectory.points.front()); + for (size_t i = 1; i < trajectory.points.size(); ++i) { + const auto & point = trajectory.points.at(i); + + const auto p1 = tier4_autoware_utils::fromMsg(cut.points.back().pose.position); + const auto p2 = tier4_autoware_utils::fromMsg(point.pose.position); + const auto points_distance = boost::geometry::distance(p1.to_2d(), p2.to_2d()); + + const auto remain_distance = length - total_length; + + // Over length + if (remain_distance <= 0.001) { + break; + } + + // Require interpolation + if (remain_distance <= points_distance) { + const Eigen::Vector3d p_interpolated = p1 + remain_distance * (p2 - p1).normalized(); + + TrajectoryPoint p; + p.pose.position.x = p_interpolated.x(); + p.pose.position.y = p_interpolated.y(); + p.pose.position.z = p_interpolated.z(); + p.pose.orientation = point.pose.orientation; + + cut.points.push_back(p); + break; + } + + cut.points.push_back(point); + total_length += points_distance; + } + motion_utils::removeOverlapPoints(cut.points); + + return cut; +} + +void PredictedPathCheckerNode::extendTrajectoryPointsArray(TrajectoryPoints & trajectory) +{ + // It extends the trajectory to the end of the footprint of the vehicle to get better distance to + // collision_point. + const double extend_distance = vehicle_info_.max_longitudinal_offset_m + node_param_.stop_margin; + const auto & goal_point = trajectory.back(); + const auto trajectory_point_extend = utils::getExtendTrajectoryPoint(extend_distance, goal_point); + trajectory.push_back(trajectory_point_extend); +} + +size_t PredictedPathCheckerNode::insertStopPoint( + TrajectoryPoints & trajectory, const geometry_msgs::msg::Point collision_point) +{ + const auto nearest_collision_segment = + motion_utils::findNearestSegmentIndex(trajectory, collision_point); + + const auto nearest_collision_point = + utils::calcInterpolatedPoint(trajectory, collision_point, nearest_collision_segment, true); + + const size_t collision_idx = nearest_collision_segment + 1; + + trajectory.insert(trajectory.begin() + static_cast(collision_idx), nearest_collision_point); + + const auto stop_point = + utils::findStopPoint(trajectory, collision_idx, node_param_.stop_margin, vehicle_info_); + + const size_t stop_idx = stop_point.first + 1; + trajectory.insert(trajectory.begin() + static_cast(stop_idx), stop_point.second); + + debug_ptr_->pushPose(stop_point.second.pose, PoseType::Stop); + return stop_idx; +} + +std::pair PredictedPathCheckerNode::calculateProjectedVelAndAcc( + const PredictedObject & object, const TrajectoryPoint & trajectory_point) +{ + const auto & orientation_obj = object.kinematics.initial_pose_with_covariance.pose.orientation; + const auto & orientation_stop_point = trajectory_point.pose.orientation; + const auto velocity_obj = object.kinematics.initial_twist_with_covariance.twist.linear.x; + const auto acceleration_obj = + object.kinematics.initial_acceleration_with_covariance.accel.linear.x; + const auto k = std::cos(tier4_autoware_utils::normalizeRadian( + tf2::getYaw(orientation_obj) - tf2::getYaw(orientation_stop_point))); + const auto projected_velocity = velocity_obj * k; + const auto projected_acceleration = acceleration_obj * k; + return std::make_pair(projected_velocity, projected_acceleration); +} + +void PredictedPathCheckerNode::filterObstacles( + const Pose & ego_pose, const TrajectoryPoints & traj, const double dist_threshold, + const bool use_prediction, PredictedObjects & filtered_objects) +{ + filtered_objects.header.frame_id = object_ptr_.get()->header.frame_id; + filtered_objects.header.stamp = this->now(); + + for (auto & object : object_ptr_.get()->objects) { + // Check is it in front of ego vehicle + if (!utils::isFrontObstacle( + ego_pose, object.kinematics.initial_pose_with_covariance.pose.position)) { + continue; + } + + // Check is it near to trajectory + const double max_length = utils::calcObstacleMaxLength(object.shape); + const size_t seg_idx = motion_utils::findNearestSegmentIndex( + traj, object.kinematics.initial_pose_with_covariance.pose.position); + const auto p_front = tier4_autoware_utils::getPoint(traj.at(seg_idx)); + const auto p_back = tier4_autoware_utils::getPoint(traj.at(seg_idx + 1)); + const auto & p_target = object.kinematics.initial_pose_with_covariance.pose.position; + const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0.0}; + const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0.0}; + + if (seg_idx == traj.size() - 2) { + // Calculate longitudinal offset + const auto longitudinal_dist = std::abs(segment_vec.dot(target_vec) / segment_vec.norm()); + if ( + longitudinal_dist - max_length - vehicle_info_.max_longitudinal_offset_m - dist_threshold > + 0.0) { + continue; + } + } + const auto lateral_dist = std::abs(segment_vec.cross(target_vec)(2) / segment_vec.norm()); + if (lateral_dist - max_length - vehicle_info_.max_lateral_offset_m - dist_threshold > 0.0) { + continue; + } + PredictedObject filtered_object = object; + if (use_prediction) { + utils::getCurrentObjectPose(filtered_object, object_ptr_.get()->header.stamp, this->now()); + } + filtered_objects.objects.push_back(filtered_object); + } +} + +} // namespace autoware::motion::control::predicted_path_checker + +#include +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::motion::control::predicted_path_checker::PredictedPathCheckerNode) diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp new file mode 100644 index 0000000000000..9cb095e908d41 --- /dev/null +++ b/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp @@ -0,0 +1,429 @@ +// Copyright 2023 LeoDrive A.Ş. 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 "predicted_path_checker/utils.hpp" + +#include +#include +#include + +namespace utils +{ + +using motion_utils::findFirstNearestIndexWithSoftConstraints; +using motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; +using tier4_autoware_utils::calcDistance2d; +using tier4_autoware_utils::getRPY; + +// Utils Functions +Polygon2d createOneStepPolygon( + const geometry_msgs::msg::Pose & base_step_pose, const geometry_msgs::msg::Pose & next_step_pose, + const vehicle_info_util::VehicleInfo & vehicle_info, const double expand_width) +{ + Polygon2d polygon; + + const double longitudinal_offset = vehicle_info.max_longitudinal_offset_m; + const double width = vehicle_info.vehicle_width_m / 2.0 + expand_width; + const double rear_overhang = vehicle_info.rear_overhang_m; + + { // base step + appendPointToPolygon( + polygon, tier4_autoware_utils::calcOffsetPose(base_step_pose, longitudinal_offset, width, 0.0) + .position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(base_step_pose, longitudinal_offset, -width, 0.0) + .position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(base_step_pose, -rear_overhang, -width, 0.0).position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(base_step_pose, -rear_overhang, width, 0.0).position); + } + + { // next step + appendPointToPolygon( + polygon, tier4_autoware_utils::calcOffsetPose(next_step_pose, longitudinal_offset, width, 0.0) + .position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(next_step_pose, longitudinal_offset, -width, 0.0) + .position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(next_step_pose, -rear_overhang, -width, 0.0).position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(next_step_pose, -rear_overhang, width, 0.0).position); + } + + polygon = tier4_autoware_utils::isClockwise(polygon) + ? polygon + : tier4_autoware_utils::inverseClockwise(polygon); + + Polygon2d hull_polygon; + boost::geometry::convex_hull(polygon, hull_polygon); + boost::geometry::correct(hull_polygon); + + return hull_polygon; +} + +TrajectoryPoint calcInterpolatedPoint( + const TrajectoryPoints & trajectory, const geometry_msgs::msg::Point & target_point, + const size_t segment_idx, const bool use_zero_order_hold_for_twist) +{ + if (trajectory.empty()) { + TrajectoryPoint interpolated_point{}; + interpolated_point.pose.position = target_point; + return interpolated_point; + } else if (trajectory.size() == 1) { + return trajectory.front(); + } + + // Calculate interpolation ratio + const auto & curr_pt = trajectory.at(segment_idx); + const auto & next_pt = trajectory.at(segment_idx + 1); + const auto v1 = tier4_autoware_utils::point2tfVector(curr_pt, next_pt); + const auto v2 = tier4_autoware_utils::point2tfVector(curr_pt, target_point); + if (v1.length2() < 1e-3) { + return curr_pt; + } + + const double ratio = v1.dot(v2) / v1.length2(); + const double clamped_ratio = std::clamp(ratio, 0.0, 1.0); + + // Interpolate + TrajectoryPoint interpolated_point{}; + + // pose interpolation + interpolated_point.pose = + tier4_autoware_utils::calcInterpolatedPose(curr_pt, next_pt, clamped_ratio); + + // twist interpolation + if (use_zero_order_hold_for_twist) { + interpolated_point.longitudinal_velocity_mps = curr_pt.longitudinal_velocity_mps; + interpolated_point.lateral_velocity_mps = curr_pt.lateral_velocity_mps; + interpolated_point.acceleration_mps2 = curr_pt.acceleration_mps2; + } else { + interpolated_point.longitudinal_velocity_mps = interpolation::lerp( + curr_pt.longitudinal_velocity_mps, next_pt.longitudinal_velocity_mps, clamped_ratio); + interpolated_point.lateral_velocity_mps = interpolation::lerp( + curr_pt.lateral_velocity_mps, next_pt.lateral_velocity_mps, clamped_ratio); + interpolated_point.acceleration_mps2 = + interpolation::lerp(curr_pt.acceleration_mps2, next_pt.acceleration_mps2, clamped_ratio); + } + + // heading rate interpolation + interpolated_point.heading_rate_rps = + interpolation::lerp(curr_pt.heading_rate_rps, next_pt.heading_rate_rps, clamped_ratio); + + // wheel interpolation + interpolated_point.front_wheel_angle_rad = interpolation::lerp( + curr_pt.front_wheel_angle_rad, next_pt.front_wheel_angle_rad, clamped_ratio); + interpolated_point.rear_wheel_angle_rad = + interpolation::lerp(curr_pt.rear_wheel_angle_rad, next_pt.rear_wheel_angle_rad, clamped_ratio); + + // time interpolation + const double interpolated_time = interpolation::lerp( + rclcpp::Duration(curr_pt.time_from_start).seconds(), + rclcpp::Duration(next_pt.time_from_start).seconds(), clamped_ratio); + interpolated_point.time_from_start = rclcpp::Duration::from_seconds(interpolated_time); + + return interpolated_point; +} + +std::pair findStopPoint( + TrajectoryPoints & trajectory_array, const size_t collision_idx, const double stop_margin, + vehicle_info_util::VehicleInfo & vehicle_info) +{ + // It returns the stop point and segment of the point on trajectory. + + double desired_distance_base_link_to_collision = + vehicle_info.max_longitudinal_offset_m + stop_margin; + size_t stop_segment_idx = 0; + bool found_stop_point = false; + double distance_point_to_collision = 0.0; + + for (size_t i = collision_idx; i > 0; i--) { + distance_point_to_collision = + motion_utils::calcSignedArcLength(trajectory_array, i - 1, collision_idx); + if (distance_point_to_collision >= desired_distance_base_link_to_collision) { + stop_segment_idx = i - 1; + found_stop_point = true; + break; + } + } + if (found_stop_point) { + const auto & base_point = trajectory_array.at(stop_segment_idx); + const auto & next_point = trajectory_array.at(stop_segment_idx + 1); + + double ratio = (distance_point_to_collision - desired_distance_base_link_to_collision) / + (std::hypot( + base_point.pose.position.x - next_point.pose.position.x, + base_point.pose.position.y - next_point.pose.position.y)); + + geometry_msgs::msg::Pose interpolated_pose = + tier4_autoware_utils::calcInterpolatedPose(base_point.pose, next_point.pose, ratio, false); + TrajectoryPoint output; + output.set__pose(interpolated_pose); + return std::make_pair(stop_segment_idx, output); + } else { + // It means that there is no enough distance between vehicle and collision point. + // So, we insert a stop at the first point of the trajectory. + return std::make_pair(0, trajectory_array.front()); + } +} + +bool isInBrakeDistance( + const TrajectoryPoints & trajectory, const size_t stop_idx, const double relative_velocity, + const double relative_acceleration, const double max_deceleration, const double delay_time_sec) +{ + if (relative_velocity < 0.0) { + return false; + } + + const auto distance_to_obstacle = motion_utils::calcSignedArcLength( + trajectory, trajectory.front().pose.position, trajectory.at(stop_idx).pose.position); + + const double distance_in_delay = relative_velocity * delay_time_sec + + relative_acceleration * delay_time_sec * delay_time_sec * 0.5; + + const double velocity_after_delay = relative_velocity + relative_acceleration * delay_time_sec; + + const double time_to_stop = velocity_after_delay / std::abs(max_deceleration); + const double distance_after_delay = + velocity_after_delay * time_to_stop - 0.5 * abs(max_deceleration) * time_to_stop * time_to_stop; + const double brake_distance = distance_in_delay + distance_after_delay; + + return brake_distance > distance_to_obstacle; +} + +TrajectoryPoint getExtendTrajectoryPoint( + const double extend_distance, const TrajectoryPoint & goal_point) +{ + tf2::Transform map2goal; + tf2::fromMsg(goal_point.pose, map2goal); + tf2::Transform local_extend_point; + local_extend_point.setOrigin(tf2::Vector3(extend_distance, 0.0, 0.0)); + tf2::Quaternion q; + q.setRPY(0, 0, 0); + local_extend_point.setRotation(q); + const auto map2extend_point = map2goal * local_extend_point; + Pose extend_pose; + tf2::toMsg(map2extend_point, extend_pose); + TrajectoryPoint extend_trajectory_point; + extend_trajectory_point.pose = extend_pose; + extend_trajectory_point.longitudinal_velocity_mps = goal_point.longitudinal_velocity_mps; + extend_trajectory_point.lateral_velocity_mps = goal_point.lateral_velocity_mps; + extend_trajectory_point.acceleration_mps2 = goal_point.acceleration_mps2; + return extend_trajectory_point; +} + +bool intersectsInZAxis(const PredictedObject & object, const double z_min, const double z_max) +{ + const auto & obj_pose = object.kinematics.initial_pose_with_covariance; + const auto & obj_height = object.shape.dimensions.z; + return obj_pose.pose.position.z - obj_height / 2.0 <= z_max && + obj_pose.pose.position.z + obj_height / 2.0 >= z_min; +} + +double getNearestPointAndDistanceForPredictedObject( + const PointArray & points, const Pose & base_pose, + geometry_msgs::msg::Point * nearest_collision_point) +{ + double min_norm = 0.0; + bool is_init = false; + + for (const auto & p : points) { + double norm = tier4_autoware_utils::calcDistance2d(p, base_pose); + if (norm < min_norm || !is_init) { + min_norm = norm; + *nearest_collision_point = p; + is_init = true; + } + } + return min_norm; +} + +Polygon2d convertBoundingBoxObjectToGeometryPolygon( + const Pose & current_pose, const double & base_to_front, const double & base_to_rear, + const double & base_to_width) +{ + const auto mapped_point = [](const double & length_scalar, const double & width_scalar) { + tf2::Vector3 map; + map.setX(length_scalar); + map.setY(width_scalar); + map.setZ(0.0); + map.setW(1.0); + return map; + }; + + // set vertices at map coordinate + const tf2::Vector3 p1_map = std::invoke(mapped_point, base_to_front, -base_to_width); + const tf2::Vector3 p2_map = std::invoke(mapped_point, base_to_front, base_to_width); + const tf2::Vector3 p3_map = std::invoke(mapped_point, -base_to_rear, base_to_width); + const tf2::Vector3 p4_map = std::invoke(mapped_point, -base_to_rear, -base_to_width); + + // transform vertices from map coordinate to object coordinate + tf2::Transform tf_map2obj; + tf2::fromMsg(current_pose, tf_map2obj); + const tf2::Vector3 p1_obj = tf_map2obj * p1_map; + const tf2::Vector3 p2_obj = tf_map2obj * p2_map; + const tf2::Vector3 p3_obj = tf_map2obj * p3_map; + const tf2::Vector3 p4_obj = tf_map2obj * p4_map; + + Polygon2d object_polygon; + object_polygon.outer().reserve(5); + object_polygon.outer().emplace_back(p1_obj.x(), p1_obj.y()); + object_polygon.outer().emplace_back(p2_obj.x(), p2_obj.y()); + object_polygon.outer().emplace_back(p3_obj.x(), p3_obj.y()); + object_polygon.outer().emplace_back(p4_obj.x(), p4_obj.y()); + + object_polygon.outer().push_back(object_polygon.outer().front()); + boost::geometry::correct(object_polygon); + + return object_polygon; +} + +Polygon2d convertCylindricalObjectToGeometryPolygon( + const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape) +{ + Polygon2d object_polygon; + + const double obj_x = current_pose.position.x; + const double obj_y = current_pose.position.y; + + constexpr int N = 20; + const double r = obj_shape.dimensions.x / 2; + object_polygon.outer().reserve(N + 1); + for (int i = 0; i < N; ++i) { + object_polygon.outer().emplace_back( + obj_x + r * std::cos(2.0 * M_PI / N * i), obj_y + r * std::sin(2.0 * M_PI / N * i)); + } + + object_polygon.outer().push_back(object_polygon.outer().front()); + boost::geometry::correct(object_polygon); + + return object_polygon; +} + +Polygon2d convertPolygonObjectToGeometryPolygon( + const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape) +{ + Polygon2d object_polygon; + tf2::Transform tf_map2obj; + fromMsg(current_pose, tf_map2obj); + const auto obj_points = obj_shape.footprint.points; + object_polygon.outer().reserve(obj_points.size() + 1); + for (const auto & obj_point : obj_points) { + tf2::Vector3 obj(obj_point.x, obj_point.y, obj_point.z); + tf2::Vector3 tf_obj = tf_map2obj * obj; + object_polygon.outer().emplace_back(tf_obj.x(), tf_obj.y()); + } + object_polygon.outer().push_back(object_polygon.outer().front()); + boost::geometry::correct(object_polygon); + + return object_polygon; +} + +void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & geom_point) +{ + Point2d point; + point.x() = geom_point.x; + point.y() = geom_point.y; + + boost::geometry::append(polygon.outer(), point); +} + +Polygon2d convertObjToPolygon(const PredictedObject & obj) +{ + Polygon2d object_polygon{}; + if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + object_polygon = utils::convertCylindricalObjectToGeometryPolygon( + obj.kinematics.initial_pose_with_covariance.pose, obj.shape); + } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + const double & length_m = obj.shape.dimensions.x / 2; + const double & width_m = obj.shape.dimensions.y / 2; + object_polygon = utils::convertBoundingBoxObjectToGeometryPolygon( + obj.kinematics.initial_pose_with_covariance.pose, length_m, length_m, width_m); + } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + object_polygon = utils::convertPolygonObjectToGeometryPolygon( + obj.kinematics.initial_pose_with_covariance.pose, obj.shape); + } else { + throw std::runtime_error("Unsupported shape type"); + } + return object_polygon; +} + +bool isFrontObstacle(const Pose & ego_pose, const geometry_msgs::msg::Point & obstacle_pos) +{ + const auto yaw = tier4_autoware_utils::getRPY(ego_pose).z; + const Eigen::Vector2d base_pose_vec(std::cos(yaw), std::sin(yaw)); + const Eigen::Vector2d obstacle_vec( + obstacle_pos.x - ego_pose.position.x, obstacle_pos.y - ego_pose.position.y); + + return base_pose_vec.dot(obstacle_vec) >= 0; +} + +double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape) +{ + if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + return std::hypot(shape.dimensions.x / 2.0, shape.dimensions.y / 2.0); + } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + return shape.dimensions.x / 2.0; + } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + double max_length_to_point = 0.0; + for (const auto rel_point : shape.footprint.points) { + const double length_to_point = std::hypot(rel_point.x, rel_point.y); + if (max_length_to_point < length_to_point) { + max_length_to_point = length_to_point; + } + } + return max_length_to_point; + } + + throw std::logic_error("The shape type is not supported in obstacle_cruise_planner."); +} + +void getCurrentObjectPose( + PredictedObject & predicted_object, const rclcpp::Time & obj_base_time, + const rclcpp::Time & current_time) +{ + const double yaw = tier4_autoware_utils::getRPY( + predicted_object.kinematics.initial_pose_with_covariance.pose.orientation) + .z; + const double vx = predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x; + const double ax = predicted_object.kinematics.initial_acceleration_with_covariance.accel.linear.x; + + const double dt = (current_time - obj_base_time).seconds(); + const double ds = vx * dt + 0.5 * ax * dt * dt; + const double delta_yaw = + predicted_object.kinematics.initial_twist_with_covariance.twist.angular.z * dt; + geometry_msgs::msg::Transform transform; + transform.translation = tier4_autoware_utils::createTranslation(ds, 0.0, 0.0); + transform.rotation = tier4_autoware_utils::createQuaternionFromRPY(0.0, 0.0, yaw); + + tf2::Transform tf_pose; + tf2::Transform tf_offset; + tf2::fromMsg(transform, tf_offset); + tf2::fromMsg(predicted_object.kinematics.initial_pose_with_covariance.pose, tf_pose); + tf2::toMsg(tf_pose * tf_offset, predicted_object.kinematics.initial_pose_with_covariance.pose); + predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x += ax * dt; + predicted_object.kinematics.initial_pose_with_covariance.pose.orientation = + tier4_autoware_utils::createQuaternionFromRPY( + 0.0, 0.0, tier4_autoware_utils::normalizeRadian(yaw + delta_yaw)); +} + +} // namespace utils 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 489709d1f5c4c..c108bec2671b3 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 @@ -19,6 +19,7 @@ #include "tf2/utils.h" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "trajectory_follower_base/lateral_controller_base.hpp" #include "trajectory_follower_base/longitudinal_controller_base.hpp" #include "trajectory_follower_node/visibility_control.hpp" @@ -111,6 +112,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node void publishDebugMarker( const trajectory_follower::InputData & input_data, const trajectory_follower::LateralOutput & lat_out) const; + + std::unique_ptr logger_configure_; }; } // namespace trajectory_follower_node } // namespace autoware::motion::control diff --git a/control/trajectory_follower_node/src/controller_node.cpp b/control/trajectory_follower_node/src/controller_node.cpp index 675ca9dffe3ae..e35848e9495af 100644 --- a/control/trajectory_follower_node/src/controller_node.cpp +++ b/control/trajectory_follower_node/src/controller_node.cpp @@ -85,6 +85,8 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control timer_control_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&Controller::callbackTimerControl, this)); } + + logger_configure_ = std::make_unique(this); } Controller::LateralControllerMode Controller::getLateralControllerMode( diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index 2f2083f626f6b..e681bc22cd24b 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -233,6 +233,8 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) rclcpp::create_timer(this, get_clock(), period_ns, std::bind(&VehicleCmdGate::onTimer, this)); timer_pub_status_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&VehicleCmdGate::publishStatus, this)); + + logger_configure_ = std::make_unique(this); } bool VehicleCmdGate::isHeartbeatTimeout( @@ -553,9 +555,7 @@ AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannCont // set prev value for both to keep consistency over switching: // Actual steer, vel, acc should be considered in manual mode to prevent sudden motion when // switching from manual to autonomous - const auto in_autonomous = - (mode.mode == OperationModeState::AUTONOMOUS && mode.is_autoware_control_enabled); - auto prev_values = in_autonomous ? out : current_status_cmd; + auto prev_values = mode.is_autoware_control_enabled ? out : current_status_cmd; if (ego_is_stopped) { prev_values.longitudinal = out.longitudinal; diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index 78d91e266c516..8f0a6aa14d08b 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -17,6 +17,7 @@ #include "adapi_pause_interface.hpp" #include "moderate_stop_interface.hpp" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "vehicle_cmd_filter.hpp" #include @@ -235,6 +236,8 @@ class VehicleCmdGate : public rclcpp::Node // debug MarkerArray createMarkerArray(const IsFilterActivated & filter_activated); + + std::unique_ptr logger_configure_; }; } // namespace vehicle_cmd_gate diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index 375819f8e7579..06718a2cf0ffa 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -63,6 +63,8 @@ def launch_setup(context, *args, **kwargs): obstacle_collision_checker_param = yaml.safe_load(f)["/**"]["ros__parameters"] with open(LaunchConfiguration("aeb_param_path").perform(context), "r") as f: aeb_param = yaml.safe_load(f)["/**"]["ros__parameters"] + with open(LaunchConfiguration("predicted_path_checker_param_path").perform(context), "r") as f: + predicted_path_checker_param = yaml.safe_load(f)["/**"]["ros__parameters"] controller_component = ComposableNode( package="trajectory_follower_node", @@ -176,6 +178,34 @@ def launch_setup(context, *args, **kwargs): target_container="/control/control_container", ) + # autonomous emergency braking + predicted_path_checker = ComposableNode( + package="predicted_path_checker", + plugin="autoware::motion::control::predicted_path_checker::PredictedPathCheckerNode", + name="predicted_path_checker", + remappings=[ + ("~/input/objects", "/perception/object_recognition/objects"), + ("~/input/reference_trajectory", "/planning/scenario_planning/trajectory"), + ("~/input/current_accel", "/localization/acceleration"), + ("~/input/odometry", "/localization/kinematic_state"), + ( + "~/input/predicted_trajectory", + "/control/trajectory_follower/lateral/predicted_trajectory", + ), + ], + parameters=[ + vehicle_info_param, + predicted_path_checker_param, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + predicted_path_checker_loader = LoadComposableNodes( + condition=IfCondition(LaunchConfiguration("enable_predicted_path_checker")), + composable_node_descriptions=[predicted_path_checker], + target_container="/control/control_container", + ) + # vehicle cmd gate vehicle_cmd_gate_component = ComposableNode( package="vehicle_cmd_gate", @@ -338,6 +368,7 @@ def launch_setup(context, *args, **kwargs): external_cmd_converter_loader, obstacle_collision_checker_loader, autonomous_emergency_braking_loader, + predicted_path_checker_loader, ] ) @@ -372,6 +403,8 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg("obstacle_collision_checker_param_path") add_launch_arg("external_cmd_selector_param_path") add_launch_arg("aeb_param_path") + add_launch_arg("predicted_path_checker_param_path") + add_launch_arg("enable_predicted_path_checker") add_launch_arg("enable_autonomous_emergency_braking") add_launch_arg("check_external_emergency_heartbeat") diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml index abbb15c797d19..7733b4e3117a1 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml @@ -20,7 +20,9 @@ - + + + @@ -42,7 +44,9 @@ - + + + @@ -74,7 +78,9 @@ - + + + @@ -99,7 +105,9 @@ - + + + @@ -131,7 +139,9 @@ - + + + diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/yabloc.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/yabloc.launch.xml index a9b0b93947054..8f0191d6156fe 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/yabloc.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/yabloc.launch.xml @@ -1,7 +1,7 @@ - + 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 178e193dc2d99..67e123ea5b018 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 @@ -15,7 +15,8 @@ - + + @@ -169,7 +170,8 @@ - + + @@ -314,6 +316,7 @@ + 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 df833296e3f66..fbe5f21c041b6 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 @@ -15,7 +15,8 @@ - + + @@ -47,6 +48,8 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -114,6 +159,8 @@ + + @@ -142,51 +189,10 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + @@ -293,7 +299,7 @@ - + @@ -302,6 +308,9 @@ + + + @@ -328,7 +337,7 @@ - + @@ -341,6 +350,7 @@ + @@ -403,5 +413,6 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml index 77a3e345ec5e8..1fcf29606891b 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml @@ -35,7 +35,8 @@ - + + @@ -64,7 +65,8 @@ - + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml index 3b6b9ba652a24..c844db39f4e9d 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml @@ -115,6 +115,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml index 52c8aedff8ef9..5b5646a061ac7 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml @@ -20,14 +20,8 @@ - - - - - - - + @@ -40,8 +34,14 @@ + + + + + + - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml index 1ba1e8de9d42e..77de1e5995ee0 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml @@ -15,6 +15,7 @@ + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index b0219376e9625..bbb7ebb2d8d25 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -17,6 +17,7 @@ + @@ -92,7 +93,8 @@ - + + @@ -168,7 +170,8 @@ - + + diff --git a/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml index 7e78c8842f7df..6ace3f423d1bc 100644 --- a/launch/tier4_planning_launch/launch/planning.launch.xml +++ b/launch/tier4_planning_launch/launch/planning.launch.xml @@ -15,8 +15,6 @@ - - diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml index 24a33b9d3f3df..5f17256df6b5c 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml @@ -14,11 +14,6 @@ - - - - - diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml index 39b95286bb6cc..5988d34cded88 100644 --- a/launch/tier4_planning_launch/package.xml +++ b/launch/tier4_planning_launch/package.xml @@ -68,7 +68,6 @@ obstacle_stop_planner planning_evaluator planning_validator - rtc_auto_mode_manager scenario_selector surround_obstacle_checker diff --git a/localization/ekf_localizer/package.xml b/localization/ekf_localizer/package.xml index 005bddd3eb22b..4101bae88b6e2 100644 --- a/localization/ekf_localizer/package.xml +++ b/localization/ekf_localizer/package.xml @@ -24,8 +24,6 @@ kalman_filter nav_msgs rclcpp - sensor_msgs - std_msgs std_srvs tf2 tf2_ros diff --git a/localization/gyro_odometer/package.xml b/localization/gyro_odometer/package.xml index cbcaadebddc2e..ad4a865a2014a 100644 --- a/localization/gyro_odometer/package.xml +++ b/localization/gyro_odometer/package.xml @@ -15,10 +15,8 @@ fmt geometry_msgs sensor_msgs - std_msgs tf2 tf2_geometry_msgs - tf2_ros tier4_autoware_utils rclcpp diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml index 2c35be181d2f0..216fa21bc951a 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml @@ -14,10 +14,8 @@ aruco cv_bridge diagnostic_msgs - geometry_msgs image_transport rclcpp - sensor_msgs tf2_eigen tf2_geometry_msgs tf2_ros diff --git a/localization/landmark_based_localizer/landmark_tf_caster/CMakeLists.txt b/localization/landmark_based_localizer/landmark_tf_caster/CMakeLists.txt index 47899d716e411..8d8cb546b6162 100644 --- a/localization/landmark_based_localizer/landmark_tf_caster/CMakeLists.txt +++ b/localization/landmark_based_localizer/landmark_tf_caster/CMakeLists.txt @@ -12,16 +12,10 @@ endif() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() -find_package(OpenCV REQUIRED) - ament_auto_add_executable(landmark_tf_caster src/landmark_tf_caster_node.cpp src/landmark_tf_caster_core.cpp ) -target_include_directories(landmark_tf_caster - SYSTEM PUBLIC - ${OpenCV_INCLUDE_DIRS} -) ament_auto_package( INSTALL_TO_SHARE diff --git a/localization/landmark_based_localizer/landmark_tf_caster/package.xml b/localization/landmark_based_localizer/landmark_tf_caster/package.xml index 9740e26e2d04a..75f42b91f502a 100644 --- a/localization/landmark_based_localizer/landmark_tf_caster/package.xml +++ b/localization/landmark_based_localizer/landmark_tf_caster/package.xml @@ -12,7 +12,6 @@ autoware_cmake autoware_auto_mapping_msgs - geometry_msgs lanelet2_extension rclcpp tf2_eigen diff --git a/localization/localization_error_monitor/CMakeLists.txt b/localization/localization_error_monitor/CMakeLists.txt index 258e942169556..3528367486350 100644 --- a/localization/localization_error_monitor/CMakeLists.txt +++ b/localization/localization_error_monitor/CMakeLists.txt @@ -4,10 +4,31 @@ project(localization_error_monitor) find_package(autoware_cmake REQUIRED) autoware_package() +ament_auto_add_library(localization_error_monitor_module SHARED + src/diagnostics.cpp +) + ament_auto_add_executable(localization_error_monitor src/main.cpp src/node.cpp ) +target_link_libraries(localization_error_monitor localization_error_monitor_module) + +if(BUILD_TESTING) + function(add_testcase filepath) + get_filename_component(filename ${filepath} NAME) + string(REGEX REPLACE ".cpp" "" test_name ${filename}) + ament_add_gtest(${test_name} ${filepath}) + target_include_directories(${test_name} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) + target_link_libraries(${test_name} localization_error_monitor_module) + ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) + endfunction() + + + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + add_testcase(test/test_diagnostics.cpp) +endif() ament_auto_package(INSTALL_TO_SHARE config diff --git a/localization/localization_error_monitor/README.md b/localization/localization_error_monitor/README.md index bf005d6a00bbb..2ddf41e4eac70 100644 --- a/localization/localization_error_monitor/README.md +++ b/localization/localization_error_monitor/README.md @@ -2,6 +2,10 @@ ## Purpose +

+ +

+ localization_error_monitor is a package for diagnosing localization errors by monitoring uncertainty of the localization results. The package monitors the following two values: @@ -25,10 +29,10 @@ The package monitors the following two values: ## Parameters -| Name | Type | Description | -| -------------------------------------- | ------ | ------------------------------------------------------------------------------------------- | -| `scale` | double | scale factor for monitored values (default: 3.0) | -| `error_ellipse_size` | double | error threshold for long radius of confidence ellipse [m] (default: 1.0) | -| `warn_ellipse_size` | double | warning threshold for long radius of confidence ellipse [m] (default: 0.8) | -| `error_ellipse_size_lateral_direction` | double | error threshold for size of confidence ellipse along lateral direction [m] (default: 0.3) | -| `warn_ellipse_size_lateral_direction` | double | warning threshold for size of confidence ellipse along lateral direction [m] (default: 0.2) | +| Name | Type | Description | +| -------------------------------------- | ------ | -------------------------------------------------------------------------------------------- | +| `scale` | double | scale factor for monitored values (default: 3.0) | +| `error_ellipse_size` | double | error threshold for long radius of confidence ellipse [m] (default: 1.0) | +| `warn_ellipse_size` | double | warning threshold for long radius of confidence ellipse [m] (default: 0.8) | +| `error_ellipse_size_lateral_direction` | double | error threshold for size of confidence ellipse along lateral direction [m] (default: 0.3) | +| `warn_ellipse_size_lateral_direction` | double | warning threshold for size of confidence ellipse along lateral direction [m] (default: 0.25) | diff --git a/localization/localization_error_monitor/config/localization_error_monitor.param.yaml b/localization/localization_error_monitor/config/localization_error_monitor.param.yaml index 026daf0532d33..def5c80164642 100644 --- a/localization/localization_error_monitor/config/localization_error_monitor.param.yaml +++ b/localization/localization_error_monitor/config/localization_error_monitor.param.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: scale: 3.0 - error_ellipse_size: 1.0 - warn_ellipse_size: 0.8 + error_ellipse_size: 1.5 + warn_ellipse_size: 1.2 error_ellipse_size_lateral_direction: 0.3 - warn_ellipse_size_lateral_direction: 0.2 + warn_ellipse_size_lateral_direction: 0.25 diff --git a/localization/localization_error_monitor/include/localization_error_monitor/diagnostics.hpp b/localization/localization_error_monitor/include/localization_error_monitor/diagnostics.hpp new file mode 100644 index 0000000000000..4fe4e5a5aac14 --- /dev/null +++ b/localization/localization_error_monitor/include/localization_error_monitor/diagnostics.hpp @@ -0,0 +1,31 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LOCALIZATION_ERROR_MONITOR__DIAGNOSTICS_HPP_ +#define LOCALIZATION_ERROR_MONITOR__DIAGNOSTICS_HPP_ + +#include + +#include +#include + +diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracy( + const double ellipse_size, const double warn_ellipse_size, const double error_ellipse_size); +diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracyLateralDirection( + const double ellipse_size, const double warn_ellipse_size, const double error_ellipse_size); + +diagnostic_msgs::msg::DiagnosticStatus mergeDiagnosticStatus( + const std::vector & stat_array); + +#endif // LOCALIZATION_ERROR_MONITOR__DIAGNOSTICS_HPP_ diff --git a/localization/localization_error_monitor/include/localization_error_monitor/node.hpp b/localization/localization_error_monitor/include/localization_error_monitor/node.hpp index 83b2ddef9215a..7bdc87d5d47f2 100644 --- a/localization/localization_error_monitor/include/localization_error_monitor/node.hpp +++ b/localization/localization_error_monitor/include/localization_error_monitor/node.hpp @@ -16,7 +16,6 @@ #define LOCALIZATION_ERROR_MONITOR__NODE_HPP_ #include -#include #include #include @@ -37,6 +36,7 @@ class LocalizationErrorMonitor : public rclcpp::Node private: rclcpp::Subscription::SharedPtr odom_sub_; rclcpp::Publisher::SharedPtr ellipse_marker_pub_; + rclcpp::Publisher::SharedPtr diag_pub_; rclcpp::TimerBase::SharedPtr timer_; double scale_; @@ -45,16 +45,11 @@ class LocalizationErrorMonitor : public rclcpp::Node double error_ellipse_size_lateral_direction_; double warn_ellipse_size_lateral_direction_; Ellipse ellipse_; - diagnostic_updater::Updater updater_; - void checkLocalizationAccuracy(diagnostic_updater::DiagnosticStatusWrapper & stat); - void checkLocalizationAccuracyLateralDirection( - diagnostic_updater::DiagnosticStatusWrapper & stat); void onOdom(nav_msgs::msg::Odometry::ConstSharedPtr input_msg); visualization_msgs::msg::Marker createEllipseMarker( const Ellipse & ellipse, nav_msgs::msg::Odometry::ConstSharedPtr odom); double measureSizeEllipseAlongBodyFrame(const Eigen::Matrix2d & Pinv, double theta); - void onTimer(); public: LocalizationErrorMonitor(); diff --git a/localization/localization_error_monitor/media/diagnostics.png b/localization/localization_error_monitor/media/diagnostics.png new file mode 100644 index 0000000000000..74aacb68a2b7c Binary files /dev/null and b/localization/localization_error_monitor/media/diagnostics.png differ diff --git a/localization/localization_error_monitor/package.xml b/localization/localization_error_monitor/package.xml index 4a186bdf95ed1..4cc5599fd81a9 100644 --- a/localization/localization_error_monitor/package.xml +++ b/localization/localization_error_monitor/package.xml @@ -13,15 +13,16 @@ ament_cmake_auto autoware_cmake + eigen diagnostic_msgs diagnostic_updater - eigen nav_msgs tf2 tf2_geometry_msgs visualization_msgs + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/localization/localization_error_monitor/src/diagnostics.cpp b/localization/localization_error_monitor/src/diagnostics.cpp new file mode 100644 index 0000000000000..0c5a4a7800639 --- /dev/null +++ b/localization/localization_error_monitor/src/diagnostics.cpp @@ -0,0 +1,94 @@ +// Copyright 2023 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 + +#include +#include + +diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracy( + const double ellipse_size, const double warn_ellipse_size, const double error_ellipse_size) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = "localization_accuracy"; + key_value.value = std::to_string(ellipse_size); + stat.values.push_back(key_value); + + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat.message = "OK"; + if (ellipse_size >= warn_ellipse_size) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat.message = "ellipse size is too large"; + } + if (ellipse_size >= error_ellipse_size) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + stat.message = "ellipse size is over the expected range"; + } + + return stat; +} + +diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracyLateralDirection( + const double ellipse_size, const double warn_ellipse_size, const double error_ellipse_size) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = "localization_accuracy_lateral_direction"; + key_value.value = std::to_string(ellipse_size); + stat.values.push_back(key_value); + + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat.message = "OK"; + if (ellipse_size >= warn_ellipse_size) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat.message = "ellipse size along lateral direction is too large"; + } + if (ellipse_size >= error_ellipse_size) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + stat.message = "ellipse size along lateral direction is over the expected range"; + } + + return stat; +} + +// The highest level within the stat_array will be reflected in the merged_stat. +diagnostic_msgs::msg::DiagnosticStatus mergeDiagnosticStatus( + const std::vector & stat_array) +{ + diagnostic_msgs::msg::DiagnosticStatus merged_stat; + + for (const auto & stat : stat_array) { + if ((stat.level > diagnostic_msgs::msg::DiagnosticStatus::OK)) { + if (!merged_stat.message.empty()) { + merged_stat.message += "; "; + } + merged_stat.message += stat.message; + } + if (stat.level > merged_stat.level) { + merged_stat.level = stat.level; + } + for (const auto & value : stat.values) { + merged_stat.values.push_back(value); + } + } + + if (merged_stat.level == diagnostic_msgs::msg::DiagnosticStatus::OK) { + merged_stat.message = "OK"; + } + + return merged_stat; +} diff --git a/localization/localization_error_monitor/src/node.cpp b/localization/localization_error_monitor/src/node.cpp index 75858c1f4be4d..43795d4036d42 100644 --- a/localization/localization_error_monitor/src/node.cpp +++ b/localization/localization_error_monitor/src/node.cpp @@ -14,6 +14,8 @@ #include "localization_error_monitor/node.hpp" +#include "localization_error_monitor/diagnostics.hpp" + #include #include @@ -30,17 +32,16 @@ #include #include -LocalizationErrorMonitor::LocalizationErrorMonitor() -: Node("localization_error_monitor"), updater_(this) +LocalizationErrorMonitor::LocalizationErrorMonitor() : Node("localization_error_monitor") { - scale_ = this->declare_parameter("scale", 3.0); - error_ellipse_size_ = this->declare_parameter("error_ellipse_size", 1.0); - warn_ellipse_size_ = this->declare_parameter("warn_ellipse_size", 0.8); + scale_ = this->declare_parameter("scale"); + error_ellipse_size_ = this->declare_parameter("error_ellipse_size"); + warn_ellipse_size_ = this->declare_parameter("warn_ellipse_size"); error_ellipse_size_lateral_direction_ = - this->declare_parameter("error_ellipse_size_lateral_direction", 0.3); + this->declare_parameter("error_ellipse_size_lateral_direction"); warn_ellipse_size_lateral_direction_ = - this->declare_parameter("warn_ellipse_size_lateral_direction", 0.2); + this->declare_parameter("warn_ellipse_size_lateral_direction"); odom_sub_ = this->create_subscription( "input/odom", 1, std::bind(&LocalizationErrorMonitor::onOdom, this, std::placeholders::_1)); @@ -51,55 +52,7 @@ LocalizationErrorMonitor::LocalizationErrorMonitor() ellipse_marker_pub_ = this->create_publisher("debug/ellipse_marker", durable_qos); - updater_.setHardwareID("localization_error_monitor"); - updater_.add("localization_accuracy", this, &LocalizationErrorMonitor::checkLocalizationAccuracy); - updater_.add( - "localization_accuracy_lateral_direction", this, - &LocalizationErrorMonitor::checkLocalizationAccuracyLateralDirection); - - // Set timer - using std::chrono_literals::operator""ms; - timer_ = rclcpp::create_timer( - this, get_clock(), 100ms, std::bind(&LocalizationErrorMonitor::onTimer, this)); -} - -void LocalizationErrorMonitor::onTimer() -{ - updater_.force_update(); -} - -void LocalizationErrorMonitor::checkLocalizationAccuracy( - diagnostic_updater::DiagnosticStatusWrapper & stat) -{ - stat.add("localization_accuracy", ellipse_.long_radius); - int8_t diag_level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string diag_message = "ellipse size is within the expected range"; - if (warn_ellipse_size_ <= ellipse_.long_radius) { - diag_level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - diag_message = "ellipse size is too large"; - } - if (error_ellipse_size_ <= ellipse_.long_radius) { - diag_level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - diag_message = "ellipse size is over the expected range"; - } - stat.summary(diag_level, diag_message); -} - -void LocalizationErrorMonitor::checkLocalizationAccuracyLateralDirection( - diagnostic_updater::DiagnosticStatusWrapper & stat) -{ - stat.add("localization_accuracy_lateral_direction", ellipse_.size_lateral_direction); - int8_t diag_level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string diag_message = "ellipse size along lateral direction is within the expected range"; - if (warn_ellipse_size_lateral_direction_ <= ellipse_.size_lateral_direction) { - diag_level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - diag_message = "ellipse size along lateral direction is too large"; - } - if (error_ellipse_size_lateral_direction_ <= ellipse_.size_lateral_direction) { - diag_level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - diag_message = "ellipse size along lateral direction is over the expected range"; - } - stat.summary(diag_level, diag_message); + diag_pub_ = this->create_publisher("/diagnostics", 10); } visualization_msgs::msg::Marker LocalizationErrorMonitor::createEllipseMarker( @@ -158,6 +111,24 @@ void LocalizationErrorMonitor::onOdom(nav_msgs::msg::Odometry::ConstSharedPtr in const auto ellipse_marker = createEllipseMarker(ellipse_, input_msg); ellipse_marker_pub_->publish(ellipse_marker); + + // diagnostics + std::vector diag_status_array; + diag_status_array.push_back( + checkLocalizationAccuracy(ellipse_.long_radius, warn_ellipse_size_, error_ellipse_size_)); + diag_status_array.push_back(checkLocalizationAccuracyLateralDirection( + ellipse_.size_lateral_direction, warn_ellipse_size_lateral_direction_, + error_ellipse_size_lateral_direction_)); + + diagnostic_msgs::msg::DiagnosticStatus diag_merged_status; + diag_merged_status = mergeDiagnosticStatus(diag_status_array); + diag_merged_status.name = "localization: " + std::string(this->get_name()); + diag_merged_status.hardware_id = this->get_name(); + + diagnostic_msgs::msg::DiagnosticArray diag_msg; + diag_msg.header.stamp = this->now(); + diag_msg.status.push_back(diag_merged_status); + diag_pub_->publish(diag_msg); } double LocalizationErrorMonitor::measureSizeEllipseAlongBodyFrame( diff --git a/localization/localization_error_monitor/test/test_diagnostics.cpp b/localization/localization_error_monitor/test/test_diagnostics.cpp new file mode 100644 index 0000000000000..7b7e8aaab4ed9 --- /dev/null +++ b/localization/localization_error_monitor/test/test_diagnostics.cpp @@ -0,0 +1,140 @@ +// Copyright 2023 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 "localization_error_monitor/diagnostics.hpp" + +#include + +TEST(TestLocalizationErrorMonitorDiagnostics, CheckLocalizationAccuracy) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + const double warn_ellipse_size = 0.8; + const double error_ellipse_size = 1.0; + + double ellipse_size = 0.0; + stat = checkLocalizationAccuracy(ellipse_size, warn_ellipse_size, error_ellipse_size); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + + ellipse_size = 0.7; + stat = checkLocalizationAccuracy(ellipse_size, warn_ellipse_size, error_ellipse_size); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + + ellipse_size = 0.8; + stat = checkLocalizationAccuracy(ellipse_size, warn_ellipse_size, error_ellipse_size); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); + + ellipse_size = 0.9; + stat = checkLocalizationAccuracy(ellipse_size, warn_ellipse_size, error_ellipse_size); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); + + ellipse_size = 1.0; + stat = checkLocalizationAccuracy(ellipse_size, warn_ellipse_size, error_ellipse_size); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); +} + +TEST(TestLocalizationErrorMonitorDiagnostics, CheckLocalizationAccuracyLateralDirection) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + const double warn_ellipse_size = 0.25; + const double error_ellipse_size = 0.3; + + double ellipse_size = 0.0; + stat = + checkLocalizationAccuracyLateralDirection(ellipse_size, warn_ellipse_size, error_ellipse_size); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + + ellipse_size = 0.24; + stat = + checkLocalizationAccuracyLateralDirection(ellipse_size, warn_ellipse_size, error_ellipse_size); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + + ellipse_size = 0.25; + stat = + checkLocalizationAccuracyLateralDirection(ellipse_size, warn_ellipse_size, error_ellipse_size); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); + + ellipse_size = 0.29; + stat = + checkLocalizationAccuracyLateralDirection(ellipse_size, warn_ellipse_size, error_ellipse_size); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); + + ellipse_size = 0.3; + stat = + checkLocalizationAccuracyLateralDirection(ellipse_size, warn_ellipse_size, error_ellipse_size); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); +} + +TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) +{ + diagnostic_msgs::msg::DiagnosticStatus merged_stat; + std::vector stat_array(2); + + stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat_array.at(0).message = "OK"; + stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat_array.at(1).message = "OK"; + merged_stat = mergeDiagnosticStatus(stat_array); + EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + EXPECT_EQ(merged_stat.message, "OK"); + + stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat_array.at(0).message = "WARN0"; + stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat_array.at(1).message = "OK"; + merged_stat = mergeDiagnosticStatus(stat_array); + EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); + EXPECT_EQ(merged_stat.message, "WARN0"); + + stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat_array.at(0).message = "OK"; + stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat_array.at(1).message = "WARN1"; + merged_stat = mergeDiagnosticStatus(stat_array); + EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); + EXPECT_EQ(merged_stat.message, "WARN1"); + + stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat_array.at(0).message = "WARN0"; + stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat_array.at(1).message = "WARN1"; + merged_stat = mergeDiagnosticStatus(stat_array); + EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); + EXPECT_EQ(merged_stat.message, "WARN0; WARN1"); + + stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat_array.at(0).message = "OK"; + stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + stat_array.at(1).message = "ERROR1"; + merged_stat = mergeDiagnosticStatus(stat_array); + EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); + EXPECT_EQ(merged_stat.message, "ERROR1"); + + stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat_array.at(0).message = "WARN0"; + stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + stat_array.at(1).message = "ERROR1"; + merged_stat = mergeDiagnosticStatus(stat_array); + EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); + EXPECT_EQ(merged_stat.message, "WARN0; ERROR1"); + + stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + stat_array.at(0).message = "ERROR0"; + stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + stat_array.at(1).message = "ERROR1"; + merged_stat = mergeDiagnosticStatus(stat_array); + EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); + EXPECT_EQ(merged_stat.message, "ERROR0; ERROR1"); +} diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index c5a14d94ebed3..a1b9339b9780a 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -54,19 +54,25 @@ One optional function is regularization. Please see the regularization chapter i ### Core Parameters -| Name | Type | Description | -| --------------------------------------- | ------ | ----------------------------------------------------------------------------------------------- | -| `base_frame` | string | Vehicle reference frame | -| `ndt_base_frame` | string | NDT reference frame | -| `input_sensor_points_queue_size` | int | Subscriber queue size | -| `trans_epsilon` | double | The maximum difference between two consecutive transformations in order to consider convergence | -| `step_size` | double | The newton line search maximum step length | -| `resolution` | double | The ND voxel grid resolution [m] | -| `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 | +| Name | Type | Description | +| --------------------------------------------------------- | ---------------------- | -------------------------------------------------------------------------------------------------- | +| `base_frame` | string | Vehicle reference frame | +| `ndt_base_frame` | string | NDT reference frame | +| `map_frame` | string | map frame | +| `input_sensor_points_queue_size` | int | Subscriber queue size | +| `trans_epsilon` | double | The max difference between two consecutive transformations to consider convergence | +| `step_size` | double | The newton line search maximum step length | +| `resolution` | double | The ND voxel grid resolution [m] | +| `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 | TP threshold for deciding whether to trust the estimation result (when converged_param_type = 0) | +| `converged_param_nearest_voxel_transformation_likelihood` | double | NVTL threshold for deciding whether to trust the estimation result (when converged_param_type = 1) | +| `initial_estimate_particles_num` | int | The number of particles to estimate initial pose | +| `lidar_topic_timeout_sec` | double | Tolerance of timestamp difference between current time and sensor pointcloud | +| `initial_pose_timeout_sec` | int | Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec] | +| `initial_pose_distance_tolerance_m` | double | Tolerance of distance difference between two initial poses used for linear interpolation. [m] | +| `num_threads` | int | Number of threads used for parallel computing | +| `output_pose_covariance` | std::array | The covariance of output pose | (TP: Transform Probability, NVTL: Nearest Voxel Transform Probability) @@ -238,7 +244,7 @@ Here is a split PCD map for `sample-map-rosbag` from Autoware tutorial: [`sample ### Abstract -This is a function that using de-grounded LiDAR scan estimate scan matching score.This score can more accurately reflect the current localization performance. +This is a function that uses de-grounded LiDAR scan to estimate the scan matching score. This score can reflect the current localization performance more accurately. [related issue](https://github.com/autowarefoundation/autoware.universe/issues/2044). ### Parameters 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 c8afa9ea1a916..cf41a4cf55d0b 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -9,6 +9,9 @@ # NDT reference frame ndt_base_frame: "ndt_base_link" + # map frame + map_frame: "map" + # Subscriber queue size input_sensor_points_queue_size: 1 @@ -38,7 +41,7 @@ converged_param_nearest_voxel_transformation_likelihood: 2.3 # The number of particles to estimate initial pose - initial_estimate_particles_num: 100 + initial_estimate_particles_num: 200 # Tolerance of timestamp difference between current time and sensor pointcloud. [sec] lidar_topic_timeout_sec: 1.0 @@ -53,7 +56,7 @@ num_threads: 4 # The covariance of output pose - # Do note that this covariance matrix is empirically derived + # Note that this covariance matrix is empirically derived output_pose_covariance: [ 0.0225, 0.0, 0.0, 0.0, 0.0, 0.0, @@ -67,7 +70,7 @@ # Regularization switch regularization_enabled: false - # regularization scale factor + # Regularization scale factor regularization_scale_factor: 0.01 # Dynamic map loading distance @@ -85,3 +88,6 @@ # If lidar_point.z - base_link.z <= this threshold , the point will be removed z_margin_for_ground_removal: 0.8 + + # The execution time which means probably NDT cannot matches scans properly. [ms] + critical_upper_bound_exe_time_ms: 100 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 ed3b99019f7d9..011cda6ba3356 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 @@ -208,6 +208,9 @@ class NDTScanMatcher : public rclcpp::Node double z_margin_for_ground_removal_; bool use_dynamic_map_loading_; + + // The execution time which means probably NDT cannot matches scans properly + int critical_upper_bound_exe_time_ms_; }; #endif // NDT_SCAN_MATCHER__NDT_SCAN_MATCHER_CORE_HPP_ 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 7bb2b79e88f33..d9054d77d2256 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -84,23 +84,9 @@ NDTScanMatcher::NDTScanMatcher() tf2_broadcaster_(*this), ndt_ptr_(new NormalDistributionsTransform), state_ptr_(new std::map), - base_frame_("base_link"), - ndt_base_frame_("ndt_base_link"), - map_frame_("map"), - converged_param_type_(ConvergedParamType::TRANSFORM_PROBABILITY), - 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), - oscillation_threshold_(10), - output_pose_covariance_(), - regularization_enabled_(declare_parameter("regularization_enabled")), - estimate_scores_for_degrounded_scan_( - declare_parameter("estimate_scores_for_degrounded_scan")), - z_margin_for_ground_removal_(declare_parameter("z_margin_for_ground_removal")) + inversion_vector_threshold_(-0.9), // Not necessary to extract to ndt_scan_matcher.param.yaml + oscillation_threshold_(10), // Not necessary to extract to ndt_scan_matcher.param.yaml + regularization_enabled_(declare_parameter("regularization_enabled")) { (*state_ptr_)["state"] = "Initializing"; is_activated_ = false; @@ -115,6 +101,9 @@ NDTScanMatcher::NDTScanMatcher() ndt_base_frame_ = this->declare_parameter("ndt_base_frame"); RCLCPP_INFO(get_logger(), "ndt_base_frame_id: %s", ndt_base_frame_.c_str()); + map_frame_ = this->declare_parameter("map_frame"); + RCLCPP_INFO(get_logger(), "map_frame_id: %s", map_frame_.c_str()); + pclomp::NdtParams ndt_params{}; ndt_params.trans_epsilon = this->declare_parameter("trans_epsilon"); ndt_params.step_size = this->declare_parameter("step_size"); @@ -141,6 +130,9 @@ NDTScanMatcher::NDTScanMatcher() lidar_topic_timeout_sec_ = this->declare_parameter("lidar_topic_timeout_sec"); + critical_upper_bound_exe_time_ms_ = + this->declare_parameter("critical_upper_bound_exe_time_ms"); + initial_pose_timeout_sec_ = this->declare_parameter("initial_pose_timeout_sec"); initial_pose_distance_tolerance_m_ = @@ -154,6 +146,11 @@ NDTScanMatcher::NDTScanMatcher() initial_estimate_particles_num_ = this->declare_parameter("initial_estimate_particles_num"); + estimate_scores_for_degrounded_scan_ = + this->declare_parameter("estimate_scores_for_degrounded_scan"); + + z_margin_for_ground_removal_ = this->declare_parameter("z_margin_for_ground_removal"); + rclcpp::CallbackGroup::SharedPtr initial_pose_callback_group; initial_pose_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -292,6 +289,13 @@ void NDTScanMatcher::timer_diagnostic() diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; diag_status_msg.message += "NDT score is unreliably low. "; } + if ( + state_ptr_->count("execution_time") && + std::stod((*state_ptr_)["execution_time"]) >= critical_upper_bound_exe_time_ms_) { + diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + diag_status_msg.message += + "NDT exe time is too long. (took " + (*state_ptr_)["execution_time"] + " [ms])"; + } // Ignore local optimal solution if ( state_ptr_->count("is_local_optimal_solution_oscillation") && @@ -528,6 +532,7 @@ void NDTScanMatcher::callback_sensor_points( } else { (*state_ptr_)["is_local_optimal_solution_oscillation"] = "0"; } + (*state_ptr_)["execution_time"] = std::to_string(exe_time); } void NDTScanMatcher::transform_sensor_measurement( diff --git a/localization/yabloc/yabloc_common/package.xml b/localization/yabloc/yabloc_common/package.xml index 1c3ba4de307f6..d7e94ebefa24b 100644 --- a/localization/yabloc/yabloc_common/package.xml +++ b/localization/yabloc/yabloc_common/package.xml @@ -13,22 +13,17 @@ autoware_auto_mapping_msgs cv_bridge - geographiclib geometry_msgs lanelet2_core lanelet2_extension - lanelet2_io libgoogle-glog-dev pcl_conversions - rcl_interfaces rclcpp - rclcpp_components sensor_msgs signal_processing sophus std_msgs tf2_ros - ublox_msgs visualization_msgs ament_lint_auto diff --git a/localization/yabloc/yabloc_image_processing/README.md b/localization/yabloc/yabloc_image_processing/README.md index eb9ae658c39a7..85e1d408a1b4b 100644 --- a/localization/yabloc/yabloc_image_processing/README.md +++ b/localization/yabloc/yabloc_image_processing/README.md @@ -108,10 +108,15 @@ This node performs image resizing and undistortion at the same time. #### Input -| Name | Type | Description | -| ---------------------------- | ----------------------------------- | -------------------- | -| `input/camera_info` | `sensor_msgs::msg::CameraInfo` | raw compressed image | -| `input/image_raw/compressed` | `sensor_msgs::msg::CompressedImage` | raw sensor info | +| Name | Type | Description | +| ---------------------------- | ----------------------------------- | ----------------------- | +| `input/camera_info` | `sensor_msgs::msg::CameraInfo` | camera info | +| `input/image_raw` | `sensor_msgs::msg::Image` | raw camera image | +| `input/image_raw/compressed` | `sensor_msgs::msg::CompressedImage` | compressed camera image | + +This node subscribes to both compressed image and raw image topics. +If raw image is subscribed to even once, compressed image will no longer be subscribed to. +This is to avoid redundant decompression within Autoware. #### Output diff --git a/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml b/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml index fd55a8163123a..aa6fc10ee6667 100644 --- a/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml +++ b/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml @@ -1,5 +1,5 @@ - + @@ -9,6 +9,7 @@ + diff --git a/localization/yabloc/yabloc_image_processing/package.xml b/localization/yabloc/yabloc_image_processing/package.xml index 961ce574f7c7a..ffdcea25b4b6d 100644 --- a/localization/yabloc/yabloc_image_processing/package.xml +++ b/localization/yabloc/yabloc_image_processing/package.xml @@ -12,14 +12,10 @@ autoware_cmake cv_bridge - nav_msgs pcl_conversions rclcpp sensor_msgs - sophus std_msgs - tf2 - tf2_ros tier4_autoware_utils visualization_msgs yabloc_common diff --git a/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp b/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp index 7412db28e1550..7fc9ad785dbe2 100644 --- a/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp +++ b/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp @@ -50,9 +50,12 @@ class UndistortNode : public rclcpp::Node } auto on_image = std::bind(&UndistortNode::on_image, this, _1); + auto on_compressed_image = std::bind(&UndistortNode::on_compressed_image, this, _1); auto on_info = std::bind(&UndistortNode::on_info, this, _1); - sub_image_ = - create_subscription("~/input/image_raw", qos, std::move(on_image)); + sub_image_ = create_subscription("~/input/image_raw", qos, std::move(on_image)); + sub_compressed_image_ = create_subscription( + "~/input/image_raw/compressed", qos, std::move(on_compressed_image)); + sub_info_ = create_subscription("~/input/camera_info", qos, std::move(on_info)); pub_info_ = create_publisher("~/output/resized_info", 10); @@ -63,7 +66,8 @@ class UndistortNode : public rclcpp::Node const int OUTPUT_WIDTH; const std::string OVERRIDE_FRAME_ID; - rclcpp::Subscription::SharedPtr sub_image_; + rclcpp::Subscription::SharedPtr sub_image_; + rclcpp::Subscription::SharedPtr sub_compressed_image_; rclcpp::Subscription::SharedPtr sub_info_; rclcpp::Publisher::SharedPtr pub_image_; rclcpp::Publisher::SharedPtr pub_info_; @@ -99,14 +103,8 @@ class UndistortNode : public rclcpp::Node scaled_info_->height = new_size.height; } - void on_image(const CompressedImage & msg) + void remap_and_publish(const cv::Mat & image, const std_msgs::msg::Header & header) { - if (!info_.has_value()) return; - if (undistort_map_x.empty()) make_remap_lut(); - - tier4_autoware_utils::StopWatch stop_watch; - cv::Mat image = common::decompress_to_cv_mat(msg); - cv::Mat undistorted_image; cv::remap(image, undistorted_image, undistort_map_x, undistort_map_y, cv::INTER_LINEAR); @@ -120,16 +118,47 @@ class UndistortNode : public rclcpp::Node // Publish Image { cv_bridge::CvImage bridge; - bridge.header.stamp = msg.header.stamp; + bridge.header.stamp = header.stamp; if (OVERRIDE_FRAME_ID != "") bridge.header.frame_id = OVERRIDE_FRAME_ID; else - bridge.header.frame_id = msg.header.frame_id; + bridge.header.frame_id = header.frame_id; bridge.encoding = "bgr8"; bridge.image = undistorted_image; pub_image_->publish(*bridge.toImageMsg()); } + } + void on_image(const Image & msg) + { + if (!info_.has_value()) { + return; + } + if (undistort_map_x.empty()) { + make_remap_lut(); + } + + // To remove redundant decompression, deactivate compressed image subscriber + if (sub_compressed_image_) { + sub_compressed_image_.reset(); + } + + tier4_autoware_utils::StopWatch stop_watch; + remap_and_publish(common::decompress_to_cv_mat(msg), msg.header); + RCLCPP_INFO_STREAM(get_logger(), "image undistort: " << stop_watch.toc() << "[ms]"); + } + + void on_compressed_image(const CompressedImage & msg) + { + if (!info_.has_value()) { + return; + } + if (undistort_map_x.empty()) { + make_remap_lut(); + } + + tier4_autoware_utils::StopWatch stop_watch; + remap_and_publish(common::decompress_to_cv_mat(msg), msg.header); RCLCPP_INFO_STREAM(get_logger(), "image undistort: " << stop_watch.toc() << "[ms]"); } diff --git a/localization/yabloc/yabloc_particle_filter/package.xml b/localization/yabloc/yabloc_particle_filter/package.xml index 6d11b12d9c922..aabb6009c2148 100644 --- a/localization/yabloc/yabloc_particle_filter/package.xml +++ b/localization/yabloc/yabloc_particle_filter/package.xml @@ -13,7 +13,6 @@ rosidl_default_generators geometry_msgs - libgoogle-glog-dev rclcpp sensor_msgs sophus @@ -22,7 +21,6 @@ tf2 tf2_geometry_msgs tf2_ros - tf2_sensor_msgs tier4_autoware_utils visualization_msgs yabloc_common diff --git a/localization/yabloc/yabloc_pose_initializer/package.xml b/localization/yabloc/yabloc_pose_initializer/package.xml index f6e8aa83bafc7..4e3ca1c1e61d1 100644 --- a/localization/yabloc/yabloc_pose_initializer/package.xml +++ b/localization/yabloc/yabloc_pose_initializer/package.xml @@ -15,11 +15,7 @@ geometry_msgs lanelet2_extension rclcpp - rclpy sensor_msgs - std_msgs - std_srvs - tf2 tier4_localization_msgs visualization_msgs yabloc_common diff --git a/map/map_projection_loader/README.md b/map/map_projection_loader/README.md index 0b0c41821d036..94b142da3dcf9 100644 --- a/map/map_projection_loader/README.md +++ b/map/map_projection_loader/README.md @@ -25,7 +25,7 @@ sample-map-rosbag ```yaml # map_projector_info.yaml -type: "local" +projector_type: local ``` ### Using MGRS diff --git a/perception/crosswalk_traffic_light_estimator/src/node.cpp b/perception/crosswalk_traffic_light_estimator/src/node.cpp index e45e3494dadce..870b8bc7c13f5 100644 --- a/perception/crosswalk_traffic_light_estimator/src/node.cpp +++ b/perception/crosswalk_traffic_light_estimator/src/node.cpp @@ -231,6 +231,7 @@ void CrosswalkTrafficLightEstimatorNode::setCrosswalkTrafficSignal( TrafficSignal output_traffic_signal; TrafficSignalElement output_traffic_signal_element; output_traffic_signal_element.color = color; + output_traffic_signal_element.shape = TrafficSignalElement::CIRCLE; output_traffic_signal_element.confidence = 1.0; output_traffic_signal.elements.push_back(output_traffic_signal_element); output_traffic_signal.traffic_signal_id = tl_reg_elem->id(); diff --git a/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml b/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml new file mode 100644 index 0000000000000..745a0fd6591a9 --- /dev/null +++ b/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml @@ -0,0 +1,13 @@ +/**: + ros__parameters: + min_points_num: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [10, 10, 10, 10, 10, 10, 10, 10] + + max_points_num: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [10, 10, 10, 10, 10, 10, 10, 10] + + min_points_and_distance_ratio: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [800.0, 800.0, 800.0, 800.0, 800.0, 800.0, 800.0, 800.0] diff --git a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp b/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp index 270e5c7bdb7ff..5819302664907 100644 --- a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp +++ b/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp @@ -32,14 +32,14 @@ #include #include - +#include namespace obstacle_pointcloud_based_validator { struct PointsNumThresholdParam { - size_t min_points_num; - size_t max_points_num; - float min_points_and_distance_ratio; + std::vector min_points_num; + std::vector max_points_num; + std::vector min_points_and_distance_ratio; }; class ObstaclePointCloudBasedValidator : public rclcpp::Node { diff --git a/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml b/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml index 8196e95f37957..799b605658345 100644 --- a/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml +++ b/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml @@ -3,11 +3,13 @@ + + 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 00e53e9de9a9c..47640c9332e4d 100644 --- a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp @@ -92,10 +92,12 @@ ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator( objects_pub_ = create_publisher( "~/output/objects", rclcpp::QoS{1}); - points_num_threshold_param_.min_points_num = declare_parameter("min_points_num", 10); - points_num_threshold_param_.max_points_num = declare_parameter("max_points_num", 10); + points_num_threshold_param_.min_points_num = + declare_parameter>("min_points_num"); + points_num_threshold_param_.max_points_num = + declare_parameter>("max_points_num"); points_num_threshold_param_.min_points_and_distance_ratio = - declare_parameter("min_points_and_distance_ratio", 800.0); + declare_parameter>("min_points_and_distance_ratio"); const bool enable_debugger = declare_parameter("enable_debugger", false); if (enable_debugger) debugger_ = std::make_shared(this); @@ -134,6 +136,7 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( for (size_t i = 0; i < transformed_objects.objects.size(); ++i) { const auto & transformed_object = transformed_objects.objects.at(i); + const auto object_label_id = transformed_object.classification.front().label; const auto & object = input_objects->objects.at(i); const auto & transformed_object_position = transformed_object.kinematics.pose_with_covariance.pose.position; @@ -155,13 +158,17 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( if (debugger_) debugger_->addNeighborPointcloud(neighbor_pointcloud); // Filter object that have few pointcloud in them. + // TODO(badai-nguyen) add 3d validator option const auto num = getPointCloudNumWithinPolygon(transformed_object, neighbor_pointcloud); const auto object_distance = std::hypot(transformed_object_position.x, transformed_object_position.y); size_t min_pointcloud_num = std::clamp( static_cast( - points_num_threshold_param_.min_points_and_distance_ratio / object_distance + 0.5f), - points_num_threshold_param_.min_points_num, points_num_threshold_param_.max_points_num); + points_num_threshold_param_.min_points_and_distance_ratio.at(object_label_id) / + object_distance + + 0.5f), + static_cast(points_num_threshold_param_.min_points_num.at(object_label_id)), + static_cast(points_num_threshold_param_.max_points_num.at(object_label_id))); if (num) { (min_pointcloud_num <= num.value()) ? output.objects.push_back(object) : removed_objects.objects.push_back(object); diff --git a/perception/heatmap_visualizer/CMakeLists.txt b/perception/heatmap_visualizer/CMakeLists.txt index ed995491f2af6..75fa02a1cdcf7 100644 --- a/perception/heatmap_visualizer/CMakeLists.txt +++ b/perception/heatmap_visualizer/CMakeLists.txt @@ -16,4 +16,5 @@ rclcpp_components_register_node(heatmap_visualizer_component ament_auto_package(INSTALL_TO_SHARE launch + config ) diff --git a/perception/heatmap_visualizer/README.md b/perception/heatmap_visualizer/README.md index dd8b6f2a7f2b2..747a158b3d487 100644 --- a/perception/heatmap_visualizer/README.md +++ b/perception/heatmap_visualizer/README.md @@ -44,14 +44,15 @@ When publishing, firstly these values are normalized to [0, 1] using maximum and ### Core Parameters -| Name | Type | Default Value | Description | -| ----------------------------- | ------ | ------------- | ------------------------------------------------- | -| `frame_count` | int | `50` | The number of frames to publish heatmap | -| `map_frame` | string | `base_link` | the frame of heatmap to be respected | -| `map_length` | float | `200.0` | the length of map in meter | -| `map_resolution` | float | `0.8` | the resolution of map | -| `use_confidence` | bool | `false` | the flag if use confidence score as heatmap value | -| `rename_car_to_truck_and_bus` | bool | `true` | the flag if rename car to truck or bus | +| Name | Type | Default Value | Description | +| --------------------- | ------------- | ------------------------------------------------------------------------------------- | ----------------------------------------------- | +| `publish_frame_count` | int | `50` | The number of frames to publish heatmap | +| `heatmap_frame_id` | string | `base_link` | The frame ID of heatmap to be respected | +| `heatmap_length` | float | `200.0` | A length of map in meter | +| `heatmap_resolution` | float | `0.8` | A resolution of map | +| `use_confidence` | bool | `false` | A flag if use confidence score as heatmap value | +| `class_names` | array | `["UNKNOWN", "CAR", "TRUCK", "BUS", "TRAILER", "BICYCLE", "MOTORBIKE", "PEDESTRIAN"]` | An array of class names to be published | +| `rename_to_car` | bool | `true` | A flag if rename car like vehicle to car | ## Assumptions / Known limits diff --git a/perception/heatmap_visualizer/config/heatmap_visualizer.param.yaml b/perception/heatmap_visualizer/config/heatmap_visualizer.param.yaml new file mode 100644 index 0000000000000..2da863ea5e37a --- /dev/null +++ b/perception/heatmap_visualizer/config/heatmap_visualizer.param.yaml @@ -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. + +/**: + ros__parameters: + publish_frame_count: 50 + heatmap_frame_id: "base_link" + heatmap_length: 200.0 + heatmap_resolution: 0.8 + use_confidence: false + class_names: + [ + "UNKNOWN", + "CAR", + "TRUCK", + "BUS", + "TRAILER", + "BICYCLE", + "MOTORBIKE", + "PEDESTRIAN", + ] + rename_to_car: false diff --git a/perception/heatmap_visualizer/launch/heatmap_visualizer.launch.xml b/perception/heatmap_visualizer/launch/heatmap_visualizer.launch.xml index e87ae4e63a4d1..7251d776b6110 100644 --- a/perception/heatmap_visualizer/launch/heatmap_visualizer.launch.xml +++ b/perception/heatmap_visualizer/launch/heatmap_visualizer.launch.xml @@ -2,20 +2,10 @@ - - - - - - + - - - - - - + diff --git a/perception/heatmap_visualizer/schema/heatmap_visualizer.schema.json b/perception/heatmap_visualizer/schema/heatmap_visualizer.schema.json new file mode 100644 index 0000000000000..cb67ae383c35a --- /dev/null +++ b/perception/heatmap_visualizer/schema/heatmap_visualizer.schema.json @@ -0,0 +1,81 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Heatmap Visualizer Node", + "type": "object", + "definitions": { + "heatmap_visualizer": { + "type": "object", + "properties": { + "publish_frame_count": { + "type": "integer", + "description": "The number of frames to be published at once.", + "default": 50, + "minimum": 1 + }, + "heatmap_frame_id": { + "type": "string", + "description": "A frame ID in which visualized heatmap is with respect to.", + "default": "base_link" + }, + "heatmap_length": { + "type": "number", + "description": "A size length of heatmap [m].", + "default": 200.0, + "exclusiveMinimum": 0.0 + }, + "heatmap_resolution": { + "type": "number", + "description": "A cell resolution of heatmap [m].", + "default": 0.8, + "exclusiveMinimum": 0.0 + }, + "use_confidence": { + "type": "boolean", + "description": "Indicates whether to use object existence probability as score. It this parameter is false, 1.0 is set to score.", + "default": false + }, + "class_names": { + "type": "array", + "description": "An array of object class names.", + "default": [ + "UNKNOWN", + "CAR", + "TRUCK", + "BUS", + "TRAILER", + "BICYCLE", + "MOTORBIKE", + "PEDESTRIAN" + ], + "uniqueItems": true + }, + "rename_to_car": { + "type": "boolean", + "description": "Indicates whether to rename car like vehicle into car.", + "default": false + } + }, + "required": [ + "publish_frame_count", + "heatmap_frame_id", + "heatmap_length", + "heatmap_resolution", + "use_confidence", + "class_names", + "rename_to_car" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/heatmap_visualizer" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/heatmap_visualizer/src/heatmap_visualizer_node.cpp b/perception/heatmap_visualizer/src/heatmap_visualizer_node.cpp index ad57d8638065f..d9337e0c9f61d 100644 --- a/perception/heatmap_visualizer/src/heatmap_visualizer_node.cpp +++ b/perception/heatmap_visualizer/src/heatmap_visualizer_node.cpp @@ -27,13 +27,13 @@ namespace heatmap_visualizer HeatmapVisualizerNode::HeatmapVisualizerNode(const rclcpp::NodeOptions & node_options) : Node("heatmap_visualizer", node_options), frame_count_(0) { - total_frame_ = declare_parameter("frame_count", 50); - map_frame_ = declare_parameter("map_frame", "base_link"); - map_length_ = declare_parameter("map_length", 200.0); - map_resolution_ = declare_parameter("map_resolution", 0.8); - use_confidence_ = declare_parameter("use_confidence", false); - class_names_ = declare_parameter("class_names", class_names_); - rename_car_to_truck_and_bus_ = declare_parameter("rename_car_to_truck_and_bus", false); + total_frame_ = static_cast(declare_parameter("publish_frame_count")); + map_frame_ = declare_parameter("heatmap_frame_id"); + map_length_ = static_cast(declare_parameter("heatmap_length")); + map_resolution_ = static_cast(declare_parameter("heatmap_resolution")); + use_confidence_ = declare_parameter("use_confidence"); + class_names_ = declare_parameter>("class_names"); + rename_car_to_truck_and_bus_ = declare_parameter("rename_to_car"); width_ = static_cast(map_length_ / map_resolution_); height_ = static_cast(map_length_ / map_resolution_); diff --git a/perception/image_projection_based_fusion/docs/roi-cluster-fusion.md b/perception/image_projection_based_fusion/docs/roi-cluster-fusion.md index 01cdc3756866b..03eaab2a3c6ca 100644 --- a/perception/image_projection_based_fusion/docs/roi-cluster-fusion.md +++ b/perception/image_projection_based_fusion/docs/roi-cluster-fusion.md @@ -32,18 +32,20 @@ The clusters are projected onto image planes, and then if the ROIs of clusters a ### Core Parameters -| Name | Type | Description | -| --------------------------- | ----- | -------------------------------------------------------------------------------- | -| `use_iou_x` | bool | calculate IoU only along x-axis | -| `use_iou_y` | bool | calculate IoU only along y-axis | -| `use_iou` | bool | calculate IoU both along x-axis and y-axis | -| `use_cluster_semantic_type` | bool | if `false`, the labels of clusters are overwritten by `UNKNOWN` before fusion | -| `only_allow_inside_cluster` | bool | if `true`, the only clusters contained inside RoIs by a detector | -| `roi_scale_factor` | float | the scale factor for offset of detector RoIs if `only_allow_inside_cluster=true` | -| `iou_threshold` | float | the IoU threshold to overwrite a label of clusters with a label of roi | -| `unknown_iou_threshold` | float | the IoU threshold to fuse cluster with unknown label of roi | -| `rois_number` | int | the number of input rois | -| `debug_mode` | bool | If `true`, subscribe and publish images for visualization. | +| Name | Type | Description | +| --------------------------- | ------ | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `fusion_distance` | double | If the detected object's distance to frame_id is less than the threshold, the fusion will be processed | +| `trust_object_distance` | double | if the detected object's distance is less than the `trust_object_distance`, `trust_object_iou_mode` will be used, otherwise `non_trust_object_iou_mode` will be used | +| `trust_object_iou_mode` | string | select mode from 3 options {`iou`, `iou_x`, `iou_y`} to calculate IoU in range of [`0`, `trust_distance`].
 `iou`: IoU along x-axis and y-axis
 `iou_x`: IoU along x-axis
 `iou_y`: IoU along y-axis | +| `non_trust_object_iou_mode` | string | the IOU mode using in range of [`trust_distance`, `fusion_distance`] if `trust_distance` < `fusion_distance` | +| `use_cluster_semantic_type` | bool | if `false`, the labels of clusters are overwritten by `UNKNOWN` before fusion | +| `only_allow_inside_cluster` | bool | if `true`, the only clusters contained inside RoIs by a detector | +| `roi_scale_factor` | double | the scale factor for offset of detector RoIs if `only_allow_inside_cluster=true` | +| `iou_threshold` | double | the IoU threshold to overwrite a label of clusters with a label of roi | +| `unknown_iou_threshold` | double | the IoU threshold to fuse cluster with unknown label of roi | +| `remove_unknown` | bool | if `true`, remove all `UNKNOWN` labeled objects from output | +| `rois_number` | int | the number of input rois | +| `debug_mode` | bool | If `true`, subscribe and publish images for visualization. | ## Assumptions / Known limits 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 70a7866e79b87..e54710ad477da 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 @@ -17,10 +17,12 @@ #include "image_projection_based_fusion/fusion_node.hpp" +#include #include - +#include namespace image_projection_based_fusion { +const std::map IOU_MODE_MAP{{"iou", 0}, {"iou_x", 1}, {"iou_y", 2}}; class RoiClusterFusionNode : public FusionNode @@ -38,9 +40,7 @@ class RoiClusterFusionNode const sensor_msgs::msg::CameraInfo & camera_info, DetectedObjectsWithFeature & output_cluster_msg) override; - bool use_iou_x_{false}; - bool use_iou_y_{false}; - bool use_iou_{false}; + std::string trust_object_iou_mode_{"iou"}; bool use_cluster_semantic_type_{false}; bool only_allow_inside_cluster_{false}; double roi_scale_factor_{1.1}; @@ -49,10 +49,14 @@ class RoiClusterFusionNode const float min_roi_existence_prob_ = 0.1; // keep small value to lessen affect on merger object stage bool remove_unknown_; - double trust_distance_; - - bool filter_by_distance(const DetectedObjectWithFeature & obj); + double fusion_distance_; + double trust_object_distance_; + std::string non_trust_object_iou_mode_{"iou_x"}; + bool is_far_enough(const DetectedObjectWithFeature & obj, const double distance_threshold); bool out_of_scope(const DetectedObjectWithFeature & obj); + double cal_iou_by_mode( + const sensor_msgs::msg::RegionOfInterest & roi_1, + const sensor_msgs::msg::RegionOfInterest & roi_2, const std::string iou_mode); // bool CheckUnknown(const DetectedObjectsWithFeature & obj); }; 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 b2b9bae80f071..abdd44a70d24a 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 @@ -1,5 +1,5 @@ - + @@ -20,7 +20,8 @@ - + + @@ -46,9 +47,8 @@ - - - + + @@ -56,7 +56,8 @@ - + + 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 316d29dae1301..4a46c8aace696 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"); - use_iou_y_ = declare_parameter("use_iou_y"); - use_iou_ = declare_parameter("use_iou"); + trust_object_iou_mode_ = declare_parameter("trust_object_iou_mode"); + non_trust_object_iou_mode_ = declare_parameter("non_trust_object_iou_mode"); 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"); + fusion_distance_ = declare_parameter("fusion_distance"); + trust_object_distance_ = declare_parameter("trust_object_distance"); } void RoiClusterFusionNode::preprocess(DetectedObjectsWithFeature & output_cluster_msg) @@ -111,7 +111,7 @@ void RoiClusterFusionNode::fuseOnSingleImage( continue; } - if (filter_by_distance(input_cluster_msg.feature_objects.at(i))) { + if (is_far_enough(input_cluster_msg.feature_objects.at(i), fusion_distance_)) { continue; } @@ -175,25 +175,23 @@ void RoiClusterFusionNode::fuseOnSingleImage( 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); - } - // 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_) { - iou_y = calcIoUY(cluster_map.second, feature_obj.feature.roi); + double iou(0.0); + bool is_use_non_trust_object_iou_mode = is_far_enough( + input_cluster_msg.feature_objects.at(cluster_map.first), trust_object_distance_); + if (is_use_non_trust_object_iou_mode || is_roi_label_known) { + iou = + cal_iou_by_mode(cluster_map.second, feature_obj.feature.roi, non_trust_object_iou_mode_); + } else { + iou = cal_iou_by_mode(cluster_map.second, feature_obj.feature.roi, trust_object_iou_mode_); } + const bool passed_inside_cluster_gate = only_allow_inside_cluster_ ? is_inside(feature_obj.feature.roi, cluster_map.second, roi_scale_factor_) : true; - if (max_iou < iou + iou_x + iou_y && passed_inside_cluster_gate) { + if (max_iou < iou && passed_inside_cluster_gate) { index = cluster_map.first; - max_iou = iou + iou_x + iou_y; + max_iou = iou; associated = true; } } @@ -274,11 +272,30 @@ bool RoiClusterFusionNode::out_of_scope(const DetectedObjectWithFeature & obj) return is_out; } -bool RoiClusterFusionNode::filter_by_distance(const DetectedObjectWithFeature & obj) +bool RoiClusterFusionNode::is_far_enough( + const DetectedObjectWithFeature & obj, const double distance_threshold) { const auto & position = obj.object.kinematics.pose_with_covariance.pose.position; - const auto square_distance = position.x * position.x + position.y + position.y; - return square_distance > trust_distance_ * trust_distance_; + return position.x * position.x + position.y * position.y > + distance_threshold * distance_threshold; +} + +double RoiClusterFusionNode::cal_iou_by_mode( + const sensor_msgs::msg::RegionOfInterest & roi_1, + const sensor_msgs::msg::RegionOfInterest & roi_2, const std::string iou_mode) +{ + switch (IOU_MODE_MAP.at(iou_mode)) { + case 0 /* use iou mode */: + return calcIoU(roi_1, roi_2); + + case 1 /* use iou_x mode */: + return calcIoUX(roi_1, roi_2); + + case 2 /* use iou_y mode */: + return calcIoUY(roi_1, roi_2); + default: + return 0.0; + } } } // namespace image_projection_based_fusion diff --git a/perception/lidar_apollo_instance_segmentation/launch/lidar_apollo_instance_segmentation.launch.xml b/perception/lidar_apollo_instance_segmentation/launch/lidar_apollo_instance_segmentation.launch.xml index bceb928dd6692..7f5024929b531 100644 --- a/perception/lidar_apollo_instance_segmentation/launch/lidar_apollo_instance_segmentation.launch.xml +++ b/perception/lidar_apollo_instance_segmentation/launch/lidar_apollo_instance_segmentation.launch.xml @@ -7,7 +7,8 @@ - + + diff --git a/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp b/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp index 905d70235e33e..e6c95f202106d 100644 --- a/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp +++ b/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp @@ -22,6 +22,7 @@ #include #include +// cspell: ignore bcnn using model_zoo::perception::lidar_obstacle_detection::baidu_cnn::onnx_bcnn::config; namespace autoware 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 8ef6d628aa2f6..3d01ab96e9b62 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 @@ -176,8 +176,7 @@ class MapBasedPredictionNode : public rclcpp::Node LaneletsData getCurrentLanelets(const TrackedObject & object); bool checkCloseLaneletCondition( - const std::pair & lanelet, const TrackedObject & object, - const bool check_distance = true); + const std::pair & lanelet, const TrackedObject & object); float calculateLocalLikelihood( const lanelet::Lanelet & current_lanelet, const TrackedObject & object) const; void updateObjectData(TrackedObject & object); 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 6a414c26b8806..58356f4f96ed6 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -1251,7 +1251,7 @@ LaneletsData MapBasedPredictionNode::getCurrentLanelets(const TrackedObject & ob for (const auto & lanelet : surrounding_opposite_lanelets) { // Check if the close lanelets meet the necessary condition for start lanelets // except for distance checking - if (!checkCloseLaneletCondition(lanelet, object, false)) { + if (!checkCloseLaneletCondition(lanelet, object)) { continue; } @@ -1271,8 +1271,7 @@ LaneletsData MapBasedPredictionNode::getCurrentLanelets(const TrackedObject & ob } bool MapBasedPredictionNode::checkCloseLaneletCondition( - const std::pair & lanelet, const TrackedObject & object, - const bool check_distance) + const std::pair & lanelet, const TrackedObject & object) { // Step1. If we only have one point in the centerline, we will ignore the lanelet if (lanelet.second.centerline().size() <= 1) { @@ -1307,7 +1306,7 @@ bool MapBasedPredictionNode::checkCloseLaneletCondition( const double object_vel = object.kinematics.twist_with_covariance.twist.linear.x; const bool is_yaw_reversed = M_PI - delta_yaw_threshold_for_searching_lanelet_ < abs_norm_delta && object_vel < 0.0; - if (check_distance && dist_threshold_for_searching_lanelet_ < lanelet.first) { + if (dist_threshold_for_searching_lanelet_ < lanelet.first) { return false; } if (!is_yaw_reversed && delta_yaw_threshold_for_searching_lanelet_ < abs_norm_delta) { diff --git a/perception/multi_object_tracker/models.md b/perception/multi_object_tracker/models.md index c83038ffa7b59..f74967943be32 100644 --- a/perception/multi_object_tracker/models.md +++ b/perception/multi_object_tracker/models.md @@ -2,7 +2,7 @@ ## Tracking model - + ### CTRV model [1] diff --git a/perception/traffic_light_classifier/README.md b/perception/traffic_light_classifier/README.md index 7df0c5466695b..2aecf66f8fb7b 100644 --- a/perception/traffic_light_classifier/README.md +++ b/perception/traffic_light_classifier/README.md @@ -148,6 +148,8 @@ DATASET_ROOT #### Prerequisites + + **Step 1.** Download and install Miniconda from the [official website](https://mmpretrain.readthedocs.io/en/latest/get_started.html). **Step 2.** Create a conda virtual environment and activate it @@ -295,7 +297,7 @@ python tools/train.py configs/mobilenet_v2/mobilenet-v2_8xb32_custom.py Training logs and weights will be saved in the `work_dirs/mobilenet-v2_8xb32_custom` folder. -### Convert PyTorh model to ONNX model +### Convert PyTorch model to ONNX model #### Install mmdeploy diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index 07d949aae8a39..0c0c7c806c119 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -173,12 +173,6 @@ struct ShiftLine Click [here](./docs/behavior_path_planner_path_generation_design.md) for details. -### Smooth goal connection - -If the target path contains a goal, modify the points of the path so that the path and the goal are connected smoothly. This process will change the shape of the path by the distance of `refine_goal_search_radius_range` from the goal. Note that this logic depends on the interpolation algorithm that will be executed in a later module (at the moment it uses spline interpolation), so it needs to be updated in the future. - -![path_goal_refinement](./image/path_goal_refinement.drawio.svg) - ## References / External links This module depends on the external [BehaviorTreeCpp](https://github.com/BehaviorTree/BehaviorTree.CPP) library. diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 589233cf5a441..df2dd806fe2c7 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -139,12 +139,16 @@ check_other_object: true # [-] # collision check parameters check_all_predicted_path: false # [-] - time_resolution: 0.5 # [s] - time_horizon_for_front_object: 10.0 # [s] - time_horizon_for_rear_object: 10.0 # [s] safety_check_backward_distance: 100.0 # [m] hysteresis_factor_expand_rate: 2.0 # [-] hysteresis_factor_safe_count: 10 # [-] + # predicted path parameters + min_velocity: 1.38 # [m/s] + max_velocity: 50.0 # [m/s] + time_resolution: 0.5 # [s] + time_horizon_for_front_object: 10.0 # [s] + time_horizon_for_rear_object: 10.0 # [s] + delay_until_departure: 0.0 # [s] # rss parameters expected_front_deceleration: -1.0 # [m/ss] expected_rear_deceleration: -1.0 # [m/ss] diff --git a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml index fe30397683494..127b70fbf7bcb 100644 --- a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml +++ b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml @@ -16,6 +16,7 @@ motorcycle: true pedestrian: false + max_obstacle_vel: 100.0 # [m/s] min_obstacle_vel: 0.0 # [m/s] successive_num_to_entry_dynamic_avoidance_condition: 5 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 babbdc60d1cf9..5ffac49847ce6 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 @@ -12,7 +12,7 @@ goal_priority: "minimum_weighted_distance" # "minimum_weighted_distance" or "minimum_longitudinal_distance" minimum_weighted_distance: lateral_weight: 40.0 - path_priority: "efficient_path" # "efficient_path" or "close_goal" + prioritize_goals_before_objects: true parking_policy: "left_side" # "left_side" or "right_side" forward_goal_search_length: 20.0 backward_goal_search_length: 20.0 @@ -47,6 +47,8 @@ decide_path_distance: 10.0 maximum_deceleration: 1.0 maximum_jerk: 1.0 + path_priority: "efficient_path" # "efficient_path" or "close_goal" + efficient_path_order: ["SHIFT", "ARC_FORWARD", "ARC_BACKWARD"] # only lane based pull over(exclude freespace parking) # shift parking shift_parking: @@ -130,7 +132,7 @@ # detection range object_check_forward_distance: 10.0 object_check_backward_distance: 100.0 - ignore_object_velocity_threshold: 0.0 + ignore_object_velocity_threshold: 1.0 # ObjectTypesToCheck object_types_to_check: check_car: true @@ -162,11 +164,11 @@ check_all_predicted_path: true publish_debug_marker: false rss_params: - rear_vehicle_reaction_time: 1.0 + rear_vehicle_reaction_time: 2.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 + lateral_distance_max_threshold: 2.0 + longitudinal_distance_min_threshold: 3.0 + longitudinal_velocity_delta_time: 0.8 # hysteresis factor to expand/shrink polygon with the value hysteresis_factor_expand_rate: 1.0 # temporary diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index 583bdeaaf8dc5..e7f3b51bd2d26 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -28,15 +28,28 @@ # safety check safety_check: - expected_front_deceleration: -1.0 - expected_rear_deceleration: -1.0 - expected_front_deceleration_for_abort: -1.0 - expected_rear_deceleration_for_abort: -2.0 - rear_vehicle_reaction_time: 2.0 - rear_vehicle_safety_time_margin: 1.0 - lateral_distance_max_threshold: 2.0 - longitudinal_distance_min_threshold: 3.0 - longitudinal_velocity_delta_time: 0.8 + allow_loose_check_for_cancel: true + execution: + expected_front_deceleration: -1.0 + expected_rear_deceleration: -1.0 + rear_vehicle_reaction_time: 2.0 + rear_vehicle_safety_time_margin: 1.0 + lateral_distance_max_threshold: 2.0 + longitudinal_distance_min_threshold: 3.0 + longitudinal_velocity_delta_time: 0.8 + cancel: + expected_front_deceleration: -1.0 + expected_rear_deceleration: -2.0 + rear_vehicle_reaction_time: 1.5 + rear_vehicle_safety_time_margin: 0.8 + lateral_distance_max_threshold: 1.0 + longitudinal_distance_min_threshold: 2.5 + longitudinal_velocity_delta_time: 0.6 + + # lane expansion for object filtering + lane_expansion: + left_offset: 0.0 # [m] + right_offset: 0.0 # [m] # lateral acceleration map lateral_acceleration: 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 9895d9b5e473f..5797704e8a0ca 100644 --- a/planning/behavior_path_planner/config/scene_module_manager.param.yaml +++ b/planning/behavior_path_planner/config/scene_module_manager.param.yaml @@ -5,7 +5,7 @@ ros__parameters: external_request_lane_change_left: enable_module: false - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: true keep_last: false @@ -14,7 +14,7 @@ external_request_lane_change_right: enable_module: false - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: true keep_last: false @@ -23,7 +23,7 @@ lane_change_left: enable_module: true - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: true keep_last: false @@ -32,7 +32,7 @@ lane_change_right: enable_module: true - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: true keep_last: false @@ -41,7 +41,7 @@ start_planner: enable_module: true - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false keep_last: false @@ -50,7 +50,7 @@ side_shift: enable_module: true - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false keep_last: false @@ -59,7 +59,7 @@ goal_planner: enable_module: true - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false keep_last: true @@ -68,7 +68,7 @@ avoidance: enable_module: true - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: false keep_last: false @@ -77,7 +77,7 @@ avoidance_by_lc: enable_module: false - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false keep_last: false @@ -86,7 +86,7 @@ dynamic_avoidance: enable_module: false - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: true keep_last: false 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 6760ec787bb03..4a42da9bc5ac3 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 @@ -95,7 +95,7 @@ # detection range object_check_forward_distance: 10.0 object_check_backward_distance: 100.0 - ignore_object_velocity_threshold: 0.0 + ignore_object_velocity_threshold: 1.0 # ObjectTypesToCheck object_types_to_check: check_car: true @@ -127,11 +127,11 @@ check_all_predicted_path: true publish_debug_marker: false rss_params: - rear_vehicle_reaction_time: 1.0 + rear_vehicle_reaction_time: 2.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 + lateral_distance_max_threshold: 2.0 + longitudinal_distance_min_threshold: 3.0 + longitudinal_velocity_delta_time: 0.8 # hysteresis factor to expand/shrink polygon hysteresis_factor_expand_rate: 1.0 # temporary 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 ee337555ceb58..888d5d9ff9826 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 @@ -89,6 +89,10 @@ Either one is activated when all conditions are met. - Route is set with `allow_goal_modification=false` by default. - ego-vehicle is in the same lane as the goal. +If the target path contains a goal, modify the points of the path so that the path and the goal are connected smoothly. This process will change the shape of the path by the distance of `refine_goal_search_radius_range` from the goal. Note that this logic depends on the interpolation algorithm that will be executed in a later module (at the moment it uses spline interpolation), so it needs to be updated in the future. + +![path_goal_refinement](../image/path_goal_refinement.drawio.svg) + ### rough_goal_planner @@ -164,7 +168,7 @@ searched for in certain range of the shoulder lane. | Name | Unit | Type | Description | Default value | | :------------------------------ | :--- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | :-------------------------- | | goal_priority | [-] | string | In case `minimum_weighted_distance`, sort with smaller longitudinal distances taking precedence over smaller lateral distances. In case `minimum_longitudinal_distance`, sort with weighted lateral distance against longitudinal distance. | `minimum_weighted_distance` | -| path_priority | [-] | string | In case `efficient_path` use a goal that can generate an efficient path( priority is `shift_parking` -> `arc_forward_parking` -> `arc_backward_parking`). In case `close_goal` use the closest goal to the original one. | efficient_path | +| prioritize_goals_before_objects | [-] | bool | If there are objects that may need to be avoided, prioritize the goal in front of them | true | | forward_goal_search_length | [m] | double | length of forward range to be explored from the original goal | 20.0 | | backward_goal_search_length | [m] | double | length of backward range to be explored from the original goal | 20.0 | | goal_search_interval | [m] | double | distance interval for goal search | 2.0 | @@ -180,13 +184,15 @@ searched for in certain range of the shoulder lane. There are three path generation methods. 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 | +| 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 | +| path_priority | [-] | string | In case `efficient_path` use a goal that can generate an efficient path which is set in `efficient_path_order`. In case `close_goal` use the closest goal to the original one. | efficient_path | +| efficient_path_order | [-] | string | efficient order of pull over planner along lanes　excluding freespace pull over | ["SHIFT", "ARC_FORWARD", "ARC_BACKWARD"] | ### **shift parking** @@ -264,7 +270,8 @@ If the vehicle gets stuck with `lane_parking`, run `freespace_parking`. To run this feature, you need to set `parking_lot` to the map, `activate_by_scenario` of [costmap_generator](../../costmap_generator/README.md) to `false` and `enable_freespace_parking` to `true` ![pull_over_freespace_parking_flowchart](../image/pull_over_freespace_parking_flowchart.drawio.svg) -\*Series execution with `avoidance_module` in the flowchart is under development. + +Simultaneous execution with `avoidance_module` in the flowchart is under development. diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md index 8322952a0be76..95a9697a1d471 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md @@ -8,11 +8,11 @@ This module is activated when a new route is received. Use cases are as follows - start smoothly from the current ego position to centerline. - - ![case1](../image/start_from_road_lane.drawio.svg) + ![case1](../image/start_from_road_lane.drawio.svg) - pull out from the side of the road lane to centerline. - - ![case2](../image/start_from_road_side.drawio.svg) + ![case2](../image/start_from_road_side.drawio.svg) - pull out from the shoulder lane to the road lane centerline. - - ![case3](../image/start_from_road_shoulder.drawio.svg) + ![case3](../image/start_from_road_shoulder.drawio.svg) ## Design @@ -155,3 +155,27 @@ If a safe path cannot be generated from the current position, search backwards f | backward_search_resolution | [m] | double | distance interval for searching backward pull out start point | 2.0 | | backward_path_update_duration | [s] | double | time interval for searching backward pull out start point. this prevents chattering between back driving and pull_out | 3.0 | | ignore_distance_from_lane_end | [m] | double | distance from end of pull out lanes for ignoring start candidates | 15.0 | + +### **freespace pull out** + +If the vehicle gets stuck with pull out along lanes, execute freespace pull out. +To run this feature, you need to set `parking_lot` to the map, `activate_by_scenario` of [costmap_generator](../../costmap_generator/README.md) to `false` and `enable_freespace_planner` to `true` + + + +#### Unimplemented parts / limitations for freespace pull out + +- When a short path is generated, the ego does can not drive with it. +- Complex cases take longer to generate or fail. +- The drivable area is not guaranteed to fit in the parking_lot. + +#### Parameters freespace parking + +| Name | Unit | Type | Description | Default value | +| :----------------------------- | :--- | :----- | :--------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| enable_freespace_planner | [-] | bool | this flag activates a free space pullout that is executed when a vehicle is stuck due to obstacles in the lanes where the ego is located | true | +| end_pose_search_start_distance | [m] | double | distance from ego to the start point of the search for the end point in the freespace_pull_out driving lane | 20.0 | +| end_pose_search_end_distance | [m] | double | distance from ego to the end point of the search for the end point in the freespace_pull_out driving lane | 30.0 | +| end_pose_search_interval | [m] | bool | interval to search for the end point in the freespace_pull_out driving lane | 2.0 | + +See [freespace_planner](../../freespace_planner/README.md) for other parameters. diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 0f51144c0b792..e8ad6c7e2f987 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -25,6 +25,7 @@ #include "behavior_path_planner/scene_module/side_shift/manager.hpp" #include "behavior_path_planner/scene_module/start_planner/manager.hpp" #include "behavior_path_planner/steering_factor_interface.hpp" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "tier4_planning_msgs/msg/detail/lane_change_debug_msg_array__struct.hpp" #include @@ -221,6 +222,8 @@ class BehaviorPathPlannerNode : public rclcpp::Node Path convertToPath( const std::shared_ptr & path_candidate_ptr, const bool is_ready, const std::shared_ptr & planner_data); + + std::unique_ptr logger_configure_; }; } // namespace behavior_path_planner 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 ee5771c98217e..72c32039da627 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 @@ -242,6 +242,11 @@ class PlannerManager */ void print() const; + /** + * @brief publish processing time of each module. + */ + void publishProcessingTime() const; + /** * @brief visit each module and get debug information. */ 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 b00818f2f79d1..ab8e3cfd70dff 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 @@ -56,6 +56,7 @@ struct DynamicAvoidanceParameters bool avoid_bicycle{false}; bool avoid_motorcycle{false}; bool avoid_pedestrian{false}; + double max_obstacle_vel{0.0}; double min_obstacle_vel{0.0}; int successive_num_to_entry_dynamic_avoidance_condition{0}; int successive_num_to_exit_dynamic_avoidance_condition{0}; 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 f6b2fa8666f2e..a82cff16756f8 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 @@ -106,6 +106,7 @@ struct FreespacePlannerDebugData struct GoalPlannerDebugData { FreespacePlannerDebugData freespace_planner{}; + std::vector ego_polygons_expanded{}; }; class GoalPlannerModule : public SceneModuleInterface @@ -267,6 +268,9 @@ class GoalPlannerModule : public SceneModuleInterface BehaviorModuleOutput planWithGoalModification(); BehaviorModuleOutput planWaitingApprovalWithGoalModification(); void selectSafePullOverPath(); + void sortPullOverPathCandidatesByGoalPriority( + std::vector & pull_over_path_candidates, + const GoalCandidates & goal_candidates) const; // deal with pull over partial paths PathWithLaneId getCurrentPath() const; 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 d4013fd97b316..7ab404d99bb12 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 @@ -63,7 +63,13 @@ struct PullOutStatus bool is_safe_dynamic_objects{false}; // current path is safe against dynamic objects bool back_finished{false}; // if backward driving is not required, this is also set to true // todo: rename to clear variable name. + bool backward_driving_complete{ + false}; // after backward driving is complete, this is set to true (warning: this is set to + // false at next cycle after backward driving is complete) Pose pull_out_start_pose{}; + bool prev_is_safe_dynamic_objects{false}; + std::shared_ptr prev_stop_path_after_approval{nullptr}; + bool has_stop_point{false}; PullOutStatus() {} }; @@ -87,12 +93,13 @@ class StartPlannerModule : public SceneModuleInterface bool isExecutionRequested() const override; bool isExecutionReady() const override; // TODO(someone): remove this, and use base class function - [[deprecated]] ModuleStatus updateState() override; + [[deprecated]] void updateCurrentState() override; BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; CandidateOutput planCandidate() const override; void processOnEntry() override; void processOnExit() override; + void updateData() override; void setParameters(const std::shared_ptr & parameters) { @@ -139,7 +146,6 @@ class StartPlannerModule : public SceneModuleInterface std::unique_ptr last_route_received_time_; std::unique_ptr last_pull_out_start_update_time_; - std::unique_ptr last_approved_pose_; // generate freespace pull out paths in a separate thread std::unique_ptr freespace_planner_; 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 634ac69bae12c..6173ecb15a5c2 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 @@ -183,9 +183,6 @@ struct AvoidanceParameters // parameters for collision check. bool check_all_predicted_path{false}; - double time_horizon_for_front_object{0.0}; - double time_horizon_for_rear_object{0.0}; - double safety_check_time_resolution{0.0}; // find adjacent lane vehicles double safety_check_backward_distance{0.0}; @@ -295,6 +292,9 @@ struct AvoidanceParameters // parameters depend on object class std::unordered_map object_parameters; + // ego predicted path params. + utils::path_safety_checker::EgoPredictedPathParams ego_predicted_path_params{}; + // rss parameters utils::path_safety_checker::RSSparams rss_params{}; 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 44d5d6f6b4e56..c0bd621cc86b4 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 @@ -87,11 +87,6 @@ std::vector generateObstaclePolygonsForDrivableArea( const ObjectDataArray & objects, const std::shared_ptr & parameters, const double vehicle_width); -std::vector convertToPredictedPath( - const PathWithLaneId & path, const std::shared_ptr & planner_data, - const bool is_object_front, const bool limit_to_max_velocity, - const std::shared_ptr & parameters); - double getLongitudinalVelocity(const Pose & p_ref, const Pose & p_target, const double v); bool isCentroidWithinLanelets( @@ -162,9 +157,6 @@ AvoidLine fillAdditionalInfo(const AvoidancePlanningData & data, const AvoidLine AvoidLineArray combineRawShiftLinesWithUniqueCheck( const AvoidLineArray & base_lines, const AvoidLineArray & added_lines); -ExtendedPredictedObject transform( - const PredictedObject & object, const std::shared_ptr & parameters); - std::vector getSafetyCheckTargetObjects( const AvoidancePlanningData & data, const std::shared_ptr & planner_data, const std::shared_ptr & parameters, const bool is_right_shift); 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 22624bd1e0a80..2b189d583b924 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 @@ -50,9 +50,8 @@ struct GoalPlannerParameters // goal search std::string goal_priority; // "minimum_weighted_distance" or "minimum_longitudinal_distance" double minimum_weighted_distance_lateral_weight{0.0}; - std::string path_priority; // "efficient_path" or "close_goal" + bool prioritize_goals_before_objects{false}; ParkingPolicy parking_policy; // "left_side" or "right_side" - double forward_goal_search_length{0.0}; double backward_goal_search_length{0.0}; double goal_search_interval{0.0}; @@ -83,6 +82,8 @@ struct GoalPlannerParameters double decide_path_distance{0.0}; double maximum_deceleration{0.0}; double maximum_jerk{0.0}; + std::string path_priority; // "efficient_path" or "close_goal" + std::vector efficient_path_order{}; // shift path bool enable_shift_parking{false}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp index d564dcaa19e29..9d957ae9767c9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp @@ -36,14 +36,16 @@ class GoalSearcher : public GoalSearcherBase const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint, const std::shared_ptr & occupancy_grid_map); - GoalCandidates search(const Pose & original_goal_pose) override; + GoalCandidates search() override; void update(GoalCandidates & goal_candidates) const override; private: + void countObjectsToAvoid( + GoalCandidates & goal_candidates, const PredictedObjects & objects) const; void createAreaPolygons(std::vector original_search_poses); - bool checkCollision(const Pose & pose) const; + bool checkCollision(const Pose & pose, const PredictedObjects & objects) const; bool checkCollisionWithLongitudinalDistance( - const Pose & ego_pose, const PredictedObjects & dynamic_objects) const; + const Pose & ego_pose, const PredictedObjects & objects) const; BasicPolygons2d getNoParkingAreaPolygons(const lanelet::ConstLanelets & lanes) const; BasicPolygons2d getNoStoppingAreaPolygons(const lanelet::ConstLanelets & lanes) const; bool isInAreas(const LinearRing2d & footprint, const BasicPolygons2d & areas) const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp index cac1d37e342c2..7364ea91339e4 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp @@ -37,6 +37,7 @@ struct GoalCandidate double lateral_offset{0.0}; size_t id{0}; bool is_safe{true}; + size_t num_objects_to_avoid{0}; }; using GoalCandidates = std::vector; @@ -46,19 +47,25 @@ class GoalSearcherBase explicit GoalSearcherBase(const GoalPlannerParameters & parameters) { parameters_ = parameters; } virtual ~GoalSearcherBase() = default; + void setReferenceGoal(const Pose & reference_goal_pose) + { + reference_goal_pose_ = reference_goal_pose; + } + void setPlannerData(const std::shared_ptr & planner_data) { planner_data_ = planner_data; } MultiPolygon2d getAreaPolygons() { return area_polygons_; } - virtual GoalCandidates search(const Pose & original_goal_pose) = 0; + virtual GoalCandidates search() = 0; virtual void update([[maybe_unused]] GoalCandidates & goal_candidates) const { return; } protected: - GoalPlannerParameters parameters_; - std::shared_ptr planner_data_; - MultiPolygon2d area_polygons_; + GoalPlannerParameters parameters_{}; + std::shared_ptr planner_data_{nullptr}; + Pose reference_goal_pose_{}; + MultiPolygon2d area_polygons_{}; }; } // namespace behavior_path_planner 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 b7a9248d569c1..1dc6cc411a31f 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 @@ -44,8 +44,6 @@ using geometry_msgs::msg::Twist; using visualization_msgs::msg::Marker; 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, const double backward_distance, const double forward_distance); @@ -66,6 +64,9 @@ MarkerArray createTextsMarkerArray( const std::vector & poses, std::string && ns, const std_msgs::msg::ColorRGBA & color); MarkerArray createGoalCandidatesMarkerArray( GoalCandidates & goal_candidates, const std_msgs::msg::ColorRGBA & color); +MarkerArray createNumObjectsToAvoidTextsMarkerArray( + const GoalCandidates & goal_candidates, std::string && ns, + const std_msgs::msg::ColorRGBA & color); } // namespace goal_planner_utils } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index 27735777c27cc..24a507d1f8695 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -67,6 +67,8 @@ struct LaneChangeParameters bool check_objects_on_current_lanes{true}; bool check_objects_on_other_lanes{true}; bool use_all_predicted_path{false}; + double lane_expansion_left_offset{0.0}; + double lane_expansion_right_offset{0.0}; // regulatory elements bool regulate_on_crosswalk{false}; @@ -80,6 +82,7 @@ struct LaneChangeParameters utils::path_safety_checker::ObjectTypesToCheck object_types_to_check; // safety check + bool allow_loose_check_for_cancel{true}; utils::path_safety_checker::RSSparams rss_params{}; utils::path_safety_checker::RSSparams rss_params_for_abort{}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp index a4c2c2e695752..b2592bc1b899b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp @@ -88,8 +88,6 @@ std::vector> getSortedLaneIds( const RouteHandler & route_handler, const Pose & current_pose, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes); -PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2); - lanelet::ConstLanelets getTargetPreferredLanes( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const Direction & direction, @@ -185,5 +183,27 @@ std::optional createPolygon( ExtendedPredictedObject transform( const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters, const LaneChangeParameters & lane_change_parameters); + +bool isCollidedPolygonsInLanelet( + const std::vector & collided_polygons, const lanelet::ConstLanelets & lanes); + +/** + * @brief Generates expanded lanelets based on the given direction and offsets. + * + * Expands the provided lanelets in either the left or right direction based on + * the specified direction. If the direction is 'LEFT', the lanelets are expanded + * using the left_offset; if 'RIGHT', they are expanded using the right_offset. + * Otherwise, no expansion occurs. + * + * @param lanes The lanelets to be expanded. + * @param direction The direction of expansion: either LEFT or RIGHT. + * @param left_offset The offset value for left expansion. + * @param right_offset The offset value for right expansion. + * @return lanelet::ConstLanelets A collection of expanded lanelets. + */ +lanelet::ConstLanelets generateExpandedLanelets( + const lanelet::ConstLanelets & lanes, const Direction direction, const double left_offset, + const double right_offset); + } // namespace behavior_path_planner::utils::lane_change #endif // BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp index e1256fd774027..15b15bac40b51 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp @@ -38,6 +38,24 @@ using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::PathPointWithLaneId; +/** + * @brief Filters objects based on object centroid position. + * + * @param objects The predicted objects to filter. + * @param lanelet + * @return result. + */ +bool isCentroidWithinLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet); + +/** + * @brief Filters objects based on object polygon overlapping with lanelet. + * + * @param objects The predicted objects to filter. + * @param lanelet + * @return result. + */ +bool isPolygonOverlapLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet); + /** * @brief Filters objects based on various criteria. * @@ -117,14 +135,16 @@ void filterObjectsByClass( * lanelet. */ std::pair, std::vector> separateObjectIndicesByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets); + const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets, + const std::function & condition); /** * @brief Separate the objects into two part based on whether the object is within lanelet. * @return Objects pair. first objects are in the lanelet, and second others are out of lanelet. */ std::pair separateObjectsByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets); + const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets, + const std::function & condition); /** * @brief Get the predicted path from an object. diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp index f9fbf70b2113b..4983060aa1c0a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp @@ -88,7 +88,6 @@ boost::optional calcInterpolatedPoseWithVelocity( boost::optional getInterpolatedPoseWithVelocityAndPolygonStamped( const std::vector & pred_path, const double current_time, const VehicleInfo & ego_info); - /** * @brief Iterate the points in the ego and target's predicted path and * perform safety check for each of the iterated points. @@ -111,15 +110,39 @@ bool checkCollision( const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, const double hysteresis_factor, CollisionCheckDebug & debug); +std::vector generatePolygonsWithStoppingAndInertialMargin( + const PathWithLaneId & ego_path, const double base_to_front, const double base_to_rear, + const double width, const double maximum_deceleration, const double max_extra_stopping_margin); + +/** + * @brief Iterate the points in the ego and target's predicted path and + * perform safety check for each of the iterated points. + * @param planned_path The predicted path of the ego vehicle. + * @param predicted_ego_path Ego vehicle's predicted path + * @param ego_current_velocity Current velocity of the ego vehicle. + * @param target_object The predicted object to check collision with. + * @param target_object_path The predicted path of the target object. + * @param common_parameters The common parameters used in behavior path planner. + * @param front_object_deceleration The deceleration of the object in the front.(used in RSS) + * @param rear_object_deceleration The deceleration of the object in the rear.(used in RSS) + * @param debug The debug information for collision checking. + * @return a list of polygon when collision is expected. + */ +std::vector getCollidedPolygons( + const PathWithLaneId & planned_path, + const std::vector & predicted_ego_path, + const ExtendedPredictedObject & target_object, + const PredictedPathWithPolygon & target_object_path, + const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, + const double hysteresis_factor, CollisionCheckDebug & debug); + /** - * @brief Check collision between ego path footprints with extra longitudinal stopping margin and - * objects. + * @brief Check collision between ego polygons with margin and objects. * @return Has collision or not */ -bool checkCollisionWithExtraStoppingMargin( - const PathWithLaneId & ego_path, const PredictedObjects & dynamic_objects, - const double base_to_front, const double base_to_rear, const double width, - const double maximum_deceleration, const double margin, const double max_stopping_margin); +bool checkCollisionWithMargin( + const std::vector & ego_polygons, const PredictedObjects & dynamic_objects, + const double collision_check_margin); } // namespace behavior_path_planner::utils::path_safety_checker #endif // BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp index e907df8c7a58a..b42502d9edec8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp @@ -102,6 +102,8 @@ PathWithLaneId calcCenterLinePath( const std::shared_ptr & planner_data, const Pose & ref_pose, const double longest_dist_to_shift_line, const std::optional & prev_module_path = std::nullopt); + +PathWithLaneId combinePath(const PathWithLaneId & path1, const PathWithLaneId & path2); } // namespace behavior_path_planner::utils #endif // BEHAVIOR_PATH_PLANNER__UTILS__PATH_UTILS_HPP_ 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 09fb9911f7386..f2dba684de991 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 @@ -44,7 +44,6 @@ using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using route_handler::RouteHandler; -PathWithLaneId combineReferencePath(const PathWithLaneId path1, const PathWithLaneId path2); PathWithLaneId getBackwardPath( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, const Pose & backed_pose, const double velocity); 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 3d81c584b7c01..3e03a09d3adf8 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 @@ -231,6 +231,15 @@ std::vector calcBound( const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, const bool is_left); +std::vector getBoundWithHatchedRoadMarkings( + const std::vector & original_bound, + const std::shared_ptr & route_handler); + +std::vector getBoundWithIntersectionAreas( + const std::vector & original_bound, + const std::shared_ptr & route_handler, + const std::vector & drivable_lanes, const bool is_left); + boost::optional getOverlappedLaneletId(const std::vector & lanes); std::vector cutOverlappedLanes( PathWithLaneId & path, const std::vector & lanes); @@ -327,6 +336,8 @@ std::optional getSignedDistanceFromBoundary( // misc +Polygon2d toPolygon2d(const lanelet::ConstLanelet & lanelet); + std::vector getTargetLaneletPolygons( const lanelet::ConstLanelets & lanelets, const Pose & pose, const double check_length, const std::string & target_type); 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 8302779313feb..755817aa22ae7 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -134,43 +134,34 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod const auto & p = planner_data_->parameters; planner_manager_ = std::make_shared(*this, p.verbose); - const auto register_and_create_publisher = [&](const auto & manager) { - const auto & module_name = manager->name(); - planner_manager_->registerSceneModuleManager(manager); - path_candidate_publishers_.emplace( - module_name, create_publisher(path_candidate_name_space + module_name, 1)); - path_reference_publishers_.emplace( - module_name, create_publisher(path_reference_name_space + module_name, 1)); - }; + const auto register_and_create_publisher = + [&](const auto & manager, const bool create_publishers) { + const auto & module_name = manager->name(); + planner_manager_->registerSceneModuleManager(manager); + if (create_publishers) { + path_candidate_publishers_.emplace( + module_name, create_publisher(path_candidate_name_space + module_name, 1)); + path_reference_publishers_.emplace( + module_name, create_publisher(path_reference_name_space + module_name, 1)); + } + }; if (p.config_start_planner.enable_module) { auto manager = std::make_shared(this, "start_planner", p.config_start_planner); - planner_manager_->registerSceneModuleManager(manager); - path_candidate_publishers_.emplace( - "start_planner", create_publisher(path_candidate_name_space + "start_planner", 1)); - path_reference_publishers_.emplace( - "start_planner", create_publisher(path_reference_name_space + "start_planner", 1)); + register_and_create_publisher(manager, true); } if (p.config_goal_planner.enable_module) { auto manager = std::make_shared(this, "goal_planner", p.config_goal_planner); - planner_manager_->registerSceneModuleManager(manager); - path_candidate_publishers_.emplace( - "goal_planner", create_publisher(path_candidate_name_space + "goal_planner", 1)); - path_reference_publishers_.emplace( - "goal_planner", create_publisher(path_reference_name_space + "goal_planner", 1)); + register_and_create_publisher(manager, true); } if (p.config_side_shift.enable_module) { auto manager = std::make_shared(this, "side_shift", p.config_side_shift); - planner_manager_->registerSceneModuleManager(manager); - path_candidate_publishers_.emplace( - "side_shift", create_publisher(path_candidate_name_space + "side_shift", 1)); - path_reference_publishers_.emplace( - "side_shift", create_publisher(path_reference_name_space + "side_shift", 1)); + register_and_create_publisher(manager, true); } if (p.config_lane_change_left.enable_module) { @@ -178,7 +169,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod auto manager = std::make_shared( this, module_topic, p.config_lane_change_left, route_handler::Direction::LEFT, LaneChangeModuleType::NORMAL); - register_and_create_publisher(manager); + register_and_create_publisher(manager, true); } if (p.config_lane_change_right.enable_module) { @@ -186,7 +177,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod auto manager = std::make_shared( this, module_topic, p.config_lane_change_right, route_handler::Direction::RIGHT, LaneChangeModuleType::NORMAL); - register_and_create_publisher(manager); + register_and_create_publisher(manager, true); } if (p.config_ext_request_lane_change_right.enable_module) { @@ -194,7 +185,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod auto manager = std::make_shared( this, module_topic, p.config_ext_request_lane_change_right, route_handler::Direction::RIGHT, LaneChangeModuleType::EXTERNAL_REQUEST); - register_and_create_publisher(manager); + register_and_create_publisher(manager, true); } if (p.config_ext_request_lane_change_left.enable_module) { @@ -202,35 +193,25 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod auto manager = std::make_shared( this, module_topic, p.config_ext_request_lane_change_left, route_handler::Direction::LEFT, LaneChangeModuleType::EXTERNAL_REQUEST); - register_and_create_publisher(manager); + register_and_create_publisher(manager, true); } if (p.config_avoidance.enable_module) { auto manager = std::make_shared(this, "avoidance", p.config_avoidance); - planner_manager_->registerSceneModuleManager(manager); - path_candidate_publishers_.emplace( - "avoidance", create_publisher(path_candidate_name_space + "avoidance", 1)); - path_reference_publishers_.emplace( - "avoidance", create_publisher(path_reference_name_space + "avoidance", 1)); + register_and_create_publisher(manager, true); } if (p.config_avoidance_by_lc.enable_module) { auto manager = std::make_shared( this, "avoidance_by_lane_change", p.config_avoidance_by_lc); - planner_manager_->registerSceneModuleManager(manager); - path_candidate_publishers_.emplace( - "avoidance_by_lane_change", - create_publisher(path_candidate_name_space + "avoidance_by_lane_change", 1)); - path_reference_publishers_.emplace( - "avoidance_by_lane_change", - create_publisher(path_reference_name_space + "avoidance_by_lane_change", 1)); + register_and_create_publisher(manager, true); } if (p.config_dynamic_avoidance.enable_module) { auto manager = std::make_shared( this, "dynamic_avoidance", p.config_dynamic_avoidance); - planner_manager_->registerSceneModuleManager(manager); + register_and_create_publisher(manager, false); } } @@ -258,6 +239,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod timer_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&BehaviorPathPlannerNode::run, this)); } + + logger_configure_ = std::make_unique(this); } std::vector BehaviorPathPlannerNode::getWaitingApprovalModules() @@ -596,6 +579,7 @@ void BehaviorPathPlannerNode::run() lk_pd.unlock(); // release planner_data_ planner_manager_->print(); + planner_manager_->publishProcessingTime(); planner_manager_->publishMarker(); planner_manager_->publishVirtualWall(); lk_manager.unlock(); // release planner_manager_ diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index 5963ddb778874..24967608775f3 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -886,13 +886,19 @@ void PlannerManager::print() const string_stream << std::right << "[" << std::setw(max_string_num + 1) << std::left << t.first << ":" << std::setw(4) << std::right << t.second << "ms]\n" << std::setw(21); - std::string name = std::string("processing_time/") + t.first; - debug_publisher_ptr_->publish(name, t.second); } RCLCPP_INFO_STREAM(logger_, string_stream.str()); } +void PlannerManager::publishProcessingTime() const +{ + for (const auto & t : processing_time_) { + std::string name = std::string("processing_time/") + t.first; + debug_publisher_ptr_->publish(name, t.second); + } +} + std::shared_ptr PlannerManager::getDebugMsg() { debug_msg_ptr_ = std::make_shared(); 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 83789818c456b..a8dd0b794245e 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 @@ -17,6 +17,7 @@ #include "behavior_path_planner/marker_utils/avoidance/debug.hpp" #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/avoidance/utils.hpp" +#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/utils.hpp" @@ -1838,10 +1839,16 @@ bool AvoidanceModule::isSafePath( } const bool limit_to_max_velocity = false; - const auto ego_predicted_path_for_front_object = utils::avoidance::convertToPredictedPath( - shifted_path.path, planner_data_, true, limit_to_max_velocity, parameters_); - const auto ego_predicted_path_for_rear_object = utils::avoidance::convertToPredictedPath( - shifted_path.path, planner_data_, false, limit_to_max_velocity, parameters_); + const auto ego_predicted_path_params = + std::make_shared( + parameters_->ego_predicted_path_params); + const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(shifted_path.path.points); + const auto ego_predicted_path_for_front_object = utils::path_safety_checker::createPredictedPath( + ego_predicted_path_params, shifted_path.path.points, getEgoPose(), getEgoSpeed(), ego_seg_idx, + true, limit_to_max_velocity); + const auto ego_predicted_path_for_rear_object = utils::path_safety_checker::createPredictedPath( + ego_predicted_path_params, shifted_path.path.points, getEgoPose(), getEgoSpeed(), ego_seg_idx, + false, limit_to_max_velocity); const auto ego_idx = planner_data_->findEgoIndex(shifted_path.path.points); const auto is_right_shift = [&]() -> std::optional { @@ -2543,9 +2550,31 @@ TurnSignalInfo AvoidanceModule::calcTurnSignalInfo(const ShiftedPath & path) con turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; } } else { - turn_signal_info.turn_signal.command = TurnIndicatorsCommand::DISABLE; + const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_); + const auto local_vehicle_footprint = + createVehicleFootprint(planner_data_->parameters.vehicle_info); + boost::geometry::model::ring shifted_vehicle_footprint; + for (const auto & cl : current_lanes) { + // get left and right bounds of current lane + const auto lane_left_bound = cl.leftBound2d().basicLineString(); + const auto lane_right_bound = cl.rightBound2d().basicLineString(); + for (size_t i = start_idx; i < end_idx; ++i) { + // transform vehicle footprint onto path points + shifted_vehicle_footprint = transformVector( + local_vehicle_footprint, + tier4_autoware_utils::pose2transform(path.path.points.at(i).point.pose)); + if ( + boost::geometry::intersects(lane_left_bound, shifted_vehicle_footprint) || + boost::geometry::intersects(lane_right_bound, shifted_vehicle_footprint)) { + if (segment_shift_length > 0.0) { + turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; + } else { + turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; + } + } + } + } } - if (ego_front_to_shift_start > 0.0) { turn_signal_info.desired_start_point = planner_data_->self_odometry->pose.pose; } else { @@ -2639,6 +2668,7 @@ void AvoidanceModule::updateDebugMarker( addObjects(data.other_objects, std::string("OutOfTargetArea")); addObjects(data.other_objects, std::string("NotNeedAvoidance")); addObjects(data.other_objects, std::string("LessThanExecutionThreshold")); + addObjects(data.other_objects, std::string("TooNearToGoal")); } // shift line pre-process 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 912f867fd5701..8c65323e5a123 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -142,11 +142,6 @@ AvoidanceModuleManager::AvoidanceModuleManager( p.check_other_object = getOrDeclareParameter(*node, ns + "check_other_object"); p.check_all_predicted_path = getOrDeclareParameter(*node, ns + "check_all_predicted_path"); - p.time_horizon_for_front_object = - getOrDeclareParameter(*node, ns + "time_horizon_for_front_object"); - p.time_horizon_for_rear_object = - getOrDeclareParameter(*node, ns + "time_horizon_for_rear_object"); - p.safety_check_time_resolution = getOrDeclareParameter(*node, ns + "time_resolution"); p.safety_check_backward_distance = getOrDeclareParameter(*node, ns + "safety_check_backward_distance"); p.hysteresis_factor_expand_rate = @@ -155,6 +150,23 @@ AvoidanceModuleManager::AvoidanceModuleManager( getOrDeclareParameter(*node, ns + "hysteresis_factor_safe_count"); } + // safety check predicted path params + { + std::string ns = "avoidance.safety_check."; + p.ego_predicted_path_params.min_velocity = + getOrDeclareParameter(*node, ns + "min_velocity"); + p.ego_predicted_path_params.max_velocity = + getOrDeclareParameter(*node, ns + "max_velocity"); + p.ego_predicted_path_params.acceleration = + getOrDeclareParameter(*node, "avoidance.constraints.longitudinal.max_acceleration"); + p.ego_predicted_path_params.time_horizon_for_rear_object = + getOrDeclareParameter(*node, ns + "time_horizon_for_rear_object"); + p.ego_predicted_path_params.time_resolution = + getOrDeclareParameter(*node, ns + "time_resolution"); + p.ego_predicted_path_params.delay_until_departure = + getOrDeclareParameter(*node, ns + "delay_until_departure"); + } + // safety check rss params { std::string ns = "avoidance.safety_check."; 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 181b9fbfd5260..24c074bcb629b 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 @@ -411,10 +411,12 @@ void DynamicAvoidanceModule::updateTargetObjects() continue; } - // 1.b. check if velocity is large enough + // 1.b. check obstacle velocity const auto [obj_tangent_vel, obj_normal_vel] = projectObstacleVelocityToTrajectory(prev_module_path->points, predicted_object); - if (std::abs(obj_tangent_vel) < parameters_->min_obstacle_vel) { + if ( + std::abs(obj_tangent_vel) < parameters_->min_obstacle_vel || + parameters_->max_obstacle_vel < std::abs(obj_tangent_vel)) { continue; } 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 616194ea66ac7..6f624a9f6f017 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 @@ -47,6 +47,7 @@ DynamicAvoidanceModuleManager::DynamicAvoidanceModuleManager( p.avoid_bicycle = node->declare_parameter(ns + "bicycle"); p.avoid_motorcycle = node->declare_parameter(ns + "motorcycle"); p.avoid_pedestrian = node->declare_parameter(ns + "pedestrian"); + p.max_obstacle_vel = node->declare_parameter(ns + "max_obstacle_vel"); p.min_obstacle_vel = node->declare_parameter(ns + "min_obstacle_vel"); p.successive_num_to_entry_dynamic_avoidance_condition = node->declare_parameter(ns + "successive_num_to_entry_dynamic_avoidance_condition"); @@ -136,6 +137,7 @@ void DynamicAvoidanceModuleManager::updateModuleParams( updateParam(parameters, ns + "motorcycle", p->avoid_motorcycle); updateParam(parameters, ns + "pedestrian", p->avoid_pedestrian); + updateParam(parameters, ns + "max_obstacle_vel", p->max_obstacle_vel); updateParam(parameters, ns + "min_obstacle_vel", p->min_obstacle_vel); updateParam( 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 1f5a823386774..9c2544cf60cec 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 @@ -21,6 +21,7 @@ #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/geometry/boost_polygon_utils.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" #include @@ -68,22 +69,17 @@ GoalPlannerModule::GoalPlannerModule( // planner when goal modification is not allowed fixed_goal_planner_ = std::make_unique(); - // set enabled planner - if (parameters_->enable_shift_parking) { - pull_over_planners_.push_back(std::make_shared( - node, *parameters, lane_departure_checker, occupancy_grid_map_)); - } - - // set geometric pull over - if (parameters_->enable_arc_forward_parking) { - constexpr bool is_forward = true; - pull_over_planners_.push_back(std::make_shared( - node, *parameters, lane_departure_checker, occupancy_grid_map_, is_forward)); - } - if (parameters_->enable_arc_backward_parking) { - constexpr bool is_forward = false; - pull_over_planners_.push_back(std::make_shared( - node, *parameters, lane_departure_checker, occupancy_grid_map_, is_forward)); + for (const std::string & planner_type : parameters_->efficient_path_order) { + if (planner_type == "SHIFT" && parameters_->enable_shift_parking) { + pull_over_planners_.push_back(std::make_shared( + node, *parameters, lane_departure_checker, occupancy_grid_map_)); + } else if (planner_type == "ARC_FORWARD" && parameters_->enable_arc_forward_parking) { + pull_over_planners_.push_back(std::make_shared( + node, *parameters, lane_departure_checker, occupancy_grid_map_, /*is_forward*/ true)); + } else if (planner_type == "ARC_BACKWARD" && parameters_->enable_arc_backward_parking) { + pull_over_planners_.push_back(std::make_shared( + node, *parameters, lane_departure_checker, occupancy_grid_map_, /*is_forward*/ false)); + } } if (pull_over_planners_.empty()) { @@ -217,6 +213,10 @@ void GoalPlannerModule::onFreespaceParkingTimer() if (!planner_data_->costmap) { return; } + // fixed goal planner do not use freespace planner + if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { + return; + } const bool is_new_costmap = (clock_->now() - planner_data_->costmap->header.stamp).seconds() < 1.0; @@ -366,12 +366,7 @@ 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 (parameters_->safety_check_params.enable_safety_check && isWaitingApproval()) { if (!isSafePath()) { RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against dynamic objects"); return false; @@ -459,6 +454,7 @@ ModuleStatus GoalPlannerModule::updateState() bool GoalPlannerModule::planFreespacePath() { mutex_.lock(); + goal_searcher_->setPlannerData(planner_data_); goal_searcher_->update(goal_candidates_); const auto goal_candidates = goal_candidates_; debug_data_.freespace_planner.num_goal_candidates = goal_candidates.size(); @@ -546,7 +542,8 @@ void GoalPlannerModule::generateGoalCandidates() refined_goal_pose_ = calcRefinedGoal(goal_pose); if (goal_planner_utils::isAllowedGoalModification(route_handler)) { goal_searcher_->setPlannerData(planner_data_); - goal_candidates_ = goal_searcher_->search(refined_goal_pose_); + goal_searcher_->setReferenceGoal(refined_goal_pose_); + goal_candidates_ = goal_searcher_->search(); } else { GoalCandidate goal_candidate{}; goal_candidate.goal_pose = goal_pose; @@ -572,15 +569,47 @@ BehaviorModuleOutput GoalPlannerModule::plan() } } +void GoalPlannerModule::sortPullOverPathCandidatesByGoalPriority( + std::vector & pull_over_path_candidates, + const GoalCandidates & goal_candidates) const +{ + // Create a map of goal_id to its index in goal_candidates + std::map goal_id_to_index; + for (size_t i = 0; i < goal_candidates.size(); ++i) { + goal_id_to_index[goal_candidates[i].id] = i; + } + + // Sort pull_over_path_candidates based on the order in goal_candidates + std::stable_sort( + pull_over_path_candidates.begin(), pull_over_path_candidates.end(), + [&goal_id_to_index](const auto & a, const auto & b) { + return goal_id_to_index[a.goal_id] < goal_id_to_index[b.goal_id]; + }); + + // Sort pull_over_path_candidates based on the order in efficient_path_order + if (parameters_->path_priority == "efficient_path") { + std::stable_sort( + pull_over_path_candidates.begin(), pull_over_path_candidates.end(), + [this](const auto & a, const auto & b) { + const auto & order = parameters_->efficient_path_order; + const auto a_pos = std::find(order.begin(), order.end(), magic_enum::enum_name(a.type)); + const auto b_pos = std::find(order.begin(), order.end(), magic_enum::enum_name(b.type)); + return a_pos < b_pos; + }); + } +} + void GoalPlannerModule::selectSafePullOverPath() { // select safe lane pull over path from candidates mutex_.lock(); goal_searcher_->setPlannerData(planner_data_); goal_searcher_->update(goal_candidates_); + sortPullOverPathCandidatesByGoalPriority(pull_over_path_candidates_, goal_candidates_); const auto pull_over_path_candidates = pull_over_path_candidates_; const auto goal_candidates = goal_candidates_; mutex_.unlock(); + status_.is_safe_static_objects = false; for (const auto & pull_over_path : pull_over_path_candidates) { // check if goal is safe @@ -1268,28 +1297,33 @@ bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const } } - if (parameters_->use_object_recognition) { - 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 [pull_over_lane_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets( - *(planner_data_->dynamic_object), pull_over_lanes); - const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( - pull_over_lane_objects, parameters_->th_moving_object_velocity); - const auto common_parameters = planner_data_->parameters; - const double base_link2front = common_parameters.base_link2front; - const double base_link2rear = common_parameters.base_link2rear; - const double vehicle_width = common_parameters.vehicle_width; - if (utils::path_safety_checker::checkCollisionWithExtraStoppingMargin( - path, pull_over_lane_stop_objects, base_link2front, base_link2rear, vehicle_width, - parameters_->maximum_deceleration, parameters_->object_recognition_collision_check_margin, - parameters_->object_recognition_collision_check_max_extra_stopping_margin)) { - return true; - } + if (!parameters_->use_object_recognition) { + return false; } - return false; + 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 [pull_over_lane_objects, others] = + utils::path_safety_checker::separateObjectsByLanelets( + *(planner_data_->dynamic_object), pull_over_lanes, + utils::path_safety_checker::isPolygonOverlapLanelet); + const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( + pull_over_lane_objects, parameters_->th_moving_object_velocity); + const auto common_parameters = planner_data_->parameters; + const double base_link2front = common_parameters.base_link2front; + const double base_link2rear = common_parameters.base_link2rear; + const double vehicle_width = common_parameters.vehicle_width; + + const auto ego_polygons_expanded = + utils::path_safety_checker::generatePolygonsWithStoppingAndInertialMargin( + path, base_link2front, base_link2rear, vehicle_width, parameters_->maximum_deceleration, + parameters_->object_recognition_collision_check_max_extra_stopping_margin); + debug_data_.ego_polygons_expanded = ego_polygons_expanded; + + return utils::path_safety_checker::checkCollisionWithMargin( + ego_polygons_expanded, pull_over_lane_stop_objects, + parameters_->object_recognition_collision_check_margin); } bool GoalPlannerModule::hasEnoughDistance(const PullOverPath & pull_over_path) const @@ -1658,6 +1692,24 @@ void GoalPlannerModule::setDebugData() add( createPathMarkerArray(partial_path, "partial_path_" + std::to_string(i), 0, 0.9, 0.5, 0.9)); } + + auto marker = tier4_autoware_utils::createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "detection_polygons", 0, Marker::LINE_LIST, + tier4_autoware_utils::createMarkerScale(0.01, 0.0, 0.0), + tier4_autoware_utils::createMarkerColor(0.0, 0.0, 1.0, 0.999)); + + for (const auto & ego_polygon : debug_data_.ego_polygons_expanded) { + for (size_t ep_idx = 0; ep_idx < ego_polygon.outer().size(); ++ep_idx) { + const auto & current_point = ego_polygon.outer().at(ep_idx); + const auto & next_point = ego_polygon.outer().at((ep_idx + 1) % ego_polygon.outer().size()); + + marker.points.push_back( + tier4_autoware_utils::createPoint(current_point.x(), current_point.y(), 0.0)); + marker.points.push_back( + tier4_autoware_utils::createPoint(next_point.x(), next_point.y(), 0.0)); + } + } + debug_marker_.markers.push_back(marker); } // safety check if (parameters_->safety_check_params.enable_safety_check) { @@ -1667,7 +1719,6 @@ void GoalPlannerModule::setDebugData() add(createPredictedPathMarkerArray( ego_predicted_path, vehicle_info_, "ego_predicted_path_goal_planner", 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)); 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 79459d6efb202..41d0299bca427 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 @@ -48,7 +48,8 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( p.goal_priority = node->declare_parameter(ns + "goal_priority"); p.minimum_weighted_distance_lateral_weight = node->declare_parameter(ns + "minimum_weighted_distance.lateral_weight"); - p.path_priority = node->declare_parameter(ns + "path_priority"); + p.prioritize_goals_before_objects = + node->declare_parameter(ns + "prioritize_goals_before_objects"); p.forward_goal_search_length = node->declare_parameter(ns + "forward_goal_search_length"); p.backward_goal_search_length = @@ -112,6 +113,9 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( p.decide_path_distance = node->declare_parameter(ns + "decide_path_distance"); p.maximum_deceleration = node->declare_parameter(ns + "maximum_deceleration"); p.maximum_jerk = node->declare_parameter(ns + "maximum_jerk"); + p.path_priority = node->declare_parameter(ns + "path_priority"); + p.efficient_path_order = + node->declare_parameter>(ns + "efficient_path_order"); } // shift parking 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 6d06b26cd1e17..1f305e3988828 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 @@ -17,6 +17,7 @@ #include "behavior_path_planner/utils/avoidance/utils.hpp" #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include #include @@ -142,7 +143,8 @@ void AvoidanceByLaneChange::fillAvoidanceTargetObjects( const auto [object_within_target_lane, object_outside_target_lane] = utils::path_safety_checker::separateObjectsByLanelets( - *planner_data_->dynamic_object, data.current_lanelets); + *planner_data_->dynamic_object, data.current_lanelets, + utils::path_safety_checker::isPolygonOverlapLanelet); // Assume that the maximum allocation for data.other object is the sum of // objects_within_target_lane and object_outside_target_lane. The maximum allocation for 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 a502c9d8e43ec..e33a6c4b05ee3 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 @@ -77,7 +77,10 @@ LaneChangeModuleManager::LaneChangeModuleManager( getOrDeclareParameter(*node, parameter("check_objects_on_other_lanes")); p.use_all_predicted_path = getOrDeclareParameter(*node, parameter("use_all_predicted_path")); - + p.lane_expansion_left_offset = + getOrDeclareParameter(*node, parameter("safety_check.lane_expansion.left_offset")); + p.lane_expansion_right_offset = + getOrDeclareParameter(*node, parameter("safety_check.lane_expansion.right_offset")); // lane change regulations p.regulate_on_crosswalk = getOrDeclareParameter(*node, parameter("regulation.crosswalk")); p.regulate_on_intersection = @@ -89,35 +92,41 @@ LaneChangeModuleManager::LaneChangeModuleManager( p.stop_time_threshold = getOrDeclareParameter(*node, parameter("stuck_detection.stop_time")); + // safety check + p.allow_loose_check_for_cancel = + getOrDeclareParameter(*node, parameter("safety_check.allow_loose_check_for_cancel")); + p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter( - *node, parameter("safety_check.longitudinal_distance_min_threshold")); + *node, parameter("safety_check.execution.longitudinal_distance_min_threshold")); + p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter( + *node, parameter("safety_check.execution.longitudinal_distance_min_threshold")); p.rss_params.longitudinal_velocity_delta_time = getOrDeclareParameter( - *node, parameter("safety_check.longitudinal_velocity_delta_time")); - p.rss_params.front_vehicle_deceleration = - getOrDeclareParameter(*node, parameter("safety_check.expected_front_deceleration")); - p.rss_params.rear_vehicle_deceleration = - getOrDeclareParameter(*node, parameter("safety_check.expected_rear_deceleration")); - p.rss_params.rear_vehicle_reaction_time = - getOrDeclareParameter(*node, parameter("safety_check.rear_vehicle_reaction_time")); - p.rss_params.rear_vehicle_safety_time_margin = - getOrDeclareParameter(*node, parameter("safety_check.rear_vehicle_safety_time_margin")); - p.rss_params.lateral_distance_max_threshold = - getOrDeclareParameter(*node, parameter("safety_check.lateral_distance_max_threshold")); + *node, parameter("safety_check.execution.longitudinal_velocity_delta_time")); + p.rss_params.front_vehicle_deceleration = getOrDeclareParameter( + *node, parameter("safety_check.execution.expected_front_deceleration")); + p.rss_params.rear_vehicle_deceleration = getOrDeclareParameter( + *node, parameter("safety_check.execution.expected_rear_deceleration")); + p.rss_params.rear_vehicle_reaction_time = getOrDeclareParameter( + *node, parameter("safety_check.execution.rear_vehicle_reaction_time")); + p.rss_params.rear_vehicle_safety_time_margin = getOrDeclareParameter( + *node, parameter("safety_check.execution.rear_vehicle_safety_time_margin")); + p.rss_params.lateral_distance_max_threshold = getOrDeclareParameter( + *node, parameter("safety_check.execution.lateral_distance_max_threshold")); p.rss_params_for_abort.longitudinal_distance_min_threshold = getOrDeclareParameter( - *node, parameter("safety_check.longitudinal_distance_min_threshold")); + *node, parameter("safety_check.cancel.longitudinal_distance_min_threshold")); p.rss_params_for_abort.longitudinal_velocity_delta_time = getOrDeclareParameter( - *node, parameter("safety_check.longitudinal_velocity_delta_time")); + *node, parameter("safety_check.cancel.longitudinal_velocity_delta_time")); p.rss_params_for_abort.front_vehicle_deceleration = getOrDeclareParameter( - *node, parameter("safety_check.expected_front_deceleration_for_abort")); + *node, parameter("safety_check.cancel.expected_front_deceleration")); p.rss_params_for_abort.rear_vehicle_deceleration = getOrDeclareParameter( - *node, parameter("safety_check.expected_rear_deceleration_for_abort")); - p.rss_params_for_abort.rear_vehicle_reaction_time = - getOrDeclareParameter(*node, parameter("safety_check.rear_vehicle_reaction_time")); - p.rss_params_for_abort.rear_vehicle_safety_time_margin = - getOrDeclareParameter(*node, parameter("safety_check.rear_vehicle_safety_time_margin")); - p.rss_params_for_abort.lateral_distance_max_threshold = - getOrDeclareParameter(*node, parameter("safety_check.lateral_distance_max_threshold")); + *node, parameter("safety_check.cancel.expected_rear_deceleration")); + p.rss_params_for_abort.rear_vehicle_reaction_time = getOrDeclareParameter( + *node, parameter("safety_check.cancel.rear_vehicle_reaction_time")); + p.rss_params_for_abort.rear_vehicle_safety_time_margin = getOrDeclareParameter( + *node, parameter("safety_check.cancel.rear_vehicle_safety_time_margin")); + p.rss_params_for_abort.lateral_distance_max_threshold = getOrDeclareParameter( + *node, parameter("safety_check.cancel.lateral_distance_max_threshold")); // target object { @@ -162,6 +171,26 @@ LaneChangeModuleManager::LaneChangeModuleManager( exit(EXIT_FAILURE); } + // validation of safety check parameters + // if loosely check is not allowed, lane change module will keep on chattering and canceling, and + // false positive situation might occur + if (!p.allow_loose_check_for_cancel) { + if ( + p.rss_params.front_vehicle_deceleration > p.rss_params_for_abort.front_vehicle_deceleration || + p.rss_params.rear_vehicle_deceleration > p.rss_params_for_abort.rear_vehicle_deceleration || + p.rss_params.rear_vehicle_reaction_time > p.rss_params_for_abort.rear_vehicle_reaction_time || + p.rss_params.rear_vehicle_safety_time_margin > + p.rss_params_for_abort.rear_vehicle_safety_time_margin || + p.rss_params.lateral_distance_max_threshold > + p.rss_params_for_abort.lateral_distance_max_threshold || + p.rss_params.longitudinal_distance_min_threshold > + p.rss_params_for_abort.longitudinal_distance_min_threshold || + p.rss_params.longitudinal_velocity_delta_time > + p.rss_params_for_abort.longitudinal_velocity_delta_time) { + RCLCPP_FATAL_STREAM(logger_, "abort parameter might be loose... Terminating the program..."); + exit(EXIT_FAILURE); + } + } if (p.cancel.delta_time < 1.0) { RCLCPP_WARN_STREAM( logger_, "cancel.delta_time: " << p.cancel.delta_time 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 d1de12a3bd5f9..71ba8c51ec6ad 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 @@ -125,7 +125,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() const auto found_extended_path = extendPath(); if (found_extended_path) { - *output.path = utils::lane_change::combineReferencePath(*output.path, *found_extended_path); + *output.path = utils::combinePath(*output.path, *found_extended_path); } extendOutputDrivableArea(output); output.reference_path = std::make_shared(getReferencePath()); @@ -674,12 +674,25 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( const auto current_polygon = utils::lane_change::createPolygon(current_lanes, 0.0, std::numeric_limits::max()); - const auto target_polygon = - utils::lane_change::createPolygon(target_lanes, 0.0, std::numeric_limits::max()); + const auto expanded_target_lanes = utils::lane_change::generateExpandedLanelets( + target_lanes, direction_, lane_change_parameters_->lane_expansion_left_offset, + lane_change_parameters_->lane_expansion_right_offset); + const auto target_polygon = utils::lane_change::createPolygon( + expanded_target_lanes, 0.0, std::numeric_limits::max()); const auto dist_ego_to_current_lanes_center = lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose); std::vector> target_backward_polygons; for (const auto & target_backward_lane : target_backward_lanes) { + // Check to see is target_backward_lane is in current_lanes + // Without this check, current lane object might be treated as target lane object + const auto is_current_lane = [&](const lanelet::ConstLanelet & current_lane) { + return current_lane.id() == target_backward_lane.id(); + }; + + if (std::any_of(current_lanes.begin(), current_lanes.end(), is_current_lane)) { + continue; + } + lanelet::ConstLanelets lanelet{target_backward_lane}; auto lane_polygon = utils::lane_change::createPolygon(lanelet, 0.0, std::numeric_limits::max()); @@ -933,6 +946,9 @@ bool NormalLaneChange::getLaneChangePaths( const auto target_neighbor_preferred_lane_poly_2d = utils::lane_change::getTargetNeighborLanesPolygon(route_handler, current_lanes, type_); + if (target_neighbor_preferred_lane_poly_2d.empty()) { + return false; + } const auto target_objects = getTargetObjects(current_lanes, target_lanes); debug_filtered_objects_ = target_objects; @@ -1449,23 +1465,41 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( target_objects.other_lane.end()); } + const auto expanded_target_lanes = utils::lane_change::generateExpandedLanelets( + lane_change_path.info.target_lanes, direction_, + lane_change_parameters_->lane_expansion_left_offset, + lane_change_parameters_->lane_expansion_right_offset); + for (const auto & obj : collision_check_objects) { auto current_debug_data = marker_utils::createObjectDebug(obj); const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( obj, lane_change_parameters_->use_all_predicted_path); for (const auto & obj_path : obj_predicted_paths) { - if (!utils::path_safety_checker::checkCollision( - path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0, - current_debug_data.second)) { - path_safety_status.is_safe = false; - marker_utils::updateCollisionCheckDebugMap( - debug_data, current_debug_data, path_safety_status.is_safe); - const auto & obj_pose = obj.initial_pose.pose; - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, obj.shape); - path_safety_status.is_object_coming_from_rear |= - !utils::path_safety_checker::isTargetObjectFront( - path, current_pose, common_parameters.vehicle_info, obj_polygon); + const auto collided_polygons = utils::path_safety_checker::getCollidedPolygons( + path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0, + current_debug_data.second); + + if (collided_polygons.empty()) { + continue; } + + const auto collision_in_current_lanes = utils::lane_change::isCollidedPolygonsInLanelet( + collided_polygons, lane_change_path.info.current_lanes); + const auto collision_in_target_lanes = + utils::lane_change::isCollidedPolygonsInLanelet(collided_polygons, expanded_target_lanes); + + if (!collision_in_current_lanes && !collision_in_target_lanes) { + continue; + } + + path_safety_status.is_safe = false; + marker_utils::updateCollisionCheckDebugMap( + debug_data, current_debug_data, path_safety_status.is_safe); + const auto & obj_pose = obj.initial_pose.pose; + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, obj.shape); + path_safety_status.is_object_coming_from_rear |= + !utils::path_safety_checker::isTargetObjectFront( + path, current_pose, common_parameters.vehicle_info, obj_polygon); } marker_utils::updateCollisionCheckDebugMap( debug_data, current_debug_data, path_safety_status.is_safe); 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 c2df11b4bede6..c828a747856d8 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 @@ -20,6 +20,7 @@ #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 "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include #include @@ -96,6 +97,7 @@ void StartPlannerModule::onFreespacePlannerTimer() BehaviorModuleOutput StartPlannerModule::run() { + updateData(); if (!isActivated()) { return planWaitingApproval(); } @@ -120,6 +122,29 @@ void StartPlannerModule::processOnExit() debug_marker_.markers.clear(); } +void StartPlannerModule::updateData() +{ + if (isBackwardDrivingComplete()) { + updateStatusAfterBackwardDriving(); + } else { + status_.backward_driving_complete = false; + } + + const bool has_received_new_route = + !planner_data_->prev_route_id || + *planner_data_->prev_route_id != planner_data_->route_handler->getRouteUuid(); + + if (has_received_new_route) { + status_ = PullOutStatus(); + } + // check safety status after back finished + if (parameters_->safety_check_params.enable_safety_check && status_.back_finished) { + status_.is_safe_dynamic_objects = isSafePath(); + } else { + status_.is_safe_dynamic_objects = true; + } +} + bool StartPlannerModule::isExecutionRequested() const { const Pose & current_pose = planner_data_->self_odometry->pose.pose; @@ -161,26 +186,35 @@ bool StartPlannerModule::isExecutionRequested() const bool StartPlannerModule::isExecutionReady() const { + // when is_safe_static_objects is false,the path is not generated and approval shouldn't be + // allowed if (!status_.is_safe_static_objects) { + RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against 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 ( + parameters_->safety_check_params.enable_safety_check && status_.back_finished && + isWaitingApproval()) { if (!isSafePath()) { RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against dynamic objects"); + stop_pose_ = planner_data_->self_odometry->pose.pose; return false; } } return true; } -ModuleStatus StartPlannerModule::updateState() +void StartPlannerModule::updateCurrentState() { - RCLCPP_DEBUG(getLogger(), "START_PLANNER updateState"); + RCLCPP_DEBUG(getLogger(), "START_PLANNER updateCurrentState"); + + const auto print = [this](const auto & from, const auto & to) { + RCLCPP_DEBUG(getLogger(), "[start_planner] Transit from %s to %s.", from.data(), to.data()); + }; + + const auto & from = current_state_; + // current_state_ = updateState(); if (isActivated() && !isWaitingApproval()) { current_state_ = ModuleStatus::RUNNING; @@ -188,16 +222,16 @@ ModuleStatus StartPlannerModule::updateState() current_state_ = ModuleStatus::IDLE; } + // TODO(someone): move to canTransitSuccessState if (hasFinishedPullOut()) { - return ModuleStatus::SUCCESS; + current_state_ = ModuleStatus::SUCCESS; } - - if (isBackwardDrivingComplete()) { - updateStatusAfterBackwardDriving(); - return ModuleStatus::SUCCESS; // for breaking loop + // TODO(someone): move to canTransitSuccessState + if (status_.backward_driving_complete) { + current_state_ = ModuleStatus::SUCCESS; // for breaking loop } - return current_state_; + print(magic_enum::enum_name(from), magic_enum::enum_name(current_state_)); } BehaviorModuleOutput StartPlannerModule::plan() @@ -215,8 +249,6 @@ BehaviorModuleOutput StartPlannerModule::plan() clearWaitingApproval(); resetPathCandidate(); resetPathReference(); - // save current_pose when approved for start_point of turn_signal for backward driving - last_approved_pose_ = std::make_unique(planner_data_->self_odometry->pose.pose); } BehaviorModuleOutput output; @@ -230,12 +262,42 @@ BehaviorModuleOutput StartPlannerModule::plan() } PathWithLaneId path; + + // Check if backward motion is finished if (status_.back_finished) { + // Increment path index if the current path is finished if (hasFinishedCurrentPath()) { RCLCPP_INFO(getLogger(), "Increment path index"); incrementPathIndex(); } - path = getCurrentPath(); + + if (!status_.is_safe_dynamic_objects && !isWaitingApproval() && !status_.has_stop_point) { + auto current_path = getCurrentPath(); + const auto stop_path = + behavior_path_planner::utils::start_goal_planner_common::generateFeasibleStopPath( + current_path, planner_data_, *stop_pose_, parameters_->maximum_deceleration_for_stop, + parameters_->maximum_jerk_for_stop); + + // Insert stop point in the path if needed + if (stop_path) { + RCLCPP_ERROR_THROTTLE( + getLogger(), *clock_, 5000, "Insert stop point in the path because of dynamic objects"); + path = *stop_path; + status_.prev_stop_path_after_approval = std::make_shared(path); + status_.has_stop_point = true; + } else { + path = current_path; + } + } else if (!isWaitingApproval() && status_.has_stop_point) { + // Delete stop point if conditions are met + if (status_.is_safe_dynamic_objects && isStopped()) { + status_.has_stop_point = false; + path = getCurrentPath(); + } + path = *status_.prev_stop_path_after_approval; + } else { + path = getCurrentPath(); + } } else { path = status_.backward_path; } @@ -616,10 +678,6 @@ void StartPlannerModule::updatePullOutStatus() !planner_data_->prev_route_id || *planner_data_->prev_route_id != planner_data_->route_handler->getRouteUuid(); - if (has_received_new_route) { - status_ = PullOutStatus(); - } - // save pull out lanes which is generated using current pose before starting pull out // (before approval) status_.pull_out_lanes = start_planner_utils::getPullOutLanes( @@ -674,6 +732,7 @@ void StartPlannerModule::updatePullOutStatus() void StartPlannerModule::updateStatusAfterBackwardDriving() { status_.back_finished = true; + status_.backward_driving_complete = true; // request start_planner approval waitApproval(); // To enable approval of the forward path, the RTC status is removed. @@ -714,7 +773,8 @@ std::vector StartPlannerModule::searchPullOutStartPoses( // filter pull out lanes stop objects const auto [pull_out_lane_objects, others] = utils::path_safety_checker::separateObjectsByLanelets( - *planner_data_->dynamic_object, status_.pull_out_lanes); + *planner_data_->dynamic_object, status_.pull_out_lanes, + utils::path_safety_checker::isPolygonOverlapLanelet); const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( pull_out_lane_objects, parameters_->th_moving_object_velocity); @@ -886,7 +946,7 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const // turn on hazard light when backward driving if (!status_.back_finished) { turn_signal.hazard_signal.command = HazardLightsCommand::ENABLE; - const auto back_start_pose = isWaitingApproval() ? current_pose : *last_approved_pose_; + const auto back_start_pose = planner_data_->route_handler->getOriginalStartPose(); turn_signal.desired_start_point = back_start_pose; turn_signal.required_start_point = back_start_pose; // pull_out start_pose is same to backward driving end_pose diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index b2b4ecd2ea42a..0e7eb87feed9b 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -564,25 +564,6 @@ double getLongitudinalVelocity(const Pose & p_ref, const Pose & p_target, const return v * std::cos(calcYawDeviation(p_ref, p_target)); } -bool isCentroidWithinLanelets( - const PredictedObject & object, const lanelet::ConstLanelets & target_lanelets) -{ - if (target_lanelets.empty()) { - return false; - } - - const auto & object_pos = object.kinematics.initial_pose_with_covariance.pose.position; - lanelet::BasicPoint2d object_centroid(object_pos.x, object_pos.y); - - for (const auto & llt : target_lanelets) { - if (boost::geometry::within(object_centroid, llt.polygon2d().basicPolygon())) { - return true; - } - } - - return false; -} - lanelet::ConstLanelets getTargetLanelets( const std::shared_ptr & planner_data, lanelet::ConstLanelets & route_lanelets, const double left_offset, const double right_offset) @@ -1476,68 +1457,6 @@ AvoidLineArray combineRawShiftLinesWithUniqueCheck( return combined; } -std::vector convertToPredictedPath( - const PathWithLaneId & path, const std::shared_ptr & planner_data, - const bool is_object_front, const bool limit_to_max_velocity, - const std::shared_ptr & parameters) -{ - const auto & vehicle_pose = planner_data->self_odometry->pose.pose; - const auto & initial_velocity = std::abs(planner_data->self_odometry->twist.twist.linear.x); - const size_t ego_seg_idx = planner_data->findEgoSegmentIndex(path.points); - - auto ego_predicted_path_params = - std::make_shared(); - - ego_predicted_path_params->min_velocity = parameters->min_slow_down_speed; - ego_predicted_path_params->acceleration = parameters->max_acceleration; - ego_predicted_path_params->max_velocity = std::numeric_limits::infinity(); - ego_predicted_path_params->time_horizon_for_front_object = - parameters->time_horizon_for_front_object; - ego_predicted_path_params->time_horizon_for_rear_object = - parameters->time_horizon_for_rear_object; - ego_predicted_path_params->time_resolution = parameters->safety_check_time_resolution; - ego_predicted_path_params->delay_until_departure = 0.0; - - return behavior_path_planner::utils::path_safety_checker::createPredictedPath( - ego_predicted_path_params, path.points, vehicle_pose, initial_velocity, ego_seg_idx, - is_object_front, limit_to_max_velocity); -} - -ExtendedPredictedObject transform( - const PredictedObject & object, const std::shared_ptr & parameters) -{ - ExtendedPredictedObject extended_object; - extended_object.uuid = object.object_id; - extended_object.initial_pose = object.kinematics.initial_pose_with_covariance; - extended_object.initial_twist = object.kinematics.initial_twist_with_covariance; - extended_object.initial_acceleration = object.kinematics.initial_acceleration_with_covariance; - extended_object.shape = object.shape; - - const auto & obj_velocity_norm = std::hypot( - extended_object.initial_twist.twist.linear.x, extended_object.initial_twist.twist.linear.y); - const auto & time_horizon = - std::max(parameters->time_horizon_for_front_object, parameters->time_horizon_for_rear_object); - const auto & time_resolution = parameters->safety_check_time_resolution; - - extended_object.predicted_paths.resize(object.kinematics.predicted_paths.size()); - for (size_t i = 0; i < object.kinematics.predicted_paths.size(); ++i) { - const auto & path = object.kinematics.predicted_paths.at(i); - extended_object.predicted_paths.at(i).confidence = path.confidence; - - // create path - for (double t = 0.0; t < time_horizon + 1e-3; t += time_resolution) { - const auto obj_pose = object_recognition_utils::calcInterpolatedPose(path, t); - if (obj_pose) { - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(*obj_pose, object.shape); - extended_object.predicted_paths.at(i).path.emplace_back( - t, *obj_pose, obj_velocity_norm, obj_polygon); - } - } - } - - return extended_object; -} - lanelet::ConstLanelets getAdjacentLane( const std::shared_ptr & planner_data, const std::shared_ptr & parameters, const bool is_right_shift) @@ -1591,14 +1510,25 @@ std::vector getSafetyCheckTargetObjects( std::vector target_objects; - const auto append_target_objects = [&](const auto & check_lanes, const auto & objects) { - std::for_each(objects.begin(), objects.end(), [&](const auto & object) { - if (isCentroidWithinLanelets(object.object, check_lanes)) { - target_objects.push_back(utils::avoidance::transform(object.object, p)); - } + const auto time_horizon = std::max( + parameters->ego_predicted_path_params.time_horizon_for_front_object, + parameters->ego_predicted_path_params.time_horizon_for_rear_object); + + const auto append = [&](const auto & objects) { + std::for_each(objects.objects.begin(), objects.objects.end(), [&](const auto & object) { + target_objects.push_back(utils::path_safety_checker::transform( + object, time_horizon, parameters->ego_predicted_path_params.time_resolution)); }); }; + const auto to_predicted_objects = [&p](const auto & objects) { + PredictedObjects ret{}; + std::for_each(objects.begin(), objects.end(), [&p, &ret](const auto & object) { + ret.objects.push_back(object.object); + }); + return ret; + }; + const auto unavoidable_objects = [&data]() { ObjectDataArray ret; std::for_each(data.target_objects.begin(), data.target_objects.end(), [&](const auto & object) { @@ -1614,11 +1544,17 @@ std::vector getSafetyCheckTargetObjects( const auto check_lanes = getAdjacentLane(planner_data, p, true); if (p->check_other_object) { - append_target_objects(check_lanes, data.other_objects); + const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( + to_predicted_objects(data.other_objects), check_lanes, + utils::path_safety_checker::isCentroidWithinLanelet); + append(targets); } if (p->check_unavoidable_object) { - append_target_objects(check_lanes, unavoidable_objects); + const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( + to_predicted_objects(unavoidable_objects), check_lanes, + utils::path_safety_checker::isCentroidWithinLanelet); + append(targets); } } @@ -1627,11 +1563,17 @@ std::vector getSafetyCheckTargetObjects( const auto check_lanes = getAdjacentLane(planner_data, p, false); if (p->check_other_object) { - append_target_objects(check_lanes, data.other_objects); + const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( + to_predicted_objects(data.other_objects), check_lanes, + utils::path_safety_checker::isCentroidWithinLanelet); + append(targets); } if (p->check_unavoidable_object) { - append_target_objects(check_lanes, unavoidable_objects); + const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( + to_predicted_objects(unavoidable_objects), check_lanes, + utils::path_safety_checker::isCentroidWithinLanelet); + append(targets); } } @@ -1640,11 +1582,17 @@ std::vector getSafetyCheckTargetObjects( const auto check_lanes = data.current_lanelets; if (p->check_other_object) { - append_target_objects(check_lanes, data.other_objects); + const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( + to_predicted_objects(data.other_objects), check_lanes, + utils::path_safety_checker::isCentroidWithinLanelet); + append(targets); } if (p->check_unavoidable_object) { - append_target_objects(check_lanes, unavoidable_objects); + const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( + to_predicted_objects(unavoidable_objects), check_lanes, + utils::path_safety_checker::isCentroidWithinLanelet); + append(targets); } } 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 d0673d8bb9ea8..60fb2ba2ff2e8 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 @@ -60,7 +60,7 @@ std::vector crop_and_resample( // 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, + points, crop_pose.position, crop_seg_idx, planner_data->drivable_area_expansion_parameters.max_path_arc_length, 0.0); planner_data->drivable_area_expansion_prev_crop_pose = crop_pose; // resample 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 c1d24f02ac140..12044980ebd81 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 @@ -21,6 +21,7 @@ #include "lanelet2_extension/regulatory_elements/no_stopping_area.hpp" #include "lanelet2_extension/utility/utilities.hpp" #include "motion_utils/trajectory/path_with_lane_id.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include #include @@ -41,8 +42,20 @@ using tier4_autoware_utils::inverseTransformPose; // Sort with smaller longitudinal distances taking precedence over smaller lateral distances. struct SortByLongitudinalDistance { + bool prioritize_goals_before_objects{false}; + explicit SortByLongitudinalDistance(bool prioritize_goals_before_objects) + : prioritize_goals_before_objects(prioritize_goals_before_objects) + { + } + bool operator()(const GoalCandidate & a, const GoalCandidate & b) const noexcept { + if (prioritize_goals_before_objects) { + if (a.num_objects_to_avoid != b.num_objects_to_avoid) { + return a.num_objects_to_avoid < b.num_objects_to_avoid; + } + } + const double diff = a.distance_from_original_goal - b.distance_from_original_goal; constexpr double eps = 0.01; // If the longitudinal distances are approximately equal, sort based on lateral offset. @@ -58,10 +71,21 @@ struct SortByLongitudinalDistance struct SortByWeightedDistance { double lateral_cost{0.0}; - explicit SortByWeightedDistance(double cost) : lateral_cost(cost) {} + bool prioritize_goals_before_objects{false}; + + SortByWeightedDistance(double cost, bool prioritize_goals_before_objects) + : lateral_cost(cost), prioritize_goals_before_objects(prioritize_goals_before_objects) + { + } bool operator()(const GoalCandidate & a, const GoalCandidate & b) const noexcept { + if (prioritize_goals_before_objects) { + if (a.num_objects_to_avoid != b.num_objects_to_avoid) { + return a.num_objects_to_avoid < b.num_objects_to_avoid; + } + } + return a.distance_from_original_goal + lateral_cost * a.lateral_offset < b.distance_from_original_goal + lateral_cost * b.lateral_offset; } @@ -77,7 +101,7 @@ GoalSearcher::GoalSearcher( { } -GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) +GoalCandidates GoalSearcher::search() { GoalCandidates goal_candidates{}; @@ -101,7 +125,7 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) lanes.insert(lanes.end(), pull_over_lanes.begin(), pull_over_lanes.end()); const auto goal_arc_coords = - lanelet::utils::getArcCoordinates(pull_over_lanes, original_goal_pose); + lanelet::utils::getArcCoordinates(pull_over_lanes, reference_goal_pose_); const double s_start = std::max(0.0, goal_arc_coords.length - backward_length); const double s_end = goal_arc_coords.length + forward_length; auto center_line_path = utils::resamplePathWithSpline( @@ -131,10 +155,11 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) const double sign = left_side_parking_ ? -1.0 : 1.0; const double offset_from_center_line = -distance_from_bound.value() + sign * margin_from_boundary; + // original means non lateral offset poses 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( - center_line_path.points, original_goal_pose.position, original_search_pose.position)); + center_line_path.points, reference_goal_pose_.position, original_search_pose.position)); original_search_poses.push_back(original_search_pose); // for createAreaPolygon Pose search_pose{}; // search goal_pose in lateral direction @@ -170,38 +195,113 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) } createAreaPolygons(original_search_poses); - if (parameters_.goal_priority == "minimum_weighted_distance") { - std::sort( - goal_candidates.begin(), goal_candidates.end(), - SortByWeightedDistance(parameters_.minimum_weighted_distance_lateral_weight)); - } else if (parameters_.goal_priority == "minimum_longitudinal_distance") { - std::sort(goal_candidates.begin(), goal_candidates.end(), SortByLongitudinalDistance()); - } + update(goal_candidates); return goal_candidates; } +void GoalSearcher::countObjectsToAvoid( + GoalCandidates & goal_candidates, const PredictedObjects & objects) const +{ + const auto & route_handler = planner_data_->route_handler; + const double forward_length = parameters_.forward_goal_search_length; + const double backward_length = parameters_.backward_goal_search_length; + + // calculate search start/end pose in pull over lanes + const auto [search_start_pose, search_end_pose] = std::invoke([&]() -> std::pair { + 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 auto goal_arc_coords = + lanelet::utils::getArcCoordinates(pull_over_lanes, reference_goal_pose_); + const double s_start = std::max(0.0, goal_arc_coords.length - backward_length); + const double s_end = goal_arc_coords.length + forward_length; + const auto center_line_path = utils::resamplePathWithSpline( + route_handler->getCenterLinePath(pull_over_lanes, s_start, s_end), + parameters_.goal_search_interval); + return std::make_pair( + center_line_path.points.front().point.pose, center_line_path.points.back().point.pose); + }); + + // generate current lane center line path to check collision with objects + const auto current_lanes = utils::getExtendedCurrentLanes( + planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, + /*forward_only_in_route*/ false); + const auto current_center_line_path = std::invoke([&]() -> PathWithLaneId { + const double s_start = + lanelet::utils::getArcCoordinates(current_lanes, search_start_pose).length; + const double s_end = lanelet::utils::getArcCoordinates(current_lanes, search_end_pose).length; + return utils::resamplePathWithSpline( + route_handler->getCenterLinePath(current_lanes, s_start, s_end), 1.0); + }); + + // reset num_objects_to_avoid + for (auto & goal_candidate : goal_candidates) { + goal_candidate.num_objects_to_avoid = 0; + } + + // count number of objects to avoid + for (const auto & object : objects.objects) { + for (const auto & p : current_center_line_path.points) { + const auto transformed_vehicle_footprint = + transformVector(vehicle_footprint_, tier4_autoware_utils::pose2transform(p.point.pose)); + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); + const double distance = boost::geometry::distance(obj_polygon, transformed_vehicle_footprint); + if (distance > parameters_.object_recognition_collision_check_margin) { + continue; + } + const Pose & object_pose = object.kinematics.initial_pose_with_covariance.pose; + const double s_object = lanelet::utils::getArcCoordinates(current_lanes, object_pose).length; + for (auto & goal_candidate : goal_candidates) { + const Pose & goal_pose = goal_candidate.goal_pose; + const double s_goal = lanelet::utils::getArcCoordinates(current_lanes, goal_pose).length; + if (s_object < s_goal) { + goal_candidate.num_objects_to_avoid++; + } + } + break; + } + } +} + void GoalSearcher::update(GoalCandidates & goal_candidates) const { + const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity( + *(planner_data_->dynamic_object), parameters_.th_moving_object_velocity); + 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 [pull_over_lane_stop_objects, others] = + utils::path_safety_checker::separateObjectsByLanelets( + stop_objects, pull_over_lanes, utils::path_safety_checker::isPolygonOverlapLanelet); + + if (parameters_.prioritize_goals_before_objects) { + countObjectsToAvoid(goal_candidates, pull_over_lane_stop_objects); + } + + if (parameters_.goal_priority == "minimum_weighted_distance") { + std::sort( + goal_candidates.begin(), goal_candidates.end(), + SortByWeightedDistance( + parameters_.minimum_weighted_distance_lateral_weight, + parameters_.prioritize_goals_before_objects)); + } else if (parameters_.goal_priority == "minimum_longitudinal_distance") { + std::sort( + goal_candidates.begin(), goal_candidates.end(), + SortByLongitudinalDistance(parameters_.prioritize_goals_before_objects)); + } + // update is_safe for (auto & goal_candidate : goal_candidates) { const Pose goal_pose = goal_candidate.goal_pose; // check collision with footprint - if (checkCollision(goal_pose)) { + if (checkCollision(goal_pose, pull_over_lane_stop_objects)) { goal_candidate.is_safe = false; continue; } // check longitudinal margin with pull over lane objects - 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 [pull_over_lane_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets( - *(planner_data_->dynamic_object), pull_over_lanes); - const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( - pull_over_lane_objects, parameters_.th_moving_object_velocity); constexpr bool filter_inside = true; const auto target_objects = goal_planner_utils::filterObjectsByLateralDistance( goal_pose, planner_data_->parameters.vehicle_width, pull_over_lane_stop_objects, @@ -215,7 +315,7 @@ void GoalSearcher::update(GoalCandidates & goal_candidates) const } } -bool GoalSearcher::checkCollision(const Pose & pose) const +bool GoalSearcher::checkCollision(const Pose & pose, const PredictedObjects & objects) const { if (parameters_.use_occupancy_grid_for_goal_search) { const Pose pose_grid_coords = global2local(occupancy_grid_map_->getMap(), pose); @@ -228,16 +328,8 @@ bool GoalSearcher::checkCollision(const Pose & pose) const } if (parameters_.use_object_recognition) { - 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 [pull_over_lane_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets( - *(planner_data_->dynamic_object), pull_over_lanes); - const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( - pull_over_lane_objects, parameters_.th_moving_object_velocity); if (utils::checkCollisionBetweenFootprintAndObjects( - vehicle_footprint_, pose, pull_over_lane_stop_objects, + vehicle_footprint_, pose, objects, parameters_.object_recognition_collision_check_margin)) { return true; } @@ -246,7 +338,7 @@ bool GoalSearcher::checkCollision(const Pose & pose) const } bool GoalSearcher::checkCollisionWithLongitudinalDistance( - const Pose & ego_pose, const PredictedObjects & dynamic_objects) const + const Pose & ego_pose, const PredictedObjects & objects) const { if ( parameters_.use_occupancy_grid_for_goal_search && @@ -282,8 +374,7 @@ bool GoalSearcher::checkCollisionWithLongitudinalDistance( if ( utils::calcLongitudinalDistanceFromEgoToObjects( ego_pose, planner_data_->parameters.base_link2front, - planner_data_->parameters.base_link2rear, - dynamic_objects) < parameters_.longitudinal_margin) { + planner_data_->parameters.base_link2rear, objects) < parameters_.longitudinal_margin) { return true; } } 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 4f24d1b682f3d..ff26c7a3654c3 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp @@ -44,16 +44,6 @@ namespace behavior_path_planner { namespace goal_planner_utils { -PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2) -{ - PathWithLaneId path; - path.points.insert(path.points.end(), path1.points.begin(), path1.points.end()); - - // skip overlapping point - path.points.insert(path.points.end(), next(path2.points.begin()), path2.points.end()); - - return path; -} lanelet::ConstLanelets getPullOverLanes( const RouteHandler & route_handler, const bool left_side, const double backward_distance, @@ -141,7 +131,7 @@ MarkerArray createPosesMarkerArray( return msg; } -MarkerArray createTextsMarkerArray( +MarkerArray createGoalPriorityTextsMarkerArray( const std::vector & poses, std::string && ns, const std_msgs::msg::ColorRGBA & color) { MarkerArray msg{}; @@ -160,24 +150,52 @@ MarkerArray createTextsMarkerArray( return msg; } +MarkerArray createNumObjectsToAvoidTextsMarkerArray( + const GoalCandidates & goal_candidates, std::string && ns, const std_msgs::msg::ColorRGBA & color) +{ + MarkerArray msg{}; + int32_t i = 0; + for (const auto & goal_candidate : goal_candidates) { + const Pose & pose = goal_candidate.goal_pose; + Marker marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, i, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.3, 0.3, 0.3), color); + marker.pose = calcOffsetPose(pose, -0.5, 0, 1.0); + marker.id = i; + marker.text = std::to_string(goal_candidate.num_objects_to_avoid); + msg.markers.push_back(marker); + i++; + } + + return msg; +} + MarkerArray createGoalCandidatesMarkerArray( GoalCandidates & goal_candidates, const std_msgs::msg::ColorRGBA & color) { - // convert to pose vector + GoalCandidates safe_goal_candidates{}; + std::copy_if( + goal_candidates.begin(), goal_candidates.end(), std::back_inserter(safe_goal_candidates), + [](const auto & goal_candidate) { return goal_candidate.is_safe; }); + std::vector pose_vector{}; - for (const auto & goal_candidate : goal_candidates) { - if (goal_candidate.is_safe) { - pose_vector.push_back(goal_candidate.goal_pose); - } - } + std::transform( + safe_goal_candidates.begin(), safe_goal_candidates.end(), std::back_inserter(pose_vector), + [](const auto & goal_candidate) { return goal_candidate.goal_pose; }); auto marker_array = createPosesMarkerArray(pose_vector, "goal_candidates", color); for (const auto & text_marker : - createTextsMarkerArray( + createGoalPriorityTextsMarkerArray( pose_vector, "goal_candidates_priority", createMarkerColor(1.0, 1.0, 1.0, 0.999)) .markers) { marker_array.markers.push_back(text_marker); } + for (const auto & text_marker : createNumObjectsToAvoidTextsMarkerArray( + safe_goal_candidates, "goal_candidates_num_objects_to_avoid", + createMarkerColor(0.5, 0.5, 0.5, 0.999)) + .markers) { + marker_array.markers.push_back(text_marker); + } return marker_array; } 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 e4fbf41e5af00..038cf9488345f 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -180,17 +180,6 @@ std::vector getAccelerationValues( return sampled_values; } -PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2) -{ - PathWithLaneId path; - path.points.insert(path.points.end(), path1.points.begin(), path1.points.end()); - - // skip overlapping point - path.points.insert(path.points.end(), next(path2.points.begin()), path2.points.end()); - - return path; -} - lanelet::ConstLanelets getTargetPreferredLanes( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const Direction & direction, @@ -247,6 +236,9 @@ lanelet::BasicPolygon2d getTargetNeighborLanesPolygon( { const auto target_neighbor_lanelets = utils::lane_change::getTargetNeighborLanes(route_handler, current_lanes, type); + if (target_neighbor_lanelets.empty()) { + return {}; + } const auto target_neighbor_preferred_lane_poly = lanelet::utils::getPolygonFromArcLength( target_neighbor_lanelets, 0, std::numeric_limits::max()); @@ -355,7 +347,7 @@ std::optional constructCandidatePath( return std::nullopt; } - candidate_path.path = combineReferencePath(prepare_segment, shifted_path.path); + candidate_path.path = utils::combinePath(prepare_segment, shifted_path.path); candidate_path.shifted_path = shifted_path; return std::optional{candidate_path}; @@ -1103,4 +1095,25 @@ ExtendedPredictedObject transform( return extended_object; } + +bool isCollidedPolygonsInLanelet( + const std::vector & collided_polygons, const lanelet::ConstLanelets & lanes) +{ + const auto lanes_polygon = createPolygon(lanes, 0.0, std::numeric_limits::max()); + + const auto is_in_lanes = [&](const auto & collided_polygon) { + return lanes_polygon && boost::geometry::intersects(lanes_polygon.value(), collided_polygon); + }; + + return std::any_of(collided_polygons.begin(), collided_polygons.end(), is_in_lanes); +} + +lanelet::ConstLanelets generateExpandedLanelets( + const lanelet::ConstLanelets & lanes, const Direction direction, const double left_offset, + const double right_offset) +{ + const auto left_extend_offset = (direction == Direction::LEFT) ? left_offset : 0.0; + const auto right_extend_offset = (direction == Direction::RIGHT) ? -right_offset : 0.0; + return lanelet::utils::getExpandedLanelets(lanes, left_extend_offset, right_extend_offset); +} } // namespace behavior_path_planner::utils::lane_change 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 ff54ce2d2dcc4..dfc9f76b25e33 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 @@ -24,6 +24,20 @@ namespace behavior_path_planner::utils::path_safety_checker { +bool isCentroidWithinLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet) +{ + const auto & object_pos = object.kinematics.initial_pose_with_covariance.pose.position; + lanelet::BasicPoint2d object_centroid(object_pos.x, object_pos.y); + return boost::geometry::within(object_centroid, lanelet.polygon2d().basicPolygon()); +} + +bool isPolygonOverlapLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet) +{ + const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); + const auto lanelet_polygon = utils::toPolygon2d(lanelet); + return !boost::geometry::disjoint(lanelet_polygon, object_polygon); +} + PredictedObjects filterObjects( const std::shared_ptr & objects, const std::shared_ptr & route_handler, const lanelet::ConstLanelets & current_lanes, @@ -128,7 +142,8 @@ void filterObjectsByClass( } std::pair, std::vector> separateObjectIndicesByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets) + const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets, + const std::function & condition) { if (target_lanelets.empty()) { return {}; @@ -138,25 +153,9 @@ std::pair, std::vector> separateObjectIndicesByLanel std::vector other_indices; for (size_t i = 0; i < objects.objects.size(); i++) { - // create object polygon - const auto & obj = objects.objects.at(i); - // create object polygon - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj); bool is_filtered_object = false; for (const auto & llt : target_lanelets) { - // create lanelet polygon - const auto polygon2d = llt.polygon2d().basicPolygon(); - if (polygon2d.empty()) { - // no lanelet polygon - continue; - } - Polygon2d lanelet_polygon; - for (const auto & lanelet_point : polygon2d) { - lanelet_polygon.outer().emplace_back(lanelet_point.x(), lanelet_point.y()); - } - lanelet_polygon.outer().push_back(lanelet_polygon.outer().front()); - // check the object does not intersect the lanelet - if (!boost::geometry::disjoint(lanelet_polygon, obj_polygon)) { + if (condition(objects.objects.at(i), llt)) { target_indices.push_back(i); is_filtered_object = true; break; @@ -172,13 +171,14 @@ std::pair, std::vector> separateObjectIndicesByLanel } std::pair separateObjectsByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets) + const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets, + const std::function & condition) { PredictedObjects target_objects; PredictedObjects other_objects; const auto [target_indices, other_indices] = - separateObjectIndicesByLanelets(objects, target_lanelets); + separateObjectIndicesByLanelets(objects, target_lanelets, condition); target_objects.objects.reserve(target_indices.size()); other_objects.objects.reserve(other_indices.size()); 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 9fb7d0acdf432..b44a3f841035d 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 @@ -258,6 +258,20 @@ boost::optional getInterpolatedPoseWithVeloci } bool checkCollision( + const PathWithLaneId & planned_path, + const std::vector & predicted_ego_path, + const ExtendedPredictedObject & target_object, + const PredictedPathWithPolygon & target_object_path, + const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, + const double hysteresis_factor, CollisionCheckDebug & debug) +{ + const auto collided_polygons = getCollidedPolygons( + planned_path, predicted_ego_path, target_object, target_object_path, common_parameters, + rss_parameters, hysteresis_factor, debug); + return collided_polygons.empty(); +} + +std::vector getCollidedPolygons( [[maybe_unused]] const PathWithLaneId & planned_path, const std::vector & predicted_ego_path, const ExtendedPredictedObject & target_object, @@ -271,6 +285,8 @@ bool checkCollision( debug.current_obj_pose = target_object.initial_pose.pose; } + std::vector collided_polygons{}; + collided_polygons.reserve(target_object_path.path.size()); for (const auto & obj_pose_with_poly : target_object_path.path) { const auto & current_time = obj_pose_with_poly.time; @@ -302,7 +318,8 @@ bool checkCollision( // check overlap if (boost::geometry::overlaps(ego_polygon, obj_polygon)) { debug.unsafe_reason = "overlap_polygon"; - return false; + collided_polygons.push_back(obj_polygon); + continue; } // compute which one is at the front of the other @@ -341,34 +358,55 @@ bool checkCollision( // check overlap with extended polygon if (boost::geometry::overlaps(extended_ego_polygon, extended_obj_polygon)) { debug.unsafe_reason = "overlap_extended_polygon"; - return false; + collided_polygons.push_back(obj_polygon); } } - return true; + return collided_polygons; } -bool checkCollisionWithExtraStoppingMargin( - const PathWithLaneId & ego_path, const PredictedObjects & dynamic_objects, - const double base_to_front, const double base_to_rear, const double width, - const double maximum_deceleration, const double collision_check_margin, - const double max_extra_stopping_margin) +std::vector generatePolygonsWithStoppingAndInertialMargin( + const PathWithLaneId & ego_path, const double base_to_front, const double base_to_rear, + const double width, const double maximum_deceleration, const double max_extra_stopping_margin) { - for (const auto & p : ego_path.points) { + std::vector polygons; + const auto curvatures = motion_utils::calcCurvature(ego_path.points); + + for (size_t i = 0; i < ego_path.points.size(); ++i) { + const auto p = ego_path.points.at(i); + const double extra_stopping_margin = std::min( std::pow(p.point.longitudinal_velocity_mps, 2) * 0.5 / maximum_deceleration, max_extra_stopping_margin); + double extra_lateral_margin = (-1) * curvatures[i] * p.point.longitudinal_velocity_mps * + std::abs(p.point.longitudinal_velocity_mps); + extra_lateral_margin = + std::clamp(extra_lateral_margin, -extra_stopping_margin, extra_stopping_margin); + + const auto lateral_offset_pose = + tier4_autoware_utils::calcOffsetPose(p.point.pose, 0.0, extra_lateral_margin / 2.0, 0.0); const auto ego_polygon = tier4_autoware_utils::toFootprint( - p.point.pose, base_to_front + extra_stopping_margin, base_to_rear, width); + lateral_offset_pose, base_to_front + extra_stopping_margin, base_to_rear, + width + std::abs(extra_lateral_margin)); + polygons.push_back(ego_polygon); + } + return polygons; +} +bool checkCollisionWithMargin( + const std::vector & ego_polygons, const PredictedObjects & dynamic_objects, + const double collision_check_margin) +{ + for (const auto & ego_polygon : ego_polygons) { for (const auto & object : dynamic_objects.objects) { const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); const double distance = boost::geometry::distance(obj_polygon, ego_polygon); - if (distance < collision_check_margin) return true; + if (distance < collision_check_margin) { + return true; + } } } - return false; } } // namespace behavior_path_planner::utils::path_safety_checker diff --git a/planning/behavior_path_planner/src/utils/path_utils.cpp b/planning/behavior_path_planner/src/utils/path_utils.cpp index 7673c9a1de9b4..509b31ad3da2b 100644 --- a/planning/behavior_path_planner/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner/src/utils/path_utils.cpp @@ -561,4 +561,25 @@ PathWithLaneId calcCenterLinePath( return centerline_path; } + +PathWithLaneId combinePath(const PathWithLaneId & path1, const PathWithLaneId & path2) +{ + if (path1.points.empty()) { + return path2; + } + if (path2.points.empty()) { + return path1; + } + + PathWithLaneId path{}; + path.points.insert(path.points.end(), path1.points.begin(), path1.points.end()); + + // skip overlapping point + path.points.insert(path.points.end(), next(path2.points.begin()), path2.points.end()); + + PathWithLaneId filtered_path = path; + filtered_path.points = motion_utils::removeOverlapPoints(filtered_path.points); + return filtered_path; +} + } // namespace behavior_path_planner::utils 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 8e5d6b7545c1d..945c386668dee 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 @@ -97,8 +97,7 @@ boost::optional FreespacePullOut::plan(const Pose & start_pose, con const auto road_center_line_path = route_handler->getCenterLinePath(road_lanes, s_start, s_end); last_path = utils::resamplePathWithSpline( - start_planner_utils::combineReferencePath(last_path, road_center_line_path), - parameters_.center_line_path_interval); + utils::combinePath(last_path, road_center_line_path), parameters_.center_line_path_interval); const double original_terminal_velocity = last_path.points.back().point.longitudinal_velocity_mps; utils::correctDividedPathVelocity(partial_paths); 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 5472885359c31..3cf42dbb5da26 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 @@ -15,8 +15,10 @@ #include "behavior_path_planner/utils/start_planner/geometric_pull_out.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_planner/util.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include @@ -26,7 +28,6 @@ using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::calcOffsetPose; namespace behavior_path_planner { -using start_planner_utils::combineReferencePath; using start_planner_utils::getPullOutLanes; GeometricPullOut::GeometricPullOut(rclcpp::Node & node, const StartPlannerParameters & parameters) @@ -65,7 +66,8 @@ boost::optional GeometricPullOut::plan(const Pose & start_pose, con 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); + utils::path_safety_checker::separateObjectsByLanelets( + stop_objects, pull_out_lanes, utils::path_safety_checker::isPolygonOverlapLanelet); if (utils::checkCollisionBetweenPathFootprintsAndObjects( vehicle_footprint_, arc_path, pull_out_lane_stop_objects, @@ -106,7 +108,7 @@ boost::optional GeometricPullOut::plan(const Pose & start_pose, con std::make_pair(velocity, velocity * velocity / (2 * arc_length_on_second_arc_path))); } else { const auto partial_paths = planner_.getPaths(); - const auto combined_path = combineReferencePath(partial_paths.at(0), partial_paths.at(1)); + const auto combined_path = utils::combinePath(partial_paths.at(0), partial_paths.at(1)); output.partial_paths.push_back(combined_path); // Calculate the acceleration required to reach the forward parking velocity at the center of 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 2de72d781994a..a430e18c330e1 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 @@ -20,6 +20,7 @@ #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/boost_polygon_utils.hpp" #include @@ -32,7 +33,6 @@ using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::calcOffsetPose; namespace behavior_path_planner { -using start_planner_utils::combineReferencePath; using start_planner_utils::getPullOutLanes; ShiftPullOut::ShiftPullOut( @@ -66,7 +66,8 @@ boost::optional ShiftPullOut::plan(const Pose & start_pose, const P // extract stop objects in pull out lane for collision check const auto [pull_out_lane_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets(*dynamic_objects, pull_out_lanes); + utils::path_safety_checker::separateObjectsByLanelets( + *dynamic_objects, pull_out_lanes, utils::path_safety_checker::isPolygonOverlapLanelet); const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( pull_out_lane_objects, parameters_.th_moving_object_velocity); 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 f5b4c9b979ee3..40b100a1bc070 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/util.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/util.cpp @@ -38,19 +38,6 @@ namespace behavior_path_planner::start_planner_utils { -PathWithLaneId combineReferencePath(const PathWithLaneId path1, const PathWithLaneId path2) -{ - PathWithLaneId path; - path.points.insert(path.points.end(), path1.points.begin(), path1.points.end()); - - // skip overlapping point - path.points.insert(path.points.end(), next(path2.points.begin()), path2.points.end()); - - PathWithLaneId filtered_path = path; - filtered_path.points = motion_utils::removeOverlapPoints(filtered_path.points); - return filtered_path; -} - PathWithLaneId getBackwardPath( const RouteHandler & route_handler, const lanelet::ConstLanelets & shoulder_lanes, const Pose & current_pose, const Pose & backed_pose, const double velocity) diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 1b14741db4a56..03063b5a9e2fe 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -1552,31 +1552,10 @@ void generateDrivableArea( } } -// calculate bounds from drivable lanes and hatched road markings -std::vector calcBound( - const std::shared_ptr route_handler, - const std::vector & drivable_lanes, - const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, - const bool is_left) +std::vector getBoundWithHatchedRoadMarkings( + const std::vector & original_bound, + const std::shared_ptr & route_handler) { - // a function to convert drivable lanes to points without duplicated points - const auto convert_to_points = [&](const std::vector & drivable_lanes) { - constexpr double overlap_threshold = 0.01; - - std::vector points; - for (const auto & drivable_lane : drivable_lanes) { - const auto bound = - is_left ? drivable_lane.left_lane.leftBound3d() : drivable_lane.right_lane.rightBound3d(); - for (const auto & point : bound) { - if ( - points.empty() || - overlap_threshold < (points.back().basicPoint2d() - point.basicPoint2d()).norm()) { - points.push_back(point); - } - } - } - return points; - }; // a function to get polygon with a designated point id const auto get_corresponding_polygon_index = [&](const auto & polygon, const auto & target_point_id) { @@ -1589,18 +1568,89 @@ std::vector calcBound( // This means calculation has some errors. return polygon.size() - 1; }; + const auto mod = [&](const int a, const int b) { return (a + b) % b; // NOTE: consider negative value }; + + std::vector expanded_bound{}; + + std::optional current_polygon{std::nullopt}; + std::vector current_polygon_border_indices; + // expand drivable area by hatched road markings. + for (size_t bound_point_idx = 0; bound_point_idx < original_bound.size(); ++bound_point_idx) { + const auto & bound_point = original_bound[bound_point_idx]; + const auto polygon = getPolygonByPoint(route_handler, bound_point, "hatched_road_markings"); + + bool will_close_polygon{false}; + if (!current_polygon) { + if (!polygon) { + expanded_bound.push_back(bound_point); + } else { + // There is a new additional polygon to expand + current_polygon = polygon; + current_polygon_border_indices.push_back( + get_corresponding_polygon_index(*current_polygon, bound_point.id())); + } + } else { + if (!polygon) { + will_close_polygon = true; + } else { + current_polygon_border_indices.push_back( + get_corresponding_polygon_index(*current_polygon, bound_point.id())); + } + } + + if (bound_point_idx == original_bound.size() - 1 && current_polygon) { + // If drivable lanes ends earlier than polygon, close the polygon + will_close_polygon = true; + } + + if (will_close_polygon) { + // The current additional polygon ends to expand + if (current_polygon_border_indices.size() == 1) { + expanded_bound.push_back((*current_polygon)[current_polygon_border_indices.front()]); + } else { + const size_t current_polygon_points_num = current_polygon->size(); + const bool is_polygon_opposite_direction = [&]() { + const size_t modulo_diff = mod( + static_cast(current_polygon_border_indices[1]) - + static_cast(current_polygon_border_indices[0]), + current_polygon_points_num); + return modulo_diff == 1; + }(); + + const int target_points_num = + current_polygon_points_num - current_polygon_border_indices.size() + 1; + for (int poly_idx = 0; poly_idx <= target_points_num; ++poly_idx) { + const int target_poly_idx = current_polygon_border_indices.front() + + poly_idx * (is_polygon_opposite_direction ? -1 : 1); + expanded_bound.push_back( + (*current_polygon)[mod(target_poly_idx, current_polygon_points_num)]); + } + } + current_polygon = std::nullopt; + current_polygon_border_indices.clear(); + } + } + + return expanded_bound; +} + +std::vector getBoundWithIntersectionAreas( + const std::vector & original_bound, + const std::shared_ptr & route_handler, + const std::vector & drivable_lanes, const bool is_left) +{ const auto extract_bound_from_polygon = [](const auto & polygon, const auto start_idx, const auto end_idx, const auto clockwise) { - std::vector ret; + std::vector ret{}; for (size_t i = start_idx; i != end_idx; i = clockwise ? i + 1 : i - 1) { - ret.push_back(lanelet::utils::conversion::toGeomMsgPt(polygon[i])); + ret.push_back(polygon[i]); if (i + 1 == polygon.size() && clockwise) { if (end_idx == 0) { - ret.push_back(lanelet::utils::conversion::toGeomMsgPt(polygon[end_idx])); + ret.push_back(polygon[end_idx]); return ret; } i = 0; @@ -1609,7 +1659,7 @@ std::vector calcBound( if (i == 0 && !clockwise) { if (end_idx == polygon.size() - 1) { - ret.push_back(lanelet::utils::conversion::toGeomMsgPt(polygon[end_idx])); + ret.push_back(polygon[end_idx]); return ret; } i = polygon.size() - 1; @@ -1617,34 +1667,30 @@ std::vector calcBound( } } + ret.push_back(polygon[end_idx]); + return ret; }; - // If no need to expand with polygons, return here. - std::vector output_points; - const auto bound_points = convert_to_points(drivable_lanes); - if (!enable_expanding_hatched_road_markings && !enable_expanding_intersection_areas) { - for (const auto & point : bound_points) { - output_points.push_back(lanelet::utils::conversion::toGeomMsgPt(point)); - } - return motion_utils::removeOverlapPoints(output_points); - } + std::vector expanded_bound = original_bound; - std::optional current_polygon{std::nullopt}; - std::vector current_polygon_border_indices; + // expand drivable area by using intersection area. for (const auto & drivable_lane : drivable_lanes) { - // extract target lane and bound. - const auto bound_lane = is_left ? drivable_lane.left_lane : drivable_lane.right_lane; - const auto bound = is_left ? bound_lane.leftBound3d() : bound_lane.rightBound3d(); + const auto edge_lanelet = is_left ? drivable_lane.left_lane : drivable_lane.right_lane; + const auto lanelet_bound = is_left ? edge_lanelet.leftBound3d() : edge_lanelet.rightBound3d(); - if (bound.size() < 2) { + if (lanelet_bound.size() < 2) { + continue; + } + + const std::string id = edge_lanelet.attributeOr("intersection_area", "else"); + if (id == "else") { continue; } - // expand drivable area by intersection areas. - const std::string id = bound_lane.attributeOr("intersection_area", "else"); - const auto use_intersection_area = enable_expanding_intersection_areas && id != "else"; - if (use_intersection_area) { + // Step1. extract intersection partial bound. + std::vector intersection_bound{}; + { const auto polygon = route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(id.c_str())); @@ -1652,96 +1698,137 @@ std::vector calcBound( boost::geometry::is_valid(lanelet::utils::to2D(polygon.basicPolygon())); const auto is_clockwise_iteration = is_clockwise_polygon ? is_left : !is_left; - const auto start_itr = std::find_if(polygon.begin(), polygon.end(), [&bound](const auto & p) { - return p.id() == bound.front().id(); - }); + const auto intersection_bound_itr_init = std::find_if( + polygon.begin(), polygon.end(), + [&lanelet_bound](const auto & p) { return p.id() == lanelet_bound.front().id(); }); - const auto end_itr = std::find_if(polygon.begin(), polygon.end(), [&bound](const auto & p) { - return p.id() == bound.back().id(); - }); + const auto intersection_bound_itr_last = std::find_if( + polygon.begin(), polygon.end(), + [&lanelet_bound](const auto & p) { return p.id() == lanelet_bound.back().id(); }); - if (start_itr == polygon.end() || end_itr == polygon.end()) { + if ( + intersection_bound_itr_init == polygon.end() || + intersection_bound_itr_last == polygon.end()) { continue; } // extract line strings between start_idx and end_idx. - const size_t start_idx = std::distance(polygon.begin(), start_itr); - const size_t end_idx = std::distance(polygon.begin(), end_itr); - for (const auto & point : - extract_bound_from_polygon(polygon, start_idx, end_idx, is_clockwise_iteration)) { - output_points.push_back(point); - } + const size_t start_idx = std::distance(polygon.begin(), intersection_bound_itr_init); + const size_t end_idx = std::distance(polygon.begin(), intersection_bound_itr_last); - continue; + intersection_bound = + extract_bound_from_polygon(polygon, start_idx, end_idx, is_clockwise_iteration); } - if (!enable_expanding_hatched_road_markings) { - for (const auto & point : bound) { - output_points.push_back(lanelet::utils::conversion::toGeomMsgPt(point)); - } + // Step2. check shared bound point. + const auto shared_point_itr_init = + std::find_if(expanded_bound.begin(), expanded_bound.end(), [&](const auto & p) { + return std::any_of( + intersection_bound.begin(), intersection_bound.end(), + [&](const auto & point) { return point.id() == p.id(); }); + }); + + const auto shared_point_itr_last = + std::find_if(expanded_bound.rbegin(), expanded_bound.rend(), [&](const auto & p) { + return std::any_of( + intersection_bound.begin(), intersection_bound.end(), + [&](const auto & point) { return point.id() == p.id(); }); + }); + + if ( + shared_point_itr_init == expanded_bound.end() || + shared_point_itr_last == expanded_bound.rend()) { continue; } - // expand drivable area by hatched road markings. - for (size_t bound_point_idx = 0; bound_point_idx < bound.size(); ++bound_point_idx) { - const auto & bound_point = bound[bound_point_idx]; - const auto polygon = getPolygonByPoint(route_handler, bound_point, "hatched_road_markings"); - - bool will_close_polygon{false}; - if (!current_polygon) { - if (!polygon) { - output_points.push_back(lanelet::utils::conversion::toGeomMsgPt(bound_point)); - } else { - // There is a new additional polygon to expand - current_polygon = polygon; - current_polygon_border_indices.push_back( - get_corresponding_polygon_index(*current_polygon, bound_point.id())); - } - } else { - if (!polygon) { - will_close_polygon = true; - } else { - current_polygon_border_indices.push_back( - get_corresponding_polygon_index(*current_polygon, bound_point.id())); - } - } + // Step3. overwrite duplicate drivable bound by intersection bound. + { + const auto trim_point_itr_init = std::find_if( + intersection_bound.begin(), intersection_bound.end(), + [&](const auto & p) { return p.id() == shared_point_itr_init->id(); }); - if (bound_point_idx == bound_points.size() - 1 && current_polygon) { - // If drivable lanes ends earlier than polygon, close the polygon - will_close_polygon = true; + const auto trim_point_itr_last = std::find_if( + intersection_bound.begin(), intersection_bound.end(), + [&](const auto & p) { return p.id() == shared_point_itr_last->id(); }); + + if ( + trim_point_itr_init == intersection_bound.end() || + trim_point_itr_last == intersection_bound.end()) { + continue; } - if (will_close_polygon) { - // The current additional polygon ends to expand - if (current_polygon_border_indices.size() == 1) { - output_points.push_back(lanelet::utils::conversion::toGeomMsgPt( - (*current_polygon)[current_polygon_border_indices.front()])); - } else { - const size_t current_polygon_points_num = current_polygon->size(); - const bool is_polygon_opposite_direction = [&]() { - const size_t modulo_diff = mod( - static_cast(current_polygon_border_indices[1]) - - static_cast(current_polygon_border_indices[0]), - current_polygon_points_num); - return modulo_diff == 1; - }(); + std::vector tmp_bound{}; - const int target_points_num = - current_polygon_points_num - current_polygon_border_indices.size() + 1; - for (int poly_idx = 0; poly_idx <= target_points_num; ++poly_idx) { - const int target_poly_idx = current_polygon_border_indices.front() + - poly_idx * (is_polygon_opposite_direction ? -1 : 1); - output_points.push_back(lanelet::utils::conversion::toGeomMsgPt( - (*current_polygon)[mod(target_poly_idx, current_polygon_points_num)])); - } + tmp_bound.insert(tmp_bound.end(), expanded_bound.begin(), shared_point_itr_init); + tmp_bound.insert(tmp_bound.end(), trim_point_itr_init, trim_point_itr_last); + tmp_bound.insert( + tmp_bound.end(), std::next(shared_point_itr_last).base(), expanded_bound.end()); + + expanded_bound = tmp_bound; + } + } + + return expanded_bound; +} + +// calculate bounds from drivable lanes and hatched road markings +std::vector calcBound( + const std::shared_ptr route_handler, + const std::vector & drivable_lanes, + const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, + const bool is_left) +{ + // a function to convert drivable lanes to points without duplicated points + const auto convert_to_points = [&](const std::vector & drivable_lanes) { + constexpr double overlap_threshold = 0.01; + + std::vector points; + for (const auto & drivable_lane : drivable_lanes) { + const auto bound = + is_left ? drivable_lane.left_lane.leftBound3d() : drivable_lane.right_lane.rightBound3d(); + for (const auto & point : bound) { + if ( + points.empty() || + overlap_threshold < (points.back().basicPoint2d() - point.basicPoint2d()).norm()) { + points.push_back(point); } - current_polygon = std::nullopt; - current_polygon_border_indices.clear(); } } + return points; + }; + + const auto to_ros_point = [](const std::vector & bound) { + std::vector ret{}; + std::for_each(bound.begin(), bound.end(), [&](const auto & p) { + ret.push_back(lanelet::utils::conversion::toGeomMsgPt(p)); + }); + return ret; + }; + + // Step1. create drivable bound from drivable lanes. + std::vector bound_points = convert_to_points(drivable_lanes); + + // Step2. if there is no drivable area defined by polygon, return original drivable bound. + if (!enable_expanding_hatched_road_markings && !enable_expanding_intersection_areas) { + return motion_utils::removeOverlapPoints(to_ros_point(bound_points)); } - return motion_utils::removeOverlapPoints(output_points); + // Step3. if there are hatched road markings, expand drivable bound with the polygon. + if (enable_expanding_hatched_road_markings) { + bound_points = getBoundWithHatchedRoadMarkings(bound_points, route_handler); + } + + if (!enable_expanding_intersection_areas) { + return motion_utils::removeOverlapPoints(to_ros_point(bound_points)); + } + + // Step4. if there are intersection areas, expand drivable bound with the polygon. + { + bound_points = + getBoundWithIntersectionAreas(bound_points, route_handler, drivable_lanes, is_left); + } + + return motion_utils::removeOverlapPoints(to_ros_point(bound_points)); } void makeBoundLongitudinallyMonotonic( @@ -1915,19 +2002,27 @@ void makeBoundLongitudinallyMonotonic( return bound; } - std::vector ret; + std::vector ret = bound; + auto itr = std::next(ret.begin()); + while (std::next(itr) != ret.end()) { + const auto p1 = *std::prev(itr); + const auto p2 = *itr; + const auto p3 = *std::next(itr); - 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; }); + const std::vector vec_1to2 = {p2.x - p1.x, p2.y - p1.y, p2.z - p1.z}; + const std::vector vec_3to2 = {p2.x - p3.x, p2.y - p3.y, p2.z - p3.z}; + const auto product = + std::inner_product(vec_1to2.begin(), vec_1to2.end(), vec_3to2.begin(), 0.0); - if (duplicated_points_itr != ret.end() && std::next(duplicated_points_itr, 2) == ret.end()) { - ret.erase(duplicated_points_itr, ret.end()); - } + const auto dist_1to2 = tier4_autoware_utils::calcDistance3d(p1, p2); + const auto dist_3to2 = tier4_autoware_utils::calcDistance3d(p3, p2); - ret.push_back(p_new); + constexpr double epsilon = 1e-3; + if (std::cos(M_PI_4) < product / dist_1to2 / dist_3to2 + epsilon) { + itr = ret.erase(itr); + } else { + itr++; + } } return ret; @@ -2580,6 +2675,19 @@ double getArcLengthToTargetLanelet( std::min(arc_front.length - arc_pose.length, arc_back.length - arc_pose.length), 0.0); } +Polygon2d toPolygon2d(const lanelet::ConstLanelet & lanelet) +{ + Polygon2d polygon; + for (const auto & p : lanelet.polygon2d().basicPolygon()) { + polygon.outer().emplace_back(p.x(), p.y()); + } + polygon.outer().push_back(polygon.outer().front()); + + return tier4_autoware_utils::isClockwise(polygon) + ? polygon + : tier4_autoware_utils::inverseClockwise(polygon); +} + std::vector getTargetLaneletPolygons( const lanelet::PolygonLayer & map_polygons, lanelet::ConstLanelets & lanelets, const Pose & pose, const double check_length, const std::string & target_type) diff --git a/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml b/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml index 3aa8c82a5e556..f9ddd3ce61099 100644 --- a/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml +++ b/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml @@ -8,4 +8,4 @@ max_future_movement_time: 10.0 # [second] threshold_yaw_diff: 0.523 # [rad] adjacent_extend_width: 1.5 # [m] - enable_rtc: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval + enable_rtc: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval diff --git a/planning/behavior_velocity_blind_spot_module/src/debug.cpp b/planning/behavior_velocity_blind_spot_module/src/debug.cpp index f95ed73c2a184..7da494bfd5231 100644 --- a/planning/behavior_velocity_blind_spot_module/src/debug.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/debug.cpp @@ -19,6 +19,8 @@ #include #include +#include + #include #include diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index b1e5bcb32bd18..a4f9e9d7ce23d 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -5,7 +5,7 @@ show_processing_time: false # [-] whether to show processing time # param for input data traffic_light_state_timeout: 3.0 # [s] timeout threshold for traffic light signal - enable_rtc: true # if true, the scene modules should be approved by (request to cooperate)rtc function. if false, the module can be run without approval from rtc. + enable_rtc: false # if true, the scene modules should be approved by (request to cooperate)rtc function. if false, the module can be run without approval from rtc. # param for stop position stop_position: @@ -54,7 +54,9 @@ ## param for yielding disable_stop_for_yield_cancel: true # for the crosswalk where there is a traffic signal disable_yield_for_new_stopped_object: true # for the crosswalk where there is a traffic signal - timeout_no_intention_to_walk: 3.0 # [s] if the pedestrian does not move for X seconds after stopping before the crosswalk, the module judge that ego is able to pass first. + # if the pedestrian does not move for X seconds after stopping before the crosswalk, the module judge that ego is able to pass first. + distance_map_for_no_intention_to_walk: [1.0, 5.0] # [m] ascending order + timeout_map_for_no_intention_to_walk: [3.0, 0.0] # [s] timeout_ego_stop_for_yield: 3.0 # [s] the amount of time which ego should be stopping to query whether it yields or not # param for target object filtering diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index 304b4bdf6f5e7..94e87d0174193 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -104,8 +104,10 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".pass_judge.disable_stop_for_yield_cancel"); cp.disable_yield_for_new_stopped_object = getOrDeclareParameter(node, ns + ".pass_judge.disable_yield_for_new_stopped_object"); - cp.timeout_no_intention_to_walk = - getOrDeclareParameter(node, ns + ".pass_judge.timeout_no_intention_to_walk"); + cp.distance_map_for_no_intention_to_walk = getOrDeclareParameter>( + node, ns + ".pass_judge.distance_map_for_no_intention_to_walk"); + cp.timeout_map_for_no_intention_to_walk = getOrDeclareParameter>( + node, ns + ".pass_judge.timeout_map_for_no_intention_to_walk"); cp.timeout_ego_stop_for_yield = getOrDeclareParameter(node, ns + ".pass_judge.timeout_ego_stop_for_yield"); diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 9ea03996c88dd..fe96985743b94 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -299,8 +299,7 @@ std::optional> CrosswalkModule::get const auto p_stop_lines = getLinestringIntersects( ego_path, lanelet::utils::to2D(stop_line).basicLineString(), ego_pos, 2); if (!p_stop_lines.empty()) { - return std::make_pair( - p_stop_lines.front(), -planner_param_.stop_distance_from_object - base_link2front); + return std::make_pair(p_stop_lines.front(), -base_link2front); } } @@ -928,7 +927,7 @@ void CrosswalkModule::updateObjectState( getCollisionPoint(sparse_resample_path, object, crosswalk_attention_range, attention_area); object_info_manager_.update( obj_uuid, obj_pos, std::hypot(obj_vel.x, obj_vel.y), clock_->now(), is_ego_yielding, - has_traffic_light, collision_point, planner_param_); + has_traffic_light, collision_point, planner_param_, crosswalk_.polygon2d().basicPolygon()); if (collision_point) { const auto collision_state = object_info_manager_.getCollisionState(obj_uuid); @@ -962,8 +961,10 @@ bool CrosswalkModule::isRedSignalForPedestrians() const continue; } - if (lights.front().color == TrafficSignalElement::RED) { - return true; + for (const auto & element : lights) { + if ( + element.color == TrafficSignalElement::RED && element.shape == TrafficSignalElement::CIRCLE) + return true; } } diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 667298cbb4c58..063ea4f83cd45 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -30,12 +30,15 @@ #include #include +#include +#include #include #include #include #include #include +#include #include #include #include @@ -45,6 +48,7 @@ namespace behavior_velocity_planner { +namespace bg = boost::geometry; using autoware_auto_perception_msgs::msg::ObjectClassification; using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_perception_msgs::msg::PredictedObjects; @@ -71,6 +75,33 @@ double interpolateEgoPassMargin( } return y_vec.back(); } + +double InterpolateMap( + const std::vector & key_map, const std::vector & value_map, const double query) +{ + // If the speed is out of range of the key_map, apply zero-order hold. + if (query <= key_map.front()) { + return value_map.front(); + } + if (query >= key_map.back()) { + return value_map.back(); + } + + // Apply linear interpolation + for (size_t i = 0; i < key_map.size() - 1; ++i) { + if (key_map.at(i) <= query && query <= key_map.at(i + 1)) { + auto ratio = (query - key_map.at(i)) / std::max(key_map.at(i + 1) - key_map.at(i), 1.0e-5); + ratio = std::clamp(ratio, 0.0, 1.0); + const auto interp = value_map.at(i) + ratio * (value_map.at(i + 1) - value_map.at(i)); + return interp; + } + } + + std::cerr << "Crosswalk's InterpolateMap interpolation logic is broken." + "Please check the code." + << std::endl; + return value_map.back(); +} } // namespace class CrosswalkModule : public SceneModuleInterface @@ -111,7 +142,8 @@ class CrosswalkModule : public SceneModuleInterface double min_object_velocity; bool disable_stop_for_yield_cancel; bool disable_yield_for_new_stopped_object; - double timeout_no_intention_to_walk; + std::vector distance_map_for_no_intention_to_walk; + std::vector timeout_map_for_no_intention_to_walk; double timeout_ego_stop_for_yield; // param for input data double traffic_light_state_timeout; @@ -132,9 +164,10 @@ class CrosswalkModule : public SceneModuleInterface std::optional collision_point{}; void transitState( - const rclcpp::Time & now, const double vel, const bool is_ego_yielding, - const bool has_traffic_light, const std::optional & collision_point, - const PlannerParam & planner_param) + const rclcpp::Time & now, const geometry_msgs::msg::Point & position, const double vel, + const bool is_ego_yielding, const bool has_traffic_light, + const std::optional & collision_point, const PlannerParam & planner_param, + const lanelet::BasicPolygon2d & crosswalk_polygon) { const bool is_stopped = vel < planner_param.stop_object_velocity; @@ -147,8 +180,13 @@ class CrosswalkModule : public SceneModuleInterface if (!time_to_start_stopped) { time_to_start_stopped = now; } + const double distance_to_crosswalk = + bg::distance(crosswalk_polygon, lanelet::BasicPoint2d(position.x, position.y)); + const double timeout_no_intention_to_walk = InterpolateMap( + planner_param.distance_map_for_no_intention_to_walk, + planner_param.timeout_map_for_no_intention_to_walk, distance_to_crosswalk); const bool intent_to_cross = - (now - *time_to_start_stopped).seconds() < planner_param.timeout_no_intention_to_walk; + (now - *time_to_start_stopped).seconds() < timeout_no_intention_to_walk; if ( (is_ego_yielding || (has_traffic_light && planner_param.disable_stop_for_yield_cancel)) && !intent_to_cross) { @@ -203,7 +241,8 @@ class CrosswalkModule : public SceneModuleInterface void update( const std::string & uuid, const geometry_msgs::msg::Point & position, const double vel, const rclcpp::Time & now, const bool is_ego_yielding, const bool has_traffic_light, - const std::optional & collision_point, const PlannerParam & planner_param) + const std::optional & collision_point, const PlannerParam & planner_param, + const lanelet::BasicPolygon2d & crosswalk_polygon) { // update current uuids current_uuids_.push_back(uuid); @@ -221,7 +260,8 @@ class CrosswalkModule : public SceneModuleInterface // update object state objects.at(uuid).transitState( - now, vel, is_ego_yielding, has_traffic_light, collision_point, planner_param); + now, position, vel, is_ego_yielding, has_traffic_light, collision_point, planner_param, + crosswalk_polygon); objects.at(uuid).collision_point = collision_point; objects.at(uuid).position = position; } diff --git a/planning/behavior_velocity_detection_area_module/config/detection_area.param.yaml b/planning/behavior_velocity_detection_area_module/config/detection_area.param.yaml index b5d075a4d6493..62b5f2458336f 100644 --- a/planning/behavior_velocity_detection_area_module/config/detection_area.param.yaml +++ b/planning/behavior_velocity_detection_area_module/config/detection_area.param.yaml @@ -8,4 +8,4 @@ state_clear_time: 2.0 hold_stop_margin_distance: 0.0 distance_to_judge_over_stop_line: 0.5 - enable_rtc: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval + enable_rtc: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval diff --git a/planning/behavior_velocity_intersection_module/README.md b/planning/behavior_velocity_intersection_module/README.md index 5f26ef34c1186..16459bb64074b 100644 --- a/planning/behavior_velocity_intersection_module/README.md +++ b/planning/behavior_velocity_intersection_module/README.md @@ -24,7 +24,7 @@ This module is activated when the path contains the lanes with `turn_direction` ### Attention area -The `Attention Area` in the intersection are defined as the set of lanes that are conflicting with ego vehicle's path and their preceding lanes up to `attention_area_length` meters. `RightOfWay` tag is used to rule out the lanes that each lane has priority given the traffic light relation and `turn_direction` priority. +The `attention area` in the intersection are defined as the set of lanes that are conflicting with ego vehicle's path and their preceding lanes up to `common.attention_area_length` meters. `RightOfWay` tag is used to rule out the lanes that each lane has priority given the traffic light relation and `turn_direction` priority(`yield lane`). `Intersection Area`, which is supposed to be defined on the HDMap, is an area converting the entire intersection. @@ -54,10 +54,10 @@ For [stuck vehicle detection](#stuck-vehicle-detection) and [collision detection Objects that satisfy all of the following conditions are considered as target objects (possible collision objects): -- The center of mass of the object is **within a certain distance** from the attention lane (threshold = `attention_area_margin`) . +- The center of mass of the object is **within a certain distance** from the attention lane (threshold = `common.attention_area_margin`) . - (Optional condition) The center of gravity is in the **intersection area**. - To deal with objects that is in the area not covered by the lanelets in the intersection. -- The posture of object is **the same direction as the attention lane** (threshold = `attention_area_angle_threshold`). +- The posture of object is **the same direction as the attention lane** (threshold = `common.attention_area_angle_threshold`). - Not being **in the adjacent lanes of the ego vehicle**. #### Stuck Vehicle Detection @@ -68,31 +68,101 @@ If there is any object on the path in inside the intersection and at the exit of #### Collision detection -The following process is performed for the targets objects to determine whether the ego vehicle can pass the intersection safely. If it is judged that the ego vehicle cannot pass through the intersection with enough _margin_, it will insert a stopline on the _path_. +The following process is performed for the targets objects to determine whether the ego vehicle can pass the intersection safely. If it is judged that the ego vehicle cannot pass through the intersection with enough margin, this module inserts a stopline on the path. 1. calculate the time interval that the ego vehicle is in the intersection. This time is set as $t_s$ ~ $t_e$ -2. extract the predicted path of the target object whose confidence is greater than `min_predicted_path_confidence`. +2. extract the predicted path of the target object whose confidence is greater than `collision_detection.min_predicted_path_confidence`. 3. detect collision between the extracted predicted path and ego's predicted path in the following process. 1. obtain the passing area of the ego vehicle $A_{ego}$ in $t_s$ ~ $t_e$. - 2. calculate the passing area of the target object $A_{target}$ at $t_s$ - `collision_start_margin_time` ~ $t_e$ + `collision_end_margin_time` for each predicted path (\*1). + 2. calculate the passing area of the target object $A_{target}$ at $t_s$ - `collision_detection.collision_start_margin_time` ~ $t_e$ + `collision_detection.collision_end_margin_time` for each predicted path (\*1). 3. check if $A_{ego}$ and $A_{target}$ polygons are overlapped (has collision). 4. when a collision is detected, the module inserts a stopline. 5. If ego is over the `pass_judge_line`, collision checking is not processed to avoid sudden braking and/or unnecessary stop in the middle of the intersection. -The parameters `collision_start_margin_time` and `collision_end_margin_time` can be interpreted as follows: +The parameters `collision_detection.collision_start_margin_time` and `collision_detection.collision_end_margin_time` can be interpreted as follows: -- If the ego vehicle was to pass through the intersection earlier than the target object, collision would be detected if the time difference between the two was less than `collision_start_margin_time`. -- If the ego vehicle was to pass through the intersection later than the target object, collision would be detected if the time difference between the two was less than `collision_end_margin_time`. +- If the ego vehicle was to pass through the intersection earlier than the target object, collision would be detected if the time difference between the two was less than `collision_detection.collision_start_margin_time`. +- If the ego vehicle was to pass through the intersection later than the target object, collision would be detected if the time difference between the two was less than `collision_detection.collision_end_margin_time`. -If collision is detected, the state transits to "STOP" immediately. On the other hand, the state does not transit to "GO" unless _safe_ judgement continues for a certain period to prevent the chattering of decisions. +If collision is detected, the state transits to "STOP" immediately. On the other hand, the state does not transit to "GO" unless safe judgement continues for a certain period `collision_detection.state_transit_margin` to prevent the chattering of decisions. #### Stop Line Automatic Generation -If a stopline is associated with the intersection lane, that line is used as the stopline for collision detection. Otherwise the path is interpolated at a certain intervals (=`path_interpolation_ds`), and the point which is `stop_line_margin` meters behind the attention lane is defined as the position of the stop line for the vehicle front. +If a stopline is associated with the intersection lane on the map, that line is used as the stopline for collision detection. Otherwise the path is interpolated at a certain intervals (=`common.path_interpolation_ds`), and the point which is `stop_line_margin` meters behind the attention area is defined as the position of the stop line for the vehicle front. #### Pass Judge Line -To avoid a rapid braking, if deceleration and jerk more than a threshold (`behavior_velocity_planner.max_accel` and `behavior_velocity_planner.max_jerk`) are needed to stop just in front of the attention area, this module does not insert stopline after passing the default stopline position. +To avoid sudden braking, if deceleration and jerk more than a threshold (`behavior_velocity_planner.max_accel` and `behavior_velocity_planner.max_jerk`) is required to stop just in front of the attention area, this module does not insert stopline after passing the default stopline position. + +### Occlusion detection + +If the flag `occlusion.enable` is true this module checks if there is sufficient field of view (FOV) on the attention area up to `occlusion.occlusion_attention_area_length`. If FOV is not clear enough the ego vehicle will once stop at the _default stop line_ for `occlusion.before_creep_stop_time`, and then slowly creep toward _occlusion peeking stop line_ at the speed of `occlusion.occlusion_creep_velocity` if `occlusion.enable_creeping` is true. During the creeping if collision is detected this module inserts a stop line instantly, and if the FOV gets sufficiently clear the _intersection occlusion_ wall will disappear. If occlusion is cleared and no collision is detected the ego vehicle will pass the intersection. + +The occlusion is detected as the common area of occlusion attention area(which is partially the same as the normal attention area) and the unknown cells of the occupancy grid map. The occupancy grid map is denoised using morphology with the window size of `occlusion.denoise_kernel`. The occlusion attention area lanes are discretized to line strings and they are used to generate a grid whose each cell represents the distance from the ego path along the lane as shown below. + +![occlusion_detection](./docs/occlusion_grid.drawio.svg) + +If the nearest occlusion cell value is below the threshold, occlusion is detected. It is expected that the occlusion gets cleared as the vehicle approaches the occlusion peeking stop line. + +In there are no traffic lights associated with the lane, the ego vehicle will make a brief stop at the _default stop line_ and the position where the vehicle heading touches the attention area for the first time(which is denoted as _first attention stop line_). After stopping at the _first attention area stop line_ this module inserts `occlusion.absence_traffic_light.creep_velocity` velocity between ego and the position which is `occlusion.absence_traffic_light.maximum_peeking_distance` ahead of _first attention area stop line_ while occlusion is not cleared. If collision is detected, ego will instantly stop. Once the occlusion is cleared or ego passed `occlusion.absence_traffic_light.maximum_peeking_distance` this module does not detect collision and occlusion because ego vehicle is already inside the intersection. + +![occlusion_detection](./docs/occlusion-without-tl.drawio.svg) + +### Data Structure + +#### `IntersectionLanelets` + +```plantuml +@startuml +entity IntersectionLanelets { + * conflicting lanes/area + -- + * first conflicting area + The conflicting lane area which the path intersects first + -- + * attention lanes/area + -- + * first attention lane area + The attention lane area which the path intersects first + -- + * occlusion attention lanes/area + Part of attention lanes/area for occlusion detection + -- + * is_priortized: bool + If ego vehicle has priority in current traffic light context +} +@enduml +``` + +#### `IntersectionStopLines` + +Each stop lines are generated from interpolated path points to obtain precise positions. + +```plantuml +@startuml +entity IntersectionStopLines { + * closest_idx: size_t + closest path point index for ego vehicle + -- + * stuck_stop_line: size_t + stop line index on stuck vehicle detection + -- + * default_stop_line: size_t + If defined on the map, its index on the path. Otherwise generated before first_attention_stop_line + -- + * first_attention_stop_line + The index of the first path point which is inside the attention area + -- + * occlusion_peeking_stop_line + The index of the path point for the peeking limit position + -- + * pass_judge_line + The index of the path point which is before first_attention_stop_line/occlusion_peeking_stop_line by braking distance +} +@enduml +``` + +![data structure](./docs/data-structure.drawio.svg) ### Module Parameters @@ -105,7 +175,6 @@ To avoid a rapid braking, if deceleration and jerk more than a threshold (`behav | `common.intersection_velocity` | double | [m/s] velocity profile for pass judge calculation | | `common.intersection_max_accel` | double | [m/s^2] acceleration profile for pass judge calculation | | `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_vel_thr` | double | [m/s] velocity threshold for stuck vehicle detection | | `collision_detection.state_transit_margin_time` | double | [m] time margin to change state | @@ -118,7 +187,13 @@ To avoid a rapid braking, if deceleration and jerk more than a threshold (`behav | `occlusion.peeking_offset` | double | [m] the offset of the front of the vehicle into the attention area for peeking to occlusion | | `occlusion.min_vehicle_brake_for_rss` | double | [m/s] assumed minimum brake of the vehicle running from behind the occlusion | | `occlusion.max_vehicle_velocity_for_rss` | double | [m/s] assumed maximum velocity of the vehicle running from behind the occlusion | -| `occlusion.denoise_kernel` | double | [m] the window size of morphology process for clearing noisy occlusion | + +#### For developers only + +| Parameter | Type | Description | +| ------------------------------ | ------ | ---------------------------------------------------------------------- | +| `common.path_interpolation_ds` | double | [m] path interpolation interval | +| `occlusion.denoise_kernel` | double | [m] the window size of morphology process for clearing noisy occlusion | ### How to turn parameters diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index a932254140878..30fefcaeee035 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -19,6 +19,7 @@ # 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 timeout_private_area: 3.0 # [s] cancel stuck vehicle stop in private area + enable_private_area_stuck_disregard: false #In the intersections which labeled as "private area", the obstacle vehicles judged as "stuck" are neglected if this param is set as true. collision_detection: state_transit_margin_time: 1.0 @@ -34,6 +35,12 @@ collision_start_margin_time: 4.0 # [s] this + state_transit_margin_time should be higher to account for collision with fast/accelerating object collision_end_margin_time: 6.0 # [s] this + state_transit_margin_time should be higher to account for collision with slow/decelerating object keep_detection_vel_thr: 0.833 # == 3.0km/h. keep detection if ego is ego.vel < keep_detection_vel_thr + use_upstream_velocity: true # flag to use the planned velocity profile from the upstream module + minimum_upstream_velocity: 0.01 # [m/s] minimum velocity to avoid null division for the stop line from the upstream velocity + yield_on_green_traffic_light: + distance_to_assigned_lanelet_start: 5.0 + duration: 2.0 + range: 30.0 # [m] occlusion: enable: false @@ -52,10 +59,15 @@ ignore_parked_vehicle_speed_threshold: 0.8333 # == 3.0km/h stop_release_margin_time: 1.5 # [s] temporal_stop_before_attention_area: false + absence_traffic_light: + creep_velocity: 1.388 # [m/s] + maximum_peeking_distance: 6.0 # [m] + attention_lane_crop_curvature_threshold: 0.25 + attention_lane_curvature_calculation_ds: 0.5 enable_rtc: - intersection: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval - intersection_to_occlusion: true + intersection: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval + intersection_to_occlusion: false merge_from_private: stop_line_margin: 3.0 diff --git a/planning/behavior_velocity_intersection_module/docs/data-structure.drawio.svg b/planning/behavior_velocity_intersection_module/docs/data-structure.drawio.svg new file mode 100644 index 0000000000000..fb512902ef856 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/docs/data-structure.drawio.svg @@ -0,0 +1,771 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + closest_idx + +
+
+
+ + closest_idx + + + + + + + + +
+
+
+ + stuck_stop_line + +
+
+
+ + stuck_stop_line + + + + + + + + + + + +
+
+
+ + default_stop_line + +
+
+
+ + default_stop_line + + + + + + + + +
+
+
+ + first_attention_stop_line + +
+
+
+ + first_attention_stop_line + + + + + + + + +
+
+
+ + + occlusion_peeking +
+ _stop_line + + +
+
+
+ + occlusion_peeking... + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + next + + +
+
+
+ + next + + + + + + + + +
+
+
+ + + prev + + +
+
+
+ + prev + + + + + + + + +
+
+
+ + + ego_or_entry2exit + + +
+
+
+ + ego_or_entry2exit + + + + + + + + +
+
+
+ + + entry2ego + + +
+
+
+ + entry2ego + + + + + + + +
+
+
+ + IntersectionStopLines + +
+
+
+ + IntersectionStopLines + + + + + +
+
+
+ + PathLanelets + +
+
+
+ + PathLanelets + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_intersection_module/docs/intersection-attention-ll-rr.drawio.svg b/planning/behavior_velocity_intersection_module/docs/intersection-attention-ll-rr.drawio.svg index d4d709eee6b99..51e926fe266d6 100644 --- a/planning/behavior_velocity_intersection_module/docs/intersection-attention-ll-rr.drawio.svg +++ b/planning/behavior_velocity_intersection_module/docs/intersection-attention-ll-rr.drawio.svg @@ -8,12 +8,48 @@ width="3723px" height="2401px" viewBox="-0.5 -0.5 3723 2401" - content="<mxfile host="Electron" modified="2023-06-06T10:28:31.375Z" 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="M_P_fA_OZlKNZHzEOQxb" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">7V1bc9s4sv41rtp9EAv3y2PsxJOZmkt2stnM7ktKtmVbiWJ5ZCVxzq8/oERQIgCRAEWQkCLPzK4tUSCF/rrR3Wh8fYYvPj//tBg/3v82v5nMzhC4eT7DL8+Q+gGQZkj9lr/4ff0ihBijTKxfvFtMb4qXNy+8nf7fpHgRFK9+md5MnioXLufz2XL6WH3xev7wMLleVl4bLxbzb9XLbuez6l0fx3cT64W31+OZflV/ifz199Ob5f36dYH45vXXk+ndvb43ZHL9zuexvrj4Lk/345v5t62X8KszfLGYz5fr3z4/X0xm+SzqmfnPxU/k+9/vbv794ubx8vLi5X//9/pytB7sMuQj5VdbTB6WrYf+9tsI/wHv/p6RP+Hr0S+/wH/h34uPgK/j2Zdixorvuvyup3Ax//JwM8kHAWf4/Nv9dDl5+zi+zt/9prCjXrtffp6pv6D69XY6m13MZ/OF+vth/qAuOi/uMFksJ8+GqBq+DywnWcF0Mv88WS6+q88Vo4wgJ6RAY4HQEUey+EbfNrLGHBaX3W8JGhFQXDouMHZX3mIzi+qXYiLdk4q+v/j5/d//ml9+/DAj4Cv89Mfk9UgjKMqsniH8kstzkH9sMV+Ol9P5g3p5BEX+ytNyMf80cV07V6NPl/lE0fzPm+lC6dv6s98mT8tSUH5ScQi0RlC0nGstKIo5tQQFEeC2oJjMJNxfUuevPz6Qqw/k/Bs/fy8n/xmh3y9HSDRL6k6J6tE9Y1vzL8H+M1iawfGVvjlomFnC9JTpmWVUz9b2zDKaUWrPLQQoAxhKpP4VmGMRa56DNAIGa8TV+FrcYBf+ESaE3njJqh4h3nBHrCIQ/aclDaB/ILQFQ6GSSyRhYA+jr4ZRS/SkWRDjp8f1un07fc6F51oBLKEAQPnKKN3OH5Zbr9+ufvoUlkRV7YHSXj6UzwMQFIwT5R0ILZZtae24pHvJwaEldw4kdUlujeQBJYdto5eS4FBfglOyuLzE6scpPVRIKTW9Ezhp6eGh1Y6tflIQHAGHJDjy47gd+rsWguHEEgzhmWQOV6ODMMg9+3RQtbmZ3I6/zJZJKA08JKVhzWJ7uh8/5r/ezibPL/I0jZq9ycNN8evL69n46Wl6XZWgEfEW351nCEgM1LejVEAs2XqkIpOEMwkIoxAyxCVgkvP12zqTA2EGIYGQqtBFWUkhaLgYJzeVFJItxC0xaV1x6c9iMlNo+lpNR7mkU9zhzXyqnq/ECK2mMgSpDvA0/7K4nhSf2c7y1A+jZqc6znK8uJssrXFWSCm/c3vwUN4MnnVA3X+oLJUGESAAV/8qQNHKRI04pCwDvAzLbH9SfShTECs/77ClHIEMUSKLH9GBjl58efXu8dur0T29+PjhzfPbV+//dz5iHoHAPhmm89U/RoZJ2gmmwvC2yS3Vw8c/uYRltkpfrDMYTE+5xj8hmdgIRFI7GsdUZtwRgvMO1kW38DyCgXwCptfj2a/jq8nszfxpWkzn1Xy5nH9WE6oveDGb3uVvLOeGRLWJ/vx8l2f4s6uxssiZkoBaCh+uX5Xr7pOyCMsXD3erO4NMcFrY8vIlqGDNMREcS0y5zN8fL661CVaAF34AIjRB/EBAlVEgAEuA8wUIGvYTiNwoSAYkI1AhiVv4oWoE4UgwU8AzxmNhyCMk6dwAYJaiBVCY3DbrRFYESJUBAHxjAIQlv1LRe1F+7pEJ3fKkiund6UbZIb4W3PodraW4cIO1X+XYEVByWXz/qxh39cd/V+pN9Z8vn7fffPld//U8Xf61th2AFH+vPwmlLP7efDT/Y/uTbyaLqZrZyaJ4LRAma7fIw9iu/R6PJTURdxAq17aCaiOEINu+CuCGo+HrLUIGa3QH19+kO1fSKRDhkaa+/rL4WsbuA6sNbK82rKI2KFhp1De/nOazO7AK4bRUiNaBm1AD3e00CJO6m1BThbw0SKF4/H3rssf8gqeaL9riGTbKur5bt6rrs08RlC7oUmELxYMVpaP1Std2bSwVE8ZQTHyYiollHV4Z7EQxSb1SsDaK2ZV+QI8syT7eu2Qcv2BnXgUipSKlVSDCKLZDragFIk5JyaAdhC4TLUbSev2TrgARNOtQmHBUPgAgM+pIbnOUMbK/DCf0cvrxzU9PgN+Bnx7//nDx5d+3PjVuA6UkGcMsAwArc4RVGGsUs1EJWSYloxBDJhG0d3SUomcKGvrjELu2DQTLBC0HwV3sGzir3jw0ZbisFkP59HACIJbqPyvFBQFmajngknNGiTRTXIx7qi7EQUbWhlwtgv1zIiYuqpuDkOeui6WcwpEFhTph0TlePHYH+0hhDyAdFVFlnDFOMGMk39Fj1ZIxZSUd4kHMIR4WrYZ1AOkEeiixxAOR26rqgj4hXdJxZRGjScfDf4yQDSHdBFNhctuOgursSCKxDa/FDuQaOqHhDKvHJLUG7i6Ecc66R+F07OScC44NibA6oCUCoGBBe+d6680atrHZUXqq/htt7ttVSsqJV+gRBaS3oHUVnnGMzOCCYTuegFA6Dgl0FF87xSI9tsOGCs4Egyo4K7eGjfnjOA/OpH4X2S4BJ3T78xDZU8sJzIQg5iCdh8Co9SzXS639NJcF9xnEaqYF5jzfjWIAT1QQ1TiXzVPJIiEWggEsSZS4Be0Q4m6doEparCyG4LBaQkUhyTiVpgS28xWCZMhhYbrYbd8hLI+diLRqbVqX14wc9TUDgMQoqCnL5MrYlmQUbIHEPnATu6BmB1I8bGR6DkI3MlOSqFhXGCoyLBx5+Yha7VH9NET0UW7pQ6lDYF0J02ZTv2VhQZsYSM9fU7BdGtTtPcd6hUokrlKqVsG4rmooAytZcecM98E3yEK4/i6g9i5d1QPQ2oeA0vUQkaMvj92BOBUBHSWtYlUA7KmN6EC1UWGvCtEqQlknykhI7U1g9SbU2OiInEBTflKQSiSWxN3og17sNjoBJWzUCvVXc+nnnsoBvJUjsdpPJCu45dWMWX7IpAPtgIZ2GHfB9Qti5HNEHmFbkmkhJqQ4nLSQhwXaMcu1QjvurNA79PWC8Zs5vCP0E/3r/jc6fT9IejlKUojukOFhJoXcsjrlhHrGSPo5ITdQjiclFCyydFNCv/88v3kmX++n5Pzd4udP/AO/ej1CHjp9ygiFuNkFYrbd7Fpruu1l12pTKk72KR/USz7Ira4elvWUDgpWRUc26BBU8YdKBjn1QdPOnXJBkRYpRyqodjVLRDN+rEyQUzVS3DgAbZQj0GmzNaAus9WoAY6jgnXXJYL/EWQcwIwxm6xHZByXx/dAy5rIXeMzSFEG+k15+px3DyKrm+W5i/Px9ae71ccqDJH5j0sXSh7P7UO09Mw6jlbSSjto2MZFSuRaITc36BHrGknOUWBhw8HdhIUDpEh0EI66ucWCMgcnWa40EWqjsZGkXrd7keT4zcvzv/4LHn+dje7f/blcXFx9lqMheHYSLhtmXDpOdcYsG65bpaJLZbcMzDx8fzIBHBkyYQ5WIwgAKzOjlRQczOrWYV+xXLHH2c+//PXrDZmPpi/f//LLH/ylF83x0coFEusE9ABycX6dfSsNbI6NsHlMxpvkGBtrDBO6NKmF7yiRtWIxP5Kk0BSe62ZMc8B2lapzitDjIOgPix0iUHfYYZAZTxUTO6SoXIiKnX2PqR4NdhiEpiMl9ohZqemWKbvjx/PbAjsUmeuaNplRseNxwDQQOzfjp/sy3rI9iMv8nwOGGNPGaCuJwbKW7EnO8ZhZiNRdRsQ9xT59F34Q++Ew4C05H1cqzYzRIq499s3U2pPRHkwIDttVceaOK0Yjx8Pb4rPzxfJ+fjd/GM9ebV49n8yuVmPp8iDL0hQgrUZBx4NKho1EjD8qqRDWaMgYrTtU2o+OGQh/QuNDe+PYmYPygHGU2sK9KcjCcB2WmbJEwYGdLIQAgph8Y87kVF9JkIMSl87Jpyeu7oOUY3c0zTw9Y8QYJ8AXgdIezc/qtzCv7j3/yGycyef4Tf/MSQ2CgKP8tqsc/1y8+Tr+/v7l3cWr31/+Df8zGt2NNN9himdApBXOS+zYF9nZdRV23XXVPYNd7yiHtz+zZVMv62bgDtJH1f3QPizXXTV1BECMgdNkNDZT7V4GZXVSRhgllHKiHGFm2BGZQYg2jTc0Z8zgTc/cX97DaRu4s2o0MTKcQSQhBZASCJnRU/2wxDh4p87GFqvxtBGSjOYkroIKpZrIOLKRrBxdLhkdXB0bO672JkdUESODGRRo06XOPl+akBhJUObiQByQ8kwbyyQpnThz958aLh6wBRWxH6tbqYY1js39WPtSKWjQYVOZMbzRKX08Jk2doh7FHAfWnrVWyonkPdSXyCTaFBxrPhVdia/ebduDi4SOHPswioeZOMazWTYwfSvu67L22xX3ddclgvOc4EJYu4O65LBFfk9gc7TSpekJzz6+5AnPjSdIkI3nuuvSwTOXJJPSPuHBM7LVGLQ1wJ3DcwX8zDqWEhnoYeUSp6NSDVXM20ivvTAhqCNi4hC2LxMxuVxGHJjb+5ERHVa4fDLd9YdYGl2R1A7/5Sf8DLPNcWs8c6vmgyPQL57DiqlPFrphx7b5ODdPDNEqUK4erh4xItsXAiNkjkZJzz5H92dLjrx8glMzIOI63d1mjTZHU55nv9wVuPsTIseNALWEWSfFzJKXkACbW6PhnhHwoxfQWIdkYc+9a91iiVw3GtJluLx2yLomSp1iiVfX5CY0SZfbliMJMyl102zDt1CoJlvvOqg81asZE+UFnNgza1zCOqC2dZ8QH6Bo2tVetFjuGmFfDxV/2kZJMsY202vkpXImOM7Ltx36kJP9cBeRbqxdSeJRpDYgG6vAgnJGKEEAQ3JmUbNy5dATwbHElMv2PK0I2UStPUPHIGk1j38ZlJ9Q2DWig5C0+pjTzhU9P/YxsLgEU9IqNVlQg3aZQCWtjR1AlrCcDWvj8bMm1+xgmLRHrfCbSbwce4q1FyYS81DKth0HUQ1Ycu5FgTaLUsutGFr1ToybkMqbVBrmKPYWpEeMPCg9MQIGPbGQ9RrRIz3xfjqj/YtGnSFp5QmokDVwxhx3ojO47iYE1d6ko8OYlNQ+g3A9w06SV1o3FnVOWlyi47DTZZEz/m1Ji1vtfNUTHe+p1L6k44kpNUZ1+GS0E6Xm9TrAWuh0V+pAUErqcBAburUZ1238116YCP5XablqXDnK0xWbApo2J0gtghrniN1B++LLq3eP316N7unFxw9vnt++ev+/85ErjcHKcucKxNnfX+b6jdHTqpT2hboA08fnFVT0++q3u+L/VwNd6Re+5U+vpL8Y394q9UBgtpIUusgltP51/Ql13ZU1ysLjFf3CP36d3C5H9+OHm839/lk3unpt/X2rLz89jh+85gD4zsHuh9/xWOtH0C8bRkcF3cuqmdHMpzM1AY4M0+fpzc2K8WMxUU9fpFhzTS/cHTUqPT+jL/ORvizn62/o2M8qvHtH9XyYyQhMjZuFPQg4zgO5DAE0d6rapAicCuSRIehrEynfrRg7ditupovJdfHZb5OnZVQpUWJzJjiSbhBBR5q9ow0Mp6C6Z+YKm8Rk1jKXhMxZ91/HiJTGaLxkRO6c/8a+mXr0bqlsnNjZt8/FMWHH9IOIztK2wo45GvQsAmiFHevRO46lndjxoTP4McBDoDDrvvYxPFRaO73xDA+1wNOL4fEiazhVKW0JSiBpmhSC2/NJOsZjmnWhpxDNi+ThBzEg1vJP2qabVjptjgZNUpsuDYj96KJrPkk3fjo4YHPikwxCpZAGjvxRiSW1RhPRUGk/OivKAoOe0PhQJBx7MGsN26y650Cbmt4UE45KQQBkTHJCt6gGyIqkLCqsawoTFNWpDj7Q3HNkm7/Wp7sIYvZo/fqXaW32HsTuVu36tL27VXthOpCW1NyLgnv41RKYo5V5+r4gHZbsPUG6YR1vhjRNDdKC0UxKkyhhKylAOuFRaLgPh5TGJFRwCgOfqHA6Qb8v+FPDvnI3rV0SsyVBQIMDK0bkUESrQ3UDOiwD1gbQ5UmvxCux9zPnjvqb2guTgTTBxIyg9uhiRpA0R6PYGC0ypH348E5hVyXsoqbMuLlXGIAAazS1UkdrnO1GwCnwDt57Nk8sM9R+Y0cSe7SeEXAAB9D7rh2y2+MUqZXq4WcHAW3M2iHqEU/+6JJi0NFKIiZ7gFtSYaU6J66iBsQ3Bz8isWXC5Sq2b1rqcBUJ7TecZ6f8bDeQ9o1+aGrRD8VmqRNjjO9z+oBAaXk/rhEjQ1t6lCasSUXsrb49F7cWPCMUcYtZcnV6l1GI827V0PZVsGQZwhhRhjHlAmIXB7vgWd4VqBjE9Gk7Wxs1f3yanBXKl1fTwwmAWKr/zkzOCggw4xRwyTmjJD/nW+GsYNzTrSK+HAheDlU9rP1ZLAxklX9vO1PCQXACuYyFlgH28F0u7/DCqToASNqiKTdxK25urCMnPtR1PVRXuAWjlP5mOtl0yiiu6lWZjIQ1dPTsdPKIRJOYTlX2zCOSv5KfVNN9L0Bq/lytwBsdOq0HjQ6dvjARhw4Bw9ibLEO+fhwy6HMQ7HdfRgxGdB0N2dvH/5PBue9GJE5rJ9LEOQYtcc6MceLR4Lhh7hGAx6bBCQB8q2wTyHLa/ArMEUENQF/9ZfPp1PPi9GXz0yqyQsQw1SaGvW2+wY2IiZ8yhBaHY6OLAi7C0rjnLDtYUNrYfVvXvNPuiSS3dJayUS9kWnrhrKdF7Rs3EWbVvpTUHX0ltQYIz9YcrkaHwvVPCJj73UOCxBQVE460xxBV8XKwnLvPQt8jIVbXZsp3+ZZphWy5YTEL9Tkge5gpaI0mo5mp89cfH8jVB3L+jZ+/l5P/jNDvlyMPhOvUu8MsDJ+NZ5RYp8mFnehR12U6TK4kTgGqtrrd35I4pzmociBKL+FmUdXiw9uiG8T1DqL1tSzKjqHQkTRdNSCOJAqf0oCo3bkBoLy2y3pfojLJDKCDRmmQ3sHOr+fFoBFVcOdAUpfg1jgeTnC60j9RufmcQ+hEbkoUl5c7nV1UCCkxrROO3eKEhIeGVjq2+klAbgQcktw8SiqOxOHA1Y1ZfcZ6Sy6EZ9JRKmrSBHU3+R4VFhGV5mZyO/5SkncOqjLV6CZxlfHIOuqqltvZ5LmI+s/DEgD6u/MMAYmB+naUCoglW4+k4/1MAsIozPsQScAk5+u3dXoAwgxCAiFVEYsykULQYCkmElLTagGaMFI53lzw1WEgNMaJHEU76RNKzlcHc+xujtiCcPe6VNvNhatEnlJg67OTu7n6yGysjEE9k+1ZhV12n/VglleGnY+vP92tPlZxwfIf19KQ/6ze2c5sUdtOveTyfO2q9Uksy4DFgoNdrWewI43QRcmIG1jxM5C+Uimva7PDWM1uuq2Vb0qxVgMTMWs5s7cwW/iV62GbRpkWQQhmxkoY28a5cljd2zh+C4DLxn2fTmY3h2Dl+CUAB2XlJHUUxvVr5eIf1ymlYluiBjmVn2xj97q2cqkds2HQPkPNW9MgMSLM8wgUGk8V2cohV5YxzMqNl0sl7Ty8Uk+9mIzTMEsbc5OkWYLAPIkNsMMsxaqwdmPBlURpXsGKdHBzF4jr+cPtbHq9nD7cFQvbU1B3CgNCdhsI324Nul3EtZJrXsG1u2GEC5hVy7sDm/rmnuCLiTQETKRJVwLdYTZxLKT5NCH324YdDbQPywEz6b4Y2LX3Z89sX/uwOKmN2F3CqseIP9jNvVhHWc2we7E+ja7T2YyNLC4zWyocRimZbKlP5+t0dmP7lRxOW3KHtx/br/jK/ZtE5YeG1ryQLdnIojNPxCcuuqR2ZSOLRhgBnf6uA27MkgPamI2tOOigFOcH2ZtNrOMpNKjWYMt+FuY4o3i9c9zw8TiL4SAYKbi3bNlFL2pmVh6UMLt4YncwDXsJpolH1qLH5WyXtOoh0Wwxv1d1Kp3omfjwm3flyQMgxsC5tRIUQscSkn7XSO5pIrhEl7XB0x9BQXRk2fFDEh0dPv8REkDHVjtxSKLrLQHSSQDdr+jsfkQpSc4j9XEEDod+196V6jtgph4Ji2QC5n4VJW0T55HoOIZ4maRV9UerHgxqScyJ6oeJHCz7sFyvg+Xe42LCmZHvZdJhJSEjGd304pF6UauLkXkHQbK7iYb3dHoSL9QLrf0smxPYT2LBPWcecVHkdb49UyHcJYqGKe8rleBuldZbOLNOJbxomUroRSxlLiFT6xahlBOGN20h9PqvVi20ZWRS8Qfc4h085GlMNPQpWYYziCSkAFLleDCjBchhSdYjJBo4D9GrzkK1cuUcyYIKpcDI4Fc5LNEGdboebhUMc0h2K2Wmlj3tWkAMqp4edbtr/cTGbvH4sNQNGRv3KTyIMxX6lWpnUHVSmTEVHGrnxnGqLiW189gsPIbYObGWLYRkkm8MM616W5xmEpfeMdDZl+CNaPMmOOQusQNvj6gnpV1qtY5aTXGQXUYyVDTunGPmEed4FtoHpRtL2R5afO6eRY9won/PpLVwSlAEHCOprm/JlQboJfYA4/kexQh1oukg9i3ZsJsxLYP4HsXJDkqaw9aitgzc+1ROfkjSDGqTlPyKuNNdqTGlw+9WM59eyolF5L2K6IB2sH3aXR5FFF5IIZEo3KyfbLuFDRu2wiOH0iwglE7zPDWFpOrNKI2g6Z2n5h7xdKInmliwAU3/PDUffJ8v2nnqcHEd1Hlq3esk4eBuMMmlfZ6a9xaWD3Oeem/xJX62kA8eiEc7Tx0uusM6T82Hj7r7E02C56l56gF3j4qDDkpxfpDomiUWXR/JeWruCq9PdNcHRXdNIDNMFsPalR2MCFa44tcT4+JhMy4SaFIOcyBs56VfxkX9ACc263ojJhnHL1gAjoY2YhINbsR208aeKIRjYkFU9xyUmYEOLHBHTjYeFnySC6f+DRuXMhFPfWVaqkXMyj8iLXfD8k6vyOwGQUC/5aWig67qDVAsF4tgkv3SZCRAsq9dg3SgWO40leARrGXomENRmkzNNF67JPT9xc/v//7X/PLjhxkBX+GnPyavR7s59gtfyl4j8zdGTysAvVAXYPr4XOuUlcvkt/zpEVguxre3Cs7K5VpJCF3kv05ul3X+/9XC4xX9wj/+zAce3Y8fbjb3+2dQeLF++elx/OA1B8B3DnY//Y7HWj+Cd9Sjo5nVfNbEMouJevpiDzxX3MccYCvI0fMz+jIf6ctyvv6GQeFUPK+CEmAGL0gb7u3ghTgMACQduBVOBeq6/rqNN1juC+7hDVphcET3UMkoM8woldghSuEQJRKxROmKQ0+irBUlM6uLlCCJY4+4X0H69Dk28uNBm1Hnq39c4ivfMft6ztXY02U+STRuiocDXb9QSoRiR7USACxjjvM/HGZ1jtF+cgnSsCOTCyQ8Wbn41E1s4pHCzFi5Vh1k4OCJTMa/5xib9oxplvcWTbSk2eCGAWA81Q7/Xs31+PvWZYV/VvPo9s3Uo0MDG+thO40gvFos/6jgKTPpXYCHlCwJfYAHFyey4oInLBNyxOBh5pmAXH3bt++jyKStBsCwY92BhyJzadNGMy54fGpewsBzM366L91724u4zP85YIwxnVDbMikk461RZo1HSbxWuG4MhHVlPmoDYptwc5wgA2JkyWOuPtbN8tUnK6gz49oQH+L2hlR8xWjkeHhbfHa+WN7P7+YP49mrzavnk9nVaiydm9uVXKtGQseDStI+YU6FubFICTVG6w6V9qNDKcKf0PhQJBwH8TTuEVqbAbRja3NHVbweob+QG1uyoI4DDBBAkFFHCShHmT4a3n2uMei01o8iLzPBn5C8fI5znXzNbd/QygwTbjgQAe4IlPZofoa/KwurBd6nxhbn0JPQTctFIzrRvK2bSNdDV2p6ZCZrILSXYmIPsexZS2GklbUOE3v+dckEPPMvmVCiWnz/a/uPrU/lf24+tvpr/1KLWgdirUUeF6ZjaYQwoalWiNaWRmBrNLMmLbKlwWHp+BOkG3ysRkjrC9OBNJckk9LYI1cROM0I3xD4tca4c3iWbz0BEI0ewo31+FWZ4VgPqXgrsQ6HNd/I13yj5LCumSo3JQS0fd6KQ7OyxCrQiw3p+NWdPwakoS+kk4t9OIAZM4uWqZQs41uktq1LmN3jq3UBZID1i/WwLZiTq7ILwr6uCkzNVckBbeCc8tbmm1s5VwZMxqTYkA7bUTqZ74Z8SbP3nZxHgrjBvUExbr8Vj5A1muh3k9SnZ/speVlZYKmZBKCyfUrBGk2FWrxfBJxKJYJrucyaFivhHJJU4vZoPSMgqEP8EaavzeoqSh2cgQg4Sm9jpq99GsfvI5bynICHWMprh9xVwNIplp53Fbzawe/nBJazfcxOoIZ3oxNI0mqB43QCETWC6/2cwJ5DdepRcTBQM1mOJMykFICrf6HVWBZAtPUusg9NqFczJsoLNI1g9fhE5RJm5F46Mxw+He07X2albWAKD67RktdDxduUY0kyxjbTa+wtQAoyzsu3qaO1PZWZiw6Cg1iCQs2C0hV6v+YH+d7Mn6bFhF/Nl8v55zP7eO1ybkhPM7d9fr5Tkr3PrsZqmciUAJYX84frVyUh35NS/uWLh7v1edxMcIEF5YxQggCG5GzN91a+DxFX5okIjiWmXOZGfby41ksIyJSz44ec/Dj7wNCBgGYEEIAlwFRAs6QYApJRIPXbUNN4VNYBmgmHg0ABL49SdY8fj03azhUds8E1XTAlrVKTBTXaCxKopLWxA8gSVqnR/Wj5voeTjiSTVyv8Ri9OL2qNXhxNK4ynlG07DqIag0MoM4E2i1LL7XTKSc1NSOVNKvsN+qlH4u/6y+Lrpnq9X13IEKAVfYBC1muE+uPNZDFVE5OfbN9LS9R3vZzm8xlHZ5CvzuC0dEbIGjhjjjvRGVx3E4Jqb9JRgT8ltc8gXM+w6/sQWjcWdU5a1OMANKl92ZZ7rO32c7VSwxhKjQ9TqTGqwyejnSg1r9cB1kKnO1OHFPd0D7BMQW8iNKfzeFL4X2Waq3Gl8o2g2BRBtjmSYJ16do4YG9quDYSYDG454beTwm2x/rUrDrdfJ7cnCjdHjumQKdyUFlpFx8SRDHTlCOJRuPl0e+9razTn7Rwze9FwtMaLJyabaY8yPSeVbTnoyLTH3Jbz6Rn/Y5z2d4qo9YEAIk1eeCpFrHPVjpsx1MMRadb9Ec7DBY/pDJVVnK3AY4xGpGdxSyvwmI+OiqRaXPCgE3gKCUBhbmXvY3mo2ecspuWhFnj6sTzd8+sdd/UdERpRG5MiYHuiIns8SsxWbJHjNM1McDIgdhOa0oS3MyDGaGr18ausaWVA7EdnvRAVsQ5OlZ2IioJQyXHrei9cHmHcjIaiodJ+dMJY+BMaH4qEY4+diSjEN95VyD2H2tT0f6irmTkAsnfOGzZAyXjKosK6sDBBUYXR2J08TMKRbf5aH1skiNmj9etf8rDU2GmLq2F9atzi0hemA2lJjQ0pIvfwqyUwt7fKVH1fkA5L2J0g3bCON0M6tV1bIhjNpDQZb7aSAqgTQpyG+1Apce/MOJpd7YT+/dCvd/eaa3ZSIzZTDqfFh2my3QZw51pRIgOoZ0jHJzb7IU7VMV9Is9QgTTAxY6g9WmQQJK3RzKMEsSHdfVr32AMvakXR5m5hAALs0aQk/SLgRK4RvPtsnsUn5nnMkDiF2KP1jIABUpqh1Ap91w9ZnFBFKyrjWD+zcRazfogfAAnG0JIi2qIOyovBO+jLcSRn9/ZyFblv7M+Ti/1tVxG1b4jlcBWR7JeFS7PfJwXpAwzouW+GlqeWoaXYKnbKz67vcQiB6OYyDSNGhrbwWFfX3CL2Zt+ei1sLuhGKuEXNuqIYYRTivBWi7oe6XZIuWYYwRpRhTLmA2LHBhwXPECwHQUZU09naKDzWxuGoKxjKp4cTALFU/52Z1BUQYMYp4JJzRkl+3LdCXcG4p1tFfKkQvByqelj7k1kYyCr/3namhIPnBJq9UbpDywCnG1wu7/DCqToAyEE0Vm7jVtzcWOdOpMeWbQ/1FW7BKKW/mU42vaKKq3pVJiNhDXWs0kQnEk9i+x4haOe/5a/kx9Xers9yYQdH37D+XK3AGx06rQeNDp2+MBGHDgHD2JtkQ75+HDJYdBDsd6NdV8X3H5lEQ/Y2C0AqOBe+2zYirW0bE+fYJC/2xbnRsQD3zIYjPbYfY7PhBAC+VbYJZHkDlArMEUENQF/9ZdPqtKDH6d7mi7SIQRExTHXbCitkUCRiz+Kq0PJwbPTDwaiH03I619Y/C+be7U37Tb+XK/1WSbEj/T5ESbHs4MBJm7XbtpfeskskQanR3+zPkqRsm7MqGrZvpUiYVb8Ee66KlkNszR6iIYLEFBVFdhZ0GEM0GHmTj7PWI7dZ12bKdx9FpraPQph13EKIPcwUNEeTuF8zBaFrsS15hxzsRbt5igrWp+sS3ZsLVzbo9tb+7OQuZ3WajVWMU8+mVGU42mlIYbMhneUbE+fj6093q49tKePl6selpvnP6p1tpaS2sS2LQHslN2LAPhzsyljq/pC9ZCwhjM8E6SuW8ro2EW7VMtPuzWGpg8mYOQbtxtbmNlFIDyKrISWU/baggNC1lHdv5vgtAC4z9306md0cgqHjlwAclKHjOk0zoKFz7XOGYWu8XKrJyl1u9YCLyTgNMGyEnCQYIDALq8qGy4PtrJZdRPs/aUIDBNVmHex81UtrryFvlWSVhwvYnqWVm+2ENqeq+lr19Czspmh1rmLrJtEehKTX84fb2fR6OX24Kxa3pyCiVEMvbEZSX+JQzVx6rbCZ7yPs5i51mcmqvu2wlPrmnhoW0+6ZnYqp1CRT23VjyKErZnTZnd3z6Vs3UJMvIjHabh1jd0cnGeBUN0e3jwsYLbyo4+wARyDLif2LH7PNbnfz7NNMbbg2X+2LdDYA8leFvJcDgqt/BebMYAeEhGRiIxJJ7fZtvff+gj5d2gZs/rVOHkbo90V8u331iiCjAxiG1fUfApEbBqnCDkagwpJNGD1IBzDo00JuwBZgvcpQwXLbuOsBChFSktv2jRFwxAe9tgWDPt3bBqr+2HNDQdkPQCrxBJSyNqJw9VQKBUpznOHdQ6xcWxOJSCCEVWRXcx+i4rUA3vKEkvIna/QH198kdgDj00UvhcKpto3AtlSHVVQHBStOq6qpOGqE0lIjWgdwQg2Et9MiTOpuQk018tKi0IIr2OIZohZjQZ8ugAdW3ttyjWzTHCxEOdGBKieWdZhlsBPlJPWKwdooZ2c6IlwO4WkD/qA24AnUXRo2G/DQziP1vC8lXM7TKft72NlfB+W2RHatR8/ZX9FPFdHBb6/nTbZesAMyY5wNb8ZcdUSn7fX4YBDmNpOgDjC48ucRwRC/JPdIisq0X5mIo7+yLlYZmLlPFlI7i83t9dIu9ea6x+fVOZJaD5FcITe2Sq+F7inZhgxSS35T69FzlyfoPKwwSDveVfvYzrrx/pkPfGrHe1zteO0mou52vMRhAzppxzuhl9OPb356AvwO/PT494eLL/++HXlsNgxUwsKR1TeTYNANcZSgm0FArLoV7LFUHhVzFLsQr1ZcuVtb6NR3A90GWC1eA0iijN09nmGH9IejjCKRq5tcUvEuboomFaPrg4qpmsTSb1GzPqHZp1R8hbKDLapzWW1rTFlNIpm9ZvXLC0VQs2j22RMvRROF1zNMSo1hBkmrfgMbdbo4py3eWrHbNnnEuDouRPXjRo48SJ/kNgF4bKiHqENaKgAKFLQ3AwyvjtuAy674YIxvUzxE1AoE4uHS75mhCUFkVyQhddhtLAHQq3kiIA8Eo3dZGzF0h/RlJJ3C8TgLnUxoyajsiJO469DyHfp6wfjNHN4R+on+df8bnb4f+Zy4O4WWgUEM2wGwgwgt3TDx8JRTDy3DpZJQaOmWiofzeJihpb+shg8tf/95fvNMvt5Pyfm7xc+f+Ad+9Xq0b2PnQ4gsCyFtR5a1xiMRn+n4IksnAj1yxocSWKa1/3kKLPeFJvRwOo8lsHSwtdWu5YmA/OgCSycQfXZM9gRiWbR4JO3KnPOoNyi3UV57YSIoz2lLuFEJlR9WbgfufDRkjcaM0SLnSrxbOvWeK7HbaDEBuFJ/qY+QO8gjCMkwK8+YE0d0ZfBLmHzcbTz6iy+v3j1+ezW6pxcfP7x5fvvq/f/ORz4cHYO1ffE6NF6LF/92PBQqkWxCrYpICccZRxuBIkuiGNBM88l1fWjcKTcfzo+0SCNa80SMIO6UKaIbwKhVPQN8AxjDCkBAMgo2kNEHb3rkiXDjJvK+vXfT2eEEWEqIbksISEdOW/Sq0vE3ptJsydl0ErZO3ttOXC3gt5242gsTceIENQwKE064hrpzEhjjwvpxu3PsnLPu0658aO6TTPlgVURD2QjppaOJTSdV93tpA0krUmGg2nGEUVYFYzuMq2EzQADkgDHljWvDqjOjVK27XOkXlISr/+0X7x4p9BTYS7ZYSEQFrvkJl3AekpZcKG3Sqo4e44dg73PMcsoEFSo6FJwJB2Y5YioWZ1iWPYODs/miqguQgDbWPzQdS41+5yqidd22q/SsExceijcUGSUHSgpCciYpIEbsOVI2UeTRp37XDiTysoqtHVzkSCcY1RmaJbnz4xLtGT9rZbb/JDMVUkmolgTIICGSoMkIVh0h5Qap9QcigaSaQgTyK5rn2hG/mXPdQZGLey1PmjeypyIX2G3n7VpQ+/MPsgpG7DKYCjwcLdailcG4gYSagTRUwUWPUoNoT7FFK5NxS22AMhl/1t99OnZ3I84R4xkjmAgmMODCbCaHMGyUZ6zaGndkEMYC6pH+MX18j2aGpdC7IgKt9j4uowpuRBVo1Y5uX1rQOuA0h8a+iSJtLhMJHKCkLo9Ewxy4YR4cOLC6u8CGu8QOqvukBN1RgxGWUeIQVRUJEdioSnUppQxhWtUoFq5RreLt7vUrrcCc0jqvQHSjXvlmRM1d4I7FquMwndU+BHc+Q1cxu1uxw8hC+9TlLcUzaHoZ6i1BNpiKplXWVa+iOdlvDzratNJGrpCBHtmAZEpkKCM0xRKZHTQ6A+yZ91AjA3eJ+BCLZHZIziOzcaqSiQmZ9MtkdiAncnZlsDqZABEmUCizQzinSpmAShkt8WZKPi3ZZsp4mZYDeHzFMjumPXI89INUywRoRFq5iKOul9khAY8CsVPBjLU+tNCGg7P6p5KZyCUzOoZLMKdQXzNDCWAp1sw41Up4pBdOdRxdhD5oB/58Czn0AO6KAKK7CA/HlSk8FPbwKjnC5WaWcoQLrl82TWfDp8hyG7CWI1ygZjGHMHa5jf0xvUU/GAens8/SXhmKUzHHNnIaIzfh69Rqk5mIT2tWcxiWy9xk0kDft5yjWhtllnOYd4kc8zk7R8UK+U7lHNE1LK0cCq1VMNGNfpl7xcZdzHoOTwXbs57DeAjufIao3KvOvl0d5i9/kHqOCDqaVmanXkfNgo5IStq01na3Cn77bYT/gHd/z8if8PXol1/gv/DvI4+sQICq9E/DY0CpHS+Pc2aADerd1w0F4RHk2OyWx4DZAcqfhIeblGYjBkFX/aTUn4t53tloc3meJPptfjPJr/h/</diagram></mxfile>" + content="<mxfile host="Electron" modified="2023-10-03T02:44:21.616Z" 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="wkqIuFIE5UyEHdVd8sjR" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" > + + + + + + + + + - + @@ -51,7 +87,7 @@ stroke-miterlimit="10" pointer-events="none" /> - + - + - + yield lane - - + + @@ -749,10 +785,10 @@ - - + + - + - attention area + attention lane - attention area + attention lane @@ -1141,12 +1177,12 @@
- attention area + attention lane
- attention area + attention lane @@ -1226,9 +1262,9 @@ stroke-miterlimit="10" pointer-events="none" /> - + + + + + + + + + + + + + + + + diff --git a/planning/behavior_velocity_intersection_module/docs/intersection-attention-lr-rl.drawio.svg b/planning/behavior_velocity_intersection_module/docs/intersection-attention-lr-rl.drawio.svg index 2d590b9fd51ca..25a7c6b519f96 100644 --- a/planning/behavior_velocity_intersection_module/docs/intersection-attention-lr-rl.drawio.svg +++ b/planning/behavior_velocity_intersection_module/docs/intersection-attention-lr-rl.drawio.svg @@ -5,10 +5,10 @@ xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" - width="3729px" + width="3727px" height="2651px" - viewBox="-0.5 -0.5 3729 2651" - content="<mxfile host="Electron" modified="2023-06-06T10:27:24.636Z" 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="EZYouzMJcjefpLDW8VVp" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" + viewBox="-0.5 -0.5 3727 2651" + content="<mxfile host="Electron" modified="2023-10-03T03:32:56.314Z" 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="aYRnTHIqVcSkLQHKGcXP" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" > @@ -25,13 +25,13 @@ transform="rotate(-130,954.7,670.3)" pointer-events="none" /> - - - + + + - + - - - + + + @@ -50,8 +50,7 @@ - - + @@ -88,9 +87,9 @@ stroke-miterlimit="10" pointer-events="none" /> - + - + - + - + - + - + - - - + + + - + - + - + - - - - - + + + + + - + - + - - - + + + - + - + - + - - + + - + - + - + - + @@ -436,8 +435,8 @@ ego lane - - + + @@ -474,16 +473,16 @@
- attention area + attention lane
- attention area + attention lane - - + + @@ -548,12 +547,12 @@ transform="rotate(225,2890.14,695.64)" pointer-events="none" /> - + - - + + - attention area + attention lane - attention area + attention lane - - + + - - - + + + - - + + - + @@ -753,13 +752,13 @@ transform="rotate(-130,954.7,1832.3)" pointer-events="none" /> - - - + + + - + - - - + + + @@ -779,7 +778,7 @@ - + - + - + - + - + - + - + - - - + + + - - - - - - - - + - - + + - - + + - + - + - - - + + + - + - + - + - - + + - + - + - + - + - + @@ -1150,8 +1118,8 @@ ego lane - - + + @@ -1186,16 +1154,16 @@
- attention area + attention lane
- attention area + attention lane - - + + @@ -1224,12 +1192,12 @@ transform="rotate(225,2890.14,1857.64)" pointer-events="none" /> - + - - + + - attention area + attention lane - attention area + attention lane - - - - - + + + + + - - + + - + @@ -1376,6 +1344,494 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_velocity_intersection_module/docs/intersection-attention-straight.drawio.svg b/planning/behavior_velocity_intersection_module/docs/intersection-attention-straight.drawio.svg index d33674a257d9b..640eba618fa49 100644 --- a/planning/behavior_velocity_intersection_module/docs/intersection-attention-straight.drawio.svg +++ b/planning/behavior_velocity_intersection_module/docs/intersection-attention-straight.drawio.svg @@ -5,41 +5,42 @@ xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" - width="3389px" - height="1411px" - viewBox="-0.5 -0.5 3389 1411" - content="<mxfile host="Electron" modified="2023-06-06T10:31:30.247Z" 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="U1JUqxOsfTr30viw8iHe" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" + width="3390px" + height="1412px" + viewBox="-0.5 -0.5 3390 1412" + content="<mxfile host="Electron" modified="2023-10-03T04:48:10.725Z" 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="02kGXKyoYgRU_RdR3ghD" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" > + - + + - - + - + - + @@ -390,7 +391,7 @@ - + - + - + - + - + - + - - - + + + - + attention area - - - - + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_velocity_intersection_module/docs/intersection-attention.drawio.svg b/planning/behavior_velocity_intersection_module/docs/intersection-attention.drawio.svg index 3e9826fd84ad6..57552f586e63b 100644 --- a/planning/behavior_velocity_intersection_module/docs/intersection-attention.drawio.svg +++ b/planning/behavior_velocity_intersection_module/docs/intersection-attention.drawio.svg @@ -6,9 +6,9 @@ xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" width="3707px" - height="2193px" - viewBox="-0.5 -0.5 3707 2193" - content="<mxfile host="Electron" modified="2023-06-06T08:17:01.045Z" 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="0BpzzxhIQKLXKpqq79kr" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" + height="2195px" + viewBox="-0.5 -0.5 3707 2195" + content="<mxfile host="Electron" modified="2023-10-03T05:15:42.124Z" 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="WBrLRHcQvF5FOJ4jcZAE" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">7V1bd+M2kv41Pmf2QTy4Xx7bfcl9JiedTif7kiNbsluJ2vLI6nT3/voFJYEiAQgEKYCEbCvZnViiQBH11YeqQqHqAr/8+OWb9fT+w0+r2Xx5gcDsywV+dYEQwoDiAqn/Kt/8unsTIsBgIXZv3q4Xs/3bhzfeLv5vvn8T7N/9tJjNHxoXblar5WZx33zzenV3N7/eNN6brterz83LblbL5l3vp7dz642319Olflc/RPn++8Vs82H3vkD88P6388XtB31vyOTuk49TffH+WR4+TGerz7W38OsL/HK9Wm12//Xxy8v5spxFPTO/vfyGfP3vu9mvL2b3b968fPXH/377ZrIb7E2Xr1SPtp7fbXoPffntX3fk6k9y+Zlfvpfz3ybo328mYv9s/0yXn/ZTtn/YzVc9h+vVp7vZvBwFXuDLzx8Wm/nb++l1+elnBR713ofNx+X+4+X0ar68nF7/fbv92svVcrXeDoPfbF/qkofNevX3vPYJ2L7UJzeru80eQ5ju/65d94rLy911i+VSv3+3ulPXX06Xi9s79ee1mqK5ev8ycM72c/vPfL2Zf6khaT+H38xXH+eb9Vd1yf7TCSQIk4LuvrdXjQmHYD/S5wPIsJ7dDzWAIf3mdA/t2+oWB+Gp/9jLr4MsteieZRkuSwYpMCWpuWsQSX78ZTX95urtHz/8yT7PFr8SxjcPEynbJVnKpJTXerWZbharcr4mEpw+WRUXT6/0rUDLJFLAaSEPL9GcUcoxVB8zCrEiVwSxNb0UkAJhjCjDmHIuqT3ZBMhC0MMYACeaeyHa576ctIVaZH4sFeTn1cNiL4Cr1Waz+qhkoC94sUfyZmWollpL7svBPn65Ldff4mr6sLgulNCUjtxdv14u1QI53+qXeuvF3e32zqAQDHEBMScAYqn+T10xv5sdPlfYxYwreUjOGSWyVKb1tV73QMG4oXF1TawjiQBLuffKuVJ0sdiUsqVutPkRHaybgjRR5NBKKKANFMhlKmQEaOWBX0E7vwYIYnQxQN4uBySwLQeWjBw7mSzdxXC5/cfFrCGCUOo8W8wPS93+qmTymRjyUUuVvXpxMaR8YLt8StIqrfvDBNXk0RSeZWFUarL7RJMbsWUx/7LY/L4ftPzvP7YcSPd/vfqyv8P2j6/6jzs1B7/X/6h9q/zz8LXtX4fvzd4syol61UMdH1af1tfzAOpRq8Ht3DeivnA+a/hFNnwaq6+NDv3eer5UOvBP08dyQWZ/h59XC/XEFToJRQ10YtIcYffg+y/VfRdzHNAcRxluzYF2E2MNtAVw9dQnYDrAtK5h+no5fVDreWJYgwIx1IA2EjwNuNUgP8/XCzWNpT2eBuIiEOKa/zOBuHIHG9CEoifGpTGOqSupIY7bIX79af1P5TwOy+GgE4cPwcW54E/buxo3tB/8IOSNcUwz+gj8FASmX2uX3ZcXPPjUxflzj/4q1LwctVxuPASWhqrsfm5UxREBiuNw1s3Yh60Vrk8M7bhRfuK3e5WzcZ7c7yfAcA4oY/uFuQ5+Rmz0M1lID9ZPc9RomtW6TlPBIgqkH42iTFgFF4gQIjigjDJO9eKg1aonx1CG63GipmrjYU06DdQQtS3V7DetszWVq3uIUIwUfCPAH3wjtkZCQFwee0KVZLx9tmPHTiaY6UB07crdy6XBlePfT4MrQAX77gTYsRQ2uGQC4p2DkWVfB6WjiFp9DA3XTNgYAmzYNT0J2GAHc5jElAtB8L7G4DRKJG1sUhjxBsokK1mWYgmAIlk7yEalQ/6YkUKI6ltcGGtcNC2GIMAKfVTbFs4wbextiwNig0kdswIf9qlMIGFe2+gSgOz9mRF3NaC24YeMp48vJoiUnJgtG0scRNmsTgcmVfwcggD35Vw3OHpIalISM8dEMIEBVxwKmmJDkBaUHjhW2jJEWBR67R9IhgHexSkBtEqGiQJo2gxTax43TDFSvRG0FbL9rGfwuEJLa1CuUppW0666MhPbjgPfekHKDLb6gtHP7vPfBCr1aK5KA5uFAc5htNBzB80ZciswDdTJGUEdEuBFYXAgG3lvI0wLLAjrXcPc/kcVzidNGqWGIJGfP/yiBJPEBrooIDlTBSRYNmHZNKRMVPYMMjC/mptrTbIYhDNJNnizZvAIBGPKdaxNTWPWJlw5xwU5xCf2wKpn+cjG1yF2BBKxYNGzKP/93Wr2hfzzYUEu362/+5v/ya++ncCnFo5wBqDVBLt4csblVdhWkRfCXUISTWA0d+0gB8WQYQg3YBJHIZzycTi9I4hnwlDBGeMEM6bsfVHtoWr5AJd8EHPIJ4ZD6xZP4piEW31EFvIpQ0UuYtXLppAu8XDHMp5OPInDDWlzLrsJrm6feakkE6OLe9EDedHXzPKjkloDx7Os3NM+pBffAZAt3rgPapkgqLOkw11l78jYBmck99j/RIf7xnKJ3YBN7xF3gWmsnW8foOseq3edzwT53QEaCn01pFepcEL6nP786vL3P8D9j8vJh3e/bNYvrz7KEMf0FANLMo5fsDADq7x2ykIMrGjHMKWxFzdhXNp5S0SvMQmSY96hf14yPlvBW0L/pr9/+Iku3qcWyvGEz8rGrYlLBtm88UQCScFMqQhKC2xvsDGdvF4XDIeFT+NDBXPF7pffff/7jzOymixevf/++//wVxOMnrJklMdhHsvbSsaRTaAkU0DH1mck4TifqVv6rW37NRdL3HkyM1m8lEWMMTDF1HcLr5Q7MkbjkBkBvEj2metm6qfjqEaZU4SnOrKPGTtEoHjYYZAZvyoldsg+mpgUOwEO6NPADoPQ2D5QytvTUFajUWRuRkBmsFg87FDErZ9O0mOnmy8Ygp3Z9OFDFeuw90rflP+cMcSYJqMDoSiXivcGmT0eS7c16J7iAGv/qfCHg8Bxb9FSZLoRCdce+2Zq7SnoABSCA7Lb2+JJDdIo8fB2/93VevNhdbu6my5fH969nC+vtmPpnVKLafYgbbpCjweVDBs7KuGopEJYoyFjtHiotH86ZqD7LzS+dDKOnZGoxGVuXOnHMtFBrVguN8eWKLjj8BYEEBTUkXfBUWFaTNFiVIkjIecpLl2NLz9xxXdSHruhaVboY4wY43SwRaC0Rwtj/R706gRAyD7po470m5FLpt+pR/r1pKSI9K/Ez/9Mv75/dfvy9b9f/Rf+NpncBuUfjZUYKC1vvtouqbMZK8/B2LOmWK4AGEqk/hWY6/pg8WcwdilZA9dX02sxcy4wCBNCZ07Y+mXdjltrfoF+QUf6FS1nOtX0hpRVqzIeW6Z2+nC/q7Z8s/hSisOcawDE1L39AQDlwLH832xfSWRQ7QUXhFFCy4oGWNnqTTdPFhAiYtWWqOfHokKAMr2OEwShDmY302Pdl8SXZYDNFkeWe/vBtgqApC4xVuV7k4mR4QIiCSmAlEConPMzFmNIwCitGFGt2vKw2ghJQcskYUGFUk0E4XnI0WWR0dHVkW1fOcgRNcTIYAEFqhY9aO8vZyRG0ilwcSYGSFUBhBWSVEYcJM1NEmqYeMAWFOGFdOQBUJBIGnRccpzNb6aflpsMVAoaDg+VBcMHnWIoZ52iAbkc+hTOzXL+ZR+3uOyWN6mfnRcISAzU01EqoPJOdyPpNgKFBMoCVEYD4hIwyXcH0Wc6FgIVWUFlU1ClAIpShaDdpZxJ2EM9RCEROLyatFyW2++5HQNJ15FTp40H0MTQx1KHKBhtA9MXR6gfevAF7esJvr7rMsH5BCo1tTYHOeqH7XI0K4+0MmkGwnOILfmM5zqefbsadTz7rssHz1ySQuoOJrXcCF4QTgx3qAfAncNzBfwCmBk+iYHeLVtioJLofYB+cj2BUKB7M5nrSPdemBHUETFxCPtniXBoohqYu/uJER2wBfCM6Hbqdpw18l2XEZ4BLBizuJWKguODwdw/j9I5vloaUAGS1T52Aj1RfWzXJuQjsVF8R7Zabe7cgF6i2QAhx72Jm1u5TRyBYYm726GBYfB8hsStMxNamRvmVR55UkaEmuc3J4zI/kSNkDkaJQMfpg8Ii4yUjqC0G5mzI5G/wWNYbSIevTaR22UMSFR4VLWJXCl81Ka50BK8fsB26Cxo9H/g9bJhlfgHKUnkxklAKCdJZuW4YjHaciDUKpdkpYjcYkl8KPuUetXBxZEjCWtbYcxUIse5eeBYfJMVIzr1KNWo4c+OomqPY+YVtcGGrY7LnMraym024Qk1mszDqhD5x01sPgVkAoxSUNhfisiLtFwA1FHQwfVYeHPcFlxGOpljPg2Pe7zGLdAA83O4HaJokRQveNu3cfKKkXREY/jmu6E8ZFSWxPELdTzysyicWmWKdPJgn3iwORpXMBgWAfHLbTxuBHCMzfIE1vmhLukK3BoND4yAp34ayVJoHdwcqO6YWyqJz+BmLxXzjBilDqnoSRlKKiS95fbY9hm86G6tRqkvzIb+XfsM0DhQd9I+A2HGaKlTHnLeZ5BlR0YBuPq3KnR2MJU4qX2K7KCXerdgorpAF8xrFsFrXMKMPZ54ZSRHKKkQ3tHNFq0fKuF9B2RZQfIwvUbaGqSg4Lz62MHwmMqCO2LLPFVsOYThx9sa4gILyhmhBAEMibU1hDgq++RxLDHlkppbQ0gEIkcR5tjQgYAWBBCAJcBUQLM4FASkoEDqj6GwT5BTNYJu09pcC3jBeCr8BNBpdEXftbkeVVwTI7BBCSwotPVZsoIk0md3MvapEY5H0vHKK+b2hD6Hvea9MBN7jVJWNxFE09eGUBYCHZafnjnZtGmHGDchjQ+pNIgn9VmEgPDOGPsgWhcKBGhDH6CQfo1Qf5i9RPtqSa/dmGCd0YTYqjMkrxAXFdIDZ8xxFJ3BvpsQ5L1JpK0fSry/Qbh+w7HnIdQ3FnVOWtI9JtStylSee0z9MoO1UsMUSh26nZWZUmPkwyejUZSa+3WA9dDpaFEOlFwdHlvCu3ezoD1wl1uCMMda5IedFgEOJ+n6VJKzClU7R4wH7ZefXr+7//x68oG+/OvPn7+8ff3+fy8nroAFq+oeNCDO/vtppT+YPGzP1L9QF2B6/2ULFf25+q/b/f9uB7rSb7xbX03vylDeevXwsF5NZw9bCSoBKkispzc3SmcQWG7Ft/+ueqYra7x1wDv6jX/9OL/ZTD5M72aHm/zP8e8F3s85L1fT679vt2o+ud4pdjk9i7vFZrGtrHt8hk6YanBdccjhTcyuxfzqxr7h/HZVzvBUmeaHB97dvfpF049lDOHu6mEbSpiWMrpfL1brkoIQWCmnvpzJD3Pz0o6TmvsU7retjqO6esyvi/lytp/Wh+PzGk2pGrN+q1jtTgvkmBb5NGZHjau7Us57Lfm0vpvMFuv59TZCdBB/0fJwLjkfufThfssE7fMAQsmlG/zUe7ufoN82VvPN/MumuX5P99HYpeKTCztI+3Exm21Laq/n6tfvdynKJXTvR6hR6eUFfVWO9Gmz2j2hI8dh7zY76tN0W4s7lbulsJmzPeHEEX2ryls3Tl3ICLE358qUuGOsMxTacQv7oCTqhvOHTVIpUWIXJXbErWHV2zvBtrZTUPFbX3SbxGyMRJeEzFkPNxCJlMZoHJrVtqIVmLdvpn563GRWJ3a6tTx43NgxHQyiS3v1wo45GgxMDOuFHeunRw5SObHTMRH6EYOHQGEmgpxCPFRayRLpiIda4BmEeILKIT9nrtYEJZBhJDJIcP+GTY7xmK5rPFDsI6iM8hMhEGv5J33juFudNkeDZtn4mARi/3QRu2GTGz8RSlg9N2zqhEohe2dLYkmt0UQyVNo/ne27IHb6hcaXEuE4oHBVlGbI/oPq9ebIozraSMrCytMVBBSuLFFXdd1I/X/cwkocFzk7YSFYWGnQAisJuoTlSuJNKazn81EdKZ8jmwJ7l5YiVWWJ2mgD25hncD5qWIUFwCqOhF2Hc1wlJxJGMQdIeelevTHvPX6vMVHf4/demA/3SGruyMMTnCAJzNEQSJa24oZ0t8j8M6RbTK52SNPcIC0YLaQ060bXIjgkSlnplvtwSGnK+tJOYeBuEb7nSur+fdhW8OeGfWVVWFtaZoPmDu2eLYeeQ5EsG98N6G7hyufjwy0meTud55aFSDAxD7YTfbKtj3ckrWPy2BgtMaRD2gM9+8cN/5iaMuPmxm4HBFijqZU6WQ0hNwKeIySdEwXMbr4M9d+Fk8QebWAEPEdIrAiJpeOOqqkEOdrxpYyQ0AB38qkJygxlMeionjp0KEsHLZ4NxRM9nz3g210fkdki4TIU+zcecRiKhA7rzLP00dnHdgLLu9i2Qzo334diMyuNMcZPOYFFoLRsH9eIiaEtAth6V0LJ3pM9cW3rUVWJcEBrrRi4OYOUkYL4WjlQ6EBJit4NzskOKbDy6Hs3lGVkwwq+BJlTflQH21OkGV/DQ/ZvcGMlIN42VqOAIQUDcatksBZC7AYBbsGM0MEhvBzakR4OQ0psYlS7Q8jeYK+KJg0iMh4hVbIp0vKgoG7sbW9AYktYdbnGKjDRKBZxqErDgWhYhqXGdK1K0xEbrYafq/SEl/UyMfyQ0WoOmiWQgjtMGCXZ05VScitAQI5l6lJK2KE2R1Wh4BA11aFcodsUQv1l1Vc6VGvCzWpNiHXXi7aqS0NpSV5hYdbENu+pIxAaymYGliPlR5vdEbihcQlynPk4lfxCVK6mH8zQD9RDP2KFzobSpLw6dxiapPzdSKo08HojAwyufKIKFHGrefe2LmKEnpAIVoOYdBbNvMUBq/ujDytMBggrVLDu0CbSqBqt/x4vsBDSNye2/+raRxtfOM1tBSRt0STrDemWTEA2wmgtO6MHFnook5EEB7V3NlpgQac/5WvXjbNL5BV4q/Wm9aDVetMXZmK9IWCQvVmpPbidIG0OhOCw5pvo5q5ETF5OhuxGrCwXnIcmN+O8sptNnGPQE+fMGGdgL0UEbOvnEBU7JYeljKTRJswRQS1Ad0fS+sS+EnB+Xge3EDGoum+EGBn9ZbDp+8dqGguN+6AB4l8ywENNs2dW6lbtwt3Lq5/jnieHdlleO6dPGb6yoAMf/ZcRjII+a/cIW2vdBNzKbRr9rdwm8+I254F4hbF+BKdGY9aZqKqw9VCByRFc7LMkIkhMUTHhCF2NQkSjtYfotIXZjXL6MGNsmgo1wWRebndJLGalDQ7ICTQFrdFkMppyd0AJsJdGamyp7BFYgENzO4MlhBSFlFVvO2RzBqegkKL6vi7+XAeGwOqS2sHwCKHwy2//uiNXf5LLz/zyvZz/NkH/PkxL91n2S+3kaYYFxGqiBeZqAWaQATyfODahzKl09HlOMJXOnjEh5VBPWVjZS/F6e3CutTBUdWVr+0AvJsIXS6qkxahkQJbtdLnhX6mVtODUpxBYkEK/PUif0JDqoyP2CaUX5hYhCuoG6oLIhNjNQEfAiNEQFNFm33ajISjSJ3rGbggaUqI0tlIfO3M3tMiUIOrLnGESBEgMCwcdp+sVGqDT0WKZlSw6NEuE0miWKIdLL2oLXfoQ09pWTSOjta2a1qZMzGalZw2A63rvOpC57QporVudw5rYfxfQvEuioGfZrNvzK6B0/oq0TQ4DuPVEL7a7mg6xW9zW5PA0ZUTnqYwKeU2ANvHJ/FoSqoyEeO8Ce+liLIXQjmCqXIqhtEGvdAeNKPNWemy4xVaN0J6+ekHLRDUgkg3QGrWROQIx1iloqIZxF+xfDeOpxv3b//w+f/Hd++9/+O5vePlu9e3tPz852yRGNMODm1F1ygxLZpjryJ6mLYEKV4ESAAvpcJgUlZqFMPsY4U45pV/Sz7NvcShv+cBfpy3fdZmwllSEQWAVFTK6jSgQFxQC/SnjPfsKUIbLypZ6HNzpJolJa4xjsieTVn2T7a5sgpEyKGTIizgL/BeODGQMcWHmmUQjsYCth+FJrKvD3ytlqj+JZUI6ClIFqMX6jR7PBBlbssFOvH9cjrmCI6heRvmMeDzjFEHAbrA+auTYQh//9BGjxCwtw4Wd2K6uK6hjlwCC0vyBEql/BeZmp6Noe2Wdyq/BznR+Nb0WM2c+hFrGCJ0FicqLj2BeRk232MHKO1lUeIcOiqawiFFGxvlEIY5ytb3SIobpw71Cq/rjZvGlFJ2r968lEmVycOBIarnZvgYUldm9T8cQ6mF4VAiASvOHIKjsA9c5Pfcl8bebQ1pGJhXcJZDUJbgdjscTnK7xkqncQmq5R5GbEsWbN0cTw9BeSJlpnXCcjs1IeCFOeVKlY9tXBnIj4Jzk1sl/PGuDQx9p0OY0seRClIHtqLdr9sWNN/khTmA6pZnNb6aflpscVKa57Z+5ygRk6OusnJvl/Mveh7/s5s7rZ+cFAhID9XSUCogl242kc2MLCQij5VYrl4BJzncfa2cfwgJCAiFVHouiSCFoZylm4nTTZhFK0dPHNoaB0BgnsRft7BXItjp41UAQ+++n1S73vPZft/v/rXR26whrtT1cuE16VwpsfXd+u1JfWU4VGexHUU+xG6g5uHr76vDeKevBssxsu5xe/327/VrDBCtfrqWhfG0/qWeBU5unqsZUDqqzQR4raZ8Bq+0r1pkO9SPy2BFGiHFE3g2s9Nn6oVKprusTHmzGKt1sFZp+79XATGhN/R4omAGnaj3snlXPODCcdYapMVpqjnPFsOJzHL8BwMVxXxfz5ewcWI6/AeCsWE5SO146MMulb3pQScVmohY5Vd/sw3uxWS63ZgUM2n2ouGGhd2A5Yuz0lk05jHSDxCyHXFHGbiw33WyUtEv3ymKrEWnpQDdZ0hI0++ZwgB20lKqilBsLIZHLJD1YA2ip+mYGtITyyh5RcEJWoy3Zt7BqucdokZyAwxpfyBXPazem9jsTRw2xio+uV3c3y8X1ZnF3u2etBzdtHTG8DK1Qer9p6oEzq8RBMdP9gahrhcwyl9M6KfVxMZstj8XNmtp2hCb1zQP1KyXpoWZMuTwq69rLcXXdSEZ6AVvVWWcEEGY176aOWR07IwAF2Lx5RugrhDyenAA8+t5yqqSA7sI6r6yAoE7hZ5kVcLLk8k4LCOqIfb5pASdLL+/NMhywv3yeeQHdBXdWiQEh1c4fi9mRX2ZASPXwc8wM6KE055QaEFJa/DGkBqC8wsuPIzcAR0uxn4zkUXPArLqa4JgPZ6NhKI8aZ5Vkf0xYfoyER49Mn9pRXnBkn/qsEu0Ti8tc7hwFb/JZ7sjo0ZAuPvWwksN5S+78cu2HFV+Vm5up/AYLisRwqxOLzuwZn7nossq4TywaYeyH6mcd0bUmZ5R0n1pxmpHE3BXnieTd47wS76GRIQF7FtEwx6mOjA3kXpOA2IyjWea+2IUtu+QH1pkwU1MIs6Nfx51pOIgzTQKiFgMuZ8ek5YdEO2N+bepUPt4zCQhmRLPkARDTWhZaXxc6lZCqUmbNjRaa9cY0GT380cmJTiw7fk6io+PHP7o40KnVTpyT6EZPB+nkQA8rOp615AJCH4/A4NCf2rk5QzvMdNwUjm4O87CKkjfFBQQ6HoO/TPI60UmbFkzviu7+YRI7yzR4L3pwv5hIpUAECMDVvwpPzT4wEw4pKwCvHDKbPzlxCL+sb4xoVXxPRFBKZ1MvFmB3pOmMF1KuM6j5nR8w4RvTWBbb6MQuQMGMMp2QkEIcBCKpnUdQVovmDnc7RvcPt/ACTI+8OvpwxDERHEtMuQxq7+MCkKO7z/j4MZr9YGNphkCUNKDL8lJiG7Wpm/24MTRGwV7McmQAhcnC2HalSuuBw/0QqND+5CCazhN3SRiv86UiCkAumi2C5EXXFkEdMdHaFFMza2tTTL1+ZmLsQWW41mwRYtRMEKRuioC+B94hM+7SuAn23yRxq18REAOL1hErgtr0LXGt1IY11AZ1Vppe/bESqBDOS4WoD9yEGujup0GY+G5CTRUK0qDO3bF6/IZYvbHcqpu+bMEJCpt7r6xwxcTnqZhY+vDKYBTFJH6lYH0UM1oQJPiE+/BBEM6MpDcmHaFiqOaPOjoK+BIFzP4l0WxqZ10w53Se5IYcO2EUPsvmBA6TXeGes/EP/weJwi/wdo9wlHwK54921nUyF8WI+RQveuZTDCKWKqGiIIwSSjlhmDHjRIksIEQ1ksllU8Qt3tH3fVuzLYaULMMFRBJSACmBiuzEGUs2IDg7cjLGoDqr7DOqph0KKpQCI6Oz9nmJdvyz6AkMkuNKWahlT5sWEIOmpUfd5towCQJu8eR+WH1I4UFcUHFQO9bcnqSyYBhV1o2jbHROavdETrPTvE6zE1JIfiBm2rS2OC0krqxjoFNQujfiNW6Cu9wlteMd4PXklKqv1lGz7mPVBDADb9w5xyzAzwmsNtAp56qS7bn55+5Z7JTOMZRl0ls4FSg6FKdsrm/ZnY8IydnI1J8fUIxQB5rOInk7JIciPyd+QHGys5LmuAdyezruQyonPydpBjiDZ7QiHjVXPFQ6fso+C/DccvPIBxXRGaXxs4BU7Efhhe+lkIkXbh4i7ZvHD1vOAyR2pVkHVzrPonIUGkkBSiMcyeBjF5XjAf50pmVdWGcCzb+oHB99ny9ZUbnu4jqronJ8MId98KJyJ0su76JyfDC3fJyicieLL/MCS3x0RzxZUbnuojuvonJ8fK97ONFkWFSO5+5wD6g46KwU54l41ywz7/qRFJXjrpzmRrs9uxFo+cHkYSvyF+oCTO+/XPiaIFdN9d6tr6Zlr9Dr9erhYb2azh628lLiQmCznt7cKBAisNwKy9OT72od8I5+41+/lMNNPkzvZoe7/M/xLwbe0DkxV1UHvsm+SWE5P4u7xWYxXXqn6IS5dvdDZNdifuVoLj2/XV20tZaefiz5/O7qYUvr01JI9+vFal0ebkFgpZi0nMkPc/PSjpOa+xRKxvGUeWBdPWa9Y/fD8XmNplWNWb9VJHanBXJMjXwqs2PC1V0p572WfFrfTQ7BtIP4i079yL2XPtxvqaB9HkAou3SDn3pv9xOCG3vqhp3L+U35ydF2neu5+vX78GC5du6P1alR6eUFfVWO9Gmz2j1hp46h9koZqz8npdLsz0movQvEnP05ZTKb1BX67dahOqR1bPm66UmVGbS7Ll/bT7Jsd02gzl+pgt9Yh1nq7a6xI/KdrN2189zoc4/h8+4xTKBRu2DCgbAd62F7DOsfkJjCjpkqdcMkaxIrH+AF64CjsUlMotFJ7Hij9FBoTTcbNVc7E8+AyIhYOMg4TywI01QC0IEFV12vdFgICXyfVggh1MyorqsKj1yE1z9oFllwR4d8gqyXMvCalJlEkbbUYtT/Y5j0zNQoT9VLZoxGzMhj4oCSCDlLdRoUq8XChksoZfQBZ2QoatMgHygisxQlE6xnWFONRplhjjFm9rVMDcWQPY3ToFgB6ryhmFfdFgUeDE3wSL1D1QOKwtzSYdysjxoPih9/WU2/uXr7xw9/ss+zxa+E8c3DJPis0ml2S5/sNGxuFDMBeCGl1IUpXQVoSYFZVbmSOEwdDlnBxKEsTqpqlCHNJ06pRsleiteXby7sftHRalF68RKeqEGhEol5Gl7XIOO44OggUIdXDGgBHaf5ktWnDOk8kVcl2qDisy64TCCOWn42DmAgxgWoHaA1WAACUlBwgIxeuseuPhvS9iK2vu+z+XMRYCUhWpcQkLYTisWgKp3eCa2k0zS3iC2InKrv+eTdWnxPA761+J6+MBMjTlCDUJhwwrWrOSeNaiKKmrzjJq4eG5Q3dULB2COI71RnuVA2WBPRULZCWv1llX+N4qmcpA0kr/gNA82TjYyyJhj7YVwNWwACIAdMOc1AE6vOoaZq3eVKv6AkXP3/YfEeUAMxWrXkE8Bfq3osGnCFonux8N61l1vqJftU4dz4vsQsp0xQobxDwZlwYJYjxsqSbRLxniFObKTOQgL6sH/XysfUKI+iPFrXbWMVO3biAp240Ng6ZAPSF2zIBGcTyDE2dmAop0ZkMTw4xJC5n2OPFglF6qcDsyWuutkA2Ak51/JUsUOlMU4H7Ehkjma5V9Gw47iZztpKip1uXt0jxo6DKaiZ+9YBO9zMKaDI3G2Jhx37ZlQov4mkh0+37cAQ+MymDx8q89I+PPem/OeMUSaIzSn9GcoezcJs4q0PGICAkfY+mCS0kJJRiEt6NbacKOei3AfhTFJwqJBXBwKEBabV15EjGEoQLBCqxsAxAqPuaQ4IRIwXUGeIC4i58mmVyQyxFV2HQM0MBVxyziiRZqidcTtS6+wTRkIDtTbY/OAN7wnGGpDgvLmnjnmBMEaUYUy5ANhRZFFnpzaKQHCZCjcB58dit3MbXUgQmVKyhFTTWVc7t0SZfu7nC4i5jNdyUxHBbDE/5DYczcCLJLuJUHxBMBFMYMAFR814MCLbvU6P8JAcVHgBG9QJOopF3xJRK6ERRC47UnaJfzljyh1hUo+KeXUlE3uOybp9oUWvt+VbbIPgcLHvJhALF5kNZPyFNCNJ3VzPpQon7/T1wG59c8OL3XaQ8/MBOSy7JNeMaNhz3w9C310E894kkiPtfVDufM60IeHR+t95lpcuu4N9N1aiax4/T80jSrmUO0UELRtulBuCTVsIKCsXUYK5kFxSKXpm89Im7o1Sy1CouxwWGJGuyZ1bCYILBA4eX+AYNR2LSdmAyjJIISCOAiIl2XgAcZJRigOW5egexS6LynFc5EgVLD1GP6ewgkW33m3HRXFoJqELwsQXSyI+r7N28Dz3tZE6yqeVcjVWM6FcVhBCJCi3gDkUghjhHVqA+qunRV/GJRFFGHIoBWekyz0S0y1G+dJtlWZ+oFsB1WRhgBjGgiCbeymQtYAcl7qEXLPMJ3AMEl/7A3ZpH30El0aODVZwDY4vIQmLWlq0JM34Eubu+O54EVwdQx5yMQ+ODqYTEwU+MSFRyumgs9BWfSxcS32qOCDuVOAwqsk1jpzq+lLJgVaL2XiCSH8qcKiYbBLDTIu41WnGoTFZfWEmFhwWSDnNTCrzTflnUDZthtKZpaLsbIiRWvJM1g5OVGwMygrMDjrAZFieRzSTLWCL71gQNuOY0dnAn+QFf9p0WSBrWrdmvYBQxBNgjAv940aKyRJj4efOu6bNrgzYnx1epQqOGlqFMPHrlSPxPbrO5HXQnGADsjKSKhhRAb8DmZr9842PMtHcUWlMmnLeAfLnXxHlvNedMYfbhYVsbH9G6J7izpLN+jzzuaZf4fAgrjv9Sh91d2dfEVfbl2GddxIQ8s08/aq7kMz0q+5SQoO6jAQNL6ShErC6S2/CcMFqCVjGkRZlgjUTTuyWL0hnow8jvVNPv5ylt98rVUWDodWQ07TV6vzoC3Ox+ABxWQza94cG8fQz/zD13QSbN0nWT88tkPRVGvo4KyUlNLwVwZN5K8Egz2vnURLfomnjqh94GfcuzX5zOjV2A2LoY2QT9opiDUHPmSBXgckHKob8jNghD9Zzl9Lo9ClIpHgUBH2e9dgjCe9g3BzshOkRztlJe7I0YCdGt3Z1JBI1bWijEmPtkxm7YpSFuDfpz5vprJcqxsGYvTHJiKPzV8p0MJIogOnNO6oENmzekZ7bTKgRS+UKq8UUEiwglbBZvhf3rQSMSFmbTflmaghRVqgwUohIISmxar8NtYx37esR2hrn18kuQIbAX5/udO+az+o3lo08enTG+deP85ujraMufEXSM+weY1bMOZH+fHlYVPvyFc1J5Dg9yxxqBUmqMICzcuBzN5mz6ibDEDUXUCId0cFUjRjcwAqIDj614vteDcxk6VVwwkZptQnFOgG0T60FbtIeSVfx3D3DcXuCKMFP86ClvHuCMDPGp+x67YI3Dh4n6gnixkL6QF+oVDKnpbySEspVznADJpQiAyYdaMlM+FHgRMPG7Wj8IkDdMJCNaCkBop62bFeD6738EOXpdRo6WnUpChnz33mAJCx6aiHeJ4OxEwreETWab+h01e/a7jxIKTwakEj7NDDWRjYnFMZTNjPwYixZlby2Ow9WMq9bOPa5Zh6RsIWWTmA8/9BDV9Nz9rTKMnb6S3nlc/C0qy9JGfYCLoNIKgQBSYujtbPi1hbb1nJQPg9inCBgp6OR7d6n+hArQ/ZwOqeRRkxkIWg1Bje3qKPNrZbkk84jjn0I+ADY8OOlTJYp+/skR7PpSPMQMNafjpdGzAI0MvczwD2khJWnA2qVOVBDSkrXSzFZSlsPyaEh84h12muOyd4nlnHsILuGDvmEg13LWjrhZJcm3O9U5MiFGkFo2FUrQ2sKsb4wE3eD0cbCgJoGGgfOlaFzCjERnuWn5SaJXRB9gj+3LMxBazomgHleXjUmrD8Cg2EufVYWRL1w3vkEMej+pEnjP6zbdknGR/QTlXUMV77QYyqZKR9hPkiWVR9jaB9hvkWGjbrIKKO+XQlGizEg6t3v4rJb0TFHthJVvouo1S9KVXKQP7V4g6vBb/x4Q+d2zMhoUZ9dhIEnjjCc1Gg9oVx00pqOKRh1xcauZsUThxRO7IedTjATu2cHtkMIZNAQAu8WQhimAzYoENeVX/qfrUxu0jk6AHsXrFaTTmtGJibdIFXH1FrZMExg0zShzeo2A5885kPGDXroSR8HZ7It7NNUCYJEa8jBEWDrGYcI1ptQVyg3vUlVrow2VaVpftF+qnKGxcv4qbmZ0RUx2+oYKZQts7hDooJoFPqUDRLap1JgtHSrfCMNHFuZ4PtshsozvLCDC6R+khtihztSFk4TnuBONIsYBVjEz+GFi27eEu3uxQqHs5RTdAEF2Ia5Rxd6iEU2tVs5CW1yGbbwGUpctNwllmESFjoIK1hnqlJnw8jm1AMPT8dwo6F2m6ahVrtNX5iL3WbUGzfKnPXfIeLNcY3KZkNvCqFu5y8Gi691rV3WF/v9a56FawAO1YC8ivojA6kyjgJgU7HGxX/AwZCziaANweOZgJPoWmPa4cWwYPgAI9a3yQRvxneb1bQxDis03j1oZTwNcD7N0TUFN78uun0bhjxy0oiZcgsdavhcSeasKsmQHCvJQJioAeSjKyVT6WAmDL89ERuxlgwZv5YM1AUin4vJDMxM2Ir5jl5MBsL0qQ2Po5pMpTYZMZPuK3w4tdy/nAzh1ByNyWSJBPdv//P7/MV377//4bu/4eW71be3//w0ybuT4pG45Gyxnl/vR7tbrUvJp6xIxQBWhjEBWAJMBWx6sRNG+La1jv7YcfiKg8LVTgeqYflxDIaSjFOuWTVm7JnvdJovDE/iI5+y1OM5vusyYS3IyxxpAKot4+a+NKecF/Twad90KS4BLQjsd5fELBcQ63zqLEekwAokFcsZBjgjFObHclk1C3wsLMcDWY7nxXISlpW4D6+m4c+ZYqcILCcphgUB/e6SmOUCmhM+sxygfpYD+bGcM0b6THMn0pwMpDmZGc0hiLw0x6LQHFMmo4/mfHeJR3Nf4V+//vD5y9uPn6/mP/6yevHn5Qs+CYjr5kdzR6gqFM/evEYs6pRmMBqXtIXRCFCWnSvbRjkNEXLUnDJ0hUNHZ7QeKQTdU3VsRjOYpR/F+TSlTnHHrxuN0QhQbiMtGYcRKIyuPpAqp1IA/SnjRsHK8C4ulNaDOMw4NNR2n8ScFhARfua0A6fBHDktfY34Z06rNKWV03C+nIbwMJzWdp/EnHb2Wwuf5w+bpDTHqLLlj9McwzhDmstyZ+HR0Zxjq+H4dWPRnGTMQ3MY8gb99GQ5uj3IWd0Ed7lHYoY7+22FsRmOUpYhw2W5q/DoGM6xzXD8ukwZTpL0DOe/R2KGO/stheQMRyhvuKrIoDgO2rJDxqC4PLcUHh3HOfYYjl83FschCRreapN/EBExOG57lP4oybXcJPWGwlnuKNRZbj5NzHJ0V9j1KMsxSHJkOfTMckNsMwTvM4x7rBFwdJznMGYxeA4yzjzGnP8mqXnuLHcZsuI5inN0WOHz1sMgPBe69wBH3Xzw8xyBUXxWP8+13CQ1z7l2Hp4PW6XetGUQGOl0XMGkcDQJQ7pEfOzzVm44pN+xqCbcPhIVKqoj+XEM+wnwaBmLhDQ46p7DFmfGcWMOe2+hQkaJiVponuhKzVeufYRnvkrNVwQyC0c6lXE8skq/+fC0yGrU7YMSZNyiFwYK3aGgz2l2s0eG4itjtN58pf5cr0piOVxeFoH8aTWbl1f8Pw==</diagram></mxfile>" > @@ -23,7 +23,7 @@ pointer-events="none" /> - + - + - - + + - + - + - - - + + + @@ -139,7 +139,7 @@ - + @@ -176,9 +176,9 @@ stroke-miterlimit="10" pointer-events="none" /> - + - + - + - + - + - + - - - + + + - + - + - + - - + + - - + + - + - + - - - + + + - + - + - + + + + + + + @@ -589,16 +613,16 @@
- attention area + attention lane
- attention area + attention lane - - + + @@ -654,7 +678,7 @@ stroke-miterlimit="10" pointer-events="none" /> - + - + - - + + - attention area + attention lane - attention area + attention lane - - + + @@ -875,7 +899,7 @@ stroke-miterlimit="10" pointer-events="none" /> - + @@ -990,10 +1014,10 @@ stroke-miterlimit="10" pointer-events="none" /> - - + + - - + + - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ attention area +
+
+
+ + attention area + + + + + + + + +
+
+
+ attention area +
+
+
+ + attention area + + + + diff --git a/planning/behavior_velocity_intersection_module/docs/occlusion-without-tl.drawio.svg b/planning/behavior_velocity_intersection_module/docs/occlusion-without-tl.drawio.svg new file mode 100644 index 0000000000000..2fc22c8a4a401 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/docs/occlusion-without-tl.drawio.svg @@ -0,0 +1,518 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + + default stop +
+ line + +
+ + +
+
+
+ + default stop... + + + + + + + + +
+
+
+ + occlusion + +
+
+
+ + occlusion + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + + first attention +
+ stop line + +
+ + +
+
+
+ + first attentio... + + + + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_intersection_module/docs/occlusion_grid.drawio.svg b/planning/behavior_velocity_intersection_module/docs/occlusion_grid.drawio.svg new file mode 100644 index 0000000000000..fc09a4212070a --- /dev/null +++ b/planning/behavior_velocity_intersection_module/docs/occlusion_grid.drawio.svg @@ -0,0 +1,2616 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + default stop line + +
+
+
+ + default stop line + + + + + + + + +
+
+
+ + occlusion peeking line + +
+
+
+ + occlusion peeking line + + + + + + + + + + + + + + + + + +
+
+
+ + ego + +
+
+
+ + ego + + + + + + + + +
+
+
+ occlusion attention area +
+
+
+ + occlusion attent... + + + + + + + + + + + +
+
+
+ + + stopped vehicle on attention area +
+ (blocking vehicle) + + +
+
+
+ + stopped vehicle on attentio... + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + The cells behind blocking vehicles +
+ are not marked as occluded +
+ + +
+
+
+ + The cells behind blocking vehicles... + + + + + + + + +
+
+
+ + + mark the cells which is unknown and +
+ belong to attention area are +
+ marked as occluded + +
+ +
+
+
+ + mark the cells which is unknown and... + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 13 +
+
+
+ + 13 + + + + + + + +
+
+
+ 12 +
+
+
+ + 12 + + + + + + + +
+
+
+ 12 +
+
+
+ + 12 + + + + + + + +
+
+
+ 11 +
+
+
+ + 11 + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 12 +
+
+
+ + 12 + + + + + + + +
+
+
+ 11 +
+
+
+ + 11 + + + + + + + +
+
+
+ 11 +
+
+
+ + 11 + + + + + + + +
+
+
+ 10 +
+
+
+ + 10 + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 11 +
+
+
+ + 11 + + + + + + + +
+
+
+ 10 +
+
+
+ + 10 + + + + + + + +
+
+
+ 10 +
+
+
+ + 10 + + + + + + + +
+
+
+ 9 +
+
+
+ + 9 + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 10 +
+
+
+ + 10 + + + + + + + +
+
+
+ 9 +
+
+
+ + 9 + + + + + + + +
+
+
+ 9 +
+
+
+ + 9 + + + + + + +
+
+
+ 8 +
+
+
+ + 8 + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 9 +
+
+
+ + 9 + + + + + + + +
+
+
+ 8 +
+
+
+ + 8 + + + + + + +
+
+
+ 8 +
+
+
+ + 8 + + + + + + +
+
+
+ 7 +
+
+
+ + 7 + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 8 +
+
+
+ + 8 + + + + + + + +
+
+
+ 7 +
+
+
+ + 7 + + + + + + +
+
+
+ 7 +
+
+
+ + 7 + + + + + + +
+
+
+ 6 +
+
+
+ + 6 + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 7 +
+
+
+ + 7 + + + + + + +
+
+
+ 6 +
+
+
+ + 6 + + + + + + +
+
+
+ 6 +
+
+
+ + 6 + + + + + + +
+
+
+ 5 +
+
+
+ + 5 + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 6 +
+
+
+ + 6 + + + + + + +
+
+
+ 5 +
+
+
+ + 5 + + + + + + +
+
+
+ 5 +
+
+
+ + 5 + + + + + + +
+
+
+ 4 +
+
+
+ + 4 + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 5 +
+
+
+ + 5 + + + + + + +
+
+
+ 4 +
+
+
+ + 4 + + + + + + +
+
+
+ 4 +
+
+
+ + 4 + + + + + + +
+
+
+ 3 +
+
+
+ + 3 + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + 4 +
+
+ +
+
+
+ + 4... + + + + + + +
+
+
+ 3 +
+
+
+ + 3 + + + + + + +
+
+
+ 3 +
+
+
+ + 3 + + + + + + +
+
+
+ 2 +
+
+
+ + 2 + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 3 +
+
+
+ + 3 + + + + + + +
+
+
+ 2 +
+
+
+ + 2 + + + + + + +
+
+
+ 2 +
+
+
+ + 2 + + + + + + +
+
+
+ 1 +
+
+
+ + 1 + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 2 +
+
+
+ + 2 + + + + + + +
+
+
+ 1 +
+
+
+ + 1 + + + + + + +
+
+
+ 1 +
+
+
+ + 1 + + + + + + +
+
+
+ 0 +
+
+
+ + 0 + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 1 +
+
+
+ + 1 + + + + + + +
+
+
+ 0 +
+
+
+ + 0 + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 0 +
+
+
+ + 0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + nearest occlusion cell + +
+
+
+ + nearest occlusion... + + + + + + + Text is not SVG - cannot display + + + 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 8802dc786b617..c29cb7ade21cd 100644 --- a/planning/behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg +++ b/planning/behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg @@ -8,7 +8,7 @@ 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>" + content="<mxfile host="Electron" modified="2023-10-03T03:33:38.127Z" 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="TqIhwSEqRvc68Imy-9XT" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" > @@ -18,7 +18,7 @@ - + @@ -57,9 +57,9 @@ stroke-miterlimit="10" pointer-events="all" /> - + - + - - - - + + + + - - - + + + - + - +
- +
Takayuki Murooka Tomoya Kimura Shumpei Wakabayashi + Kyoichi Sugahara Apache License 2.0 @@ -25,7 +26,6 @@ interpolation lanelet2_extension libopencv-dev - magic_enum motion_utils nav_msgs pluginlib @@ -39,8 +39,6 @@ vehicle_info_util visualization_msgs - grid_map_rviz_plugin - ament_lint_auto autoware_lint_common diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index af8972424f54a..9ff638cb61876 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -173,6 +173,14 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array); } + if (debug_data_.occlusion_attention_area) { + appendMarkerArray( + createLaneletPolygonsMarkerArray( + debug_data_.occlusion_attention_area.value(), "occlusion_attention_area", lane_id_, 0.917, + 0.568, 0.596), + &debug_marker_array); + } + if (debug_data_.adjacent_area) { appendMarkerArray( createLaneletPolygonsMarkerArray( @@ -230,6 +238,12 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( debug_data_.stuck_targets, "stuck_targets", module_id_, now, 0.99, 0.99, 0.2), &debug_marker_array, now); + appendMarkerArray( + debug::createObjectsMarkerArray( + debug_data_.blocking_attention_objects, "blocking_attention_objects", module_id_, now, 0.99, + 0.99, 0.6), + &debug_marker_array, now); + /* appendMarkerArray( createPoseMarkerArray( @@ -265,29 +279,37 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( motion_utils::VirtualWalls IntersectionModule::createVirtualWalls() { - // TODO(Mamoru Sobue): collision stop pose depends on before/after occlusion clearance motion_utils::VirtualWalls virtual_walls; motion_utils::VirtualWall wall; - wall.style = motion_utils::VirtualWallType::stop; if (debug_data_.collision_stop_wall_pose) { + wall.style = motion_utils::VirtualWallType::stop; wall.text = "intersection"; wall.ns = "intersection" + std::to_string(module_id_) + "_"; wall.pose = debug_data_.collision_stop_wall_pose.value(); virtual_walls.push_back(wall); } if (debug_data_.occlusion_first_stop_wall_pose) { + wall.style = motion_utils::VirtualWallType::stop; wall.text = "intersection"; wall.ns = "intersection_occlusion_first_stop" + std::to_string(module_id_) + "_"; wall.pose = debug_data_.occlusion_first_stop_wall_pose.value(); virtual_walls.push_back(wall); } if (debug_data_.occlusion_stop_wall_pose) { + wall.style = motion_utils::VirtualWallType::stop; wall.text = "intersection_occlusion"; wall.ns = "intersection_occlusion" + std::to_string(module_id_) + "_"; wall.pose = debug_data_.occlusion_stop_wall_pose.value(); virtual_walls.push_back(wall); } + if (debug_data_.absence_traffic_light_creep_wall) { + wall.style = motion_utils::VirtualWallType::slowdown; + wall.text = "intersection_occlusion"; + wall.ns = "intersection_occlusion" + std::to_string(module_id_) + "_"; + wall.pose = debug_data_.absence_traffic_light_creep_wall.value(); + virtual_walls.push_back(wall); + } return virtual_walls; } diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 28cc7a1ad2881..4137090a5b6ae 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -77,6 +77,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) */ ip.stuck_vehicle.timeout_private_area = getOrDeclareParameter(node, ns + ".stuck_vehicle.timeout_private_area"); + ip.stuck_vehicle.enable_private_area_stuck_disregard = + getOrDeclareParameter(node, ns + ".stuck_vehicle.enable_private_area_stuck_disregard"); ip.collision_detection.min_predicted_path_confidence = getOrDeclareParameter(node, ns + ".collision_detection.min_predicted_path_confidence"); @@ -103,6 +105,18 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) node, ns + ".collision_detection.not_prioritized.collision_end_margin_time"); ip.collision_detection.keep_detection_vel_thr = getOrDeclareParameter(node, ns + ".collision_detection.keep_detection_vel_thr"); + ip.collision_detection.use_upstream_velocity = + getOrDeclareParameter(node, ns + ".collision_detection.use_upstream_velocity"); + ip.collision_detection.minimum_upstream_velocity = + getOrDeclareParameter(node, ns + ".collision_detection.minimum_upstream_velocity"); + ip.collision_detection.yield_on_green_traffic_light.distance_to_assigned_lanelet_start = + getOrDeclareParameter( + node, + ns + ".collision_detection.yield_on_green_traffic_light.distance_to_assigned_lanelet_start"); + ip.collision_detection.yield_on_green_traffic_light.duration = getOrDeclareParameter( + node, ns + ".collision_detection.yield_on_green_traffic_light.duration"); + ip.collision_detection.yield_on_green_traffic_light.range = getOrDeclareParameter( + node, ns + ".collision_detection.yield_on_green_traffic_light.range"); ip.occlusion.enable = getOrDeclareParameter(node, ns + ".occlusion.enable"); ip.occlusion.occlusion_attention_area_length = @@ -132,6 +146,14 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".occlusion.stop_release_margin_time"); ip.occlusion.temporal_stop_before_attention_area = getOrDeclareParameter(node, ns + ".occlusion.temporal_stop_before_attention_area"); + ip.occlusion.absence_traffic_light.creep_velocity = + getOrDeclareParameter(node, ns + ".occlusion.absence_traffic_light.creep_velocity"); + ip.occlusion.absence_traffic_light.maximum_peeking_distance = getOrDeclareParameter( + node, ns + ".occlusion.absence_traffic_light.maximum_peeking_distance"); + ip.occlusion.attention_lane_crop_curvature_threshold = + getOrDeclareParameter(node, ns + ".occlusion.attention_lane_crop_curvature_threshold"); + ip.occlusion.attention_lane_curvature_calculation_ds = + getOrDeclareParameter(node, ns + ".occlusion.attention_lane_curvature_calculation_ds"); } void IntersectionModuleManager::launchNewModules( @@ -161,13 +183,21 @@ void IntersectionModuleManager::launchNewModules( continue; } - const auto associative_ids = - planning_utils::getAssociativeIntersectionLanelets(ll, lanelet_map, routing_graph); const std::string location = ll.attributeOr("location", "else"); const bool is_private_area = (location.compare("private") == 0); + const auto associative_ids = + planning_utils::getAssociativeIntersectionLanelets(ll, lanelet_map, routing_graph); + bool has_traffic_light = false; + if (const auto tl_reg_elems = ll.regulatoryElementsAs(); + tl_reg_elems.size() != 0) { + const auto tl_reg_elem = tl_reg_elems.front(); + const auto stop_line_opt = tl_reg_elem->stopLine(); + if (!!stop_line_opt) has_traffic_light = true; + } const auto new_module = std::make_shared( - module_id, lane_id, planner_data_, intersection_param_, associative_ids, is_private_area, - enable_occlusion_detection, node_, logger_.get_child("intersection_module"), clock_); + module_id, lane_id, planner_data_, intersection_param_, associative_ids, turn_direction, + has_traffic_light, enable_occlusion_detection, is_private_area, node_, + logger_.get_child("intersection_module"), clock_); generateUUID(module_id); /* set RTC status as non_occluded status initially */ const UUID uuid = getUUID(new_module->getModuleId()); diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index fec7589f3c18f..75bb4e861a60f 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -17,7 +17,6 @@ #include "util.hpp" #include -#include #include #include #include @@ -31,14 +30,15 @@ #include #include +#include #include #include #include +#include #include #include #include - namespace behavior_velocity_planner { namespace bg = boost::geometry; @@ -65,14 +65,18 @@ static bool isTargetCollisionVehicleType( } IntersectionModule::IntersectionModule( - const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, + const int64_t module_id, const int64_t lane_id, + [[maybe_unused]] std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, - const bool is_private_area, const bool enable_occlusion_detection, rclcpp::Node & node, + const std::string & turn_direction, const bool has_traffic_light, + const bool enable_occlusion_detection, const bool is_private_area, rclcpp::Node & node, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), node_(node), lane_id_(lane_id), associative_ids_(associative_ids), + turn_direction_(turn_direction), + has_traffic_light_(has_traffic_light), enable_occlusion_detection_(enable_occlusion_detection), occlusion_attention_divisions_(std::nullopt), is_private_area_(is_private_area), @@ -81,11 +85,10 @@ IntersectionModule::IntersectionModule( velocity_factor_.init(VelocityFactor::INTERSECTION); planner_param_ = planner_param; - const auto & assigned_lanelet = - planner_data->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id); - turn_direction_ = assigned_lanelet.attributeOr("turn_direction", "else"); - collision_state_machine_.setMarginTime( - planner_param_.collision_detection.state_transit_margin_time); + { + collision_state_machine_.setMarginTime( + planner_param_.collision_detection.state_transit_margin_time); + } { before_creep_state_machine_.setMarginTime(planner_param_.occlusion.before_creep_stop_time); before_creep_state_machine_.setState(StateMachine::State::STOP); @@ -94,10 +97,6 @@ IntersectionModule::IntersectionModule( occlusion_stop_state_machine_.setMarginTime(planner_param_.occlusion.stop_release_margin_time); occlusion_stop_state_machine_.setState(StateMachine::State::GO); } - { - stuck_private_area_timeout_.setMarginTime(planner_param_.stuck_vehicle.timeout_private_area); - stuck_private_area_timeout_.setState(StateMachine::State::STOP); - } { temporal_stop_before_attention_state_machine_.setMarginTime( planner_param_.occlusion.before_creep_stop_time); @@ -226,6 +225,23 @@ void prepareRTCByDecisionResult( return; } +template <> +void prepareRTCByDecisionResult( + const IntersectionModule::OccludedAbsenceTrafficLight & result, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, + double * default_distance, bool * occlusion_safety, double * occlusion_distance) +{ + RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "OccludedAbsenceTrafficLight"); + const auto closest_idx = result.closest_idx; + const auto collision_stop_line_idx = result.closest_idx; + *default_safety = !result.collision_detected; + *default_distance = + motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stop_line_idx); + *occlusion_safety = result.is_actually_occlusion_cleared; + *occlusion_distance = 0; + return; +} + template <> void prepareRTCByDecisionResult( const IntersectionModule::OccludedCollisionStop & result, @@ -570,6 +586,59 @@ void reactRTCApprovalByDecisionResult( return; } +template <> +void reactRTCApprovalByDecisionResult( + const bool rtc_default_approved, const bool rtc_occlusion_approved, + const IntersectionModule::OccludedAbsenceTrafficLight & decision_result, + [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, + const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, util::DebugData * debug_data) +{ + RCLCPP_DEBUG( + rclcpp::get_logger("reactRTCApprovalByDecisionResult"), + "OccludedAbsenceTrafficLight, approval = (default: %d, occlusion: %d)", rtc_default_approved, + rtc_occlusion_approved); + if (!rtc_default_approved) { + const auto stop_line_idx = decision_result.closest_idx; + planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); + debug_data->collision_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); + } + } + if (!rtc_occlusion_approved && decision_result.temporal_stop_before_attention_required) { + const auto stop_line_idx = decision_result.first_attention_area_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); + } + } + if (!rtc_occlusion_approved && !decision_result.temporal_stop_before_attention_required) { + const auto closest_idx = decision_result.closest_idx; + const auto peeking_limit_line = decision_result.peeking_limit_line_idx; + for (auto i = closest_idx; i <= peeking_limit_line; ++i) { + planning_utils::setVelocityFromIndex( + i, planner_param.occlusion.absence_traffic_light.creep_velocity, path); + } + debug_data->absence_traffic_light_creep_wall = + planning_utils::getAheadPose(closest_idx, baselink2front, *path); + } + return; +} + template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, @@ -675,7 +744,8 @@ void reactRTCApproval( static std::string formatDecisionResult(const IntersectionModule::DecisionResult & decision_result) { if (std::holds_alternative(decision_result)) { - return "Indecisive"; + const auto indecisive = std::get(decision_result); + return "Indecisive because " + indecisive.error; } if (std::holds_alternative(decision_result)) { return "Safe"; @@ -695,6 +765,9 @@ static std::string formatDecisionResult(const IntersectionModule::DecisionResult if (std::holds_alternative(decision_result)) { return "OccludedCollisionStop"; } + if (std::holds_alternative(decision_result)) { + return "OccludedAbsenceTrafficLight"; + } if (std::holds_alternative(decision_result)) { return "TrafficLightArrowSolidOn"; } @@ -735,6 +808,36 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * return true; } +static bool isGreenSolidOn( + lanelet::ConstLanelet lane, const std::map & tl_infos) +{ + using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; + + std::optional tl_id = std::nullopt; + for (auto && tl_reg_elem : lane.regulatoryElementsAs()) { + tl_id = tl_reg_elem->id(); + break; + } + if (!tl_id) { + // this lane has no traffic light + return false; + } + const auto tl_info_it = tl_infos.find(tl_id.value()); + if (tl_info_it == tl_infos.end()) { + // the info of this traffic light is not available + return false; + } + const auto & tl_info = tl_info_it->second; + for (auto && tl_light : tl_info.signal.elements) { + if ( + tl_light.color == TrafficSignalElement::GREEN && + tl_light.shape == TrafficSignalElement::CIRCLE) { + return true; + } + } + return false; +} + IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) { @@ -747,15 +850,15 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const auto interpolated_path_info_opt = util::generateInterpolatedPath( lane_id_, associative_ids_, *path, planner_param_.common.path_interpolation_ds, logger_); if (!interpolated_path_info_opt) { - RCLCPP_DEBUG(logger_, "splineInterpolate failed"); - return IntersectionModule::Indecisive{}; + return IntersectionModule::Indecisive{"splineInterpolate failed"}; } 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_); - return IntersectionModule::Indecisive{}; + return IntersectionModule::Indecisive{ + "Path has no interval on intersection lane " + std::to_string(lane_id_)}; } + // cache intersection lane information because it is invariant const auto & current_pose = planner_data_->current_odometry->pose; const auto lanelets_on_path = planning_utils::getLaneletsOnPath(*path, lanelet_map_ptr, current_pose); @@ -766,30 +869,40 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( planner_param_.occlusion.occlusion_attention_area_length, planner_param_.common.consider_wrong_direction_vehicle); } + auto & intersection_lanelets = intersection_lanelets_.value(); + + // at the very first time of registration of this module, the path may not be conflicting with the + // attention area, so update() is called to update the internal data as well as traffic light info const auto traffic_prioritized_level = util::getTrafficPrioritizedLevel(assigned_lanelet, planner_data_->traffic_light_id_map); const bool is_prioritized = traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED; - intersection_lanelets_.value().update(is_prioritized, interpolated_path_info); + intersection_lanelets.update(is_prioritized, interpolated_path_info); - const auto & conflicting_lanelets = intersection_lanelets_.value().conflicting(); - const auto & first_conflicting_area_opt = intersection_lanelets_.value().first_conflicting_area(); + // this is abnormal + const auto & conflicting_lanelets = intersection_lanelets.conflicting(); + const auto & first_conflicting_area_opt = intersection_lanelets.first_conflicting_area(); if (conflicting_lanelets.empty() || !first_conflicting_area_opt) { - RCLCPP_DEBUG(logger_, "conflicting area is empty"); - return IntersectionModule::Indecisive{}; + return IntersectionModule::Indecisive{"conflicting area is empty"}; } const auto first_conflicting_area = first_conflicting_area_opt.value(); - const auto & first_attention_area_opt = intersection_lanelets_.value().first_attention_area(); + // generate all stop line candidates + // see the doc for struct IntersectionStopLines + const auto & first_attention_area_opt = intersection_lanelets.first_attention_area(); + /// even if the attention area is null, stuck vehicle stop line needs to be generated from + /// conflicting lanes const auto & dummy_first_attention_area = first_attention_area_opt ? first_attention_area_opt.value() : first_conflicting_area; + const double peeking_offset = + has_traffic_light_ ? planner_param_.occlusion.peeking_offset + : planner_param_.occlusion.absence_traffic_light.maximum_peeking_distance; const auto intersection_stop_lines_opt = util::generateIntersectionStopLines( 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); + peeking_offset, path); if (!intersection_stop_lines_opt) { - RCLCPP_DEBUG(logger_, "failed to generate intersection_stop_lines"); - return IntersectionModule::Indecisive{}; + return IntersectionModule::Indecisive{"failed to generate intersection_stop_lines"}; } const auto & intersection_stop_lines = intersection_stop_lines_opt.value(); const auto @@ -797,54 +910,63 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( first_attention_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(); + // see the doc for struct PathLanelets + const auto & conflicting_area = intersection_lanelets.conflicting_area(); const auto path_lanelets_opt = util::generatePathLanelets( 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); + conflicting_area, first_attention_area_opt, intersection_lanelets.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{}; + return IntersectionModule::Indecisive{"failed to generate PathLanelets"}; } const auto path_lanelets = path_lanelets_opt.value(); - const bool stuck_detected = checkStuckVehicle(planner_data_, path_lanelets); + // utility functions + auto fromEgoDist = [&](const size_t index) { + return motion_utils::calcSignedArcLength(path->points, closest_idx, index); + }; + auto stoppedForDuration = + [&](const size_t pos, const double duration, StateMachine & state_machine) { + const double dist_stopline = fromEgoDist(pos); + const bool approached_dist_stopline = + (std::fabs(dist_stopline) < planner_param_.common.stop_overshoot_margin); + const bool over_stopline = (dist_stopline < 0.0); + const bool is_stopped_duration = planner_data_->isVehicleStopped(duration); + if (over_stopline) { + state_machine.setState(StateMachine::State::GO); + } else if (is_stopped_duration && approached_dist_stopline) { + state_machine.setState(StateMachine::State::GO); + } + return state_machine.getState() == StateMachine::State::GO; + }; + // stuck vehicle detection is viable even if attention area is empty + // so this needs to be checked before attention area validation + const bool stuck_detected = checkStuckVehicle(planner_data_, path_lanelets); 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); - const bool approached_stop_line = - (std::fabs(dist_stopline) < planner_param_.common.stop_overshoot_margin); - const bool is_stopped = planner_data_->isVehicleStopped(); - if (is_stopped && approached_stop_line) { - stuck_private_area_timeout_.setStateWithMarginTime( - StateMachine::State::GO, logger_.get_child("stuck_private_area_timeout"), *clock_); - } - const bool timeout = - (is_private_area_ && stuck_private_area_timeout_.getState() == StateMachine::State::GO); - if (!timeout) { + if (is_private_area_ && planner_param_.stuck_vehicle.enable_private_area_stuck_disregard) { if ( default_stop_line_idx_opt && - motion_utils::calcSignedArcLength(path->points, stuck_stop_line_idx, closest_idx) > - planner_param_.common.stop_overshoot_margin) { + fromEgoDist(stuck_stop_line_idx) < -planner_param_.common.stop_overshoot_margin) { stuck_stop_line_idx = default_stop_line_idx_opt.value(); } + } else { return IntersectionModule::StuckStop{ closest_idx, stuck_stop_line_idx, occlusion_peeking_stop_line_idx_opt}; } } + // if attention area is empty, collision/occlusion detection is impossible if (!first_attention_area_opt) { - RCLCPP_DEBUG(logger_, "attention area is empty"); - return IntersectionModule::Indecisive{}; + return IntersectionModule::Indecisive{"attention area is empty"}; } const auto first_attention_area = first_attention_area_opt.value(); + // if attention area is not null but default stop line is not available, ego/backward-path has + // already passed the stop line if (!default_stop_line_idx_opt) { - RCLCPP_DEBUG(logger_, "default stop line is null"); - return IntersectionModule::Indecisive{}; + return IntersectionModule::Indecisive{"default stop line is null"}; } const auto default_stop_line_idx = default_stop_line_idx_opt.value(); @@ -873,25 +995,26 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( // 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."); - return IntersectionModule::Indecisive{}; + return IntersectionModule::Indecisive{"over the pass judge line. no plan needed"}; } + // occlusion stop line is generated from the intersection of ego footprint along the path with the + // attention area, so if this is null, eog has already passed the intersection if (!first_attention_stop_line_idx_opt || !occlusion_peeking_stop_line_idx_opt) { - RCLCPP_DEBUG(logger_, "occlusion stop line is null"); - return IntersectionModule::Indecisive{}; + return IntersectionModule::Indecisive{"occlusion stop line is null"}; } const auto collision_stop_line_idx = is_over_default_stop_line ? closest_idx : default_stop_line_idx; const auto first_attention_stop_line_idx = first_attention_stop_line_idx_opt.value(); 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(); - const auto & occlusion_attention_area = intersection_lanelets_.value().occlusion_attention_area(); - debug_data_.attention_area = intersection_lanelets_.value().attention_area(); - debug_data_.adjacent_area = intersection_lanelets_.value().adjacent_area(); + const auto & attention_lanelets = intersection_lanelets.attention(); + const auto & adjacent_lanelets = intersection_lanelets.adjacent(); + const auto & occlusion_attention_lanelets = intersection_lanelets.occlusion_attention(); + const auto & occlusion_attention_area = intersection_lanelets.occlusion_attention_area(); + debug_data_.attention_area = intersection_lanelets.attention_area(); + debug_data_.occlusion_attention_area = occlusion_attention_area; + debug_data_.adjacent_area = intersection_lanelets.adjacent_area(); // get intersection area const auto intersection_area = planner_param_.common.use_intersection_area @@ -902,16 +1025,57 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( debug_data_.intersection_area = toGeomPoly(intersection_area_2d); } - // calculate dynamic collision around detection area - const double time_delay = (is_go_out_ || is_prioritized) - ? 0.0 - : (planner_param_.collision_detection.state_transit_margin_time - - collision_state_machine_.getDuration()); const auto target_objects = filterTargetObjects(attention_lanelets, adjacent_lanelets, intersection_area); + // If there are any vehicles on the attention area when ego entered the intersection on green + // light, do pseudo collision detection because the vehicles are very slow and no collisions may + // be detected. check if ego vehicle entered assigned lanelet + const bool is_green_solid_on = + isGreenSolidOn(assigned_lanelet, planner_data_->traffic_light_id_map); + if (is_green_solid_on) { + if (!initial_green_light_observed_time_) { + const auto assigned_lane_begin_point = assigned_lanelet.centerline().front(); + const bool approached_assigned_lane = + motion_utils::calcSignedArcLength( + path->points, closest_idx, + tier4_autoware_utils::createPoint( + assigned_lane_begin_point.x(), assigned_lane_begin_point.y(), + assigned_lane_begin_point.z())) < + planner_param_.collision_detection.yield_on_green_traffic_light + .distance_to_assigned_lanelet_start; + if (approached_assigned_lane) { + initial_green_light_observed_time_ = clock_->now(); + } + } + if (initial_green_light_observed_time_) { + const auto now = clock_->now(); + const bool exist_close_vehicles = std::any_of( + target_objects.objects.begin(), target_objects.objects.end(), [&](const auto & object) { + return tier4_autoware_utils::calcDistance3d( + object.kinematics.initial_pose_with_covariance.pose, current_pose) < + planner_param_.collision_detection.yield_on_green_traffic_light.range; + }); + if ( + exist_close_vehicles && + rclcpp::Duration((now - initial_green_light_observed_time_.value())).seconds() < + planner_param_.collision_detection.yield_on_green_traffic_light.duration) { + return IntersectionModule::NonOccludedCollisionStop{ + closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; + } + } + } + + // calculate dynamic collision around attention area + const double time_to_restart = (is_go_out_ || is_prioritized) + ? 0.0 + : (planner_param_.collision_detection.state_transit_margin_time - + collision_state_machine_.getDuration()); + const bool has_collision = checkCollision( - *path, target_objects, path_lanelets, closest_idx, time_delay, traffic_prioritized_level); + *path, target_objects, path_lanelets, closest_idx, + std::min(occlusion_stop_line_idx, path->points.size() - 1), time_to_restart, + traffic_prioritized_level); collision_state_machine_.setStateWithMarginTime( has_collision ? StateMachine::State::STOP : StateMachine::State::GO, logger_.get_child("collision state_machine"), *clock_); @@ -927,28 +1091,31 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( if (!occlusion_attention_divisions_) { occlusion_attention_divisions_ = util::generateDetectionLaneDivisions( occlusion_attention_lanelets, routing_graph_ptr, - planner_data_->occupancy_grid->info.resolution / std::sqrt(2.0)); + planner_data_->occupancy_grid->info.resolution, + planner_param_.occlusion.attention_lane_crop_curvature_threshold, + planner_param_.occlusion.attention_lane_curvature_calculation_ds); } 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) / (2 * planner_param_.occlusion.min_vehicle_brake_for_rss)); - std::vector parked_attention_objects; + std::vector blocking_attention_objects; std::copy_if( target_objects.objects.begin(), target_objects.objects.end(), - std::back_inserter(parked_attention_objects), + std::back_inserter(blocking_attention_objects), [thresh = planner_param_.occlusion.ignore_parked_vehicle_speed_threshold](const auto & object) { return std::hypot( object.kinematics.initial_twist_with_covariance.twist.linear.x, object.kinematics.initial_twist_with_covariance.twist.linear.y) <= thresh; }); + debug_data_.blocking_attention_objects.objects = blocking_attention_objects; const bool is_occlusion_cleared = (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !is_prioritized) ? isOcclusionCleared( *planner_data_->occupancy_grid, occlusion_attention_area, adjacent_lanelets, first_attention_area, interpolated_path_info, occlusion_attention_divisions, - parked_attention_objects, occlusion_dist_thr) + blocking_attention_objects, occlusion_dist_thr) : true; occlusion_stop_state_machine_.setStateWithMarginTime( is_occlusion_cleared ? StateMachine::State::GO : StateMachine::STOP, @@ -956,74 +1123,63 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const bool is_occlusion_cleared_with_margin = (occlusion_stop_state_machine_.getState() == StateMachine::State::GO); - // check safety + // distinguish if ego detected occlusion or RTC detects occlusion const bool ext_occlusion_requested = (is_occlusion_cleared_with_margin && !occlusion_activated_); - if ( - occlusion_stop_state_machine_.getState() == StateMachine::State::STOP || - ext_occlusion_requested) { - const double dist_default_stopline = motion_utils::calcSignedArcLength( - path->points, path->points.at(closest_idx).point.pose.position, - path->points.at(default_stop_line_idx).point.pose.position); - const bool approached_default_stop_line = - (std::fabs(dist_default_stopline) < planner_param_.common.stop_overshoot_margin); - const bool over_default_stop_line = (dist_default_stopline < 0.0); - const bool is_stopped_at_default = - planner_data_->isVehicleStopped(planner_param_.occlusion.before_creep_stop_time); - if (over_default_stop_line) { - before_creep_state_machine_.setState(StateMachine::State::GO); - } - if (before_creep_state_machine_.getState() == StateMachine::State::GO) { - const double dist_first_attention_stopline = motion_utils::calcSignedArcLength( - path->points, path->points.at(closest_idx).point.pose.position, - path->points.at(first_attention_stop_line_idx).point.pose.position); - const bool approached_first_attention_stop_line = - (std::fabs(dist_first_attention_stopline) < planner_param_.common.stop_overshoot_margin); - const bool over_first_attention_stop_line = (dist_first_attention_stopline < 0.0); - const bool is_stopped_at_first_attention = - planner_data_->isVehicleStopped(planner_param_.occlusion.before_creep_stop_time); - if (planner_param_.occlusion.temporal_stop_before_attention_area) { - if (over_first_attention_stop_line) { - temporal_stop_before_attention_state_machine_.setState(StateMachine::State::GO); - } - if (is_stopped_at_first_attention && approached_first_attention_stop_line) { - temporal_stop_before_attention_state_machine_.setState(StateMachine::State::GO); - } - } - const bool temporal_stop_before_attention_required = - planner_param_.occlusion.temporal_stop_before_attention_area && - temporal_stop_before_attention_state_machine_.getState() == StateMachine::State::STOP; - if (has_collision_with_margin) { - return IntersectionModule::OccludedCollisionStop{is_occlusion_cleared_with_margin, - temporal_stop_before_attention_required, - closest_idx, - collision_stop_line_idx, - first_attention_stop_line_idx, - occlusion_stop_line_idx}; - } else { - return IntersectionModule::PeekingTowardOcclusion{is_occlusion_cleared_with_margin, - temporal_stop_before_attention_required, - closest_idx, - collision_stop_line_idx, - first_attention_stop_line_idx, - occlusion_stop_line_idx}; + const bool is_occlusion_state = (!is_occlusion_cleared_with_margin || ext_occlusion_requested); + // Safe + if (!is_occlusion_state && !has_collision_with_margin) { + return IntersectionModule::Safe{closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; + } + // Only collision + if (!is_occlusion_state && has_collision_with_margin) { + return IntersectionModule::NonOccludedCollisionStop{ + closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; + } + // Occluded + const bool stopped_at_default_line = stoppedForDuration( + default_stop_line_idx, planner_param_.occlusion.before_creep_stop_time, + before_creep_state_machine_); + if (stopped_at_default_line) { + // in this case ego will temporarily stop before entering attention area + const bool temporal_stop_before_attention_required = + (planner_param_.occlusion.temporal_stop_before_attention_area || !has_traffic_light_) + ? !stoppedForDuration( + first_attention_stop_line_idx, planner_param_.occlusion.before_creep_stop_time, + temporal_stop_before_attention_state_machine_) + : false; + if (!has_traffic_light_) { + if (fromEgoDist(first_attention_stop_line_idx) <= -peeking_offset) { + return IntersectionModule::Indecisive{ + "already passed maximum peeking line in the absence of traffic light"}; } + return IntersectionModule::OccludedAbsenceTrafficLight{ + is_occlusion_cleared_with_margin, has_collision_with_margin, + temporal_stop_before_attention_required, closest_idx, + first_attention_stop_line_idx, occlusion_stop_line_idx}; + } + if (has_collision_with_margin) { + return IntersectionModule::OccludedCollisionStop{is_occlusion_cleared_with_margin, + temporal_stop_before_attention_required, + closest_idx, + collision_stop_line_idx, + first_attention_stop_line_idx, + occlusion_stop_line_idx}; } else { - if (is_stopped_at_default && approached_default_stop_line) { - // start waiting at the first stop line - before_creep_state_machine_.setState(StateMachine::State::GO); - } - const auto occlusion_stop_line = planner_param_.occlusion.temporal_stop_before_attention_area - ? first_attention_stop_line_idx - : occlusion_stop_line_idx; - return IntersectionModule::FirstWaitBeforeOcclusion{ - is_occlusion_cleared_with_margin, closest_idx, default_stop_line_idx, occlusion_stop_line}; + return IntersectionModule::PeekingTowardOcclusion{is_occlusion_cleared_with_margin, + temporal_stop_before_attention_required, + closest_idx, + collision_stop_line_idx, + first_attention_stop_line_idx, + occlusion_stop_line_idx}; } - } else if (has_collision_with_margin) { - return IntersectionModule::NonOccludedCollisionStop{ - closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; + } else { + const auto occlusion_stop_line = + (planner_param_.occlusion.temporal_stop_before_attention_area || !has_traffic_light_) + ? first_attention_stop_line_idx + : occlusion_stop_line_idx; + return IntersectionModule::FirstWaitBeforeOcclusion{ + is_occlusion_cleared_with_margin, closest_idx, default_stop_line_idx, occlusion_stop_line}; } - - return IntersectionModule::Safe{closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; } bool IntersectionModule::checkStuckVehicle( @@ -1062,9 +1218,11 @@ autoware_auto_perception_msgs::msg::PredictedObjects IntersectionModule::filterT // check direction of objects const auto object_direction = util::getObjectPoseWithVelocityDirection(object.kinematics); const auto is_in_adjacent_lanelets = util::checkAngleForTargetLanelets( - object_direction, adjacent_lanelets, planner_param_.common.attention_area_angle_thr, + object_direction, object.kinematics.initial_twist_with_covariance.twist.linear.x, + adjacent_lanelets, planner_param_.common.attention_area_angle_thr, planner_param_.common.consider_wrong_direction_vehicle, - planner_param_.common.attention_area_margin); + planner_param_.common.attention_area_margin, + planner_param_.occlusion.ignore_parked_vehicle_speed_threshold); if (is_in_adjacent_lanelets) { continue; } @@ -1076,17 +1234,19 @@ autoware_auto_perception_msgs::msg::PredictedObjects IntersectionModule::filterT if (is_in_intersection_area) { target_objects.objects.push_back(object); } else if (util::checkAngleForTargetLanelets( - object_direction, attention_area_lanelets, - planner_param_.common.attention_area_angle_thr, + object_direction, object.kinematics.initial_twist_with_covariance.twist.linear.x, + attention_area_lanelets, planner_param_.common.attention_area_angle_thr, planner_param_.common.consider_wrong_direction_vehicle, - planner_param_.common.attention_area_margin)) { + planner_param_.common.attention_area_margin, + planner_param_.occlusion.ignore_parked_vehicle_speed_threshold)) { target_objects.objects.push_back(object); } } else if (util::checkAngleForTargetLanelets( - object_direction, attention_area_lanelets, - planner_param_.common.attention_area_angle_thr, + object_direction, object.kinematics.initial_twist_with_covariance.twist.linear.x, + attention_area_lanelets, planner_param_.common.attention_area_angle_thr, planner_param_.common.consider_wrong_direction_vehicle, - planner_param_.common.attention_area_margin)) { + planner_param_.common.attention_area_margin, + planner_param_.occlusion.ignore_parked_vehicle_speed_threshold)) { // intersection_area is not available, use detection_area_with_margin as before target_objects.objects.push_back(object); } @@ -1097,7 +1257,8 @@ autoware_auto_perception_msgs::msg::PredictedObjects IntersectionModule::filterT bool IntersectionModule::checkCollision( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const autoware_auto_perception_msgs::msg::PredictedObjects & objects, - const util::PathLanelets & path_lanelets, const int closest_idx, const double time_delay, + const util::PathLanelets & path_lanelets, const size_t closest_idx, + const size_t last_intersection_stop_line_candidate_idx, const double time_delay, const util::TrafficPrioritizedLevel & traffic_prioritized_level) { using lanelet::utils::getArcCoordinates; @@ -1106,9 +1267,11 @@ bool IntersectionModule::checkCollision( // check collision between target_objects predicted path and ego lane // cut the predicted path at passing_time const auto time_distance_array = util::calcIntersectionPassingTime( - path, planner_data_, associative_ids_, closest_idx, time_delay, - planner_param_.common.intersection_velocity, - planner_param_.collision_detection.minimum_ego_predicted_velocity); + path, planner_data_, associative_ids_, closest_idx, last_intersection_stop_line_candidate_idx, + time_delay, planner_param_.common.intersection_velocity, + planner_param_.collision_detection.minimum_ego_predicted_velocity, + planner_param_.collision_detection.use_upstream_velocity, + planner_param_.collision_detection.minimum_upstream_velocity); const double passing_time = time_distance_array.back().first; auto target_objects = objects; util::cutPredictPathWithDuration(&target_objects, clock_, passing_time); @@ -1235,7 +1398,7 @@ bool IntersectionModule::isOcclusionCleared( const lanelet::CompoundPolygon3d & first_attention_area, const util::InterpolatedPathInfo & interpolated_path_info, const std::vector & lane_divisions, - [[maybe_unused]] const std::vector & + const std::vector & blocking_attention_objects, const double occlusion_dist_thr) { @@ -1260,6 +1423,13 @@ bool IntersectionModule::isOcclusionCleared( const int height = occ_grid.info.height; const double resolution = occ_grid.info.resolution; const auto & origin = occ_grid.info.origin.position; + auto coord2index = [&](const double x, const double y) { + const int idx_x = (x - origin.x) / resolution; + const int idx_y = (y - origin.y) / resolution; + if (idx_x < 0 || idx_x >= width) return std::make_tuple(false, -1, -1); + if (idx_y < 0 || idx_y >= height) return std::make_tuple(false, -1, -1); + return std::make_tuple(true, idx_x, idx_y); + }; Polygon2d grid_poly; grid_poly.outer().emplace_back(origin.x, origin.y); @@ -1346,8 +1516,39 @@ bool IntersectionModule::isOcclusionCleared( cv::getStructuringElement(cv::MORPH_RECT, cv::Size(morph_size, morph_size))); // (3) occlusion mask + static constexpr unsigned char OCCLUDED = 255; + static constexpr unsigned char BLOCKED = 127; cv::Mat occlusion_mask(width, height, CV_8UC1, cv::Scalar(0)); cv::bitwise_and(attention_mask, unknown_mask, occlusion_mask); + // re-use attention_mask + attention_mask = cv::Mat(width, height, CV_8UC1, cv::Scalar(0)); + // (3.1) draw all cells on attention_mask behind blocking vehicles as not occluded + std::vector> blocking_polygons; + for (const auto & blocking_attention_object : blocking_attention_objects) { + const Polygon2d obj_poly = tier4_autoware_utils::toPolygon2d(blocking_attention_object); + findCommonCvPolygons(obj_poly.outer(), blocking_polygons); + } + for (const auto & blocking_polygon : blocking_polygons) { + cv::fillPoly(attention_mask, blocking_polygon, cv::Scalar(BLOCKED), cv::LINE_AA); + } + for (const auto & lane_division : lane_divisions) { + const auto & divisions = lane_division.divisions; + for (const auto & division : divisions) { + bool blocking_vehicle_found = false; + for (const auto & point_it : division) { + const auto [valid, idx_x, idx_y] = coord2index(point_it.x(), point_it.y()); + if (!valid) continue; + if (blocking_vehicle_found) { + occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; + continue; + } + if (attention_mask.at(height - 1 - idx_y, idx_x) == BLOCKED) { + blocking_vehicle_found = true; + occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; + } + } + } + } // (4) extract occlusion polygons const auto & possible_object_bbox = planner_param_.occlusion.possible_object_bbox; @@ -1392,19 +1593,11 @@ bool IntersectionModule::isOcclusionCleared( } // (4.1) re-draw occluded cells using valid_contours occlusion_mask = cv::Mat(width, height, CV_8UC1, cv::Scalar(0)); - for (unsigned i = 0; i < valid_contours.size(); ++i) { + for (const auto & valid_contour : valid_contours) { // NOTE: drawContour does not work well - cv::fillPoly(occlusion_mask, valid_contours[i], cv::Scalar(255), cv::LINE_AA); + cv::fillPoly(occlusion_mask, valid_contour, cv::Scalar(OCCLUDED), cv::LINE_AA); } - auto coord2index = [&](const double x, const double y) { - const int idx_x = (x - origin.x) / resolution; - const int idx_y = (y - origin.y) / resolution; - if (idx_x < 0 || idx_x >= width) return std::make_tuple(false, -1, -1); - if (idx_y < 0 || idx_y >= height) return std::make_tuple(false, -1, -1); - return std::make_tuple(true, idx_x, idx_y); - }; - // (5) find distance // (5.1) discretize path_ip with resolution for computational cost LineString2d path_linestring; @@ -1491,7 +1684,11 @@ bool IntersectionModule::isOcclusionCleared( const auto [valid, idx_x, idx_y] = coord2index(point_it->x(), point_it->y()); // TODO(Mamoru Sobue): add handling for blocking vehicles if (!valid) continue; - if (occlusion_mask.at(height - 1 - idx_y, idx_x) == 255) { + const auto pixel = occlusion_mask.at(height - 1 - idx_y, idx_x); + if (pixel == BLOCKED) { + break; + } + if (pixel == OCCLUDED) { if (acc_dist < min_dist) { min_dist = acc_dist; nearest_occlusion_point = { diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 89444bb71c947..de7d97a50c133 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -18,9 +18,7 @@ #include "util_type.hpp" #include -#include #include -#include #include #include @@ -72,6 +70,7 @@ class IntersectionModule : public SceneModuleInterface bool enable_front_car_decel_prediction; //! flag for using above feature */ double timeout_private_area; + bool enable_private_area_stuck_disregard; } stuck_vehicle; struct CollisionDetection { @@ -95,6 +94,14 @@ class IntersectionModule : public SceneModuleInterface double collision_end_margin_time; //! end margin time to check collision } not_prioritized; double keep_detection_vel_thr; //! keep detection if ego is ego.vel < keep_detection_vel_thr + bool use_upstream_velocity; + double minimum_upstream_velocity; + struct YieldOnGreeTrafficLight + { + double distance_to_assigned_lanelet_start; + double duration; + double range; + } yield_on_green_traffic_light; } collision_detection; struct Occlusion { @@ -114,10 +121,20 @@ class IntersectionModule : public SceneModuleInterface double ignore_parked_vehicle_speed_threshold; double stop_release_margin_time; bool temporal_stop_before_attention_area; + struct AbsenceTrafficLight + { + double creep_velocity; + double maximum_peeking_distance; + } absence_traffic_light; + double attention_lane_crop_curvature_threshold; + double attention_lane_curvature_calculation_ds; } occlusion; }; - using Indecisive = std::monostate; + struct Indecisive + { + std::string error; + }; struct StuckStop { size_t closest_idx{0}; @@ -157,6 +174,15 @@ class IntersectionModule : public SceneModuleInterface size_t first_attention_stop_line_idx{0}; size_t occlusion_stop_line_idx{0}; }; + struct OccludedAbsenceTrafficLight + { + bool is_actually_occlusion_cleared{false}; + bool collision_detected{false}; + bool temporal_stop_before_attention_required{false}; + size_t closest_idx{0}; + size_t first_attention_area_stop_line_idx{0}; + size_t peeking_limit_line_idx{0}; + }; struct Safe { // NOTE: if RTC is disapproved status, default stop lines are still needed. @@ -172,20 +198,22 @@ class IntersectionModule : public SceneModuleInterface size_t occlusion_stop_line_idx{0}; }; using DecisionResult = std::variant< - Indecisive, // internal process error, or over the pass judge line - StuckStop, // detected stuck vehicle - NonOccludedCollisionStop, // detected collision while FOV is clear - FirstWaitBeforeOcclusion, // stop for a while before peeking to occlusion - PeekingTowardOcclusion, // peeking into occlusion while collision is not detected - OccludedCollisionStop, // occlusion and collision are both detected - Safe, // judge as safe - TrafficLightArrowSolidOn // only detect vehicles violating traffic rules + Indecisive, // internal process error, or over the pass judge line + StuckStop, // detected stuck vehicle + NonOccludedCollisionStop, // detected collision while FOV is clear + FirstWaitBeforeOcclusion, // stop for a while before peeking to occlusion + PeekingTowardOcclusion, // peeking into occlusion while collision is not detected + OccludedCollisionStop, // occlusion and collision are both detected + OccludedAbsenceTrafficLight, // occlusion is detected in the absence of traffic light + Safe, // judge as safe + TrafficLightArrowSolidOn // only detect vehicles violating traffic rules >; IntersectionModule( const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, - const bool is_private_area, const bool enable_occlusion_detection, rclcpp::Node & node, + const std::string & turn_direction, const bool has_traffic_light, + const bool enable_occlusion_detection, const bool is_private_area, rclcpp::Node & node, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); /** @@ -209,38 +237,40 @@ class IntersectionModule : public SceneModuleInterface rclcpp::Node & node_; const int64_t lane_id_; const std::set associative_ids_; - std::string turn_direction_; + const std::string turn_direction_; + const bool has_traffic_light_; - bool is_go_out_ = false; - bool is_permanent_go_ = false; - DecisionResult prev_decision_result_; + bool is_go_out_{false}; + bool is_permanent_go_{false}; + DecisionResult prev_decision_result_{Indecisive{""}}; // Parameter PlannerParam planner_param_; - std::optional intersection_lanelets_; + std::optional intersection_lanelets_{std::nullopt}; // for occlusion detection const bool enable_occlusion_detection_; - std::optional> occlusion_attention_divisions_; - // OcclusionState prev_occlusion_state_ = OcclusionState::NONE; + std::optional> occlusion_attention_divisions_{std::nullopt}; StateMachine collision_state_machine_; //! for stable collision checking StateMachine before_creep_state_machine_; //! for two phase stop StateMachine occlusion_stop_state_machine_; StateMachine temporal_stop_before_attention_state_machine_; - // NOTE: uuid_ is base member + + // for pseudo-collision detection when ego just entered intersection on green light and upcoming + // vehicles are very slow + std::optional initial_green_light_observed_time_{std::nullopt}; // for stuck vehicle detection const bool is_private_area_; - StateMachine stuck_private_area_timeout_; // for RTC const UUID occlusion_uuid_; - bool occlusion_safety_ = true; - double occlusion_stop_distance_; - bool occlusion_activated_ = true; + bool occlusion_safety_{true}; + double occlusion_stop_distance_{0.0}; + bool occlusion_activated_{true}; // for first stop in two-phase stop - bool occlusion_first_stop_required_ = false; + bool occlusion_first_stop_required_{false}; void initializeRTCStatus(); void prepareRTCStatus( @@ -260,7 +290,8 @@ class IntersectionModule : public SceneModuleInterface bool checkCollision( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const autoware_auto_perception_msgs::msg::PredictedObjects & target_objects, - const util::PathLanelets & path_lanelets, const int closest_idx, const double time_delay, + const util::PathLanelets & path_lanelets, const size_t closest_idx, + const size_t last_intersection_stop_line_candidate_idx, const double time_delay, const util::TrafficPrioritizedLevel & traffic_prioritized_level); bool isOcclusionCleared( diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index a3bebecbd2bca..2f3fcc6f6fde0 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -16,10 +16,11 @@ #include "util_type.hpp" +#include #include #include #include -#include +#include #include #include #include @@ -42,6 +43,22 @@ #include #include #include + +namespace tier4_autoware_utils +{ + +template <> +inline geometry_msgs::msg::Point getPoint(const lanelet::ConstPoint3d & p) +{ + geometry_msgs::msg::Point point; + point.x = p.x(); + point.y = p.y(); + point.z = p.z(); + return point; +} + +} // namespace tier4_autoware_utils + namespace behavior_velocity_planner { namespace bg = boost::geometry; @@ -809,10 +826,26 @@ TrafficPrioritizedLevel getTrafficPrioritizedLevel( return TrafficPrioritizedLevel::NOT_PRIORITIZED; } +double getHighestCurvature(const lanelet::ConstLineString3d & centerline) +{ + std::vector points; + for (auto point = centerline.begin(); point != centerline.end(); point++) { + points.push_back(*point); + } + + SplineInterpolationPoints2d interpolation(points); + const std::vector curvatures = interpolation.getSplineInterpolatedCurvatures(); + std::vector curvatures_positive; + for (const auto & curvature : curvatures) { + curvatures_positive.push_back(std::fabs(curvature)); + } + return *std::max_element(curvatures_positive.begin(), curvatures_positive.end()); +} + std::vector generateDetectionLaneDivisions( lanelet::ConstLanelets detection_lanelets_all, - [[maybe_unused]] const lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const double resolution) + const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution, + const double curvature_threshold, const double curvature_calculation_ds) { using lanelet::utils::getCenterlineWithOffset; using lanelet::utils::to2D; @@ -824,6 +857,12 @@ std::vector generateDetectionLaneDivisions( if (turn_direction.compare("left") == 0 || turn_direction.compare("right") == 0) { continue; } + const auto fine_centerline = + lanelet::utils::generateFineCenterline(detection_lanelet, curvature_calculation_ds); + const double highest_curvature = getHighestCurvature(fine_centerline); + if (highest_curvature > curvature_threshold) { + continue; + } detection_lanelets.push_back(detection_lanelet); } @@ -1058,17 +1097,18 @@ Polygon2d generateStuckVehicleDetectAreaPolygon( } bool checkAngleForTargetLanelets( - const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, - const double detection_area_angle_thr, const bool consider_wrong_direction_vehicle, - const double margin) + const geometry_msgs::msg::Pose & pose, const double longitudinal_velocity, + const lanelet::ConstLanelets & target_lanelets, const double detection_area_angle_thr, + const bool consider_wrong_direction_vehicle, const double dist_margin, + const double parked_vehicle_speed_threshold) { for (const auto & ll : target_lanelets) { - if (!lanelet::utils::isInLanelet(pose, ll, margin)) { + if (!lanelet::utils::isInLanelet(pose, ll, dist_margin)) { continue; } const double ll_angle = lanelet::utils::getLaneletAngle(ll, pose.position); const double pose_angle = tf2::getYaw(pose.orientation); - const double angle_diff = tier4_autoware_utils::normalizeRadian(ll_angle - pose_angle); + const double angle_diff = tier4_autoware_utils::normalizeRadian(ll_angle - pose_angle, -M_PI); if (consider_wrong_direction_vehicle) { if (std::fabs(angle_diff) > 1.57 || std::fabs(angle_diff) < detection_area_angle_thr) { return true; @@ -1077,6 +1117,14 @@ bool checkAngleForTargetLanelets( if (std::fabs(angle_diff) < detection_area_angle_thr) { return true; } + // NOTE: sometimes parked vehicle direction is reversed even if its longitudinal velocity is + // positive + if ( + std::fabs(longitudinal_velocity) < parked_vehicle_speed_threshold && + (std::fabs(angle_diff) < detection_area_angle_thr || + (std::fabs(angle_diff + M_PI) < detection_area_angle_thr))) { + return true; + } } } return false; @@ -1108,8 +1156,9 @@ void cutPredictPathWithDuration( TimeDistanceArray calcIntersectionPassingTime( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::shared_ptr & planner_data, const std::set & associative_ids, - const int closest_idx, const double time_delay, const double intersection_velocity, - const double minimum_ego_velocity) + const size_t closest_idx, const size_t last_intersection_stop_line_candidate_idx, + const double time_delay, const double intersection_velocity, const double minimum_ego_velocity, + const bool use_upstream_velocity, const double minimum_upstream_velocity) { double dist_sum = 0.0; int assigned_lane_found = false; @@ -1119,7 +1168,9 @@ TimeDistanceArray calcIntersectionPassingTime( PathWithLaneId reference_path; for (size_t i = closest_idx; i < path.points.size(); ++i) { auto reference_point = path.points.at(i); - reference_point.point.longitudinal_velocity_mps = intersection_velocity; + if (!use_upstream_velocity) { + reference_point.point.longitudinal_velocity_mps = intersection_velocity; + } reference_path.points.push_back(reference_point); bool has_objective_lane_id = hasLaneIds(path.points.at(i), associative_ids); if (assigned_lane_found && !has_objective_lane_id) { @@ -1150,10 +1201,13 @@ TimeDistanceArray calcIntersectionPassingTime( // use average velocity between p1 and p2 const double average_velocity = (p1.point.longitudinal_velocity_mps + p2.point.longitudinal_velocity_mps) / 2.0; - passing_time += - (dist / std::max( - minimum_ego_velocity, - average_velocity)); // to avoid zero-division + const double minimum_ego_velocity_division = + (use_upstream_velocity && i > last_intersection_stop_line_candidate_idx) + ? minimum_upstream_velocity /* to avoid null division */ + : minimum_ego_velocity; + const double passing_velocity = + std::max(minimum_ego_velocity_division, average_velocity); + passing_time += (dist / passing_velocity); time_distance_array.emplace_back(passing_time, dist_sum); } diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index 72e96876f02c9..a75545353fb7a 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -20,14 +20,6 @@ #include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - #include #include #include @@ -117,7 +109,8 @@ TrafficPrioritizedLevel getTrafficPrioritizedLevel( std::vector generateDetectionLaneDivisions( lanelet::ConstLanelets detection_lanelets, - const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution); + const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution, + const double curvature_threshold, const double curvature_calculation_ds); std::optional generateInterpolatedPath( const int lane_id, const std::set & associative_lane_ids, @@ -136,9 +129,10 @@ Polygon2d generateStuckVehicleDetectAreaPolygon( const util::PathLanelets & path_lanelets, const double stuck_vehicle_detect_dist); bool checkAngleForTargetLanelets( - const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, - const double detection_area_angle_thr, const bool consider_wrong_direction_vehicle, - const double margin = 0.0); + const geometry_msgs::msg::Pose & pose, const double longitudinal_velocity, + const lanelet::ConstLanelets & target_lanelets, const double detection_area_angle_thr, + const bool consider_wrong_direction_vehicle, const double dist_margin, + const double parked_vehicle_speed_threshold); void cutPredictPathWithDuration( autoware_auto_perception_msgs::msg::PredictedObjects * objects_ptr, @@ -147,8 +141,9 @@ void cutPredictPathWithDuration( TimeDistanceArray calcIntersectionPassingTime( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::shared_ptr & planner_data, const std::set & associative_ids, - const int closest_idx, const double time_delay, const double intersection_velocity, - const double minimum_ego_velocity); + const size_t closest_idx, const size_t last_intersection_stop_line_candidate_idx, + const double time_delay, const double intersection_velocity, const double minimum_ego_velocity, + const bool use_upstream_velocity, const double minimum_upstream_velocity); double calcDistanceUntilIntersectionLanelet( const lanelet::ConstLanelet & assigned_lanelet, diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index 4135884f03c74..aed81d771e480 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -39,6 +39,7 @@ struct DebugData std::optional occlusion_first_stop_wall_pose{std::nullopt}; std::optional pass_judge_wall_pose{std::nullopt}; std::optional> attention_area{std::nullopt}; + std::optional> occlusion_attention_area{std::nullopt}; std::optional intersection_area{std::nullopt}; std::optional ego_lane{std::nullopt}; std::optional> adjacent_area{std::nullopt}; @@ -50,6 +51,8 @@ struct DebugData std::vector occlusion_polygons; std::optional> nearest_occlusion_projection{std::nullopt}; + autoware_auto_perception_msgs::msg::PredictedObjects blocking_attention_objects; + std::optional absence_traffic_light_creep_wall{std::nullopt}; }; struct InterpolatedPathInfo diff --git a/planning/behavior_velocity_no_stopping_area_module/config/no_stopping_area.param.yaml b/planning/behavior_velocity_no_stopping_area_module/config/no_stopping_area.param.yaml index f550188d4f2c1..c84848f8cc31d 100644 --- a/planning/behavior_velocity_no_stopping_area_module/config/no_stopping_area.param.yaml +++ b/planning/behavior_velocity_no_stopping_area_module/config/no_stopping_area.param.yaml @@ -8,4 +8,4 @@ stop_line_margin: 1.0 # [m] margin to auto-gen stop line at no stopping area detection_area_length: 200.0 # [m] used to create detection area polygon stuck_vehicle_front_margin: 6.0 # [m] obstacle stop max distance(5.0m) - enable_rtc: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval + enable_rtc: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval diff --git a/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt b/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt index 633d5fdd613af..8ce1a334c4b35 100644 --- a/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt +++ b/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt @@ -6,13 +6,7 @@ autoware_package() pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED - src/debug.cpp - src/decisions.cpp - src/footprint.cpp - src/lanelets_selection.cpp - src/manager.cpp - src/overlapping_range.cpp - src/scene_out_of_lane.cpp + DIRECTORY src ) ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_out_of_lane_module/README.md b/planning/behavior_velocity_out_of_lane_module/README.md index 885c0dc2859ab..99396eb0edf22 100644 --- a/planning/behavior_velocity_out_of_lane_module/README.md +++ b/planning/behavior_velocity_out_of_lane_module/README.md @@ -149,14 +149,13 @@ Moreover, parameter `action.distance_buffer` adds an extra distance between the | `minimum_distance` | double | [m] minimum distance inside a lanelet for an overlap to be considered | | `extra_length` | double | [m] extra arc length to add to the front and back of an overlap (used to calculate enter/exit times) | -| Parameter /action | Type | Description | -| ----------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | -| `skip_if_over_max_decel` | bool | [-] if true, do not take an action that would cause more deceleration than the maximum allowed | -| `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/s] slow down velocity | -| `stop.distance_threshold` | double | [m] insert a stop when closer than this distance from an overlap | +| Parameter /action | Type | Description | +| ----------------------------- | ------ | ---------------------------------------------------------------------------------------------- | +| `skip_if_over_max_decel` | bool | [-] if true, do not take an action that would cause more deceleration than the maximum allowed | +| `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 | +| `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 510dc86ef651d..c74c5b3d87c1d 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 @@ -24,8 +24,7 @@ action: # action to insert in the path if an object causes a conflict at an overlap skip_if_over_max_decel: true # if true, do not take an action that would cause more deceleration than the maximum allowed - strict: true # 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. + precision: 0.1 # [m] precision when inserting a stop pose in the path distance_buffer: 1.5 # [m] buffer distance to try to keep between the ego footprint and lane slowdown: distance_threshold: 30.0 # [m] insert a slowdown when closer than this distance from an overlap diff --git a/planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp b/planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp new file mode 100644 index 0000000000000..5d22f01222919 --- /dev/null +++ b/planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp @@ -0,0 +1,87 @@ +// 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 CALCULATE_SLOWDOWN_POINTS_HPP_ +#define CALCULATE_SLOWDOWN_POINTS_HPP_ + +#include "footprint.hpp" +#include "types.hpp" + +#include +#include + +#include + +#include +#include + +namespace behavior_velocity_planner::out_of_lane +{ + +bool can_decelerate( + const EgoData & ego_data, const PathPointWithLaneId & point, const double target_vel) +{ + if (ego_data.velocity < 0.5) return true; + const auto dist_ahead_of_ego = motion_utils::calcSignedArcLength( + ego_data.path.points, ego_data.pose.position, point.point.pose.position); + const auto acc_to_target_vel = + (ego_data.velocity * ego_data.velocity - target_vel * target_vel) / (2 * dist_ahead_of_ego); + return acc_to_target_vel < std::abs(ego_data.max_decel); +} + +std::optional calculate_last_in_lane_pose( + const EgoData & ego_data, const Slowdown & decision, const Polygon2d & footprint, + const PlannerParam & params) +{ + const auto from_arc_length = + motion_utils::calcSignedArcLength(ego_data.path.points, 0, ego_data.first_path_idx); + const auto to_arc_length = + motion_utils::calcSignedArcLength(ego_data.path.points, 0, decision.target_path_idx); + PathPointWithLaneId interpolated_point; + for (auto l = to_arc_length - params.precision; l > from_arc_length; l -= params.precision) { + // TODO(Maxime): binary search + interpolated_point.point.pose = motion_utils::calcInterpolatedPose(ego_data.path.points, l); + const auto respect_decel_limit = + !params.skip_if_over_max_decel || + can_decelerate(ego_data, interpolated_point, decision.velocity); + if ( + respect_decel_limit && !boost::geometry::overlaps( + project_to_pose(footprint, interpolated_point.point.pose), + decision.lane_to_avoid.polygon2d().basicPolygon())) + return interpolated_point; + } + return std::nullopt; +} + +/// @brief calculate the slowdown point to insert in the path +/// @param ego_data ego data (path, velocity, etc) +/// @param decisions decision (before which point to stop, what lane to avoid entering, etc) +/// @param params parameters +/// @return optional slowdown point to insert in the path +std::optional calculate_slowdown_point( + const EgoData & ego_data, const std::vector & decisions, PlannerParam params) +{ + params.extra_front_offset += params.dist_buffer; + const auto base_footprint = make_base_footprint(params); + + // search for the first slowdown decision for which a stop point can be inserted + for (const auto & decision : decisions) { + const auto last_in_lane_pose = + calculate_last_in_lane_pose(ego_data, decision, base_footprint, params); + if (last_in_lane_pose) return SlowdownToInsert{decision, *last_in_lane_pose}; + } + return std::nullopt; +} +} // namespace behavior_velocity_planner::out_of_lane +#endif // CALCULATE_SLOWDOWN_POINTS_HPP_ 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 62114f5a95b00..78d9b73c86a17 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp @@ -35,7 +35,7 @@ namespace behavior_velocity_planner::out_of_lane double distance_along_path(const EgoData & ego_data, const size_t target_idx) { return motion_utils::calcSignedArcLength( - ego_data.path->points, ego_data.pose.position, ego_data.first_path_idx + 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, const double min_velocity) @@ -43,7 +43,7 @@ double time_along_path(const EgoData & ego_data, const size_t target_idx, const const auto dist = distance_along_path(ego_data, target_idx); const auto v = std::max( std::max(ego_data.velocity, min_velocity), - ego_data.path->points[ego_data.first_path_idx + target_idx].point.longitudinal_velocity_mps * + ego_data.path.points[ego_data.first_path_idx + target_idx].point.longitudinal_velocity_mps * 0.5); return dist / v; } @@ -78,17 +78,18 @@ std::optional> object_time_to_range( auto worst_exit_time = std::optional(); for (const auto & predicted_path : object.kinematics.predicted_paths) { + const auto unique_path_points = motion_utils::removeOverlapPoints(predicted_path.path); 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()); const auto enter_segment_idx = - motion_utils::findNearestSegmentIndex(predicted_path.path, enter_point); + motion_utils::findNearestSegmentIndex(unique_path_points, enter_point); const auto enter_offset = motion_utils::calcLongitudinalOffsetToSegment( - predicted_path.path, enter_segment_idx, enter_point); - const auto enter_lat_dist = std::abs( - motion_utils::calcLateralOffset(predicted_path.path, enter_point, enter_segment_idx)); + unique_path_points, enter_segment_idx, enter_point); + const auto enter_lat_dist = + std::abs(motion_utils::calcLateralOffset(unique_path_points, enter_point, enter_segment_idx)); const auto enter_segment_length = tier4_autoware_utils::calcDistance2d( - predicted_path.path[enter_segment_idx], predicted_path.path[enter_segment_idx + 1]); + unique_path_points[enter_segment_idx], unique_path_points[enter_segment_idx + 1]); const auto enter_offset_ratio = enter_offset / enter_segment_length; const auto enter_time = static_cast(enter_segment_idx) * time_step + enter_offset_ratio * time_step; @@ -96,13 +97,13 @@ std::optional> object_time_to_range( const auto exit_point = geometry_msgs::msg::Point().set__x(range.exiting_point.x()).set__y(range.exiting_point.y()); const auto exit_segment_idx = - motion_utils::findNearestSegmentIndex(predicted_path.path, exit_point); + motion_utils::findNearestSegmentIndex(unique_path_points, exit_point); const auto exit_offset = motion_utils::calcLongitudinalOffsetToSegment( - predicted_path.path, exit_segment_idx, exit_point); + unique_path_points, exit_segment_idx, exit_point); const auto exit_lat_dist = - std::abs(motion_utils::calcLateralOffset(predicted_path.path, exit_point, exit_segment_idx)); + std::abs(motion_utils::calcLateralOffset(unique_path_points, exit_point, exit_segment_idx)); const auto exit_segment_length = tier4_autoware_utils::calcDistance2d( - predicted_path.path[exit_segment_idx], predicted_path.path[exit_segment_idx + 1]); + unique_path_points[exit_segment_idx], unique_path_points[exit_segment_idx + 1]); const auto exit_offset_ratio = exit_offset / static_cast(exit_segment_length); const auto exit_time = static_cast(exit_segment_idx) * time_step + exit_offset_ratio * time_step; @@ -125,11 +126,9 @@ std::optional> object_time_to_range( const auto same_driving_direction_as_ego = enter_time < exit_time; if (same_driving_direction_as_ego) { - RCLCPP_DEBUG(logger, " / SAME DIR \\\n"); worst_enter_time = worst_enter_time ? std::min(*worst_enter_time, enter_time) : enter_time; worst_exit_time = worst_exit_time ? std::max(*worst_exit_time, exit_time) : exit_time; } else { - RCLCPP_DEBUG(logger, " / OPPOSITE DIR \\\n"); worst_enter_time = worst_enter_time ? std::max(*worst_enter_time, enter_time) : enter_time; worst_exit_time = worst_exit_time ? std::min(*worst_exit_time, exit_time) : exit_time; } @@ -211,8 +210,11 @@ std::optional> object_time_to_range( bool threshold_condition(const RangeTimes & range_times, const PlannerParam & params) { - return std::min(range_times.object.enter_time, range_times.object.exit_time) < - params.time_threshold; + const auto enter_within_threshold = + range_times.object.enter_time > 0.0 && range_times.object.enter_time < params.time_threshold; + const auto exit_within_threshold = + range_times.object.exit_time > 0.0 && range_times.object.exit_time < params.time_threshold; + return enter_within_threshold || exit_within_threshold; } bool intervals_condition( @@ -356,11 +358,10 @@ std::optional calculate_decision( { std::optional decision; if (should_not_enter(range, inputs, params, logger)) { - const auto stop_before_range = params.strict ? find_most_preceding_range(range, inputs) : range; decision.emplace(); - decision->target_path_idx = inputs.ego_data.first_path_idx + - stop_before_range.entering_path_idx; // add offset from curr pose - decision->lane_to_avoid = stop_before_range.lane; + decision->target_path_idx = + inputs.ego_data.first_path_idx + range.entering_path_idx; // add offset from curr pose + decision->lane_to_avoid = range.lane; const auto ego_dist_to_range = distance_along_path(inputs.ego_data, range.entering_path_idx); set_decision_velocity(decision, ego_dist_to_range, params); } 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 9ad7437901309..31cc035703a7b 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp @@ -57,9 +57,9 @@ std::vector calculate_path_footprints( { const auto base_footprint = make_base_footprint(params); std::vector path_footprints; - path_footprints.reserve(ego_data.path->points.size()); - for (auto i = ego_data.first_path_idx; i < ego_data.path->points.size(); ++i) { - const auto & path_pose = ego_data.path->points[i].point.pose; + path_footprints.reserve(ego_data.path.points.size()); + for (auto i = ego_data.first_path_idx; i < ego_data.path.points.size(); ++i) { + const auto & path_pose = ego_data.path.points[i].point.pose; const auto angle = tf2::getYaw(path_pose.orientation); const auto rotated_footprint = tier4_autoware_utils::rotatePolygon(base_footprint, angle); lanelet::BasicPolygon2d footprint; 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 7f3b69f40840b..c5dedf23785ab 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 @@ -20,6 +20,7 @@ #include #include +#include namespace behavior_velocity_planner::out_of_lane { @@ -68,6 +69,8 @@ lanelet::ConstLanelets calculate_other_lanelets( return false; }; for (const auto & ll : lanelets_within_range) { + if (std::string(ll.second.attributeOr(lanelet::AttributeName::Subtype, "none")) != "road") + continue; const auto is_path_lanelet = contains_lanelet(path_lanelets, ll.second.id()); const auto is_ignored_lanelet = contains_lanelet(ignored_lanelets, ll.second.id()); if (!is_path_lanelet && !is_ignored_lanelet && !is_overlapped_by_path_lanelets(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 5765363024aed..2422c6fe84e56 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp @@ -56,7 +56,7 @@ OutOfLaneModuleManager::OutOfLaneModuleManager(rclcpp::Node & node) pp.skip_if_over_max_decel = getOrDeclareParameter(node, ns + ".action.skip_if_over_max_decel"); - pp.strict = getOrDeclareParameter(node, ns + ".action.strict"); + pp.precision = getOrDeclareParameter(node, ns + ".action.precision"); pp.dist_buffer = getOrDeclareParameter(node, ns + ".action.distance_buffer"); pp.slow_velocity = getOrDeclareParameter(node, ns + ".action.slowdown.velocity"); pp.slow_dist_threshold = 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 013643b6bd6d4..1487d9e63c8b0 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 @@ -14,6 +14,7 @@ #include "scene_out_of_lane.hpp" +#include "calculate_slowdown_points.hpp" #include "debug.hpp" #include "decisions.hpp" #include "filter_predicted_objects.hpp" @@ -31,6 +32,8 @@ #include #include +#include + #include #include @@ -62,9 +65,9 @@ bool OutOfLaneModule::modifyPathVelocity( stopwatch.tic(); EgoData ego_data; ego_data.pose = planner_data_->current_odometry->pose; - ego_data.path = path; + ego_data.path.points = path->points; ego_data.first_path_idx = - motion_utils::findNearestSegmentIndex(path->points, ego_data.pose.position); + motion_utils::findNearestSegmentIndex(ego_data.path.points, ego_data.pose.position); ego_data.velocity = planner_data_->current_velocity->twist.linear.x; ego_data.max_decel = -planner_data_->max_stop_acceleration_threshold; stopwatch.tic("calculate_path_footprints"); @@ -73,7 +76,7 @@ bool OutOfLaneModule::modifyPathVelocity( const auto calculate_path_footprints_us = stopwatch.toc("calculate_path_footprints"); // Calculate lanelets to ignore and consider const auto path_lanelets = planning_utils::getLaneletsOnPath( - *path, planner_data_->route_handler_->getLaneletMapPtr(), + ego_data.path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); const auto ignored_lanelets = calculate_ignored_lanelets(ego_data, path_lanelets, *planner_data_->route_handler_, params_); @@ -113,22 +116,27 @@ bool OutOfLaneModule::modifyPathVelocity( auto decisions = calculate_decisions(inputs, params_, logger_); const auto calculate_decisions_us = stopwatch.toc("calculate_decisions"); stopwatch.tic("calc_slowdown_points"); - const auto points_to_insert = calculate_slowdown_points(ego_data, decisions, params_); - debug_data_.slowdowns = points_to_insert; + const auto point_to_insert = calculate_slowdown_point(ego_data, decisions, params_); const auto calc_slowdown_points_us = stopwatch.toc("calc_slowdown_points"); stopwatch.tic("insert_slowdown_points"); - for (const auto & point : points_to_insert) { - auto path_idx = point.slowdown.target_path_idx; - planning_utils::insertVelocity(*ego_data.path, point.point, point.slowdown.velocity, path_idx); - if (point.slowdown.velocity == 0.0) { + debug_data_.slowdowns.clear(); + if (point_to_insert) { + debug_data_.slowdowns = {*point_to_insert}; + auto path_idx = motion_utils::findNearestSegmentIndex( + path->points, point_to_insert->point.point.pose.position) + + 1; + planning_utils::insertVelocity( + *path, point_to_insert->point, point_to_insert->slowdown.velocity, path_idx, + params_.precision); + if (point_to_insert->slowdown.velocity == 0.0) { tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = point.point.point.pose; + stop_factor.stop_pose = point_to_insert->point.point.pose; stop_factor.dist_to_stop_pose = motion_utils::calcSignedArcLength( - ego_data.path->points, ego_data.pose.position, point.point.point.pose.position); + ego_data.path.points, ego_data.pose.position, point_to_insert->point.point.pose.position); planning_utils::appendStopReason(stop_factor, stop_reason); } velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, point.point.point.pose, + path->points, planner_data_->current_odometry->pose, point_to_insert->point.point.pose, VelocityFactor::UNKNOWN); } const auto insert_slowdown_points_us = stopwatch.toc("insert_slowdown_points"); @@ -181,62 +189,4 @@ motion_utils::VirtualWalls OutOfLaneModule::createVirtualWalls() return virtual_walls; } -std::vector calculate_slowdown_points( - const EgoData & ego_data, const std::vector & decisions, PlannerParam params) -{ - std::vector to_insert; - params.extra_front_offset += params.dist_buffer; - const auto base_footprint = make_base_footprint(params); - - const auto can_decel = [&](const auto dist_ahead_of_ego, const auto target_vel) { - const auto acc_to_target_vel = - (ego_data.velocity * ego_data.velocity - target_vel * target_vel) / (2 * dist_ahead_of_ego); - return acc_to_target_vel < std::abs(ego_data.max_decel); - }; - const auto insert_decision = [&](const auto & path_point, const auto & decision) -> bool { - const auto dist_ahead_of_ego = motion_utils::calcSignedArcLength( - ego_data.path->points, ego_data.pose.position, path_point.point.pose.position); - if (!params.skip_if_over_max_decel || can_decel(dist_ahead_of_ego, decision.velocity)) { - to_insert.push_back({decision, path_point}); - return true; - } - return false; - }; - const auto insert_interpolated_decision = - [&](const auto & path_point, const auto & decision) -> bool { - auto interpolated_point = path_point; - const auto & path_pose = path_point.point.pose; - const auto & prev_path_pose = ego_data.path->points[decision.target_path_idx - 1].point.pose; - constexpr auto precision = 0.1; - for (auto ratio = precision; ratio <= 1.0; ratio += precision) { - interpolated_point.point.pose = - tier4_autoware_utils::calcInterpolatedPose(path_pose, prev_path_pose, ratio, false); - const auto is_overlap = boost::geometry::overlaps( - project_to_pose(base_footprint, interpolated_point.point.pose), - decision.lane_to_avoid.polygon2d().basicPolygon()); - if (!is_overlap) { - return insert_decision(path_point, decision); - } - } - return false; - }; - for (const auto & decision : decisions) { - const auto & path_point = ego_data.path->points[decision.target_path_idx]; - const auto decision_is_at_beginning_of_path = - decision.target_path_idx == ego_data.first_path_idx; - bool inserted = false; - if (decision_is_at_beginning_of_path) { - inserted = insert_decision(path_point, decision); - } else { - inserted = insert_interpolated_decision(path_point, decision); - // if no valid point found, fallback to using the previous index (known to not overlap) - if (!inserted) - inserted = insert_decision(ego_data.path->points[decision.target_path_idx], decision); - } - // only insert the first (i.e., lowest arc length) decision - if (inserted) break; - } - return to_insert; -} - } // namespace behavior_velocity_planner::out_of_lane 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 1a64c11a3c921..cdf8eef8f277b 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 @@ -57,15 +57,6 @@ class OutOfLaneModule : public SceneModuleInterface // Debug mutable DebugData debug_data_; }; - -/// @brief calculate points along the path where we want ego to slowdown/stop -/// @param ego_data ego data (path, velocity, etc) -/// @param decisions decisions (before which point to stop, what lane to avoid entering, etc) -/// @param params parameters -/// @return precise points to insert in the path -std::vector calculate_slowdown_points( - const EgoData & ego_data, const std::vector & decisions, PlannerParam params); - } // namespace behavior_velocity_planner::out_of_lane #endif // SCENE_OUT_OF_LANE_HPP_ 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 b81fa09884819..64d88f15e6966 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/types.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/types.hpp @@ -52,11 +52,11 @@ struct PlannerParam double overlap_min_dist; // [m] min distance inside another lane to consider an overlap // action to insert in the path if an object causes a conflict at an overlap bool skip_if_over_max_decel; // if true, skip the action if it causes more than the max decel - bool strict; // if true stop before entering *any* other lane, not only the lane to avoid double dist_buffer; double slow_velocity; double slow_dist_threshold; double stop_dist_threshold; + double precision; // [m] precision when inserting a stop pose in the path // ego dimensions used to create its polygon footprint double front_offset; // [m] front offset (from vehicle info) double rear_offset; // [m] rear offset (from vehicle info) @@ -135,7 +135,7 @@ struct OtherLane /// @brief data related to the ego vehicle struct EgoData { - autoware_auto_planning_msgs::msg::PathWithLaneId * path{}; + autoware_auto_planning_msgs::msg::PathWithLaneId path{}; size_t first_path_idx{}; double velocity{}; // [m/s] double max_decel{}; // [m/s²] diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index f30043ad42588..ec28656d8f866 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -151,6 +151,8 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio for (const auto & name : declare_parameter>("launch_modules")) { planner_manager_.launchScenePlugin(*this, name); } + + logger_configure_ = std::make_unique(this); } void BehaviorVelocityPlannerNode::onLoadPlugin( diff --git a/planning/behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/src/node.hpp index bdc280434ef41..ded1d08700a1d 100644 --- a/planning/behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/src/node.hpp @@ -16,6 +16,7 @@ #define NODE_HPP_ #include "planner_manager.hpp" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include #include @@ -126,6 +127,8 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node autoware_auto_planning_msgs::msg::Path generatePath( const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg, const PlannerData & planner_data); + + std::unique_ptr logger_configure_; }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml b/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml index e8e0357daa4a1..a837ae1b46b9b 100644 --- a/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml +++ b/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml @@ -5,4 +5,4 @@ tl_state_timeout: 1.0 yellow_lamp_period: 2.75 enable_pass_judge: true - enable_rtc: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval + enable_rtc: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval diff --git a/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp b/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp index fde0767006e33..9c13f14d180d0 100644 --- a/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp +++ b/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp @@ -31,6 +31,8 @@ #ifndef FREESPACE_PLANNER__FREESPACE_PLANNER_NODE_HPP_ #define FREESPACE_PLANNER__FREESPACE_PLANNER_NODE_HPP_ +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" + #include #include #include @@ -158,6 +160,8 @@ class FreespacePlannerNode : public rclcpp::Node void initializePlanningAlgorithm(); TransformStamped getTransform(const std::string & from, const std::string & to); + + std::unique_ptr logger_configure_; }; } // namespace freespace_planner 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 025cc32d4ed24..6400ae6d135ba 100644 --- a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp +++ b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp @@ -285,6 +285,8 @@ FreespacePlannerNode::FreespacePlannerNode(const rclcpp::NodeOptions & node_opti timer_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&FreespacePlannerNode::onTimer, this)); } + + logger_configure_ = std::make_unique(this); } PlannerCommonParam FreespacePlannerNode::getPlannerCommonParam() diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index 7e071e3b926b2..513a5b4c25a2e 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -126,6 +126,8 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) // otherwise the mission planner rejects the request for the API. data_check_timer_ = create_wall_timer( std::chrono::milliseconds(100), std::bind(&MissionPlanner::checkInitialization, this)); + + logger_configure_ = std::make_unique(this); } void MissionPlanner::checkInitialization() diff --git a/planning/mission_planner/src/mission_planner/mission_planner.hpp b/planning/mission_planner/src/mission_planner/mission_planner.hpp index f7af76f43bbb0..ea44d2643e186 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.hpp @@ -16,6 +16,7 @@ #define MISSION_PLANNER__MISSION_PLANNER_HPP_ #include "arrival_checker.hpp" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include #include @@ -147,6 +148,8 @@ class MissionPlanner : public rclcpp::Node std::shared_ptr normal_route_{nullptr}; std::shared_ptr mrm_route_{nullptr}; + + std::unique_ptr logger_configure_; }; } // namespace mission_planner diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp index bb49700464715..347d19f631588 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp @@ -30,6 +30,7 @@ #include "tf2_ros/transform_listener.h" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "tier4_autoware_utils/ros/self_pose_listener.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" @@ -263,6 +264,8 @@ class MotionVelocitySmootherNode : public rclcpp::Node bool isReverse(const TrajectoryPoints & points) const; void flipVelocity(TrajectoryPoints & points) const; void publishStopWatchTime(); + + std::unique_ptr logger_configure_; }; } // namespace motion_velocity_smoother diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp index a4ad118ffe012..75c84d3c482f6 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -99,6 +99,8 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions pub_velocity_limit_->publish(max_vel_msg); clock_ = get_clock(); + + logger_configure_ = std::make_unique(this); } void MotionVelocitySmootherNode::setupSmoother(const double wheelbase) 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 14925a8facf88..d6f49afad4391 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp @@ -21,6 +21,7 @@ #include "obstacle_avoidance_planner/replan_checker.hpp" #include "obstacle_avoidance_planner/type_alias.hpp" #include "rclcpp/rclcpp.hpp" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include @@ -127,6 +128,8 @@ class ObstacleAvoidancePlanner : public rclcpp::Node private: double vehicle_stop_margin_outside_drivable_area_; + + std::unique_ptr logger_configure_; }; } // namespace obstacle_avoidance_planner diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index f911fa87bedeb..9cee933a1c9a7 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -140,6 +140,8 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n // initialized. set_param_res_ = this->add_on_set_parameters_callback( std::bind(&ObstacleAvoidancePlanner::onParam, this, std::placeholders::_1)); + + logger_configure_ = std::make_unique(this); } rcl_interfaces::msg::SetParametersResult ObstacleAvoidancePlanner::onParam( diff --git a/planning/obstacle_cruise_planner/README.md b/planning/obstacle_cruise_planner/README.md index 314f9dcc29393..260f302791079 100644 --- a/planning/obstacle_cruise_planner/README.md +++ b/planning/obstacle_cruise_planner/README.md @@ -230,14 +230,19 @@ $$ ### Slow down planning -| Parameter | Type | Description | -| ---------------------------- | ------ | ------------------------------------------------------------------- | -| `slow_down.min_lat_velocity` | double | minimum velocity to linearly calculate slow down velocity [m] | -| `slow_down.max_lat_velocity` | double | maximum velocity to linearly calculate slow down velocity [m] | -| `slow_down.min_lat_margin` | double | minimum lateral margin to linearly calculate slow down velocity [m] | -| `slow_down.max_lat_margin` | double | maximum lateral margin to linearly calculate slow down velocity [m] | - -The role of the slow down planning is inserting slow down velocity in the trajectory where the trajectory points are close to the obstacles. +| Parameter | Type | Description | +| ----------------------------------------------- | -------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | +| `slow_down.labels` | vector(string) | A vector of labels for customizing obstacle-label-based slow down behavior. Each label represents an obstacle type that will be treated differently when applying slow down. The possible labels are ("default" (Mandatory), "unknown","car","truck","bus","trailer","motorcycle","bicycle" or "pedestrian") | +| `slow_down.default.min_lat_velocity` | double | minimum velocity to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels | +| `slow_down.default.max_lat_velocity` | double | maximum velocity to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels | +| `slow_down.default.min_lat_margin` | double | minimum lateral margin to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels | +| `slow_down.default.max_lat_margin` | double | maximum lateral margin to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels | +| `(optional) slow_down."label".min_lat_velocity` | double | minimum velocity to linearly calculate slow down velocity [m]. Note: only for obstacles specified in `slow_down.labels` | +| `(optional) slow_down."label".max_lat_velocity` | double | maximum velocity to linearly calculate slow down velocity [m]. Note: only for obstacles specified in `slow_down.labels` | +| `(optional) slow_down."label".min_lat_margin` | double | minimum lateral margin to linearly calculate slow down velocity [m]. Note: only for obstacles specified in `slow_down.labels` | +| `(optional) slow_down."label".max_lat_margin` | double | maximum lateral margin to linearly calculate slow down velocity [m]. Note: only for obstacles specified in `slow_down.labels` | + +The role of the slow down planning is inserting slow down velocity in the trajectory where the trajectory points are close to the obstacles. The parameters can be customized depending on the obstacle type (see `slow_down.labels`), making it possible to adjust the slow down behavior depending if the obstacle is a pedestrian, bicycle, car, etc. The closest point on the obstacle to the ego's trajectory is calculated. Then, the slow down velocity is calculated by linear interpolation with the distance between the point and trajectory as follows. 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 73bc4a83249a4..123d0dda93b7a 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -179,10 +179,19 @@ slow_down: # parameters to calculate slow down velocity by linear interpolation - min_lat_margin: 0.2 - max_lat_margin: 1.0 - min_ego_velocity: 2.0 - max_ego_velocity: 8.0 + labels: + - "default" + - "pedestrian" + default: + min_lat_margin: 0.2 + max_lat_margin: 1.0 + min_ego_velocity: 2.0 + max_ego_velocity: 8.0 + pedestrian: + min_lat_margin: 0.5 + max_lat_margin: 1.5 + min_ego_velocity: 1.0 + max_ego_velocity: 2.0 time_margin_on_target_velocity: 1.5 # [s] 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 e958074971f2a..c6cb73a680e5a 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 @@ -137,19 +137,21 @@ struct SlowDownObstacle : public TargetObstacleInterface { SlowDownObstacle( 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 double arg_precise_lat_dist, + const ObjectClassification & object_classification, const geometry_msgs::msg::Pose & arg_pose, + const double arg_lon_velocity, const double arg_lat_velocity, const double arg_precise_lat_dist, const geometry_msgs::msg::Point & arg_front_collision_point, const geometry_msgs::msg::Point & arg_back_collision_point) : TargetObstacleInterface(arg_uuid, arg_stamp, arg_pose, arg_lon_velocity, arg_lat_velocity), precise_lat_dist(arg_precise_lat_dist), front_collision_point(arg_front_collision_point), - back_collision_point(arg_back_collision_point) + back_collision_point(arg_back_collision_point), + classification(object_classification) { } double precise_lat_dist; // for efficient calculation geometry_msgs::msg::Point front_collision_point; geometry_msgs::msg::Point back_collision_point; + ObjectClassification classification; }; struct LongitudinalInfo 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 5786a96f72c41..9b2de4c8f2de0 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -20,6 +20,7 @@ #include "obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp" #include "obstacle_cruise_planner/type_alias.hpp" #include "signal_processing/lowpass_filter_1d.hpp" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" #include @@ -259,6 +260,8 @@ class ObstacleCruisePlannerNode : public rclcpp::Node // previous closest obstacle std::shared_ptr prev_closest_stop_obstacle_ptr_{nullptr}; + + std::unique_ptr logger_configure_; }; } // namespace motion_planning 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 9a5a6521a6494..d8c31fb35df9b 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 @@ -27,6 +27,7 @@ #include #include #include +#include #include #include @@ -221,12 +222,42 @@ class PlannerInterface struct SlowDownParam { + std::vector obstacle_labels{"default"}; + std::unordered_map types_map; + struct ObstacleSpecificParams + { + double max_lat_margin; + double min_lat_margin; + double max_ego_velocity; + double min_ego_velocity; + }; explicit SlowDownParam(rclcpp::Node & node) { - max_lat_margin = node.declare_parameter("slow_down.max_lat_margin"); - min_lat_margin = node.declare_parameter("slow_down.min_lat_margin"); - max_ego_velocity = node.declare_parameter("slow_down.max_ego_velocity"); - min_ego_velocity = node.declare_parameter("slow_down.min_ego_velocity"); + types_map = {{ObjectClassification::UNKNOWN, "unknown"}, + {ObjectClassification::CAR, "car"}, + {ObjectClassification::TRUCK, "truck"}, + {ObjectClassification::BUS, "bus"}, + {ObjectClassification::TRAILER, "trailer"}, + {ObjectClassification::MOTORCYCLE, "motorcycle"}, + {ObjectClassification::BICYCLE, "bicycle"}, + {ObjectClassification::PEDESTRIAN, "pedestrian"}}; + obstacle_labels = + node.declare_parameter>("slow_down.labels", obstacle_labels); + // obstacle label dependant parameters + for (const auto & label : obstacle_labels) { + ObstacleSpecificParams params; + params.max_lat_margin = + node.declare_parameter("slow_down." + label + ".max_lat_margin"); + params.min_lat_margin = + node.declare_parameter("slow_down." + label + ".min_lat_margin"); + params.max_ego_velocity = + node.declare_parameter("slow_down." + label + ".max_ego_velocity"); + params.min_ego_velocity = + node.declare_parameter("slow_down." + label + ".min_ego_velocity"); + obstacle_to_param_struct_map.emplace(std::make_pair(label, params)); + } + + // common parameters time_margin_on_target_velocity = node.declare_parameter("slow_down.time_margin_on_target_velocity"); lpf_gain_slow_down_vel = node.declare_parameter("slow_down.lpf_gain_slow_down_vel"); @@ -235,16 +266,35 @@ class PlannerInterface node.declare_parameter("slow_down.lpf_gain_dist_to_slow_down"); } + ObstacleSpecificParams getObstacleParamByLabel(const ObjectClassification & label_id) const + { + const std::string label = types_map.at(label_id.label); + if (obstacle_to_param_struct_map.count(label) > 0) { + return obstacle_to_param_struct_map.at(label); + } + return obstacle_to_param_struct_map.at("default"); + } + void onParam(const std::vector & parameters) { - tier4_autoware_utils::updateParam( - parameters, "slow_down.max_lat_margin", max_lat_margin); - tier4_autoware_utils::updateParam( - parameters, "slow_down.min_lat_margin", min_lat_margin); - tier4_autoware_utils::updateParam( - parameters, "slow_down.max_ego_velocity", max_ego_velocity); - tier4_autoware_utils::updateParam( - parameters, "slow_down.min_ego_velocity", min_ego_velocity); + // obstacle type dependant parameters + for (const auto & label : obstacle_labels) { + auto & param_by_obstacle_label = obstacle_to_param_struct_map[label]; + tier4_autoware_utils::updateParam( + parameters, "slow_down." + label + ".max_lat_margin", + param_by_obstacle_label.max_lat_margin); + tier4_autoware_utils::updateParam( + parameters, "slow_down." + label + ".min_lat_margin", + param_by_obstacle_label.min_lat_margin); + tier4_autoware_utils::updateParam( + parameters, "slow_down." + label + ".max_ego_velocity", + param_by_obstacle_label.max_ego_velocity); + tier4_autoware_utils::updateParam( + parameters, "slow_down." + label + ".min_ego_velocity", + param_by_obstacle_label.min_ego_velocity); + } + + // common parameters tier4_autoware_utils::updateParam( parameters, "slow_down.time_margin_on_target_velocity", time_margin_on_target_velocity); tier4_autoware_utils::updateParam( @@ -255,10 +305,8 @@ class PlannerInterface parameters, "slow_down.lpf_gain_dist_to_slow_down", lpf_gain_dist_to_slow_down); } - double max_lat_margin; - double min_lat_margin; - double max_ego_velocity; - double min_ego_velocity; + std::unordered_map obstacle_to_param_struct_map; + double time_margin_on_target_velocity; double lpf_gain_slow_down_vel; double lpf_gain_lat_dist; diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 8ef610cc9df10..d50054da46237 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -100,14 +100,15 @@ std::pair projectObstacleVelocityToTrajectory( const std::vector & traj_points, const Obstacle & obstacle) { const size_t object_idx = motion_utils::findNearestIndex(traj_points, obstacle.pose.position); - - const double object_vel_norm = std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y); - const double object_vel_yaw = std::atan2(obstacle.twist.linear.y, obstacle.twist.linear.x); const double traj_yaw = tf2::getYaw(traj_points.at(object_idx).pose.orientation); - return std::make_pair( - object_vel_norm * std::cos(object_vel_yaw - traj_yaw), - object_vel_norm * std::sin(object_vel_yaw - traj_yaw)); + const double obstacle_yaw = tf2::getYaw(obstacle.pose.orientation); + + const Eigen::Rotation2Dd R_ego_to_obstacle(obstacle_yaw - traj_yaw); + const Eigen::Vector2d obstacle_velocity(obstacle.twist.linear.x, obstacle.twist.linear.y); + const Eigen::Vector2d projected_velocity = R_ego_to_obstacle * obstacle_velocity; + + return std::make_pair(projected_velocity[0], projected_velocity[1]); } double calcObstacleMaxLength(const Shape & shape) @@ -438,6 +439,8 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & // set parameter callback set_param_res_ = this->add_on_set_parameters_callback( std::bind(&ObstacleCruisePlannerNode::onParam, this, std::placeholders::_1)); + + logger_configure_ = std::make_unique(this); } ObstacleCruisePlannerNode::PlanningAlgorithm ObstacleCruisePlannerNode::getPlanningAlgorithmType( @@ -1172,9 +1175,9 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl } const auto [tangent_vel, normal_vel] = projectObstacleVelocityToTrajectory(traj_points, obstacle); - return SlowDownObstacle{obstacle.uuid, obstacle.stamp, obstacle.pose, - tangent_vel, normal_vel, precise_lat_dist, - front_collision_point, back_collision_point}; + return SlowDownObstacle{obstacle.uuid, obstacle.stamp, obstacle.classification, + obstacle.pose, tangent_vel, normal_vel, + precise_lat_dist, front_collision_point, back_collision_point}; } void ObstacleCruisePlannerNode::checkConsistency( diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index 287df10dcebb5..76469364cfb39 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -665,7 +665,7 @@ std::vector PlannerInterface::generateSlowDownTrajectory( double PlannerInterface::calculateSlowDownVelocity( const SlowDownObstacle & obstacle, const std::optional & prev_output) const { - const auto & p = slow_down_param_; + const auto & p = slow_down_param_.getObstacleParamByLabel(obstacle.classification); const double stable_precise_lat_dist = [&]() { if (prev_output) { @@ -691,7 +691,6 @@ PlannerInterface::calculateDistanceToSlowDownWithConstraints( const SlowDownObstacle & obstacle, const std::optional & prev_output, const double dist_to_ego) const { - const auto & p = slow_down_param_; 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); @@ -728,8 +727,8 @@ PlannerInterface::calculateDistanceToSlowDownWithConstraints( // calculate distance during deceleration, slow down preparation, and slow down const double min_slow_down_prepare_dist = 3.0; - const double slow_down_prepare_dist = - std::max(min_slow_down_prepare_dist, slow_down_vel * p.time_margin_on_target_velocity); + const double slow_down_prepare_dist = std::max( + min_slow_down_prepare_dist, slow_down_vel * slow_down_param_.time_margin_on_target_velocity); const double deceleration_dist = offset_dist_to_collision + dist_to_front_collision - abs_ego_offset - dist_to_ego - slow_down_prepare_dist; const double slow_down_dist = 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 4cb71c710a225..93a9736bcdb36 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -18,6 +18,7 @@ #include "obstacle_stop_planner/adaptive_cruise_control.hpp" #include "obstacle_stop_planner/debug_marker.hpp" #include "obstacle_stop_planner/planner_data.hpp" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include #include @@ -274,6 +275,8 @@ class ObstacleStopPlannerNode : public rclcpp::Node return ret; } + + std::unique_ptr logger_configure_; }; } // namespace motion_planning diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index bec94335c4739..4e4b5d2f91ef3 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -242,6 +242,8 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod "~/input/expand_stop_range", 1, std::bind(&ObstacleStopPlannerNode::onExpandStopRange, this, std::placeholders::_1), createSubscriptionOptions(this)); + + logger_configure_ = std::make_unique(this); } void ObstacleStopPlannerNode::onPointCloud(const PointCloud2::ConstSharedPtr input_msg) diff --git a/planning/obstacle_velocity_limiter/README.md b/planning/obstacle_velocity_limiter/README.md index d670f07d26e9d..36aa35bb31156 100644 --- a/planning/obstacle_velocity_limiter/README.md +++ b/planning/obstacle_velocity_limiter/README.md @@ -161,7 +161,7 @@ For example a value of `1` means all trajectory points will be evaluated while a | ------------------------------- | ---------------------------------------- | -------------------------------------------------------- | | `~/output/trajectory` | `autoware_auto_planning_msgs/Trajectory` | Trajectory with adjusted velocities | | `~/output/debug_markers` | `visualization_msgs/MarkerArray` | Debug markers (envelopes, obstacle polygons) | -| `~/output/runtime_microseconds` | `std_msgs/Int64` | Time taken to calculate the trajectory (in microseconds) | +| `~/output/runtime_microseconds` | `tier4_debug_msgs/Float64` | Time taken to calculate the trajectory (in microseconds) | ## Parameters diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp index 8916137c84077..cbdd390183d61 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp @@ -18,6 +18,7 @@ #include "obstacle_velocity_limiter/obstacles.hpp" #include "obstacle_velocity_limiter/parameters.hpp" #include "obstacle_velocity_limiter/types.hpp" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include #include @@ -28,7 +29,7 @@ #include #include #include -#include +#include #include #include @@ -55,7 +56,7 @@ class ObstacleVelocityLimiterNode : public rclcpp::Node pub_trajectory_; //!< @brief publisher for output trajectory rclcpp::Publisher::SharedPtr pub_debug_markers_; //!< @brief publisher for debug markers - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr pub_runtime_; //!< @brief publisher for callback runtime rclcpp::Subscription::SharedPtr sub_trajectory_; //!< @brief subscriber for reference trajectory @@ -101,6 +102,8 @@ class ObstacleVelocityLimiterNode : public rclcpp::Node /// @brief validate the inputs of the node /// @return true if the inputs are valid bool validInputs(); + + std::unique_ptr logger_configure_; }; } // namespace obstacle_velocity_limiter diff --git a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp index 123ed997d4445..086d76e9ef0fe 100644 --- a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp +++ b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp @@ -68,7 +68,8 @@ ObstacleVelocityLimiterNode::ObstacleVelocityLimiterNode(const rclcpp::NodeOptio pub_trajectory_ = create_publisher("~/output/trajectory", 1); pub_debug_markers_ = create_publisher("~/output/debug_markers", 1); - pub_runtime_ = create_publisher("~/output/runtime_microseconds", 1); + pub_runtime_ = + create_publisher("~/output/runtime_microseconds", 1); const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); vehicle_lateral_offset_ = static_cast(vehicle_info.max_lateral_offset_m); @@ -79,6 +80,8 @@ ObstacleVelocityLimiterNode::ObstacleVelocityLimiterNode(const rclcpp::NodeOptio set_param_res_ = add_on_set_parameters_callback([this](const auto & params) { return onParameter(params); }); + + logger_configure_ = std::make_unique(this); } rcl_interfaces::msg::SetParametersResult ObstacleVelocityLimiterNode::onParameter( @@ -221,7 +224,8 @@ void ObstacleVelocityLimiterNode::onTrajectory(const Trajectory::ConstSharedPtr const auto t_end = std::chrono::system_clock::now(); const auto runtime = std::chrono::duration_cast(t_end - t_start); - pub_runtime_->publish(std_msgs::msg::Int64().set__data(runtime.count())); + pub_runtime_->publish(tier4_debug_msgs::msg::Float64Stamped().set__stamp(now()).set__data( + static_cast(runtime.count()))); if (pub_debug_markers_->get_subscription_count() > 0) { const auto safe_projected_linestrings = 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 8bd73292a56de..a67983393681f 100644 --- a/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp +++ b/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp @@ -21,6 +21,7 @@ #include "path_smoother/replan_checker.hpp" #include "path_smoother/type_alias.hpp" #include "rclcpp/rclcpp.hpp" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include #include @@ -104,6 +105,8 @@ class ElasticBandSmoother : public rclcpp::Node std::vector extendTrajectory( const std::vector & traj_points, const std::vector & optimized_points) const; + + std::unique_ptr logger_configure_; }; } // namespace path_smoother diff --git a/planning/path_smoother/src/elastic_band_smoother.cpp b/planning/path_smoother/src/elastic_band_smoother.cpp index 31fe23a47b0ca..cd314b7b141bf 100644 --- a/planning/path_smoother/src/elastic_band_smoother.cpp +++ b/planning/path_smoother/src/elastic_band_smoother.cpp @@ -95,6 +95,8 @@ ElasticBandSmoother::ElasticBandSmoother(const rclcpp::NodeOptions & node_option // set parameter callback set_param_res_ = this->add_on_set_parameters_callback( std::bind(&ElasticBandSmoother::onParam, this, std::placeholders::_1)); + + logger_configure_ = std::make_unique(this); } rcl_interfaces::msg::SetParametersResult ElasticBandSmoother::onParam( diff --git a/planning/planning_debug_tools/scripts/trajectory_visualizer.py b/planning/planning_debug_tools/scripts/trajectory_visualizer.py index 73e41b052cd90..10bd41c5c9c93 100755 --- a/planning/planning_debug_tools/scripts/trajectory_visualizer.py +++ b/planning/planning_debug_tools/scripts/trajectory_visualizer.py @@ -178,15 +178,15 @@ def __init__(self): # main process if PLOT_TYPE == "VEL_ACC_JERK": + self.setPlotTrajectory() self.ani = animation.FuncAnimation( self.fig, self.plotTrajectory, interval=100, blit=True ) - self.setPlotTrajectory() else: + self.setPlotTrajectoryVelocity() self.ani = animation.FuncAnimation( self.fig, self.plotTrajectoryVelocity, interval=100, blit=True ) - self.setPlotTrajectoryVelocity() plt.show() diff --git a/planning/planning_validator/include/planning_validator/planning_validator.hpp b/planning/planning_validator/include/planning_validator/planning_validator.hpp index 321eb23aa7fd4..3b09ebe51ff6b 100644 --- a/planning/planning_validator/include/planning_validator/planning_validator.hpp +++ b/planning/planning_validator/include/planning_validator/planning_validator.hpp @@ -17,6 +17,7 @@ #include "planning_validator/debug_marker.hpp" #include "planning_validator/msg/planning_validator_status.hpp" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include @@ -117,6 +118,8 @@ class PlanningValidator : public rclcpp::Node Odometry::ConstSharedPtr current_kinematics_; std::shared_ptr debug_pose_publisher_; + + std::unique_ptr logger_configure_; }; } // namespace planning_validator diff --git a/planning/planning_validator/src/planning_validator.cpp b/planning/planning_validator/src/planning_validator.cpp index db4510c2e5d0d..13a63a7e7d4ed 100644 --- a/planning/planning_validator/src/planning_validator.cpp +++ b/planning/planning_validator/src/planning_validator.cpp @@ -49,6 +49,8 @@ PlanningValidator::PlanningValidator(const rclcpp::NodeOptions & options) if (publish_diag_) { setupDiag(); } + + logger_configure_ = std::make_unique(this); } void PlanningValidator::setupParameters() diff --git a/planning/rtc_auto_mode_manager/CMakeLists.txt b/planning/rtc_auto_mode_manager/CMakeLists.txt deleted file mode 100644 index 52c0c68384a57..0000000000000 --- a/planning/rtc_auto_mode_manager/CMakeLists.txt +++ /dev/null @@ -1,29 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(rtc_auto_mode_manager) - -### Compile options -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Werror) -endif() - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(${PROJECT_NAME} SHARED - src/rtc_auto_mode_manager_interface.cpp - src/node.cpp -) - -rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "rtc_auto_mode_manager::RTCAutoModeManagerNode" - EXECUTABLE ${PROJECT_NAME}_node -) - -ament_auto_package( - INSTALL_TO_SHARE - config - launch -) diff --git a/planning/rtc_auto_mode_manager/README.md b/planning/rtc_auto_mode_manager/README.md deleted file mode 100644 index 0e2614baaf1c8..0000000000000 --- a/planning/rtc_auto_mode_manager/README.md +++ /dev/null @@ -1,47 +0,0 @@ -# RTC Auto Mode Manager - -## Purpose - -RTC Auto Mode Manager is a node to approve request to cooperate from behavior planning modules automatically. - -## Inputs / Outputs - -### Input - -| Name | Type | Description | -| ------------------------------- | --------------------------- | ------------------------------------------ | -| `/planning/enable_auto_mode/**` | tier4_rtc_msgs/srv/AutoMode | Service to enable auto mode for the module | - -### Output - -| Name | Type | Description | -| ---------------------------------------- | --------------------------- | ------------------------------------------ | -| `/planning/enable_auto_mode/internal/**` | tier4_rtc_msgs/srv/AutoMode | Service to enable auto mode for the module | - -## Parameters - -| Name | Type | Description | -| :-------------------- | :--------------- | :----------------------------------------------- | -| `module_list` | List of `string` | Module names managing in `rtc_auto_mode_manager` | -| `default_enable_list` | List of `string` | Module names enabled auto mode at initialization | - -## Inner-workings / Algorithms - -```plantuml - -start -:Read parameters; -:Send enable auto mode service to the module listed in `default_enable_list`; -repeat - if (Enable auto mode command received?) then (yes) - :Send enable auto mode command to rtc_interface; - else (no) - endif -repeat while (Is node running?) is (yes) not (no) -end - -``` - -## Assumptions / Known limits - -## Future extensions / Unimplemented parts diff --git a/planning/rtc_auto_mode_manager/config/rtc_auto_mode_manager.param.yaml b/planning/rtc_auto_mode_manager/config/rtc_auto_mode_manager.param.yaml deleted file mode 100644 index 0aa3cbd49e8e9..0000000000000 --- a/planning/rtc_auto_mode_manager/config/rtc_auto_mode_manager.param.yaml +++ /dev/null @@ -1,31 +0,0 @@ -/**: - ros__parameters: - module_list: - - "blind_spot" - - "crosswalk" - - "detection_area" - - "intersection" - - "no_stopping_area" - - "traffic_light" - - "lane_change_left" - - "lane_change_right" - - "avoidance_left" - - "avoidance_right" - - "goal_planner" - - "start_planner" - - "intersection_occlusion" - - default_enable_list: - - "blind_spot" - - "crosswalk" - - "detection_area" - - "intersection" - - "no_stopping_area" - - "traffic_light" - - "lane_change_left" - - "lane_change_right" - - "avoidance_left" - - "avoidance_right" - - "goal_planner" - - "start_planner" - - "intersection_occlusion" diff --git a/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/node.hpp b/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/node.hpp deleted file mode 100644 index d36974508d2df..0000000000000 --- a/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/node.hpp +++ /dev/null @@ -1,44 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef RTC_AUTO_MODE_MANAGER__NODE_HPP_ -#define RTC_AUTO_MODE_MANAGER__NODE_HPP_ - -#include "rclcpp/rclcpp.hpp" -#include "rtc_auto_mode_manager/rtc_auto_mode_manager_interface.hpp" - -#include "tier4_rtc_msgs/msg/auto_mode_status_array.hpp" - -#include -#include -#include - -namespace rtc_auto_mode_manager -{ -using tier4_rtc_msgs::msg::AutoModeStatusArray; -class RTCAutoModeManagerNode : public rclcpp::Node -{ -public: - explicit RTCAutoModeManagerNode(const rclcpp::NodeOptions & node_options); - -private: - rclcpp::TimerBase::SharedPtr timer_; - AutoModeStatusArray auto_mode_statuses_; - rclcpp::Publisher::SharedPtr statuses_pub_; - std::vector> managers_; -}; - -} // namespace rtc_auto_mode_manager - -#endif // RTC_AUTO_MODE_MANAGER__NODE_HPP_ diff --git a/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/rtc_auto_mode_manager_interface.hpp b/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/rtc_auto_mode_manager_interface.hpp deleted file mode 100644 index 098852c397161..0000000000000 --- a/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/rtc_auto_mode_manager_interface.hpp +++ /dev/null @@ -1,53 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef RTC_AUTO_MODE_MANAGER__RTC_AUTO_MODE_MANAGER_INTERFACE_HPP_ -#define RTC_AUTO_MODE_MANAGER__RTC_AUTO_MODE_MANAGER_INTERFACE_HPP_ - -#include "rclcpp/rclcpp.hpp" - -#include "tier4_rtc_msgs/msg/auto_mode_status.hpp" -#include "tier4_rtc_msgs/msg/module.hpp" -#include "tier4_rtc_msgs/srv/auto_mode.hpp" - -#include -#include -#include - -namespace rtc_auto_mode_manager -{ -using tier4_rtc_msgs::msg::AutoModeStatus; -using tier4_rtc_msgs::msg::Module; -using tier4_rtc_msgs::srv::AutoMode; - -class RTCAutoModeManagerInterface -{ -public: - RTCAutoModeManagerInterface( - rclcpp::Node * node, const std::string & module_name, const bool default_enable); - AutoModeStatus getAutoModeStatus() const { return auto_mode_status_; } - -private: - void onEnableService( - const AutoMode::Request::SharedPtr request, const AutoMode::Response::SharedPtr response); - AutoMode::Request createRequest(const AutoMode::Request::SharedPtr request) const; - AutoModeStatus auto_mode_status_; - rclcpp::Client::SharedPtr enable_cli_; - rclcpp::Service::SharedPtr enable_srv_; - - std::string enable_auto_mode_namespace_ = "/planning/enable_auto_mode"; -}; -} // namespace rtc_auto_mode_manager - -#endif // RTC_AUTO_MODE_MANAGER__RTC_AUTO_MODE_MANAGER_INTERFACE_HPP_ diff --git a/planning/rtc_auto_mode_manager/launch/rtc_auto_mode_manager.launch.xml b/planning/rtc_auto_mode_manager/launch/rtc_auto_mode_manager.launch.xml deleted file mode 100644 index 85d4b3446fb89..0000000000000 --- a/planning/rtc_auto_mode_manager/launch/rtc_auto_mode_manager.launch.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/planning/rtc_auto_mode_manager/package.xml b/planning/rtc_auto_mode_manager/package.xml deleted file mode 100644 index e9c16aac1bb91..0000000000000 --- a/planning/rtc_auto_mode_manager/package.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - - rtc_auto_mode_manager - 0.1.0 - The rtc_auto_mode_manager package - - Fumiya Watanabe - Taiki Tanaka - - Apache License 2.0 - - Fumiya Watanabe - - ament_cmake_auto - autoware_cmake - - rclcpp - rclcpp_components - tier4_rtc_msgs - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/planning/rtc_auto_mode_manager/src/node.cpp b/planning/rtc_auto_mode_manager/src/node.cpp deleted file mode 100644 index 6092d2741d909..0000000000000 --- a/planning/rtc_auto_mode_manager/src/node.cpp +++ /dev/null @@ -1,51 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "rtc_auto_mode_manager/node.hpp" - -#include - -namespace rtc_auto_mode_manager -{ - -RTCAutoModeManagerNode::RTCAutoModeManagerNode(const rclcpp::NodeOptions & node_options) -: Node("rtc_auto_mode_manager_node", node_options) -{ - const std::vector module_list = - declare_parameter>("module_list"); - const std::vector default_enable_list = - declare_parameter>("default_enable_list"); - - for (const auto & module_name : module_list) { - const bool enabled = - std::count(default_enable_list.begin(), default_enable_list.end(), module_name) != 0; - managers_.push_back(std::make_shared(this, module_name, enabled)); - } - statuses_pub_ = create_publisher("output/auto_mode_statuses", 1); - using std::chrono_literals::operator""ms; - timer_ = rclcpp::create_timer(this, get_clock(), 100ms, [this] { - AutoModeStatusArray auto_mode_statuses; - for (const auto & m : managers_) { - auto_mode_statuses.stamp = get_clock()->now(); - auto_mode_statuses.statuses.emplace_back(m->getAutoModeStatus()); - } - statuses_pub_->publish(auto_mode_statuses); - auto_mode_statuses.statuses.clear(); - }); -} - -} // namespace rtc_auto_mode_manager - -#include -RCLCPP_COMPONENTS_REGISTER_NODE(rtc_auto_mode_manager::RTCAutoModeManagerNode) diff --git a/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp b/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp deleted file mode 100644 index e28957cee161b..0000000000000 --- a/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp +++ /dev/null @@ -1,109 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "rtc_auto_mode_manager/rtc_auto_mode_manager_interface.hpp" - -namespace rtc_auto_mode_manager -{ -Module getModuleType(const std::string & module_name) -{ - Module module; - if (module_name == "blind_spot") { - module.type = Module::BLIND_SPOT; - } else if (module_name == "crosswalk") { - module.type = Module::CROSSWALK; - } else if (module_name == "detection_area") { - module.type = Module::DETECTION_AREA; - } else if (module_name == "intersection") { - module.type = Module::INTERSECTION; - } else if (module_name == "no_stopping_area") { - module.type = Module::NO_STOPPING_AREA; - } else if (module_name == "occlusion_spot") { - module.type = Module::OCCLUSION_SPOT; - } else if (module_name == "traffic_light") { - module.type = Module::TRAFFIC_LIGHT; - } else if (module_name == "virtual_traffic_light") { - module.type = Module::TRAFFIC_LIGHT; - } else if (module_name == "external_request_lane_change_left") { - module.type = Module::EXT_REQUEST_LANE_CHANGE_LEFT; - } else if (module_name == "external_request_lane_change_right") { - module.type = Module::EXT_REQUEST_LANE_CHANGE_RIGHT; - } else if (module_name == "lane_change_left") { - module.type = Module::LANE_CHANGE_LEFT; - } else if (module_name == "lane_change_right") { - module.type = Module::LANE_CHANGE_RIGHT; - } else if (module_name == "avoidance_by_lane_change_left") { - module.type = Module::AVOIDANCE_BY_LC_LEFT; - } else if (module_name == "avoidance_by_lane_change_right") { - module.type = Module::AVOIDANCE_BY_LC_RIGHT; - } else if (module_name == "avoidance_left") { - module.type = Module::AVOIDANCE_LEFT; - } else if (module_name == "avoidance_right") { - module.type = Module::AVOIDANCE_RIGHT; - } else if (module_name == "goal_planner") { - module.type = Module::GOAL_PLANNER; - } else if (module_name == "start_planner") { - module.type = Module::START_PLANNER; - } else if (module_name == "intersection_occlusion") { - module.type = Module::INTERSECTION_OCCLUSION; - } else { - module.type = Module::NONE; - } - return module; -} - -RTCAutoModeManagerInterface::RTCAutoModeManagerInterface( - rclcpp::Node * node, const std::string & module_name, const bool default_enable) -{ - using std::chrono_literals::operator""s; - using std::placeholders::_1; - using std::placeholders::_2; - - // Service client - enable_cli_ = - node->create_client(enable_auto_mode_namespace_ + "/internal/" + module_name); - - while (!enable_cli_->wait_for_service(1s)) { - if (!rclcpp::ok()) { - RCLCPP_ERROR(node->get_logger(), "Interrupted while waiting for service."); - rclcpp::shutdown(); - return; - } - RCLCPP_INFO_STREAM(node->get_logger(), "Waiting for service... [" << module_name << "]"); - } - - // Service - enable_srv_ = node->create_service( - enable_auto_mode_namespace_ + "/" + module_name, - std::bind(&RTCAutoModeManagerInterface::onEnableService, this, _1, _2)); - - auto_mode_status_.module = getModuleType(module_name); - // Send enable auto mode request - if (default_enable) { - auto_mode_status_.is_auto_mode = true; - AutoMode::Request::SharedPtr request = std::make_shared(); - request->enable = true; - enable_cli_->async_send_request(request); - } -} - -void RTCAutoModeManagerInterface::onEnableService( - const AutoMode::Request::SharedPtr request, const AutoMode::Response::SharedPtr response) -{ - auto_mode_status_.is_auto_mode = request->enable; - enable_cli_->async_send_request(request); - response->success = true; -} - -} // namespace rtc_auto_mode_manager diff --git a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp b/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp index 4929d0f8e27f3..d6e083421c0b7 100644 --- a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp +++ b/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp @@ -85,7 +85,7 @@ class RTCInterface std::string cooperate_status_namespace_ = "/planning/cooperate_status"; std::string cooperate_commands_namespace_ = "/planning/cooperate_commands"; - std::string enable_auto_mode_namespace_ = "/planning/enable_auto_mode/internal"; + std::string enable_auto_mode_namespace_ = "/planning/enable_auto_mode"; mutable std::mutex mutex_; }; diff --git a/planning/rtc_replayer/launch/rtc_replayer.launch.xml b/planning/rtc_replayer/launch/rtc_replayer.launch.xml index 9b832b369c77c..70d7baf2dce4a 100644 --- a/planning/rtc_replayer/launch/rtc_replayer.launch.xml +++ b/planning/rtc_replayer/launch/rtc_replayer.launch.xml @@ -1,7 +1,3 @@ - - - - - + 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 f31bb7ddee331..c40fef5b0c43e 100644 --- a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp +++ b/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp @@ -16,6 +16,7 @@ #define SURROUND_OBSTACLE_CHECKER__NODE_HPP_ #include "surround_obstacle_checker/debug_marker.hpp" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include #include @@ -138,6 +139,8 @@ class SurroundObstacleCheckerNode : public rclcpp::Node // State Machine State state_ = State::PASS; std::shared_ptr last_obstacle_found_time_; + + std::unique_ptr logger_configure_; }; } // namespace surround_obstacle_checker diff --git a/planning/surround_obstacle_checker/src/node.cpp b/planning/surround_obstacle_checker/src/node.cpp index ab542bdfa73ed..1b981fcddb155 100644 --- a/planning/surround_obstacle_checker/src/node.cpp +++ b/planning/surround_obstacle_checker/src/node.cpp @@ -188,6 +188,8 @@ SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptio 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"); + + logger_configure_ = std::make_unique(this); } vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); diff --git a/sensing/gnss_poser/CMakeLists.txt b/sensing/gnss_poser/CMakeLists.txt index 167f2c51337ce..775cde514945f 100644 --- a/sensing/gnss_poser/CMakeLists.txt +++ b/sensing/gnss_poser/CMakeLists.txt @@ -34,5 +34,6 @@ rclcpp_components_register_node(gnss_poser_node ) ament_auto_package(INSTALL_TO_SHARE + config launch ) diff --git a/sensing/gnss_poser/config/gnss_poser.param.yaml b/sensing/gnss_poser/config/gnss_poser.param.yaml new file mode 100644 index 0000000000000..30be98bba8cf1 --- /dev/null +++ b/sensing/gnss_poser/config/gnss_poser.param.yaml @@ -0,0 +1,9 @@ +/**: + ros__parameters: + base_frame: base_link + gnss_base_frame: gnss_base_link + gnss_frame: gnss + map_frame: map + buff_epoch: 1 + use_gnss_ins_orientation: true + gnss_pose_pub_method: 0 diff --git a/sensing/gnss_poser/launch/gnss_poser.launch.xml b/sensing/gnss_poser/launch/gnss_poser.launch.xml index 60e952c6845f4..d9b850e6995ae 100755 --- a/sensing/gnss_poser/launch/gnss_poser.launch.xml +++ b/sensing/gnss_poser/launch/gnss_poser.launch.xml @@ -1,5 +1,7 @@ + + @@ -7,15 +9,6 @@ - - - - - - - - - @@ -23,13 +16,6 @@ - - - - - - - - + diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp index 1ae8bce2ed7f2..599a017bffed7 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -30,14 +30,14 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options) : rclcpp::Node("gnss_poser", node_options), tf2_listener_(tf2_buffer_), tf2_broadcaster_(*this), - base_frame_(declare_parameter("base_frame", "base_link")), - gnss_frame_(declare_parameter("gnss_frame", "gnss")), - gnss_base_frame_(declare_parameter("gnss_base_frame", "gnss_base_link")), - map_frame_(declare_parameter("map_frame", "map")), - use_gnss_ins_orientation_(declare_parameter("use_gnss_ins_orientation", true)), + base_frame_(declare_parameter("base_frame")), + gnss_frame_(declare_parameter("gnss_frame")), + gnss_base_frame_(declare_parameter("gnss_base_frame")), + map_frame_(declare_parameter("map_frame")), + use_gnss_ins_orientation_(declare_parameter("use_gnss_ins_orientation")), msg_gnss_ins_orientation_stamped_( std::make_shared()), - gnss_pose_pub_method(declare_parameter("gnss_pose_pub_method", 0)) + gnss_pose_pub_method(declare_parameter("gnss_pose_pub_method")) { // Subscribe to map_projector_info topic const auto adaptor = component_interface_utils::NodeAdaptor(this); @@ -45,7 +45,7 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options) sub_map_projector_info_, [this](const MapProjectorInfo::Message::ConstSharedPtr msg) { callbackMapProjectorInfo(msg); }); - int buff_epoch = declare_parameter("buff_epoch", 1); + int buff_epoch = declare_parameter("buff_epoch"); position_buffer_.set_capacity(buff_epoch); nav_sat_fix_sub_ = create_subscription( diff --git a/sensing/pointcloud_preprocessor/docs/blockage_diag.md b/sensing/pointcloud_preprocessor/docs/blockage_diag.md index 519bd831fd39c..32e5a5869109d 100644 --- a/sensing/pointcloud_preprocessor/docs/blockage_diag.md +++ b/sensing/pointcloud_preprocessor/docs/blockage_diag.md @@ -2,11 +2,13 @@ ## Purpose -To ensure the performance of LiDAR and safety for autonomous driving, the abnormal condition diagnostics feature is needed. -LiDAR blockage is abnormal condition of LiDAR when some unwanted objects stitch to and block the light pulses and return signal. +To ensure the performance of LiDAR and safety for autonomous driving, the abnormal condition diagnostics feature is +needed. +LiDAR blockage is abnormal condition of LiDAR when some unwanted objects stitch to and block the light pulses and return +signal. This node's purpose is to detect the existing of blockage on LiDAR and its related size and location. -## Inner-workings / Algorithms +## Inner-workings / Algorithms(Blockage detection) This node bases on the no-return region and its location to decide if it is a blockage. @@ -16,6 +18,14 @@ The logic is showed as below ![blockage_diag_flowchart](./image/blockage_diag_flowchart.drawio.svg) +## Inner-workings /Algorithms(Dust detection) + +About dust detection, morphological processing is implemented. +If the lidar's ray cannot be acquired due to dust in the lidar area where the point cloud is considered to return from +the ground, +black pixels appear as noise in the depth image. +The area of noise is found by erosion and dilation these black pixels. + ## Inputs / Outputs This implementation inherits `pointcloud_preprocessor::Filter` class, please refer [README](../README.md). @@ -28,30 +38,41 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref ### Output -| Name | Type | Description | -| ---------------------------------------------------- | --------------------------------------- | -------------------------------------------------- | -| `~/output/blockage_diag/debug/blockage_mask_image` | `sensor_msgs::msg::Image` | The mask image of detected blockage | -| `~/output/blockage_diag/debug/ground_blockage_ratio` | `tier4_debug_msgs::msg::Float32Stamped` | The area ratio of blockage region in ground region | -| `~/output/blockage_diag/debug/sky_blockage_ratio` | `tier4_debug_msgs::msg::Float32Stamped` | The area ratio of blockage region in sky region | -| `~/output/blockage_diag/debug/lidar_depth_map` | `sensor_msgs::msg::Image` | The depth map image of input point cloud | +| Name | Type | Description | +| :-------------------------------------------------------- | :-------------------------------------- | ------------------------------------------------------------------------------------------------ | +| `~/output/blockage_diag/debug/blockage_mask_image` | `sensor_msgs::msg::Image` | The mask image of detected blockage | +| `~/output/blockage_diag/debug/ground_blockage_ratio` | `tier4_debug_msgs::msg::Float32Stamped` | The area ratio of blockage region in ground region | +| `~/output/blockage_diag/debug/sky_blockage_ratio` | `tier4_debug_msgs::msg::Float32Stamped` | The area ratio of blockage region in sky region | +| `~/output/blockage_diag/debug/lidar_depth_map` | `sensor_msgs::msg::Image` | The depth map image of input point cloud | +| `~/output/blockage_diag/debug/single_frame_dust_mask` | `sensor_msgs::msg::Image` | The mask image of detected dusty area in latest single frame | +| `~/output/blockage_diag/debug/multi_frame_dust_mask` | `sensor_msgs::msg::Image` | The mask image of continuous detected dusty area | +| `~/output/blockage_diag/debug/blockage_dust_merged_image` | `sensor_msgs::msg::Image` | The merged image of blockage detection(red) and multi frame dusty area detection(yellow) results | +| `~/output/blockage_diag/debug/ground_dust_ratio` | `tier4_debug_msgs::msg::Float32Stamped` | The ratio of dusty area divided by area where ray usually returns from the ground. | ## Parameters -| Name | Type | Description | -| -------------------------- | ------ | -------------------------------------------------- | -| `blockage_ratio_threshold` | float | The threshold of blockage area ratio | -| `blockage_count_threshold` | float | The threshold of number continuous blockage frames | -| `horizontal_ring_id` | int | The id of horizontal ring of the LiDAR | -| `angle_range` | vector | The effective range of LiDAR | -| `vertical_bins` | int | The LiDAR channel number | -| `model` | string | The LiDAR model | -| `buffering_frames` | uint | The number of buffering [range:1-200] | -| `buffering_interval` | uint | The interval of buffering | +| Name | Type | Description | +| ----------------------------- | ------ | ----------------------------------------------------------------------------------------------------------------------------- | +| `blockage_ratio_threshold` | float | The threshold of blockage area ratio.If the blockage value exceeds this threshold, the diagnostic state will be set to ERROR. | +| `blockage_count_threshold` | float | The threshold of number continuous blockage frames | +| `horizontal_ring_id` | int | The id of horizontal ring of the LiDAR | +| `angle_range` | vector | The effective range of LiDAR | +| `vertical_bins` | int | The LiDAR channel number | +| `model` | string | The LiDAR model | +| `blockage_buffering_frames` | int | The number of buffering about blockage detection [range:1-200] | +| `blockage_buffering_interval` | int | The interval of buffering about blockage detection | +| `dust_ratio_threshold` | float | The threshold of dusty area ratio | +| `dust_count_threshold` | int | The threshold of number continuous frames include dusty area | +| `dust_kernel_size` | int | The kernel size of morphology processing in dusty area detection | +| `dust_buffering_frames` | int | The number of buffering about dusty area detection [range:1-200] | +| `dust_buffering_interval` | int | The interval of buffering about dusty area detection | ## Assumptions / Known limits -1. Only Hesai Pandar40P and Hesai PandarQT were tested. For a new LiDAR, it is necessary to check order of channel id in vertical distribution manually and modify the code. -2. The current method is still limited for dust type of blockage when dust particles are sparsely distributed. +1. Only Hesai Pandar40P and Hesai PandarQT were tested. For a new LiDAR, it is necessary to check order of channel id in + vertical distribution manually and modify the code. +2. About dusty area detection, False positives occur when there are water puddles on the road surface due to rain, etc. + Also, the area of the ray to the sky is currently undetectable. ## (Optional) Error detection and handling diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp index 116dd2f832b93..66e14c418b52c 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp @@ -53,27 +53,42 @@ class BlockageDiagComponent : public pointcloud_preprocessor::Filter rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); image_transport::Publisher lidar_depth_map_pub_; image_transport::Publisher blockage_mask_pub_; + image_transport::Publisher single_frame_dust_mask_pub; + image_transport::Publisher multi_frame_dust_mask_pub; + image_transport::Publisher blockage_dust_merged_pub; rclcpp::Publisher::SharedPtr ground_blockage_ratio_pub_; rclcpp::Publisher::SharedPtr sky_blockage_ratio_pub_; + rclcpp::Publisher::SharedPtr ground_dust_ratio_pub_; + rclcpp::Publisher::SharedPtr blockage_type_pub_; private: void onBlockageChecker(DiagnosticStatusWrapper & stat); + void dustChecker(DiagnosticStatusWrapper & stat); Updater updater_{this}; - uint vertical_bins_; + int vertical_bins_; std::vector angle_range_deg_; - uint horizontal_ring_id_ = 12; + int horizontal_ring_id_; float blockage_ratio_threshold_; + float dust_ratio_threshold_; float ground_blockage_ratio_ = -1.0f; float sky_blockage_ratio_ = -1.0f; + float ground_dust_ratio_ = -1.0f; std::vector ground_blockage_range_deg_ = {0.0f, 0.0f}; std::vector sky_blockage_range_deg_ = {0.0f, 0.0f}; - uint erode_kernel_ = 10; - uint ground_blockage_count_ = 0; - uint sky_blockage_count_ = 0; - uint blockage_count_threshold_; + int blockage_kernel_ = 10; + int blockage_frame_count_ = 0; + int ground_blockage_count_ = 0; + int sky_blockage_count_ = 0; + int blockage_count_threshold_; std::string lidar_model_; - uint buffering_frames_ = 100; - uint buffering_interval_ = 5; + int blockage_buffering_frames_; + int blockage_buffering_interval_; + int dust_kernel_size_; + int dust_buffering_frames_; + int dust_buffering_interval_; + int dust_buffering_frame_counter_ = 0; + int dust_count_threshold_; + int dust_frame_count_ = 0; public: PCL_MAKE_ALIGNED_OPERATOR_NEW diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp index f30c02a97a6c0..8c3cb98ba266e 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp @@ -68,9 +68,10 @@ #include #include -#include #include #include +#include +#include #include #include #include @@ -127,11 +128,14 @@ class PointCloudDataSynchronizerComponent : public rclcpp::Node /** \brief A vector of subscriber. */ std::vector::SharedPtr> filters_; - rclcpp::Subscription::SharedPtr sub_twist_; + rclcpp::Subscription::SharedPtr sub_twist_; + rclcpp::Subscription::SharedPtr sub_odom_; rclcpp::TimerBase::SharedPtr timer_; diagnostic_updater::Updater updater_{this}; + const std::string input_twist_topic_type_; + /** \brief Output TF frame the concatenated points should be transformed to. */ std::string output_frame_; @@ -168,7 +172,8 @@ class PointCloudDataSynchronizerComponent : public rclcpp::Node void cloud_callback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, const std::string & topic_name); - void twist_callback(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr input); + void twist_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr input); + void odom_callback(const nav_msgs::msg::Odometry::ConstSharedPtr input); void timer_callback(); void checkSyncStatus(diagnostic_updater::DiagnosticStatusWrapper & stat); diff --git a/sensing/pointcloud_preprocessor/launch/blockage_diag.launch.xml b/sensing/pointcloud_preprocessor/launch/blockage_diag.launch.xml index 47dc1e73eef12..8fa5796e2802d 100644 --- a/sensing/pointcloud_preprocessor/launch/blockage_diag.launch.xml +++ b/sensing/pointcloud_preprocessor/launch/blockage_diag.launch.xml @@ -2,8 +2,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py b/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py index 0d65b45b1b3de..d82c23152aee4 100644 --- a/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py +++ b/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py @@ -58,7 +58,10 @@ def launch_setup(context, *args, **kwargs): package=pkg, plugin="pointcloud_preprocessor::PointCloudDataSynchronizerComponent", name="synchronizer_filter", - remappings=[("output", "points_raw/concatenated")], + remappings=[ + ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), + ("output", "points_raw/concatenated"), + ], parameters=[ { "input_topics": LaunchConfiguration("input_points_raw_list"), diff --git a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp index 3563bf2b6c408..06da91e61b3bd 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -19,6 +19,7 @@ #include #include +#include namespace pointcloud_preprocessor { @@ -30,34 +31,45 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options { { // initialize params: - horizontal_ring_id_ = static_cast(declare_parameter("horizontal_ring_id", 12)); - blockage_ratio_threshold_ = - static_cast(declare_parameter("blockage_ratio_threshold", 0.1)); - vertical_bins_ = static_cast(declare_parameter("vertical_bins", 40)); - angle_range_deg_ = declare_parameter("angle_range", std::vector{0.0, 360.0}); - lidar_model_ = static_cast(declare_parameter("model", "Pandar40P")); - blockage_count_threshold_ = - static_cast(declare_parameter("blockage_count_threshold", 50)); - buffering_frames_ = static_cast(declare_parameter("buffering_frames", 100)); - buffering_interval_ = static_cast(declare_parameter("buffering_interval", 5)); + horizontal_ring_id_ = declare_parameter("horizontal_ring_id"); + blockage_ratio_threshold_ = declare_parameter("blockage_ratio_threshold"); + vertical_bins_ = declare_parameter("vertical_bins"); + angle_range_deg_ = declare_parameter>("angle_range"); + lidar_model_ = declare_parameter("model"); + blockage_count_threshold_ = declare_parameter("blockage_count_threshold"); + blockage_buffering_frames_ = declare_parameter("blockage_buffering_frames"); + blockage_buffering_interval_ = declare_parameter("blockage_buffering_interval"); + dust_ratio_threshold_ = declare_parameter("dust_ratio_threshold"); + dust_count_threshold_ = declare_parameter("dust_count_threshold"); + dust_kernel_size_ = declare_parameter("dust_kernel_size"); + dust_buffering_frames_ = declare_parameter("dust_buffering_frames"); + dust_buffering_interval_ = declare_parameter("dust_buffering_interval"); } updater_.setHardwareID("blockage_diag"); updater_.add( std::string(this->get_namespace()) + ": blockage_validation", this, &BlockageDiagComponent::onBlockageChecker); + updater_.add( + std::string(this->get_namespace()) + ": dust_validation", this, + &BlockageDiagComponent::dustChecker); updater_.setPeriod(0.1); - + single_frame_dust_mask_pub = + image_transport::create_publisher(this, "blockage_diag/debug/single_frame_dust_mask_image"); + multi_frame_dust_mask_pub = + image_transport::create_publisher(this, "blockage_diag/debug/multi_frame_dust_mask_image"); lidar_depth_map_pub_ = image_transport::create_publisher(this, "blockage_diag/debug/lidar_depth_map"); blockage_mask_pub_ = image_transport::create_publisher(this, "blockage_diag/debug/blockage_mask_image"); - + blockage_dust_merged_pub = + image_transport::create_publisher(this, "blockage_diag/debug/blockage_dust_merged_image"); ground_blockage_ratio_pub_ = create_publisher( "blockage_diag/debug/ground_blockage_ratio", rclcpp::SensorDataQoS()); sky_blockage_ratio_pub_ = create_publisher( "blockage_diag/debug/sky_blockage_ratio", rclcpp::SensorDataQoS()); - + ground_dust_ratio_pub_ = create_publisher( + "blockage_diag/debug/ground_dust_ratio", rclcpp::SensorDataQoS()); using std::placeholders::_1; set_param_res_ = this->add_on_set_parameters_callback( std::bind(&BlockageDiagComponent::paramCallback, this, _1)); @@ -105,19 +117,59 @@ void BlockageDiagComponent::onBlockageChecker(DiagnosticStatusWrapper & stat) stat.summary(level, msg); } +void BlockageDiagComponent::dustChecker(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + stat.add("ground_dust_ratio", std::to_string(ground_dust_ratio_)); + auto level = DiagnosticStatus::OK; + std::string msg; + if (ground_dust_ratio_ < 0.0f) { + level = DiagnosticStatus::STALE; + msg = "STALE"; + } else if ( + (ground_dust_ratio_ > dust_ratio_threshold_) && (dust_frame_count_ > dust_count_threshold_)) { + level = DiagnosticStatus::ERROR; + msg = "ERROR"; + } else if (ground_dust_ratio_ > 0.0f) { + level = DiagnosticStatus::WARN; + msg = "WARN"; + } else { + level = DiagnosticStatus::OK; + msg = "OK"; + } + + if (ground_dust_ratio_ > 0.0f) { + msg = msg + ": LIDAR ground dust"; + } + stat.summary(level, msg); +} + void BlockageDiagComponent::filter( - const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) + const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, + PointCloud2 & output) { std::scoped_lock lock(mutex_); - if (indices) { - RCLCPP_WARN(get_logger(), "Indices are not supported and will be ignored"); + int vertical_bins = vertical_bins_; + int ideal_horizontal_bins; + float distance_coefficient = 327.67f; + float horizontal_resolution_ = 0.4f; + if (lidar_model_ == "Pandar40P") { + distance_coefficient = 327.67f; + horizontal_resolution_ = 0.4f; + } else if (lidar_model_ == "PandarQT") { + distance_coefficient = 3276.75f; + horizontal_resolution_ = 0.6f; } - uint horizontal_bins = static_cast((angle_range_deg_[1] - angle_range_deg_[0])); - uint vertical_bins = vertical_bins_; + ideal_horizontal_bins = + static_cast((angle_range_deg_[1] - angle_range_deg_[0]) / horizontal_resolution_); pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); pcl::fromROSMsg(*input, *pcl_input); - cv::Mat lidar_depth_map(cv::Size(horizontal_bins, vertical_bins), CV_16UC1, cv::Scalar(0)); - cv::Mat lidar_depth_map_8u(cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); + std::vector horizontal_bin_reference(ideal_horizontal_bins); + std::vector> each_ring_pointcloud(vertical_bins); + cv::Mat full_size_depth_map( + cv::Size(ideal_horizontal_bins, vertical_bins), CV_16UC1, cv::Scalar(0)); + cv::Mat lidar_depth_map(cv::Size(ideal_horizontal_bins, vertical_bins), CV_16UC1, cv::Scalar(0)); + cv::Mat lidar_depth_map_8u( + cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); if (pcl_input->points.empty()) { ground_blockage_ratio_ = 1.0f; sky_blockage_ratio_ = 1.0f; @@ -132,114 +184,186 @@ void BlockageDiagComponent::filter( sky_blockage_range_deg_[0] = angle_range_deg_[0]; sky_blockage_range_deg_[1] = angle_range_deg_[1]; } else { - for (const auto & p : pcl_input->points) { - if ((p.azimuth / 100.0 > angle_range_deg_[0]) && (p.azimuth / 100.0 < angle_range_deg_[1])) { - if (lidar_model_ == "Pandar40P") { - lidar_depth_map.at( - p.ring, static_cast((p.azimuth / 100.0 - angle_range_deg_[0]))) += - static_cast(6250.0 / p.distance); // make image clearly - lidar_depth_map_8u.at( - p.ring, static_cast((p.azimuth / 100.0 - angle_range_deg_[0]))) = 255; - } else if (lidar_model_ == "PandarQT") { - lidar_depth_map.at( - vertical_bins - p.ring - 1, - static_cast((p.azimuth / 100.0 - angle_range_deg_[0]))) += - static_cast(6250.0 / p.distance); - lidar_depth_map_8u.at( - vertical_bins - p.ring - 1, - static_cast((p.azimuth / 100.0 - angle_range_deg_[0]))) = 255; + for (int i = 0; i < ideal_horizontal_bins; ++i) { + horizontal_bin_reference.at(i) = angle_range_deg_[0] + i * horizontal_resolution_; + } + for (const auto p : pcl_input->points) { + for (int horizontal_bin = 0; + horizontal_bin < static_cast(horizontal_bin_reference.size()); horizontal_bin++) { + if ( + (p.azimuth / 100 > + (horizontal_bin_reference.at(horizontal_bin) - horizontal_resolution_ / 2)) && + (p.azimuth / 100 <= + (horizontal_bin_reference.at(horizontal_bin) + horizontal_resolution_ / 2))) { + if (lidar_model_ == "Pandar40P") { + full_size_depth_map.at(p.ring, horizontal_bin) = + UINT16_MAX - distance_coefficient * p.distance; + } else if (lidar_model_ == "PandarQT") { + full_size_depth_map.at(vertical_bins - p.ring - 1, horizontal_bin) = + UINT16_MAX - distance_coefficient * p.distance; + } } } } - lidar_depth_map.convertTo(lidar_depth_map, CV_8UC1, 1.0 / 100.0); - cv::Mat no_return_mask; - cv::inRange(lidar_depth_map_8u, 0, 1, no_return_mask); - - cv::Mat erosion_dst; - cv::Mat element = cv::getStructuringElement( - cv::MORPH_RECT, cv::Size(2 * erode_kernel_ + 1, 2 * erode_kernel_ + 1), - cv::Point(erode_kernel_, erode_kernel_)); - cv::erode(no_return_mask, erosion_dst, element); - cv::dilate(erosion_dst, no_return_mask, element); - - static boost::circular_buffer no_return_mask_buffer(buffering_frames_); - - cv::Mat no_return_mask_result(cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); - cv::Mat time_series_blockage_mask( - cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); - cv::Mat no_return_mask_binarized( - cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); + } + full_size_depth_map.convertTo(lidar_depth_map_8u, CV_8UC1, 1.0 / 300); + cv::Mat no_return_mask(cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); + cv::inRange(lidar_depth_map_8u, 0, 1, no_return_mask); + cv::Mat erosion_dst; + cv::Mat blockage_element = cv::getStructuringElement( + cv::MORPH_RECT, cv::Size(2 * blockage_kernel_ + 1, 2 * blockage_kernel_ + 1), + cv::Point(blockage_kernel_, blockage_kernel_)); + cv::erode(no_return_mask, erosion_dst, blockage_element); + cv::dilate(erosion_dst, no_return_mask, blockage_element); + static boost::circular_buffer no_return_mask_buffer(blockage_buffering_frames_); + cv::Mat time_series_blockage_result( + cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); + cv::Mat time_series_blockage_mask( + cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); + cv::Mat no_return_mask_binarized( + cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); - static uint frame_count; - frame_count++; - if (buffering_interval_ != 0) { - no_return_mask_binarized = no_return_mask / 255; - if (frame_count == buffering_interval_) { - no_return_mask_buffer.push_back(no_return_mask_binarized); - frame_count = 0; - } - for (const auto & binary_mask : no_return_mask_buffer) { - time_series_blockage_mask += binary_mask; - } - cv::inRange( - time_series_blockage_mask, no_return_mask_buffer.size() - 1, no_return_mask_buffer.size(), - no_return_mask_result); + if (blockage_buffering_interval_ == 0) { + no_return_mask.copyTo(time_series_blockage_result); + } else { + no_return_mask_binarized = no_return_mask / 255; + if (blockage_frame_count_ >= blockage_buffering_interval_) { + no_return_mask_buffer.push_back(no_return_mask_binarized); + blockage_frame_count_ = 0; } else { - no_return_mask.copyTo(no_return_mask_result); + blockage_frame_count_++; } - cv::Mat ground_no_return_mask; - cv::Mat sky_no_return_mask; - no_return_mask_result(cv::Rect(0, 0, horizontal_bins, horizontal_ring_id_)) - .copyTo(sky_no_return_mask); - no_return_mask_result( - cv::Rect(0, horizontal_ring_id_, horizontal_bins, vertical_bins - horizontal_ring_id_)) - .copyTo(ground_no_return_mask); - ground_blockage_ratio_ = - static_cast(cv::countNonZero(ground_no_return_mask)) / - static_cast(horizontal_bins * (vertical_bins - horizontal_ring_id_)); - sky_blockage_ratio_ = static_cast(cv::countNonZero(sky_no_return_mask)) / - static_cast(horizontal_bins * horizontal_ring_id_); - - if (ground_blockage_ratio_ > blockage_ratio_threshold_) { - cv::Rect ground_blockage_bb = cv::boundingRect(ground_no_return_mask); - ground_blockage_range_deg_[0] = - static_cast(ground_blockage_bb.x) + angle_range_deg_[0]; - ground_blockage_range_deg_[1] = - static_cast(ground_blockage_bb.x + ground_blockage_bb.width) + angle_range_deg_[0]; + for (const auto & binary_mask : no_return_mask_buffer) { + time_series_blockage_mask += binary_mask; + } + cv::inRange( + time_series_blockage_mask, no_return_mask_buffer.size() - 1, no_return_mask_buffer.size(), + time_series_blockage_result); + } + cv::Mat ground_no_return_mask; + cv::Mat sky_no_return_mask; + no_return_mask(cv::Rect(0, 0, ideal_horizontal_bins, horizontal_ring_id_)) + .copyTo(sky_no_return_mask); + no_return_mask( + cv::Rect(0, horizontal_ring_id_, ideal_horizontal_bins, vertical_bins - horizontal_ring_id_)) + .copyTo(ground_no_return_mask); + ground_blockage_ratio_ = + static_cast(cv::countNonZero(ground_no_return_mask)) / + static_cast(ideal_horizontal_bins * (vertical_bins - horizontal_ring_id_)); + sky_blockage_ratio_ = static_cast(cv::countNonZero(sky_no_return_mask)) / + static_cast(ideal_horizontal_bins * horizontal_ring_id_); - if (ground_blockage_count_ <= 2 * blockage_count_threshold_) { - ground_blockage_count_ += 1; - } - } else { - ground_blockage_count_ = 0; + if (ground_blockage_ratio_ > blockage_ratio_threshold_) { + cv::Rect ground_blockage_bb = cv::boundingRect(ground_no_return_mask); + ground_blockage_range_deg_[0] = static_cast(ground_blockage_bb.x) + angle_range_deg_[0]; + ground_blockage_range_deg_[1] = + static_cast(ground_blockage_bb.x + ground_blockage_bb.width) + angle_range_deg_[0]; + if (ground_blockage_count_ <= 2 * blockage_count_threshold_) { + ground_blockage_count_ += 1; } + } else { + ground_blockage_count_ = 0; + } + if (sky_blockage_ratio_ > blockage_ratio_threshold_) { + cv::Rect sky_blockage_bx = cv::boundingRect(sky_no_return_mask); + sky_blockage_range_deg_[0] = static_cast(sky_blockage_bx.x) + angle_range_deg_[0]; + sky_blockage_range_deg_[1] = + static_cast(sky_blockage_bx.x + sky_blockage_bx.width) + angle_range_deg_[0]; + if (sky_blockage_count_ <= 2 * blockage_count_threshold_) { + sky_blockage_count_ += 1; + } + } else { + sky_blockage_count_ = 0; + } + // dust + cv::Mat ground_depth_map = lidar_depth_map_8u( + cv::Rect(0, horizontal_ring_id_, ideal_horizontal_bins, vertical_bins - horizontal_ring_id_)); + cv::Mat sky_blank(horizontal_ring_id_, ideal_horizontal_bins, CV_8UC1, cv::Scalar(0)); + cv::Mat ground_blank( + vertical_bins - horizontal_ring_id_, ideal_horizontal_bins, CV_8UC1, cv::Scalar(0)); + cv::Mat single_dust_img( + cv::Size(lidar_depth_map_8u.rows, lidar_depth_map_8u.cols), CV_8UC1, cv::Scalar(0)); + cv::Mat single_dust_ground_img = ground_depth_map.clone(); + cv::inRange(single_dust_ground_img, 0, 1, single_dust_ground_img); + cv::Mat dust_element = getStructuringElement( + cv::MORPH_RECT, cv::Size(2 * dust_kernel_size_ + 1, 2 * dust_kernel_size_ + 1), + cv::Point(-1, -1)); + cv::dilate(single_dust_ground_img, single_dust_ground_img, dust_element); + cv::erode(single_dust_ground_img, single_dust_ground_img, dust_element); + cv::inRange(single_dust_ground_img, 254, 255, single_dust_ground_img); + cv::Mat ground_mask(cv::Size(ideal_horizontal_bins, horizontal_ring_id_), CV_8UC1); + cv::vconcat(sky_blank, single_dust_ground_img, single_dust_img); + static boost::circular_buffer dust_mask_buffer(dust_buffering_frames_); + cv::Mat binarized_dust_mask_( + cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); + cv::Mat multi_frame_dust_mask( + cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); + cv::Mat multi_frame_ground_dust_result( + cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); - if (sky_blockage_ratio_ > blockage_ratio_threshold_) { - cv::Rect sky_blockage_bx = cv::boundingRect(sky_no_return_mask); - sky_blockage_range_deg_[0] = static_cast(sky_blockage_bx.x) + angle_range_deg_[0]; - sky_blockage_range_deg_[1] = - static_cast(sky_blockage_bx.x + sky_blockage_bx.width) + angle_range_deg_[0]; - if (sky_blockage_count_ <= 2 * blockage_count_threshold_) { - sky_blockage_count_ += 1; - } + if (dust_buffering_interval_ == 0) { + single_dust_img.copyTo(multi_frame_ground_dust_result); + dust_buffering_frame_counter_ = 0; + } else { + binarized_dust_mask_ = single_dust_img / 255; + if (dust_buffering_frame_counter_ >= dust_buffering_interval_) { + dust_mask_buffer.push_back(binarized_dust_mask_); + dust_buffering_frame_counter_ = 0; } else { - sky_blockage_count_ = 0; + dust_buffering_frame_counter_++; } - - cv::Mat lidar_depth_colorized; - cv::applyColorMap(lidar_depth_map, lidar_depth_colorized, cv::COLORMAP_JET); - sensor_msgs::msg::Image::SharedPtr lidar_depth_msg = - cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", lidar_depth_colorized).toImageMsg(); - lidar_depth_msg->header = input->header; - lidar_depth_map_pub_.publish(lidar_depth_msg); - - cv::Mat blockage_mask_colorized; - cv::applyColorMap(no_return_mask_result, blockage_mask_colorized, cv::COLORMAP_JET); - sensor_msgs::msg::Image::SharedPtr blockage_mask_msg = - cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", blockage_mask_colorized).toImageMsg(); - blockage_mask_msg->header = input->header; - blockage_mask_pub_.publish(blockage_mask_msg); + for (const auto & binarized_dust_mask : dust_mask_buffer) { + multi_frame_dust_mask += binarized_dust_mask; + } + cv::inRange( + multi_frame_dust_mask, dust_mask_buffer.size() - 1, dust_mask_buffer.size(), + multi_frame_ground_dust_result); + } + cv::Mat single_frame_ground_dust_colorized( + cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC3, cv::Scalar(0, 0, 0)); + cv::applyColorMap(single_dust_img, single_frame_ground_dust_colorized, cv::COLORMAP_JET); + cv::Mat multi_frame_ground_dust_colorized; + cv::Mat blockage_dust_merged_img( + cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC3, cv::Scalar(0, 0, 0)); + cv::Mat blockage_dust_merged_mask( + cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); + blockage_dust_merged_img.setTo( + cv::Vec3b(0, 0, 255), time_series_blockage_result); // red:blockage + blockage_dust_merged_img.setTo( + cv::Vec3b(0, 255, 255), multi_frame_ground_dust_result); // yellow:dust + sensor_msgs::msg::Image::SharedPtr single_frame_dust_mask_msg = + cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", single_frame_ground_dust_colorized) + .toImageMsg(); + single_frame_dust_mask_pub.publish(single_frame_dust_mask_msg); + sensor_msgs::msg::Image::SharedPtr multi_frame_dust_mask_msg = + cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", multi_frame_ground_dust_colorized) + .toImageMsg(); + multi_frame_dust_mask_pub.publish(multi_frame_dust_mask_msg); + sensor_msgs::msg::Image::SharedPtr lidar_depth_map_msg = + cv_bridge::CvImage(std_msgs::msg::Header(), "mono16", full_size_depth_map).toImageMsg(); + lidar_depth_map_msg->header = input->header; + lidar_depth_map_pub_.publish(lidar_depth_map_msg); + cv::Mat blockage_mask_colorized; + cv::applyColorMap(time_series_blockage_result, blockage_mask_colorized, cv::COLORMAP_JET); + sensor_msgs::msg::Image::SharedPtr blockage_mask_msg = + cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", blockage_mask_colorized).toImageMsg(); + blockage_mask_msg->header = input->header; + blockage_mask_pub_.publish(blockage_mask_msg); + cv::Mat blockage_dust_merged_colorized( + cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC3, cv::Scalar(0, 0, 0)); + blockage_dust_merged_img.copyTo(blockage_dust_merged_colorized); + sensor_msgs::msg::Image::SharedPtr blockage_dust_merged_msg = + cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", blockage_dust_merged_colorized) + .toImageMsg(); + if (ground_dust_ratio_ > dust_ratio_threshold_) { + if (dust_frame_count_ < 2 * dust_count_threshold_) { + dust_frame_count_++; + } + } else { + dust_frame_count_ = 0; } + blockage_dust_merged_msg->header = input->header; + blockage_dust_merged_pub.publish(blockage_dust_merged_msg); tier4_debug_msgs::msg::Float32Stamped ground_blockage_ratio_msg; ground_blockage_ratio_msg.data = ground_blockage_ratio_; @@ -250,7 +374,14 @@ void BlockageDiagComponent::filter( sky_blockage_ratio_msg.data = sky_blockage_ratio_; sky_blockage_ratio_msg.stamp = now(); sky_blockage_ratio_pub_->publish(sky_blockage_ratio_msg); + tier4_debug_msgs::msg::Float32Stamped blockage_ratio_msg; + tier4_debug_msgs::msg::Float32Stamped ground_dust_ratio_msg; + ground_dust_ratio_ = static_cast(cv::countNonZero(single_dust_ground_img)) / + (single_dust_ground_img.cols * single_dust_ground_img.rows); + ground_dust_ratio_msg.data = ground_dust_ratio_; + ground_dust_ratio_msg.stamp = now(); + ground_dust_ratio_pub_->publish(ground_dust_ratio_msg); pcl::toROSMsg(*pcl_input, output); output.header = input->header; } @@ -280,11 +411,29 @@ rcl_interfaces::msg::SetParametersResult BlockageDiagComponent::paramCallback( get_logger(), " Setting new angle_range to: [%f , %f].", angle_range_deg_[0], angle_range_deg_[1]); } - if (get_param(p, "buffering_frames", buffering_frames_)) { - RCLCPP_DEBUG(get_logger(), "Setting new buffering_frames to: %d.", buffering_frames_); + if (get_param(p, "blockage_buffering_frames", blockage_buffering_frames_)) { + RCLCPP_DEBUG( + get_logger(), "Setting new blockage_buffering_frames_ to: %d.", blockage_buffering_frames_); + } + if (get_param(p, "blockage_buffering_interval", blockage_buffering_interval_)) { + RCLCPP_DEBUG( + get_logger(), "Setting new blockage_buffering_interval_ to: %d.", + blockage_buffering_interval_); + } + if (get_param(p, "dust_kernel_size", dust_kernel_size_)) { + RCLCPP_DEBUG(get_logger(), "Setting new dust_kernel_size_ to: %d.", dust_kernel_size_); } - if (get_param(p, "buffering_interval", buffering_interval_)) { - RCLCPP_DEBUG(get_logger(), "Setting new buffering_interval to: %d.", buffering_interval_); + if (get_param(p, "dust_buffering_frames", dust_buffering_frames_)) { + RCLCPP_DEBUG( + get_logger(), "Setting new dust_buffering_frames_ to: %d.", dust_buffering_frames_); + // note:NOT affects to actual variable. + // if you want change this param/variable, change the parameter called at launch this + // node(aip_launcher). + } + if (get_param(p, "dust_buffering_interval", dust_buffering_interval_)) { + RCLCPP_DEBUG( + get_logger(), "Setting new dust_buffering_interval_ to: %d.", dust_buffering_interval_); + dust_buffering_frame_counter_ = 0; } rcl_interfaces::msg::SetParametersResult result; result.successful = true; diff --git a/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp b/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp index a6bf64b835f61..5d261f42391e9 100644 --- a/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp @@ -41,7 +41,8 @@ namespace pointcloud_preprocessor { PointCloudDataSynchronizerComponent::PointCloudDataSynchronizerComponent( const rclcpp::NodeOptions & node_options) -: Node("point_cloud_time_synchronizer_component", node_options) +: Node("point_cloud_time_synchronizer_component", node_options), + input_twist_topic_type_(declare_parameter("input_twist_topic_type", "twist")) { // initialize debug tool { @@ -129,10 +130,21 @@ PointCloudDataSynchronizerComponent::PointCloudDataSynchronizerComponent( } // Subscribe to the twist - auto twist_cb = - std::bind(&PointCloudDataSynchronizerComponent::twist_callback, this, std::placeholders::_1); - sub_twist_ = this->create_subscription( - "/vehicle/status/velocity_status", rclcpp::QoS{100}, twist_cb); + if (input_twist_topic_type_ == "twist") { + auto twist_cb = std::bind( + &PointCloudDataSynchronizerComponent::twist_callback, this, std::placeholders::_1); + sub_twist_ = this->create_subscription( + "~/input/twist", rclcpp::QoS{100}, twist_cb); + } else if (input_twist_topic_type_ == "odom") { + auto odom_cb = + std::bind(&PointCloudDataSynchronizerComponent::odom_callback, this, std::placeholders::_1); + sub_odom_ = this->create_subscription( + "~/input/odom", rclcpp::QoS{100}, odom_cb); + } else { + RCLCPP_ERROR_STREAM( + get_logger(), "input_twist_topic_type is invalid: " << input_twist_topic_type_); + throw std::runtime_error("input_twist_topic_type is invalid: " + input_twist_topic_type_); + } } // Transformed Raw PointCloud2 Publisher @@ -474,7 +486,33 @@ void PointCloudDataSynchronizerComponent::timer_callback() } void PointCloudDataSynchronizerComponent::twist_callback( - const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr input) + const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr input) +{ + // if rosbag restart, clear buffer + if (!twist_ptr_queue_.empty()) { + if (rclcpp::Time(twist_ptr_queue_.front()->header.stamp) > rclcpp::Time(input->header.stamp)) { + twist_ptr_queue_.clear(); + } + } + + // pop old data + while (!twist_ptr_queue_.empty()) { + if ( + rclcpp::Time(twist_ptr_queue_.front()->header.stamp) + rclcpp::Duration::from_seconds(1.0) > + rclcpp::Time(input->header.stamp)) { + break; + } + twist_ptr_queue_.pop_front(); + } + + auto twist_ptr = std::make_shared(); + twist_ptr->header.stamp = input->header.stamp; + twist_ptr->twist = input->twist.twist; + twist_ptr_queue_.push_back(twist_ptr); +} + +void PointCloudDataSynchronizerComponent::odom_callback( + const nav_msgs::msg::Odometry::ConstSharedPtr input) { // if rosbag restart, clear buffer if (!twist_ptr_queue_.empty()) { @@ -495,9 +533,7 @@ void PointCloudDataSynchronizerComponent::twist_callback( auto twist_ptr = std::make_shared(); twist_ptr->header.stamp = input->header.stamp; - twist_ptr->twist.linear.x = input->longitudinal_velocity; - twist_ptr->twist.linear.y = input->lateral_velocity; - twist_ptr->twist.angular.z = input->heading_rate; + twist_ptr->twist = input->twist.twist; twist_ptr_queue_.push_back(twist_ptr); } diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md index d70aca794dc07..ceecacfe8cd8c 100644 --- a/simulator/simple_planning_simulator/README.md +++ b/simulator/simple_planning_simulator/README.md @@ -89,6 +89,12 @@ _Note_: The steering/velocity/acceleration dynamics is modeled by a first order Since the vehicle outputs `odom`->`base_link` tf, this simulator outputs the tf with the same frame_id configuration. In the simple_planning_simulator.launch.py, the node that outputs the `map`->`odom` tf, that usually estimated by the localization module (e.g. NDT), will be launched as well. Since the tf output by this simulator module is an ideal value, `odom`->`map` will always be 0. +### (Caveat) Pitch calculation + +Ego vehicle pitch angle is calculated in the following manner. + +![pitch calculation](./media/pitch-calculation.drawio.svg) + ## Error detection and handling The only validation on inputs being done is testing for a valid vehicle model type. diff --git a/simulator/simple_planning_simulator/media/pitch-calculation.drawio.svg b/simulator/simple_planning_simulator/media/pitch-calculation.drawio.svg new file mode 100644 index 0000000000000..d022b29d2a730 --- /dev/null +++ b/simulator/simple_planning_simulator/media/pitch-calculation.drawio.svg @@ -0,0 +1,329 @@ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + forward
$diff_xy > 0
 + +
+
+
+ + forward... + + + + + +
+
+
+ + + ego +
+ direction +
+ + +
+
+
+ + ego... + + + + + +
+
+
+ + reverse
$diff_xy < 0
 + +
+
+
+ + reverse... + + + + + +
+
+
+ + + lane +
+ direction +
+ + +
+
+
+ + lane... + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + $diff_z > 0
 + +
+
+
+ + $diff_z > 0 + + + + + +
+
+
+ + $diff_z < 0
 + +
+
+
+ + $diff_z < 0 + + + + + +
+
+
+ + pitch < 0
 + +
+
+
+ + pitch < 0 + + + + + +
+
+
+ + pitch > 0
 + +
+
+
+ + pitch > 0 + + + + + +
+
+
+ + pitch < 0
 + +
+
+
+ + pitch < 0 + + + + + +
+
+
+ + pitch > 0
 + +
+
+
+ + pitch > 0 + + + + + + + + Text is not SVG - cannot display + + + 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 8c2688928b4c9..208fd80d57a08 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 @@ -313,7 +313,7 @@ double SimplePlanningSimulator::calculate_ego_pitch() const 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); + reverse_sign ? -std::atan2(-diff_z, -diff_xy) : -std::atan2(diff_z, diff_xy); return ego_pitch_angle; } diff --git a/system/autoware_auto_msgs_adapter/CMakeLists.txt b/system/autoware_auto_msgs_adapter/CMakeLists.txt index ff8dcf6cedbb3..8b1d31ff2d01b 100644 --- a/system/autoware_auto_msgs_adapter/CMakeLists.txt +++ b/system/autoware_auto_msgs_adapter/CMakeLists.txt @@ -12,6 +12,7 @@ ament_auto_add_library(${NODE_NAME} src/include/adapter_base.hpp src/include/adapter_base_interface.hpp src/include/adapter_control.hpp + src/include/adapter_map.hpp src/include/adapter_perception.hpp src/include/autoware_auto_msgs_adapter_core.hpp src/autoware_auto_msgs_adapter_core.cpp) diff --git a/system/autoware_auto_msgs_adapter/config/adapter_map.param.yaml b/system/autoware_auto_msgs_adapter/config/adapter_map.param.yaml new file mode 100644 index 0000000000000..dcaa49e089b44 --- /dev/null +++ b/system/autoware_auto_msgs_adapter/config/adapter_map.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + msg_type_target: "autoware_auto_mapping_msgs/msg/HADMapBin" + topic_name_source: "/map/vector_map" + topic_name_target: "/map/vector_map_auto" diff --git a/system/autoware_auto_msgs_adapter/launch/autoware_auto_msgs_adapter.launch.xml b/system/autoware_auto_msgs_adapter/launch/autoware_auto_msgs_adapter.launch.xml index f914fe6e824c4..d80c1bd8cbedf 100755 --- a/system/autoware_auto_msgs_adapter/launch/autoware_auto_msgs_adapter.launch.xml +++ b/system/autoware_auto_msgs_adapter/launch/autoware_auto_msgs_adapter.launch.xml @@ -1,11 +1,15 @@ + + + + diff --git a/system/autoware_auto_msgs_adapter/package.xml b/system/autoware_auto_msgs_adapter/package.xml index de19032e1aee9..99a94fa043565 100644 --- a/system/autoware_auto_msgs_adapter/package.xml +++ b/system/autoware_auto_msgs_adapter/package.xml @@ -10,8 +10,6 @@ Apache License 2.0 - M. Fatih Cırıt - ament_cmake_auto autoware_cmake @@ -22,8 +20,10 @@ autoware_lint_common autoware_auto_control_msgs + autoware_auto_mapping_msgs autoware_auto_perception_msgs autoware_control_msgs + autoware_map_msgs autoware_perception_msgs rclcpp rclcpp_components diff --git a/system/autoware_auto_msgs_adapter/schema/autoware_auto_msgs_adapter.schema.json b/system/autoware_auto_msgs_adapter/schema/autoware_auto_msgs_adapter.schema.json index 35e3aef1511cc..f6aa87f774528 100644 --- a/system/autoware_auto_msgs_adapter/schema/autoware_auto_msgs_adapter.schema.json +++ b/system/autoware_auto_msgs_adapter/schema/autoware_auto_msgs_adapter.schema.json @@ -11,6 +11,7 @@ "description": "Target message type", "enum": [ "autoware_auto_control_msgs/msg/AckermannControlCommand", + "autoware_auto_mapping_msgs/msg/HADMapBin", "autoware_auto_perception_msgs/msg/PredictedObjects" ], "default": "autoware_auto_control_msgs/msg/AckermannControlCommand" diff --git a/system/autoware_auto_msgs_adapter/src/autoware_auto_msgs_adapter_core.cpp b/system/autoware_auto_msgs_adapter/src/autoware_auto_msgs_adapter_core.cpp index 0f65571aba07b..15e3c90d227dd 100644 --- a/system/autoware_auto_msgs_adapter/src/autoware_auto_msgs_adapter_core.cpp +++ b/system/autoware_auto_msgs_adapter/src/autoware_auto_msgs_adapter_core.cpp @@ -57,6 +57,11 @@ MapStringAdapter AutowareAutoMsgsAdapterNode::create_adapter_map( return std::static_pointer_cast( std::make_shared(*this, topic_name_source, topic_name_target)); }}, + {"autoware_auto_mapping_msgs/msg/HADMapBin", + [&] { + return std::static_pointer_cast( + std::make_shared(*this, topic_name_source, topic_name_target)); + }}, {"autoware_auto_perception_msgs/msg/PredictedObjects", [&] { return std::static_pointer_cast( diff --git a/system/autoware_auto_msgs_adapter/src/include/adapter_map.hpp b/system/autoware_auto_msgs_adapter/src/include/adapter_map.hpp new file mode 100644 index 0000000000000..8150b7d7dba08 --- /dev/null +++ b/system/autoware_auto_msgs_adapter/src/include/adapter_map.hpp @@ -0,0 +1,59 @@ +// Copyright 2023 The Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef ADAPTER_MAP_HPP_ +#define ADAPTER_MAP_HPP_ + +#include "adapter_base.hpp" + +#include + +#include +#include + +#include + +namespace autoware_auto_msgs_adapter +{ +using autoware_auto_mapping_msgs::msg::HADMapBin; +using autoware_map_msgs::msg::LaneletMapBin; + +class AdapterMap : public autoware_auto_msgs_adapter::AdapterBase +{ +public: + AdapterMap( + rclcpp::Node & node, const std::string & topic_name_source, + const std::string & topic_name_target, const rclcpp::QoS & qos = rclcpp::QoS{1}) + : AdapterBase(node, topic_name_source, topic_name_target, qos) + { + RCLCPP_DEBUG( + node.get_logger(), "AdapterMap is initialized to convert: %s -> %s", + topic_name_source.c_str(), topic_name_target.c_str()); + } + +protected: + HADMapBin convert(const LaneletMapBin & msg_source) override + { + autoware_auto_mapping_msgs::msg::HADMapBin msg_auto; + msg_auto.header = msg_source.header; + msg_auto.format_version = msg_source.version_map_format; + msg_auto.map_version = msg_source.version_map; + msg_auto.map_format = 0; + msg_auto.data = msg_source.data; + + return msg_auto; + } +}; +} // namespace autoware_auto_msgs_adapter + +#endif // ADAPTER_MAP_HPP_ diff --git a/system/autoware_auto_msgs_adapter/src/include/autoware_auto_msgs_adapter_core.hpp b/system/autoware_auto_msgs_adapter/src/include/autoware_auto_msgs_adapter_core.hpp index f595359dcc1e7..f0e28f48aacd8 100644 --- a/system/autoware_auto_msgs_adapter/src/include/autoware_auto_msgs_adapter_core.hpp +++ b/system/autoware_auto_msgs_adapter/src/include/autoware_auto_msgs_adapter_core.hpp @@ -15,6 +15,7 @@ #define AUTOWARE_AUTO_MSGS_ADAPTER_CORE_HPP_ #include "adapter_control.hpp" +#include "adapter_map.hpp" #include "adapter_perception.hpp" #include diff --git a/system/autoware_auto_msgs_adapter/test/test_msg_had_map_bin.cpp b/system/autoware_auto_msgs_adapter/test/test_msg_had_map_bin.cpp new file mode 100644 index 0000000000000..b312bd144f6f3 --- /dev/null +++ b/system/autoware_auto_msgs_adapter/test/test_msg_had_map_bin.cpp @@ -0,0 +1,104 @@ +// Copyright 2023 The 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 + +#include + +#include + +autoware_map_msgs::msg::LaneletMapBin generate_map_msg() +{ + // generate deterministic random int + std::mt19937 gen(0); + std::uniform_int_distribution<> dis_int(0, 1000000); + auto rand_int = [&dis_int, &gen]() { return dis_int(gen); }; + + autoware_map_msgs::msg::LaneletMapBin msg_map; + msg_map.header.stamp = rclcpp::Time(rand_int()); + msg_map.header.frame_id = "test_frame"; + + msg_map.version_map_format = "1.1.1"; + msg_map.version_map = "1.0.0"; + msg_map.name_map = "florence-prato-city-center"; + msg_map.data.push_back(rand_int()); + + return msg_map; +} + +TEST(AutowareAutoMsgsAdapter, TestHADMapBin) // NOLINT for gtest +{ + const std::string msg_type_target = "autoware_auto_mapping_msgs/msg/HADMapBin"; + const std::string topic_name_source = "topic_name_source"; + const std::string topic_name_target = "topic_name_target"; + + std::cout << "Creating the adapter node..." << std::endl; + + rclcpp::NodeOptions node_options; + node_options.append_parameter_override("msg_type_target", msg_type_target); + node_options.append_parameter_override("topic_name_source", topic_name_source); + node_options.append_parameter_override("topic_name_target", topic_name_target); + + using autoware_auto_msgs_adapter::AutowareAutoMsgsAdapterNode; + AutowareAutoMsgsAdapterNode::SharedPtr node_adapter; + node_adapter = std::make_shared(node_options); + + std::cout << "Creating the subscriber node..." << std::endl; + + auto node_subscriber = std::make_shared("node_subscriber", rclcpp::NodeOptions{}); + + bool test_completed = false; + + const auto msg_map = generate_map_msg(); + auto sub = node_subscriber->create_subscription( + topic_name_target, rclcpp::QoS{1}, + [&msg_map, &test_completed](const autoware_auto_mapping_msgs::msg::HADMapBin::SharedPtr msg) { + EXPECT_EQ(msg->header.stamp, msg_map.header.stamp); + EXPECT_EQ(msg->header.frame_id, msg_map.header.frame_id); + + EXPECT_EQ(msg->map_format, 0); + EXPECT_EQ(msg->format_version, msg_map.version_map_format); + EXPECT_EQ(msg->map_version, msg_map.version_map); + EXPECT_EQ(msg->data, msg_map.data); + + test_completed = true; + }); + + std::cout << "Creating the publisher node..." << std::endl; + + auto node_publisher = std::make_shared("node_publisher", rclcpp::NodeOptions{}); + auto pub = node_publisher->create_publisher( + topic_name_source, rclcpp::QoS{1}); + pub->publish(msg_map); + + auto start_time = std::chrono::system_clock::now(); + auto max_test_dur = std::chrono::seconds(5); + auto timed_out = false; + + while (rclcpp::ok() && !test_completed) { + rclcpp::spin_some(node_subscriber); + rclcpp::spin_some(node_adapter); + rclcpp::spin_some(node_publisher); + rclcpp::sleep_for(std::chrono::milliseconds(50)); + if (std::chrono::system_clock::now() - start_time > max_test_dur) { + timed_out = true; + break; + } + } + + EXPECT_TRUE(test_completed); + EXPECT_FALSE(timed_out); + + // rclcpp::shutdown(); +} diff --git a/system/default_ad_api_helpers/ad_api_adaptors/README.md b/system/default_ad_api_helpers/ad_api_adaptors/README.md index 121befb68c88a..2e299fd7a2e51 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/README.md +++ b/system/default_ad_api_helpers/ad_api_adaptors/README.md @@ -21,9 +21,11 @@ The clear API is called automatically before setting the route. | Interface | Local Name | Global Name | Description | | ------------ | ------------------ | ------------------------------------- | -------------------------------------------------- | +| Subscription | - | /api/routing/state | The state of the routing API. | | Subscription | ~/input/fixed_goal | /planning/mission_planning/goal | The goal pose of route. Disable goal modification. | | Subscription | ~/input/rough_goal | /rviz/routing/rough_goal | The goal pose of route. Enable goal modification. | | Subscription | ~/input/reroute | /rviz/routing/reroute | The goal pose of reroute. | | Subscription | ~/input/waypoint | /planning/mission_planning/checkpoint | The waypoint pose of route. | | Client | - | /api/routing/clear_route | The route clear API. | | Client | - | /api/routing/set_route_points | The route points set API. | +| Client | - | /api/routing/change_route_points | The route points change API. | diff --git a/system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml index 2d0b2b00af343..f82f7d732f5dc 100644 --- a/system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml +++ b/system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml @@ -27,5 +27,5 @@ localization_accuracy: type: diagnostic_aggregator/GenericAnalyzer path: localization_accuracy - contains: [": localization_accuracy"] + contains: ["localization: localization_error_monitor"] timeout: 1.0 diff --git a/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp b/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp index 75d80b56ea711..0bb95882700a5 100644 --- a/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp +++ b/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp @@ -15,6 +15,8 @@ #ifndef SYSTEM_ERROR_MONITOR__SYSTEM_ERROR_MONITOR_CORE_HPP_ #define SYSTEM_ERROR_MONITOR__SYSTEM_ERROR_MONITOR_CORE_HPP_ +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" + #include #include @@ -29,6 +31,7 @@ #include #include +#include #include #include #include @@ -144,6 +147,8 @@ class AutowareErrorMonitor : public rclcpp::Node void updateTimeoutHazardStatus(); bool canAutoRecovery() const; bool isEmergencyHoldingRequired() const; + + std::unique_ptr logger_configure_; }; #endif // SYSTEM_ERROR_MONITOR__SYSTEM_ERROR_MONITOR_CORE_HPP_ diff --git a/system/system_error_monitor/src/system_error_monitor_core.cpp b/system/system_error_monitor/src/system_error_monitor_core.cpp index 1bd7caa19a220..e74ac9c360164 100644 --- a/system/system_error_monitor/src/system_error_monitor_core.cpp +++ b/system/system_error_monitor/src/system_error_monitor_core.cpp @@ -274,6 +274,8 @@ AutowareErrorMonitor::AutowareErrorMonitor() const auto period_ns = rclcpp::Rate(params_.update_rate).period(); timer_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&AutowareErrorMonitor::onTimer, this)); + + logger_configure_ = std::make_unique(this); } void AutowareErrorMonitor::loadRequiredModules(const std::string & key) diff --git a/system/system_monitor/include/system_monitor/process_monitor/process_monitor.hpp b/system/system_monitor/include/system_monitor/process_monitor/process_monitor.hpp index e114d58770883..42bd2a3ea9236 100644 --- a/system/system_monitor/include/system_monitor/process_monitor/process_monitor.hpp +++ b/system/system_monitor/include/system_monitor/process_monitor/process_monitor.hpp @@ -88,6 +88,14 @@ class ProcessMonitor : public rclcpp::Node */ void getHighMemoryProcesses(const std::string & output); + /** + * @brief get command line from process id + * @param [in] pid process id + * @param [out] command output command line + * @return true if success to get command line name + */ + bool getCommandLineFromPiD(const std::string & pid, std::string * command); + /** * @brief get top-rated processes * @param [in] tasks list of diagnostics tasks for high load procs diff --git a/system/system_monitor/src/process_monitor/process_monitor.cpp b/system/system_monitor/src/process_monitor/process_monitor.cpp index f8ee314e5a0ef..6f58339f2ff4b 100644 --- a/system/system_monitor/src/process_monitor/process_monitor.cpp +++ b/system/system_monitor/src/process_monitor/process_monitor.cpp @@ -414,6 +414,19 @@ void ProcessMonitor::getHighMemoryProcesses(const std::string & output) getTopratedProcesses(&memory_tasks_, &p2); } +bool ProcessMonitor::getCommandLineFromPiD(const std::string & pid, std::string * command) +{ + std::string commandLineFilePath = "/proc/" + pid + "/cmdline"; + std::ifstream commandFile(commandLineFilePath); + if (commandFile.is_open()) { + std::getline(commandFile, *command); + commandFile.close(); + return true; + } else { + return false; + } +} + void ProcessMonitor::getTopratedProcesses( std::vector> * tasks, bp::pipe * p) { @@ -462,7 +475,14 @@ void ProcessMonitor::getTopratedProcesses( info.virtualImage >> info.residentSize >> info.sharedMemSize >> info.processStatus >> info.cpuUsage >> info.memoryUsage >> info.cpuTime; - std::getline(stream, info.commandName); + std::string program_name; + std::getline(stream, program_name); + + bool flag_find_command_line = getCommandLineFromPiD(info.processId, &info.commandName); + + if (!flag_find_command_line) { + info.commandName = program_name; // if command line is not found, use program name instead + } tasks->at(index)->setDiagnosticsStatus(DiagStatus::OK, "OK"); tasks->at(index)->setProcessInformation(info); @@ -515,7 +535,7 @@ void ProcessMonitor::onTimer() std::ostringstream os; // Get processes - bp::child c("top -bcn1 -o %CPU -w 256", bp::std_out > is_out, bp::std_err > is_err); + bp::child c("top -bn1 -o %CPU -w 128", bp::std_out > is_out, bp::std_err > is_err); c.wait(); if (c.exit_code() != 0) { diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp index 1d8ec80c46e17..3580f0546bbfb 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp @@ -22,6 +22,7 @@ #include "raw_vehicle_cmd_converter/brake_map.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "tier4_autoware_utils/ros/transform_listener.hpp" #include @@ -369,6 +370,8 @@ class AccelBrakeMapCalibrator : public rclcpp::Node COMMAND = 1, }; + std::unique_ptr logger_configure_; + public: explicit AccelBrakeMapCalibrator(const rclcpp::NodeOptions & node_options); }; diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp index 1910631b11e53..ecfc312d2bdfd 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp @@ -232,6 +232,8 @@ AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & nod // timer initTimer(1.0 / update_hz_); initOutputCSVTimer(30.0); + + logger_configure_ = std::make_unique(this); } void AccelBrakeMapCalibrator::initOutputCSVTimer(double period_s) diff --git a/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/node.hpp b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/node.hpp index 27346e99b60fb..98693337b5c28 100644 --- a/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/node.hpp +++ b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/node.hpp @@ -19,6 +19,7 @@ #include "raw_vehicle_cmd_converter/brake_map.hpp" #include "raw_vehicle_cmd_converter/pid.hpp" #include "raw_vehicle_cmd_converter/steer_map.hpp" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include @@ -115,6 +116,8 @@ class RawVehicleCommandConverterNode : public rclcpp::Node // for debugging rclcpp::Publisher::SharedPtr debug_pub_steer_pid_; DebugValues debug_steer_; + + std::unique_ptr logger_configure_; }; } // namespace raw_vehicle_cmd_converter diff --git a/vehicle/raw_vehicle_cmd_converter/src/node.cpp b/vehicle/raw_vehicle_cmd_converter/src/node.cpp index e45439ecb4d41..0b0e14a845200 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/node.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/node.cpp @@ -84,6 +84,8 @@ RawVehicleCommandConverterNode::RawVehicleCommandConverterNode( "~/input/steering", 1, std::bind(&RawVehicleCommandConverterNode::onSteering, this, _1)); debug_pub_steer_pid_ = create_publisher( "/vehicle/raw_vehicle_cmd_converter/debug/steer_pid", 1); + + logger_configure_ = std::make_unique(this); } void RawVehicleCommandConverterNode::publishActuationCmd() diff --git a/vehicle/vehicle_info_util/package.xml b/vehicle/vehicle_info_util/package.xml index 41cb10003a1de..139f8439d60c9 100644 --- a/vehicle/vehicle_info_util/package.xml +++ b/vehicle/vehicle_info_util/package.xml @@ -4,7 +4,6 @@ vehicle_info_util 0.1.0 The vehicle_info_util package - Yamato ANDO Taiki Tanaka @@ -12,6 +11,7 @@ Shumpei Wakabayashi Apache License 2.0 + Yamato ANDO ament_cmake_auto autoware_cmake